室内定位质心定位

CSDN上面也有相关的算法，但是有一个需要下载48积分，然而他也是从GitHub上转的，Github原文地址https://github.com/megagao/IndoorPos，这个是全部的项目，我就提取了质心定位的一部分，经测试可用。关于csdn上另一位大佬发布的处理的并不好，代码有很多没必要的地方以及漏掉的地方我已经修改过了，供大家学习，有错误请指正谢谢！附上大佬的链接https://download.csdn.net/download/cuiyf2004/9315241

GisMapTest.java

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
public class GisMapTest {
    /**
     * @param args
     */
    public static void main(String[] args) {
        // TODO 自动生成的方法存根
    	
        // d(x4,y4)
        // sqrt((x1-x4)^2+(y1-y4)^2) = X
        // sqrt((x2-x4)^2+(y2-y4)^2) = Y
        // sqrt((x3-x4)^2+(y3-y4)^2) = Z

		
    	
		Round r1=new Round(0,0,29);
		Round r2=new Round(36,0,25);
		Round r3=new Round(16,32,13);
		Coordinate zx1= tcl(r1, r2, r3);
		System.out.println(zx1.x);
		System.out.println(zx1.y);
        

  
    }

  
   
    /**
     * 三角形质心定位算法实现
     * Triangle centroid location
     * @param r1 坐标1为圆心,距离为半径
     * @param r2
     * @param r3
     * @return
     */

    public static Coordinate tcl(Round r1, Round r2, Round r3) {
        Coordinate p1 = null;// 有效交叉点1
        Coordinate p2 = null;// 有效交叉点2
        Coordinate p3 = null;// 有效交叉点3
        Coordinate zx=new Coordinate();//计算三点质心
        List<Coordinate> jds1 = jd(r1.getX(), r1.getY(), r1.getR(), r2.getX(), r2.getY(), r2.getR());// r1,r2交点
        if (jds1 != null && !jds1.isEmpty()) {
            for (Coordinate jd : jds1) {//有交点
                if (p1==null&&Math.pow(jd.getX()-r3.getX(),2) + Math.pow(jd.getY()-r3.getY(),2) <= Math.pow(r3.getR(),2)) {
                    p1 = jd;
                }else if(p1!=null){
                    if(Math.pow(jd.getX()-r3.getX(),2) + Math.pow(jd.getY()-r3.getY(),2)<= Math.pow(r3.getR(),2)){
                        if(Math.sqrt(Math.pow(jd.getX()-r3.getX(),2) + Math.pow(jd.getY()-r3.getY(),2))>Math.sqrt(Math.pow(p1.getX()-r3.getX(),2) + Math.pow(p1.getY()-r3.getY(),2))){
                            p1 = jd;
                        }
                    }
                }
            }
        } else {//没有交点定位错误
            return null;
        }
        List<Coordinate> jds2 = jd(r1.getX(), r1.getY(), r1.getR(), r3.getX(), r3.getY(), r3.getR());// r1,r2交点
        if (jds2 != null && !jds2.isEmpty()) {
            for (Coordinate jd : jds2) {//有交点
                if (p2==null&&Math.pow(jd.getX()-r2.getX(),2) + Math.pow(jd.getY()-r2.getY(),2) <= Math.pow(r2.getR(),2)) {
                    p2 = jd;

                }else if(p2!=null){
                    if(Math.pow(jd.getX()-r2.getX(),2) + Math.pow(jd.getY()-r2.getY(),2) <= Math.pow(r2.getR(),2)){
                        if(Math.pow(jd.getX()-r2.getX(),2) + Math.pow(jd.getY()-r2.getY(),2)>Math.sqrt(Math.pow(p2.getX()-r2.getX(),2) + Math.pow(p2.getY()-r2.getY(),2))){
                            p1 = jd;
                        }
                    }
                }
            }
        } else {//没有交点定位错误
            return null;
        }
        List<Coordinate> jds3 = jd(r2.getX(), r2.getY(), r2.getR(), r3.getX(), r3.getY(), r3.getR());// r1,r2交点
        if (jds3 != null && !jds3.isEmpty()) {
            for (Coordinate jd : jds3) {//有交点
                if (Math.pow(jd.getX()-r1.getX(),2) + Math.pow(jd.getY()-r1.getY(),2) <= Math.pow(r1.getR(),2)) {
                    p3 = jd;
                }else if(p3!=null){
                    if(Math.pow(jd.getX()-r1.getX(),2) + Math.pow(jd.getY()-r1.getY(),2) <= Math.pow(r1.getR(),2)){
                        if(Math.pow(jd.getX()-r1.getX(),2) + Math.pow(jd.getY()-r1.getY(),2)>Math.sqrt(Math.pow(p3.getX()-r1.getX(),2) + Math.pow(p3.getY()-r1.getY(),2))){
                            p3 = jd;
                        }
                    }
                }
            }
        } else {//没有交点定位错误
            return null;
        }

        zx.x=(p1.x+p2.x+p3.x)/3;//质心
        zx.y=(p1.y+p2.y+p3.y)/3;
        return zx;

}

    /**
     * 求两个圆的交点
     * @param x1,y1 圆心1坐标
     * @param y1
     * @param r1 半径
     * @param x2
     * @param y2
     * @param r2
     * @return
     */
    public static List<Coordinate> jd(double x1, double y1, double r1, double x2, double y2, double r2) {

        Map<Integer, double[]> p = new HashMap<Integer, double[]>();
        double d = Math.sqrt(Math.pow(x1 - x2, 2) + Math.pow(y1 - y2, 2));// 两圆心距离
        if (Math.sqrt(Math.pow(x1 - x2, 2) + Math.pow(y1 - y2, 2)) < (r1 + r2)) {// 两圆向交

        }
        List<Coordinate>points  =new ArrayList<Coordinate>();//交点坐标
        Coordinate coor;
        if (d > r1 + r2 || d < Math.abs(r1 - r2)) {//相离或内含
            return null;
        } else if (x1 == x2 && y1 == y2) {//同心圆
            return null;// 同心圆 )
        }
        else if (y1 == y2 && x1 != x2) {
            double a = ((r1 * r1 - r2 * r2) - (x1 * x1 - x2 * x2)) / (2 * x2 - 2 * x1);
            if (d == Math.abs(r1 - r2) || d == r1 + r2) {// 只有一个交点时\
                coor=new Coordinate();
                coor.x=a;
                coor.y=y1;
                points.add(coor);
            } else{// 两个交点
                double t = r1 * r1 - (a - x1) * (a - x1);
                coor=new Coordinate();
                coor.x=a;
                coor.y=y1 + Math.sqrt(t);
                points.add(coor);
                coor=new Coordinate();
                coor.x=a;
                coor.y=y1 - Math.sqrt(t);
                points.add(coor);
            }
        } else if (y1 != y2) {
            double k, disp;
            k = (2 * x1 - 2 * x2) / (2 * y2 - 2 * y1);
            disp = ((r1 * r1 - r2 * r2) - (x1 * x1 - x2 * x2) - (y1 * y1 - y2 * y2)) / (2 * y2 - 2 * y1);// 直线偏移量
            double a, b, c;
            a = (k * k + 1);
            b = (2 * (disp - y1) * k - 2 * x1);
            c = (disp - y1) * (disp - y1) - r1 * r1 + x1 * x1;
            double disc;
            disc = b * b - 4 * a * c;// 一元二次方程判别式
            if (d == Math.abs(r1 - r2) || d == r1 + r2) {
                coor=new Coordinate();
                coor.x=(-b) / (2 * a);;
                coor.y= k * coor.x + disp;
                points.add(coor);
            } else {
                coor=new Coordinate();
                coor.x=((-b) + Math.sqrt(disc)) / (2 * a);
                coor.y= k * coor.x + disp;
                points.add(coor);
                coor=new Coordinate();
                coor.x=((-b) - Math.sqrt(disc)) / (2 * a);
                coor.y= k * coor.x + disp;
                points.add(coor);
            }
        }

        return points;
    }
}

Round.java

public class Round {
    private double x;
    private double y;
    private double r;

    public Round(double x, double y, double r) {
        this.x = x;
        this.y = y;
        this.r = r;
    }

    public double getX() {
        return this.x;
    }

    public void setX(double x) {
        this.x = x;
    }

    public double getY() {
        return this.y;
    }

    public void setY(double y) {
        this.y = y;
    }

    public double getR() {
        return this.r;
    }

    public void setR(double r) {
        this.r = r;
    }
}

Corrdinate.java

	/**
	 * 坐标对象
	 * @author cuiyuanfeng
	 *
	 */
public class Coordinate {

		public double x;
		public double y;

		public Double getX() {
			return x;
		}

		public void setX(Double x) {
			this.x = x;
		}

		public Double getY() {
			return y;
		}

		public void setY(Double y) {
			this.y = y;
		}



}