diff options
author | Mike Turvey <mturvey6@gmail.com> | 2017-12-28 08:03:37 -0700 |
---|---|---|
committer | Mike Turvey <mturvey6@gmail.com> | 2017-12-28 08:03:37 -0700 |
commit | ec564d70daa8c1a66018f9606b02b873ae792c84 (patch) | |
tree | 74cc2f8ad167fb468abe2dfcbd026d8445762f63 /calibrate.c | |
parent | 417ced84b51bfcc392f06a87a1328f679364fc38 (diff) | |
download | libsurvive-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.c | 66 |
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])); } |