ReadySteadyGo
2011-11-23 16:44:53
(* start math.simba additions *)
type
TVector = record
a: Extended;
b: Extended;
Magnitude: Extended;
end;
function fixRad(rad: Extended): Extended; // by Nava2
var
math_2pi: Extended;
begin
math_2pi := 2*pi;
result := rad;
while (result >= math_2pi) do
result := result - math_2pi;
while (result < 0) do
result := result + math_2pi;
end;
function AngleBetween(v1, v2: TVector): Extended;
var
a1, a2: Extended;
begin
a1 := ArcTan2(v1.b, v1.a);
a2 := ArcTan2(v2.b, v2.a);
Result := fixRad(a1 - a2)
end;
procedure Normalise(var vect: TVector);
begin
vect.a := vect.a / vect.Magnitude;
vect.b := vect.b / vect.Magnitude;
vect.Magnitude := 1;
end;
function CreateVector(Origin, vPoint: TPoint; NormaliseV: Boolean): TVector;
begin
Result.a := vPoint.x - Origin.x;
Result.b := 0 - (vPoint.y - Origin.y); // inverts y axis as well
Result.Magnitude := Sqrt(Sqr(Result.a) + Sqr(Result.b));
if NormaliseV then
Normalise(Result);
end;
(* end math.simba additions *)
function rs_GetCompassAngleRadians: Extended;
var
tpa: TPointArray;
atpa: T2DPointArray;
i, h, l: Integer;
VectorArray: Array of TVector;
RefPoint: TPoint;
angles: TExtendedArray;
math_2pi: Extended;
begin
math_2pi := 2*pi;
{ Colour of 3 E/S/W dots }
FindColorsTolerance(tpa, 2239277, 529, 9, 558, 39, 0);
atpa := SplitTPA(tpa, 1);
h := High(atpa);
setLength(tpa, h+1);
l := 0;
for i := 0 to h do
{ Gets rid of dots under length of 3 and get the MiddleTPA }
if high(atpa[i]) > 4 then
begin
tpa[l] := MiddleTPA(atpa[i]);
inc(l);
end;
if l < 3 then
Exit;
setLength(tpa, l);
SetLength(VectorArray, 3);
for i := 0 to 2 do
VectorArray[i] := CreateVector(Point(543, 24), tpa[i], True);
angles := [AngleBetween(VectorArray[0], VectorArray[1]),
AngleBetween(VectorArray[1], VectorArray[2]),
AngleBetween(VectorArray[2], VectorArray[0])];
for i := 0 to 2 do
begin
if angles[i] > pi then
angles[i] := math_2pi - angles[i];
{ compare angle between, > 100 then it's the south dot }
if angles[i] > radians(100) then
begin
case i of
0: RefPoint := tpa[2];
1: RefPoint := tpa[0];
2: RefPoint := tpa[1];
end;
{ break the loop, we have reference pt. }
break;
end;
end;
{ Using the south dot to measure compass angle.. }
Result := ArcTan2(-(RefPoint.Y - 24), RefPoint.X - 543) - Radians(80);
Result := fixRad(Result - Pi - 0.05*Pi);
end;
function rs_GetCompassAngleDegrees: Extended;
begin
Result := Degrees(rs_GetCompassAngleRadians());
end;I made a new thread as the other one was apparently resolved. Unfortunately, the rs_GetCompassAngle was very inaccurate, so I have written a new one.
Edit: Corrected to anticlockwise
Edit: Made Nava2's suggested changes