算法3的重心计算方法改为和算法4一样

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