aboutsummaryrefslogtreecommitdiff
path: root/calibrate.c
diff options
context:
space:
mode:
authorMike Turvey <mturvey6@gmail.com>2017-12-28 08:03:37 -0700
committerMike Turvey <mturvey6@gmail.com>2017-12-28 08:03:37 -0700
commitec564d70daa8c1a66018f9606b02b873ae792c84 (patch)
tree74cc2f8ad167fb468abe2dfcbd026d8445762f63 /calibrate.c
parent417ced84b51bfcc392f06a87a1328f679364fc38 (diff)
downloadlibsurvive-ec564d70daa8c1a66018f9606b02b873ae792c84.tar.gz
libsurvive-ec564d70daa8c1a66018f9606b02b873ae792c84.tar.bz2
Start work on determining rotation using quaternions only
Rotation was previously approximated using axis/angle This change starts down the path of using quaternions exclusively. This change appears to give at least as good as answers as the axis/angle model in basic cases (also only tested with 1 lighthouse), but it is currently much slower and runs in unpredictable time.
Diffstat (limited to 'calibrate.c')
-rw-r--r--calibrate.c66
1 files changed, 58 insertions, 8 deletions
diff --git a/calibrate.c b/calibrate.c
index 8fb0986..cfd3e17 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -207,9 +207,9 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution)
CNFGTackSegment(
(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
- yResolution-(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+ (short)yResolution-(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
- yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+ (short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
}
// line for the (0,-y) to (0,+y))
@@ -233,11 +233,36 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution)
CNFGTackSegment(
(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
- yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+ (short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
- yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+ (short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
}
+ // Small line to indicate (0,+y)
+ {
+ FLT tmp1[3];
+ FLT tmp1out[3];
+ FLT tmp2[3];
+ FLT tmp2out[3];
+
+ tmp1[1] = minRectSize + ((pose.Pos[2] * 40.0)*1.3);
+ tmp1[2] = 0;
+ tmp1[0] = ((pose.Pos[2] * 40.0)) * 0.3;
+
+ quatrotatevector(tmp1out, pose.Rot, tmp1);
+
+ tmp2[1] = minRectSize + ((pose.Pos[2] * 40.0)*0.7);
+ tmp2[2] = 0;
+ tmp2[0] = -(((pose.Pos[2] * 40.0)) * 0.3);
+
+ quatrotatevector(tmp2out, pose.Rot, tmp2);
+
+ CNFGTackSegment(
+ (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
+ (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+ (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
+ (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+ }
// Small line to indicate (+x,0)
{
FLT tmp1[3];
@@ -245,13 +270,38 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution)
FLT tmp2[3];
FLT tmp2out[3];
- tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0));
+ tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0)*1.3);
tmp1[2] = 0;
tmp1[1] = tmp1[0] * 0.3;
quatrotatevector(tmp1out, pose.Rot, tmp1);
- tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0));
+ tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)*.7);
+ tmp2[2] = 0;
+ tmp2[1] = -(tmp1[0] * 0.3);
+
+ quatrotatevector(tmp2out, pose.Rot, tmp2);
+
+ CNFGTackSegment(
+ (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
+ (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+ (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
+ (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+ }
+ // Small line to indicate (+x,0)
+ {
+ FLT tmp1[3];
+ FLT tmp1out[3];
+ FLT tmp2[3];
+ FLT tmp2out[3];
+
+ tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0)*.7);
+ tmp1[2] = 0;
+ tmp1[1] = tmp1[0] * 0.3;
+
+ quatrotatevector(tmp1out, pose.Rot, tmp1);
+
+ tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)*1.3);
tmp2[2] = 0;
tmp2[1] = -(tmp1[0] * 0.3);
@@ -259,9 +309,9 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution)
CNFGTackSegment(
(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
- yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+ (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
- yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+ (short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
}