diff options
Diffstat (limited to 'calibrate.c')
-rw-r--r-- | calibrate.c | 98 |
1 files changed, 49 insertions, 49 deletions
diff --git a/calibrate.c b/calibrate.c index 8c56e43..88f8288 100644 --- a/calibrate.c +++ b/calibrate.c @@ -49,6 +49,7 @@ void HandleDestroy() //int bufferpts[32*2*3][2]; int bufferpts[32*2*3][2]; SurvivePose objPose[2]; +SurvivePose lhPose[2]; char buffermts[32*128*3]; @@ -76,6 +77,17 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int objPose[0].Rot[1] = so->FromLHPose[0].Rot[1]; objPose[0].Rot[2] = so->FromLHPose[0].Rot[2]; objPose[0].Rot[3] = so->FromLHPose[0].Rot[3]; + + lhPose[0].Pos[0] = so->ctx->bsd[0].Pose.Pos[0]; + lhPose[0].Pos[1] = so->ctx->bsd[0].Pose.Pos[1]; + lhPose[0].Pos[2] = so->ctx->bsd[0].Pose.Pos[2]; + + lhPose[0].Rot[0] = so->ctx->bsd[0].Pose.Rot[0]; + lhPose[0].Rot[1] = so->ctx->bsd[0].Pose.Rot[1]; + lhPose[0].Rot[2] = so->ctx->bsd[0].Pose.Rot[2]; + lhPose[0].Rot[3] = so->ctx->bsd[0].Pose.Rot[3]; + + //quatgetreciprocal(lhPose[0].Rot, lhPose[0].Rot); } if (so == so->ctx->objs[0] && so->FromLHPose[1].Pos[0] != 0) { @@ -87,6 +99,18 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int objPose[1].Rot[1] = so->FromLHPose[1].Rot[1]; objPose[1].Rot[2] = so->FromLHPose[1].Rot[2]; objPose[1].Rot[3] = so->FromLHPose[1].Rot[3]; + + lhPose[1].Pos[0] = so->ctx->bsd[1].Pose.Pos[0]; + lhPose[1].Pos[1] = so->ctx->bsd[1].Pose.Pos[1]; + lhPose[1].Pos[2] = so->ctx->bsd[1].Pose.Pos[2]; + + lhPose[1].Rot[0] = so->ctx->bsd[1].Pose.Rot[0]; + lhPose[1].Rot[1] = so->ctx->bsd[1].Pose.Rot[1]; + lhPose[1].Rot[2] = so->ctx->bsd[1].Pose.Rot[2]; + lhPose[1].Rot[3] = so->ctx->bsd[1].Pose.Rot[3]; + + //quatgetreciprocal(lhPose[1].Rot, lhPose[1].Rot); + } if( acode % 2 == 0 && lh == 0) //data = 0 @@ -135,8 +159,8 @@ char* sensor_name[32]; void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) { const FLT toScale = 2.0 / yResolution; // 2 meters across yResolution pixels - const int windowCenterX = xResolution / 2; - const int windowCenterY = yResolution / 2; + const int windowCenterX = (int)xResolution / 2; + const int windowCenterY = (int)yResolution / 2; const FLT sizeScale = 100.0; // used to indicate change in size with change in Z const int minRectSize = 10; // size at z=0 @@ -149,10 +173,10 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) int x1, x2, y1, y2; - x1 = windowCenterX - minRectSize - ((pose.Pos[2] * 40.0)) + (pose.Pos[0] * sizeScale); - y1 = windowCenterY - minRectSize - ((pose.Pos[2] * 40.0)) + (pose.Pos[1] * sizeScale); - x2 = windowCenterX + minRectSize + ((pose.Pos[2] * 40.0)) + (pose.Pos[0] * sizeScale); - y2 = windowCenterY + minRectSize + ((pose.Pos[2] * 40.0)) + (pose.Pos[1] * sizeScale); + x1 = (int)(windowCenterX - minRectSize - ((pose.Pos[2] * 40.0)) + (pose.Pos[0] * sizeScale)); + y1 = (int)(windowCenterY - minRectSize - ((pose.Pos[2] * 40.0)) + (pose.Pos[1] * sizeScale)); + x2 = (int)(windowCenterX + minRectSize + ((pose.Pos[2] * 40.0)) + (pose.Pos[0] * sizeScale)); + y2 = (int)(windowCenterY + minRectSize + ((pose.Pos[2] * 40.0)) + (pose.Pos[1] * sizeScale)); FLT xCenter = windowCenterX + (pose.Pos[0] * sizeScale); FLT yCenter = windowCenterY + (pose.Pos[1] * sizeScale); @@ -173,19 +197,19 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) tmp1[1] = 0; tmp1[2] = 0; - quatrotatevector(&tmp1out, pose.Rot, tmp1); + quatrotatevector(tmp1out, pose.Rot, tmp1); tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)); tmp2[1] = 0; tmp2[2] = 0; - quatrotatevector(&tmp2out, pose.Rot, tmp2); + quatrotatevector(tmp2out, pose.Rot, tmp2); CNFGTackSegment( - windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0], - windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1], - windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0], - windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]); + (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), + 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])); } // line for the (0,-y) to (0,+y)) @@ -199,19 +223,19 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) tmp1[1] = -minRectSize - ((pose.Pos[2] * 40.0)); tmp1[2] = 0; - quatrotatevector(&tmp1out, pose.Rot, tmp1); + quatrotatevector(tmp1out, pose.Rot, tmp1); tmp2[0] = 0; tmp2[1] = minRectSize + ((pose.Pos[2] * 40.0)); tmp2[2] = 0; - quatrotatevector(&tmp2out, pose.Rot, tmp2); + quatrotatevector(tmp2out, pose.Rot, tmp2); CNFGTackSegment( - windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0], - windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1], - windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0], - windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]); + (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), + 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])); } // Small line to indicate (0,+y) @@ -225,19 +249,19 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) tmp1[2] = 0; tmp1[0] = tmp1[1] * 0.3; - quatrotatevector(&tmp1out, pose.Rot, tmp1); + quatrotatevector(tmp1out, pose.Rot, tmp1); tmp2[1] = minRectSize + ((pose.Pos[2] * 40.0)); tmp2[2] = 0; tmp2[0] = -(tmp1[1] * 0.3); - quatrotatevector(&tmp2out, pose.Rot, tmp2); + quatrotatevector(tmp2out, pose.Rot, tmp2); CNFGTackSegment( - windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0], - windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1], - windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0], - windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]); + (short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]), + 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])); } @@ -291,33 +315,9 @@ void * GuiThread( void * jnk ) for (int lh = 0; lh < 2; lh++) { DisplayPose(objPose[lh], 640, 480); + DisplayPose(lhPose[lh], 640, 480); - //if (objPose[lh].Pos[0] != 0) - //{ - // const FLT toScale = 2.0 / 480.0; // 2 meters across 480 pixels - // const int centerX = 640 / 2; - // const int centerY = 480 / 2; - - // const FLT sizeScale = 1.0 / 40.0; - // const int minRectSize = 10; - - // uint8_t r = 0xff; - // uint8_t g = 0xff; - // uint8_t b = 0xff; - - // CNFGColor((b << 16) | (g << 8) | r); - - // int x1, x2, y1, y2; - - // x1 = centerX - minRectSize - ((objPose[lh].Pos[2] * 40.0)) + (objPose[lh].Pos[0] * 200.0); - // y1 = centerY - minRectSize - ((objPose[lh].Pos[2] * 40.0)) + (objPose[lh].Pos[1] * 200.0); - // x2 = centerX + minRectSize + ((objPose[lh].Pos[2] * 40.0)) + (objPose[lh].Pos[0] * 200.0); - // y2 = centerY + minRectSize + ((objPose[lh].Pos[2] * 40.0)) + (objPose[lh].Pos[1] * 200.0); - - // CNFGTackRectangle(x1, y1, x2, y2); - //} } - //CNFGTackRectangle(bufferpts[i * 2 + 0][nn], bufferpts[i * 2 + 1][nn], bufferpts[i * 2 + 0][nn] + 5, bufferpts[i * 2 + 1][nn] + 5); buffertimeto[i][nn]++; |