private void Get2CirclesIntersections(TargetData target1, TargetData target2, out double[] intersect1, out double[] intersect2)
{
double[] P0 = target1.Position;
double r0 = target1.HzDist;
double[] P1 = target2.Position;
double r1 = target2.HzDist;
double d, a, h;
double[] P2;
//חישוב המרחק בין הנקודות ובירור האם יש פתרון
d = Math.Sqrt(Math.Pow(P1[0] - P0[0], 2) + Math.Pow(P1[1] - P0[1], 2));
if
(
(d > r0 + r1) //המעגלים אינם נחתכים
||
(d < Math.Abs(r0 - r1)) //המעגלים מוכלים
||
((d == 0) && (r0 == r1)) //המעגלים חופפים
)
{
throw new Exception("No Solution");
}
//חישוב ערכי ביניים לצורך הפתרון
a = (Math.Pow(r0, 2) - Math.Pow(r1, 2) + Math.Pow(d, 2)) / (2 * d);
h = Math.Sqrt(Math.Pow(r0, 2) - Math.Pow(a, 2));
P2 = new double[]
{
P0[0] + (a / d) * (P1[0] - P0[0]),
P0[1] + (a / d) * (P1[1] - P0[1])
};
//מצא את גובה התחנה, על ידי ממוצע מדידות המטרות
double stZ = ((target1.Position[2] + target2.Position[2]) - (target1.VDist + target2.VDist)) / 2;
//חישוב נקודות החיתוך
intersect1 = new double[]
{
P2[0] + (h / d) * (P1[1] - P0[1]),
P2[1] - (h / d) * (P1[0] - P0[0]),
stZ
};
intersect2 = new double[]
{
P2[0] - (h / d) * (P1[1] - P0[1]),
P2[1] + (h / d) * (P1[0] - P0[0]),
stZ
};
}
private Station DecideByAngles(TargetData target1, TargetData target2, double[] st1, double[] st2)
{
double[] selectedSt;
double selectedAngle;
//מציאת הזוית שנמדדה בין שתי המטרות הסדורות
//הזוית מוגדרת כגדלה נגד כיוון השעון
//בנוסף אנו מחשבים רק זויות חיוביות
double t1t2Ang = target2.HzAngle - target1.HzAngle;
if (t1t2Ang < 0)
{
t1t2Ang += Math.PI * 2;
}
//מציאת הזוית בין שתי המטרות מנקודת מבט הפתרון הראשון
double st1Ang = GetAngle(st1, target1.Position, target2.Position);
//מציאת הזוית בין שתי המטרות מנקודת מבט הפתרון השני
double st2Ang = GetAngle(st2, target1.Position, target2.Position);
//בחירת הפתרון על פי הזוית
//הפתרון שיבחר הוא זה שנמצא באותו צד עם הזוית שנמדדה
if (Math.Sign(t1t2Ang - Math.PI) == Math.Sign(st1Ang - Math.PI))
{
selectedSt = st1;
selectedAngle = st1Ang;
log.Add("...............Selected: #1");
}
else
{
selectedSt = st2;
selectedAngle = st2Ang;
log.Add("...............Selected: #2");
}
Station SelectedStation = new Station(selectedSt);
//בדיקת הדיוק שהושג בזוית
SelectedStation.AngleDeviation = Math.Abs(1 - (selectedAngle / t1t2Ang));
if (SelectedStation.AngleDeviation > this.AngleMaxDev)
{
//הורד את דירוג המטרות, מכיוון שכנראה אחת מהן לא נמדדה באופן מדויק
target1.Evaluation -= 1;
target2.Evaluation -= 1;
throw new Exception("Angle Deviation Too high: " + SelectedStation.AngleDeviation.ToString("0.00000"));
}
return(SelectedStation);
}
private Station GetStationBy2Targets(TargetData target1, TargetData target2)
{
//קבלת שתי נקודות החיתוך של המעגלים
double[] intersect1, intersect2;
Get2CirclesIntersections(target1, target2, out intersect1, out intersect2);
log.Add("Intersection No. 1:");
log.Add(intersect1[0].ToString("0.00000") + ",");
log.Add(intersect1[1].ToString("0.00000") + ",");
log.Add(intersect1[2].ToString("0.00000"));
log.Add("Intersection No. 2:");
log.Add(intersect2[0].ToString("0.00000") + ",");
log.Add(intersect2[1].ToString("0.00000") + ",");
log.Add(intersect2[2].ToString("0.00000"));
//בחירת הנקודה המתאימה על פי הזוויות
return(DecideByAngles(target1, target2, intersect1, intersect2));
}