3点測位の座標計算がうまく行かないことの改善を行いたい．

前提・実現したいこと

3点測位の座標計算を行うプログラムを製作しているが，Ｚ座標の計算は，計算サイトで計算したものと同じになるが，Ｘ座標の計算結果は同じにならず，悩んでいる．そのため，Ｘ座標の計算がうまくいくようにしたい．現在，固定値を入れて結果が正しく出るかデバッグしている．計算サイトでは．１つ目の円の中心（0,0,80）距離：79.18839と，2つ目の円の中心（50，0，0）距離63.26987で，１つ目の交点座標（75.981,0,57.69),２つ目の交点座標は，（-13.241，1.926）となる．しかし，現状では，（113.240，0.5，-1.92638），（24.019，-57.689）となっています．

該当のソースコード

C#
1using System.Collections;
2using System.Collections.Generic;
3using UnityEngine;
4using System;
5using UnityEngine.UI;
6using Mathd;
7
8public class ThreePoint : MonoBehaviour
9{
10    //入る変数
11    GameObject iBeacon;
12    //取得したscriptが入る変数
13    Test001 script;
14
15    double speed = 2.0f;
16
17   
18    //Unityは10倍
19    //Unity内での座標
20    Vector3 IB1 = new Vector3d(0, 0, 80);
21    //Unity内での座標
22    Vector3 IB2 = new Vector3d(100, 0, 80);
23    //Unity内での座標
24    Vector3 IB3 = new Vector3d(50, 0, 0);
25
26    Vector3d devicepoint;
27    Vector3d basepoint;
28
29    //RSSI
30    public int RSSI1;
31    public int RSSI2;
32    public int RSSI3;
33
34    //RSSI@1m
35    public int R1;
36    public int R2;
37    public int R3;
38
39    //距離
40    public double distance1;
41    public double distance2;
42    public double distance3;
43    public double ddistance1;
44    public double ddistance2;
45    public double ddistance3;
46
47    public static double Pow;
48    public Text textUI;
49    public Text textUUI;
50    public Text textUUUI;
51    bool onetime;
52
53    // Start is called before the first frame update
54    void Start()
55    {
56        iBeacon = GameObject.Find("IBeacon");
57        script = iBeacon.GetComponent<Test001>();
58        onetime = true;
59    }
60
61    // Update is called once per frame
62    void Update()
63    {
64        
65
66        //RSSIを持ってくる
67        RSSI1 = script.rssi;
68        RSSI2 = script.rssi1;
69        RSSI3 = script.rssi2;
70
71        //RSSI@1mを持ってくる
72        R1 = script.Strength;
73        R2 = script.Strength1;
74        R3 = script.Strength2;
75
76        distance1 = 79.18839;
77        distance2 = 105.3969;
78        distance3 = 63.26987;
79
80        IbeaconMath();
81        
82    }
83
84    private void IbeaconMath()
85    {
86        double a, aa;
87        double b, bb;
88        double c, cc;
89        double D, DD;
90        double e;
91        double X1, X2;
92        double tmpX = 0;
93        double tmpZ = 0;
94        double Z1, Z2;
95        double dis1, ddis1;
96        double dis2, ddis2;
97        double result1, result2;
98        double step;
99        step = speed * Time.deltaTime;
100
101        
102        a = 2 * (IB3.x - IB1.x);
103        b = 2 * (IB3.z - IB1.z);
104        c = ((IB1.x + IB3.x) * (IB1.x - IB3.x) + (IB1.z + IB3.z) * (IB1.z - IB3.z) + (distance3 + distance1) * (distance3 - distance1));
105        D = Math.Abs(a * IB3.x + b * IB3.z + c);
106
107        aa = Math.Pow(a, 2);
108        bb = Math.Pow(b, 2);
109        cc = Math.Pow(c, 2);
110        DD = Math.Pow(D, 2);
111        ddistance1 = Math.Pow(distance3, 2);
112        
113        //textUUUI.text = a+"\n"+aa.ToString() +"\n"+b+"\n" + bb.ToString() + "\n" +c+"\n"+ DD.ToString();
114        e = Math.Sqrt((aa + bb) * ddistance1 - DD);
115        X1 = (((a * D) - b * e) / (aa + bb)) + IB3.x;
116        Z1 = (((b * D) + a * e) / (aa + bb)) + IB3.z;
117
118        X2 = (((a * D) + b * e) / (aa + bb)) + IB3.x;
119        Z2 = (((b * D) - a * e) / (aa + bb)) + IB3.z;
120        
121        textUUUI.text = X1 + "\n" + Z1 + "\n" + X2 + "\n" + Z2;
122        dis1 = Math.Pow((IB2.x - X1), 2) + Math.Pow((IB2.z - Z1), 2);
123        ddis1 = Math.Sqrt(dis1);
124
125        dis2 = Math.Pow((IB2.x - X2), 2) + Math.Pow((IB2.z - Z2), 2);
126        ddis2 = Math.Sqrt(dis2);
127        
128        
129        
130        result1 = Math.Abs(ddis1 - distance2);
131        result2 = Math.Abs(ddis2 - distance2);
132
133        if(result2>result1)
134        {
135            devicepoint = new Vector3d(X1, 0.5, Z1);
136            tmpX = X1;
137            tmpZ = Z1;
138        }
139
140        if(result1>result2)
141        {
142            devicepoint = new Vector3d(X2, 0.5, Z2);
143            tmpX = X2;
144            tmpZ = Z2;
145            
146        }
147
148        if(onetime==true)
149        {
150            devicepoint = basepoint;
151            onetime = false;
152        }
153
154        transform.position = Vector3d.MoveTowards(basepoint, devicepoint, step);
155        basepoint = new Vector3d(tmpX, 0.5, tmpZ);
156        textUI.text = devicepoint.ToString();
157    }
158        
159}
160

試したこと

演算子と式の確認

補足情報（FW/ツールのバージョンなど）

Unity(2019.4.15f1)
C#

fana

2021/12/13 02:23 編集

何らかの演算実装（しかもどこかが間違っている）だけを示して「どんな計算を実装しているのかを読み解きつつ間違いも適切に見つけてみせてみろよ」という話？まず質問文の座標も２次元だったり３次元だったりで意味不明だし，コードでは > new Vector3d(X1, 0.5, Z1); とか唐突に固定値 0.5 が出てきてるけど，この点も他者には謎と思う．全体的に説明が足りてないし丸投げすぎる．どのような立式を行った結果を実装しているのかという話と，その理屈上の式とコードとの対応関係くらいは述べねばならんのでは？