算法一圈

Question

鑑於以下系統行進位置的三角測量：

其中：

• A：點上圓的邊緣與半徑r任何地方存在xz平面。
• θ：正x軸與從原點到點A的矢量之間的夾角。這應該從-PI/2到PI/2。
• B1：圓點與正x軸交點處的高度爲h1的點。
• B2：圓點和正z軸在h2高度的交點處的點。
• d1：B1和A之間的距離。
• d2：B2和A之間的距離。

假設：

• h1，h2，和r是已知常數。
• d1和d2是已知的變量。

我該如何找到θ？

這將最終用C在嵌入式系統在那裏我有相當快的功能arctan2，sine和cosine實施。因此，性能絕對是一個優先事項，如果它們對於小數點後三位是正確的（這是我的三角函數的準確度），則可以使用估計。

然而，即使給定了一個數學算法，我相信我可以制定出具體的實現。

對於它的價值，我得到了儘可能：

(d1^2 - h1^2)/r = (sin(θ))^2  + (cos(θ))^2 
(d2^2 - h2^2)/r = (sin(PI/4 - θ))^2 + (cos(PI/4 - θ))^2 

之前，我意識到，數學，這是方式我配不上。

Answer 1

這不是一個完整的答案，而是一個開始。

您可以進行兩種簡單的簡化。

設H1和H2爲B1和B2以下的平面中的點。 既然你知道h1和d1，h2和d2，你可以計算2個距離A-H1和A-H2（與畢達哥拉斯）。 現在你已經減少了拼圖飛機。

此外，你並不需要看H1和H2。給定距離A-H1，A只有2個可能的位置，這些位置在x軸上鏡像。然後，通過查看A-H2距離是高於還是低於閾值距離H2-H1，可以找到它們中的哪一個。

這似乎是一個良好的開端:-)

Answer 2

用人@Rhialto，額外的簡化和試驗角落情況：

// c1 is the signed chord distance A to (B1 projected to the xz plane) 
// c1*c1 + h1*h1 = d1*d1 
// c1 = +/- sqrt(d1*d1 - h1*h1) (choose sign later) 
// c1 = Cord(r, theta) = fabs(r*2*sin(theta/2)) 
// theta = asin(c1/(r*2))*2 
// 
// theta is < 0 when d2 > sqrt(h2*h2 + sqrt(2)*r*sqrt(2)*r) 
// theta is < 0 when d2 > sqrt(h2*h2 + 2*r*r) 
// theta is < 0 when d2*d2 > h2*h2 + 2*r*r 

#define h1 (0.1) 
#define h2 (0.25) 
#define r (1.333) 

#define h1Squared (h1*h1) 
#define rSquared (r*r) 
#define rSquaredTimes2 (r*r*2) 
#define rTimes2 (r*2) 
#define d2Big (h2*h2 + 2*r*r) 

// Various steps to avoid issues with d1 < 0, d2 < 0, d1 ~= h1 and theta near pi 
double zashu(double d1, double d2) { 
    double c1Squared = d1*d1 - h1Squared; 
    if (c1Squared < 0.0) 
    c1Squared = 0.0; // _May_ be needed when in select times |d1| ~= |h1| 
    double a = sqrt(c1Squared)/rTimes2; 
    double theta = (a <= 1.0) ? asin(a)*2.0 : asin(1.0)*2.0; // Possible a is _just_ greater than 1.0 
    if (d2*d2 > d2Big) // this could be done with fabs(d2) > pre_computed_sqrt(d2Big) 
    theta = -theta; 
    return theta; 
}