室内定位三边定位

public static Point evaluateCoordinates(BigDecimal xa,BigDecimal ya, BigDecimal xb, BigDecimal yb, BigDecimal xc, BigDecimal yc, BigDecimal ra, BigDecimal rb, BigDecimal rc){

        BigDecimal a2 = ra.multiply(ra);
        BigDecimal b2 = rb.multiply(rb);
        BigDecimal c2 = rc.multiply(rc);
        BigDecimal xa2 = xa.multiply(xa);
        BigDecimal xb2 = xb.multiply(xb);
        BigDecimal xc2 = xc.multiply(xc);
        BigDecimal ya2 = ya.multiply(ya);
        BigDecimal yb2 = yb.multiply(yb);
        BigDecimal yc2 = yc.multiply(yc);
        
        BigDecimal rbc = b2.subtract(c2);
        BigDecimal xbc = xb2.subtract(xc2);
        BigDecimal ybc = yb2.subtract(yc2);
        
        BigDecimal va = rbc.subtract(xbc).subtract(ybc).divide(new BigDecimal("2.0"));
        
        BigDecimal rba = b2.subtract(a2);
        BigDecimal xba = xb2.subtract(xa2);
        BigDecimal yba = yb2.subtract(ya2);
        
        BigDecimal vb = rba.subtract(xba).subtract(yba).divide(new BigDecimal("2.0"));
        
        BigDecimal xcb = xc.subtract(xb);
        BigDecimal xab = xa.subtract(xb);
        BigDecimal yab = ya.subtract(yb);
        BigDecimal ycb = yc.subtract(yb);
        
        BigDecimal va1 = va.multiply(xab);
        BigDecimal vb1 = vb.multiply(xcb);
        BigDecimal aa = yab.multiply(xcb);
        BigDecimal ab = ycb.multiply(xab);
        
        BigDecimal y1 = vb1.subtract(va1);
        BigDecimal y2 = aa.subtract(ab);
        BigDecimal y = y1.divide(y2,10,RoundingMode.DOWN);
        
        BigDecimal x;
        if (xcb.signum() != 0) {
            x = va.subtract((y.multiply(ycb))).divide(xcb,10,RoundingMode.DOWN);
        } else {
            x = vb.subtract(y.multiply(yab)).divide(xab,10,RoundingMode.DOWN);
        }

        return new Point(x,y);
    }

 

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值