java 空间四点定位,可跟据已知的四点坐标(x,y,z)及距离计算所在位置坐标

      public static void main(String args[]) {
          
          try{
            
            float point[]=new float[3];
            Location loc = new Location();

            //获得坐标
            point[0] = 0;
            point[1] = 0;
            point[2] = (float) 0.5;
            loc.set_point(point,1);

            point[0] = 0;
            point[1] = -1;
            point[2] = 2;
            loc.set_point(point,2);

            point[0] = 0;
            point[1] = 1;
            point[2] = 0;
            loc.set_point(point,3);

            point[0] = 1;
            point[1] = 0;
            point[2] = 3;
            loc.set_point(point,4);

            //distance
            loc.set_distance(1,1);
            loc.set_distance(1,2);
            loc.set_distance(2,3);
            loc.set_distance(1,4);

            //calc
            float x[] = loc.calc();
            if (x == null)
            {
                System.out.println("fail");
            }
            else
            {
               System.out.println(x[0]+","+x[1]+","+ x[2]);
            }
            
          } catch(Exception ex){
              ex.printStackTrace();
          }

      }

package com.qmall.location;

public class Location {

        //空间已知4点坐标   
        float p[][] = new float[4][3];  
        //空间已知4点距离   
        float d[] = new float[4] ;  
    
      //初始化空间4点坐标   
      //p:坐标,数组   
      //num:1-4   
      void set_point(float point[],int num)  throws Exception
      {  
          int j = 0;  
        
          for (j = 0;j < 3;j++)  
          {  
              p[num - 1][j] = point[j];  
          }  
      }  
        
      //初始化空间4点距离   
      //distance:距离   
      //num:1-4   
      void set_distance(float distance,int num)  throws Exception
      {  
          d[num - 1] = distance;  
      }  
        
      //计算未知点坐标   
      //p:计算后的返回值   
      //fail:back -1   
      float[] calc()  throws Exception
      {  
          float point[]=new float[3];
              //矩阵A   
              float A[][] = new float[3][3];  
              //矩阵B   
              float B[]= new float[3];  
              int i = 0;  
              int j = 0;  
        
              //初始化B矩阵   
              for (i = 0;i < 3;i++)  
              {  
                      B[i] = (LocationMath.d_p_square(p[i + 1]) - LocationMath.d_p_square(p[i]) - (d[i + 1] * d[i + 1] - d[i] * d[i])) / 2;  
              }  
        
              //初始化A矩阵   
              for (i = 0;i < 3;i++)  
              {  
                      for (j = 0;j < 3;j++)  
                      {  
                              A[i][j] = p[i + 1][j] - p[i][j];  
                      }  
              }  
        
              //计算未知点坐标   
              point = LocationMath.solve(A,B);
              
              return point;  
      }  
      
}

package com.qmall.location;

public class LocationMath {

    
    public static void printf_matrix(float m[][]) throws Exception{
        int i = 0;  
        int j = 0;  
  
        for (i = 0;i < 3;i++)  
        {  
                for (j = 0;j < 3;j++)  
                {   
                        System.out.println(m[i*3][j]);  
                }  
        }  

    }
    
    //三维行列式的值
    //m:3 * 3数组
    public static double det(float m[][]) throws Exception{
        double value = 0.0;  
          
        value = m[0][0] * m[1][1] * m[2][2] +  
                        m[0][1] * m[1][2] * m[2][0] + 
                        m[0][2] * m[1][0] * m[2][1] -   
                        m[0][1] * m[1][0] * m[2][2] -   
                        m[0][2] * m[1][1] * m[2][0] -   
                        m[0][0] * m[1][2] * m[2][1];  
  
        return value;  
    }
    
    //将一个行列式的值赋给另一个
    //src,dst:3 * 3数组
    public static void copy_matrix(float src[][],float dst[][]) throws Exception {
        int i = 0;  
        int j = 0;  
  
        for (i = 0;i < 3;i++)  
        {  
                for (j = 0;j < 3;j++)  
                {  
                        dst[i][j] = src[i][j];  
                }  
        }  

    }
    
    //解方程
    //m:方阵,3 * 3数组
    //b:解
    //x:返回值
    //fail:back -1
    public static float[] solve(float m[][],float b[]) throws Exception {
        
        float det_m;  
        float det_m_temp;  
        float m_temp[][] = new float[3][3];  
        int i = 0;  
        int j = 0;  
        
        float x[]=new float[3];
  
        det_m = (float) det(m);  
        if (det_m == 0)  
        {  
            return null;  
        }  
        for (j = 0;j < 3;j++)  
        {  
                //得到新的行列式   
                copy_matrix(m,m_temp);  
                for (i = 0;i < 3;i++)  
                {  
                        m_temp[i][j] = b [i];  
                }  
                det_m_temp = (float) det(m_temp);  
  
                //求解   
                x[j] = det_m_temp / det_m;  
        }  
  
        return x;  

    }
    
    //计算空间点到原点距离的平方
    public static float d_p_square(float p[]) throws Exception {
        
        float d = 0;  
        int i = 0;  
  
        for (i = 0;i < 3;i++)  
        {  
                d += p[i] * p [i];  
        }  
  
        return d;  

    }

}