aboutsummaryrefslogtreecommitdiff
path: root/calibrate.c
diff options
context:
space:
mode:
Diffstat (limited to 'calibrate.c')
-rw-r--r--calibrate.c98
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]++;