diff options
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])); } |