JAva空間の4点位置決めは、既知の4点座標(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;  



    }



}