Sane World View
The z=0 plane is now initially aligned to the
device's z=0 plane during calibration.
(works really well for the tracker, which is
oriented with Z in the proper direction when
sitting on a horizontal surface)
@@ -895,9 +895,9 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni
static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint)
{
- posOut[0] = lhPoint.x;
- posOut[1] = lhPoint.y;
- posOut[2] = lhPoint.z;
+ posOut[0] = -lhPoint.x;
+ posOut[1] = -lhPoint.y;
+ posOut[2] = -lhPoint.z;
rotatearoundaxis(posOut, posOut, rotation, rotation[3]);
@@ -1273,8 +1273,12 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
quatrotatevector(tmpPos, rotQuat, tmpPos);
//}
+ //static int foo = 0;
+
+ //if (0 == foo)
if (setLhCalibration)
{
+ //foo = 1;
if (so->ctx->bsd[lh].PositionSet)
{
printf("Warning: resetting base station calibration data");
@@ -1297,20 +1301,22 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos);
- wcPos[0] -= so->ctx->bsd[lh].Pose.Pos[0];
- wcPos[1] -= so->ctx->bsd[lh].Pose.Pos[1];
- wcPos[2] -= so->ctx->bsd[lh].Pose.Pos[2];
+ wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0];
+ wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1];
+ wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2];
- printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]);
+ so->OutPose.Pos[0] = wcPos[0];
+ so->OutPose.Pos[1] = wcPos[1];
+ so->OutPose.Pos[2] = wcPos[2];
- //posOut =
+ printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]);
if (logFile)
{
updateHeader(logFile);
fclose(logFile);
}
- //fgetc(stdin);
+
return refinedEstimateGd;
}
@@ -1526,8 +1532,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] };
FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] };
- quatrotatevector(norm, downQuat, norm);
- quatrotatevector(point, downQuat, point);
+ //quatrotatevector(norm, downQuat, norm);
+ //quatrotatevector(point, downQuat, point);
to->sensor[sensorCount].normal.x = norm[0];
to->sensor[sensorCount].normal.y = norm[1];
