From f8e110b9f10e613087273801113bf3cfa6fe787e Mon Sep 17 00:00:00 2001
From: weidong <weidong@im-droid.com>
Date: Fri, 19 Jul 2024 10:19:33 +0800
Subject: [PATCH] =?UTF-8?q?=E7=AE=97=E6=B3=953=E7=9A=84=E9=87=8D=E5=BF=83?=
 =?UTF-8?q?=E8=AE=A1=E7=AE=97=E6=96=B9=E6=B3=95=E6=94=B9=E4=B8=BA=E5=92=8C?=
 =?UTF-8?q?=E7=AE=97=E6=B3=954=E4=B8=80=E6=A0=B7?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../sideslope/bd/FocusCalculator3.java        | 50 +++++++++++++++++--
 1 file changed, 47 insertions(+), 3 deletions(-)

diff --git a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator3.java b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator3.java
index 5f1ee2ab..f300c85a 100644
--- a/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator3.java
+++ b/sec-beidou-rtcm/src/main/java/com/imdroid/sideslope/bd/FocusCalculator3.java
@@ -15,7 +15,7 @@ public class FocusCalculator3 extends FocusCalculator1{
         super();
         this.bsDevice = bsDevice;
     }
-
+/*
     static class EComparator implements Comparator<double[]>{
          @Override
          public int compare(double[] point1, double[] point2) {
@@ -39,7 +39,7 @@ public class FocusCalculator3 extends FocusCalculator1{
             //return (int) ((point1[2] - point2[2])*100);
         }
     }
-
+*/
     @Override
     public void addGGA(Gga gga) {
         if(gga == null) return;
@@ -90,7 +90,7 @@ public class FocusCalculator3 extends FocusCalculator1{
 
     }
 
-    double calcGravity(List<double[]> list , int index){
+    /*double calcGravity(List<double[]> list , int index){
         double sum = 0;
         int begin = (int) (list.size() * 0.25);
         int end = (int) (list.size() * 0.75);
@@ -115,6 +115,50 @@ public class FocusCalculator3 extends FocusCalculator1{
         }
 
         return null;
+    }*/
+
+    @Override
+    public double[] resultB562(){
+        try {
+            if (pointList.size() >= gravityMinCount) {
+                List<Point> selectPoints = new ArrayList<>();
+                if (referPoint != null && referPoint.length > 0) {
+                    // 计算所有点位与变量之间的距离，并存入集合
+                    for (double[] point : pointList) {
+                        Point pointObj = new Point(point[0], point[1], point[2]);
+                        pointObj.setXDistance(Math.abs(point[0] - referPoint[0])); // 设置该点到变量的水平距离
+                        pointObj.setYDistance(Math.abs(point[1] - referPoint[1])); // 设置该点到变量的水平距离
+                        pointObj.setZDistance(Math.abs(point[2] - referPoint[2])); // 设置该点到变量的垂直距离
+                        selectPoints.add(pointObj); // 将点位加入集合
+                    }
+                    //分别对东北天排序
+                    Collections.sort(selectPoints, Comparator.comparing(Point::getXDistance));//排序
+                    List<Point> points = selectPoints.subList(0, selectPoints.size() / 2);
+                    double[] focusX = focusPointObj(points);
+                    Collections.sort(selectPoints, Comparator.comparing(Point::getYDistance));//排序
+                    points = selectPoints.subList(0, selectPoints.size() / 2);
+                    double[] focusY = focusPointObj(points);
+                    Collections.sort(selectPoints, Comparator.comparing(Point::getZDistance));//排序
+                    points = selectPoints.subList(0, selectPoints.size() / 2);
+                    double[] focusZ = focusPointObj(points);
+                    return new double[]{focusX[0], focusY[1], focusZ[2]};
+
+                }
+                else {
+                    for (int i = pointList.size()/2; i < pointList.size(); i++) {
+                        selectPoints.add(new Point(pointList.get(i)[0], pointList.get(i)[1], pointList.get(i)[2]));
+                    }
+                    // 求重心
+                    double[] focus = focusPointObj(selectPoints);
+                    return focus;
+                }
+            }
+        }
+        catch (Exception e){
+            logger.error("error!",e);
+            return null;
+        }
+        return resultB562Sub();
     }
 
     @Override