aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--calibrate.c98
-rw-r--r--redist/linmath.c39
-rw-r--r--redist/linmath.h2
-rw-r--r--src/poser_turveytori.c132
4 files changed, 198 insertions, 73 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]++;
diff --git a/redist/linmath.c b/redist/linmath.c
index b3896ff..5d51708 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -104,6 +104,45 @@ void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle)
outvec3[2] = w*(u*x + v*y + w*z)*(1-c) + z*c + (-v*x + u*y)*s;
}
+void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest)
+{
+ FLT v0[3];
+ FLT v1[3];
+ normalize3d(v0, src);
+ normalize3d(v1, dest);
+
+ FLT d = dot3d(v0, v1);// v0.dotProduct(v1);
+
+ // If dot == 1, vectors are the same
+ // If dot == -1, vectors are opposite
+ if (FLT_FABS(d - 1) < DEFAULT_EPSILON)
+ {
+ axis[0] = 0;
+ axis[1] = 1;
+ axis[2] = 0;
+ *angle = 0;
+ return;
+ }
+ else if (FLT_FABS(d + 1) < DEFAULT_EPSILON)
+ {
+ axis[0] = 0;
+ axis[1] = 1;
+ axis[2] = 0;
+ *angle = LINMATHPI;
+ return;
+ }
+
+ FLT v0Len = magnitude3d(v0);
+ FLT v1Len = magnitude3d(v1);
+
+ *angle = FLT_ACOS(d / (v0Len * v1Len));
+
+ //cross3d(c, v0, v1);
+ cross3d(axis, v1, v0);
+
+}
+
+
/////////////////////////////////////QUATERNIONS//////////////////////////////////////////
//Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman)
//Under the mit/X11 license.
diff --git a/redist/linmath.h b/redist/linmath.h
index 6f0bf60..8d6cf05 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -71,6 +71,8 @@ FLT magnitude3d(const FLT * a );
FLT anglebetween3d( FLT * a, FLT * b );
void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle);
+void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest);
+
//Quaternion things...
void quatsetnone( FLT * q );
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index b4f685c..80e8d89 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -386,7 +386,7 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
return dist;
}
-int compareFlts(const void * b, const void * a)
+int compareFlts(const void * a, const void * b)
{
FLT a2 = *(const FLT*)a;
FLT b2 = *(const FLT*)b;
@@ -399,7 +399,6 @@ FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deu
FLT resultSum = 0;
FLT *fitnesses = alloca(sizeof(FLT) * pnaCount);
- int i = 0, j = 0;
FLT worstFitness = 0;
@@ -409,12 +408,10 @@ FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deu
if (worstFitness < fitness)
{
- i = pna[i].ai;
- j = pna[i].bi;
worstFitness = fitness;
}
- fitnesses[i] = fitness;
+ fitnesses[i] = FLT_FABS(fitness);
if (deubgPrint)
{
printf(" [%d, %d](%f)\n", pna[i].ai, pna[i].bi, fitness);
@@ -1154,7 +1151,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
}
-static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput, int lh, int setLhCalibration)
+static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput,const int lh,const int setLhCalibration)
{
ToriData *toriData = so->PoserData;
@@ -1256,21 +1253,28 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
p1.z = toriData->lastLhPos[lh].z;
}
+ // refinedEstimateGd is the estimate for the location of the lighthouse in the tracked
+ // object's local coordinate system.
Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile);
FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z };
+ // here we're checking the direction of the found point against the average direction of the
+ // normal direction of the sensors that saw the light pulse.
+ // This is because there are two possible points of convergence for the tori. One is the correct
+ // location of the lighthouse. The other is in almost exactly the opposite direction.
+ // The easiest way to determine that we've converged correctly is to see if the sensors' normal
+ // are pointing in the direction of the point we've converged on.
+ // if we have converged on the wrong point, we can try to converge one more time, using a starting estimate of
+ // the point we converged on rotated to be directly opposite of its current position. Such a point
+ // is guaranteed, in practice, to converge on the other location.
+ // Note: in practice, we pretty much always converge on the correct point in the first place,
+ // but this check just makes extra sure.
FLT a1 = anglebetween3d(pf1, avgNormF);
-
if (a1 > M_PI / 2)
{
Point p2 = { .x = -refinedEstimateGd.x,.y = -refinedEstimateGd.y,.z = -refinedEstimateGd.z };
refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile);
-
- //FLT pf2[3] = { refinedEstimageGd2.x, refinedEstimageGd2.y, refinedEstimageGd2.z };
-
- //FLT a2 = anglebetween3d(pf2, avgNormF);
-
}
FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0);
@@ -1281,6 +1285,9 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
FLT rot[4]; // this is axis/ angle rotation, not a quaternion!
+ // if we've already guessed at the rotation of the lighthouse,
+ // then let's use that as a starting guess, because it's probably
+ // going to make convergence happen much faster.
if (toriData->lastLhRotAxisAngle[lh][0] != 0)
{
rot[0] = toriData->lastLhRotAxisAngle[lh][0];
@@ -1289,7 +1296,12 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
rot[3] = toriData->lastLhRotAxisAngle[lh][3];
}
-
+ // Given the relative position of the lighthouse
+ // to the tracked object, in the tracked object's coordinate
+ // system, find the rotation of the lighthouse, again in the
+ // tracked object's coordinate system.
+ // TODO: I believe this could be radically improved
+ // using an SVD.
SolveForRotation(rot, obj, refinedEstimateGd);
FLT objPos[3];
@@ -1308,9 +1320,9 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
quatfromaxisangle(rotQuat, rot, rot[3]);
//{
- FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z};
+ //FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z};
- quatrotatevector(tmpPos, rotQuat, tmpPos);
+ //quatrotatevector(tmpPos, rotQuat, tmpPos);
//}
//static int foo = 0;
@@ -1337,18 +1349,44 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
so->ctx->bsd[lh].PositionSet = 1;
}
+
FLT wcPos[3]; // position in wold coordinates
+ // Take the position of the tracked object from the lighthouse's
+ // frame of reference, and then rotate it in the reverse
+ // direction of the orientation of the lighthouse
+ // in the world reference freame.
+ // Then, change the offset based on the position of the lighthouse
+ // in the world reference frame.
+ // The result is the position of the tracked object
+ // in the world reference frame.
quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos);
-
- FLT newOrientation[4];
- //quatrotateabout(newOrientation, rotQuat, so->ctx->bsd[lh].Pose.Rot);
- quatrotateabout(newOrientation, so->ctx->bsd[lh].Pose.Rot, rotQuat);
-
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];
+ FLT newOrientation[4];
+ //quatrotateabout(newOrientation, rotQuat, so->ctx->bsd[lh].Pose.Rot); // turns the wrong way
+ quatrotateabout(newOrientation, so->ctx->bsd[lh].Pose.Rot, rotQuat); // turns the wrong way
+
+ FLT invRot[4];
+ quatgetreciprocal(invRot, rotQuat);
+ //quatrotateabout(newOrientation, invRot, so->ctx->bsd[lh].Pose.Rot); // turns correctly, rotations not aligned
+ //quatrotateabout(newOrientation, so->ctx->bsd[lh].Pose.Rot, invRot); // turns correctly, rotations not aligned
+
+ FLT invPoseRot[4];
+ quatgetreciprocal(invPoseRot, so->ctx->bsd[lh].Pose.Rot);
+
+ //quatrotateabout(newOrientation, rotQuat, invPoseRot); // turns the wrong way, rotations not aligned
+ //quatrotateabout(newOrientation, invPoseRot, rotQuat); // turns the wrong way, rotations not aligned
+
+ //FLT invRot[4];
+ //quatgetreciprocal(invRot, rotQuat);
+ //quatrotateabout(newOrientation, invRot, invPoseRot); // turns correctly, rotations aligned <-- This seems to be the best.
+ //quatrotateabout(newOrientation, invPoseRot, invRot); // turns correctly, rotations aligned, (x & y flipped?)
+
+ quatnormalize(newOrientation, newOrientation);
+
so->OutPose.Pos[0] = wcPos[0];
so->OutPose.Pos[1] = wcPos[1];
so->OutPose.Pos[2] = wcPos[2];
@@ -1470,6 +1508,25 @@ static void QuickPose(SurviveObject *so, int lh)
{
FLT pos[3], quat[4];
+ // TODO: This countdown stuff is a total hack!
+ // it basically ignores all the logic to find the most reliable data points
+ // and just grabs a sample and uses it.
+
+ //static int countdown = 5;
+
+ //if (countdown > 0 && so->ctx->objs[0] == so)
+ //{
+ // SolveForLighthouse(pos, quat, to, so, 0, lh, 1);
+ // countdown--;
+ //}
+ //else
+ //{
+ // SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
+ //}
+
+
+
+
SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
printf("!\n");
}
@@ -1584,6 +1641,11 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
//quatfrom2vectors(downQuat, negZ, td->down);
quatfrom2vectors(downQuat, td->down, negZ);
+ FLT angle;
+ FLT axis[3];
+ angleaxisfrom2vect(&angle, axis, td->down, negZ);
+ //angleaxisfrom2vect(&angle, &axis, negZ, td->down);
+
{
int sensorCount = 0;
@@ -1595,8 +1657,12 @@ 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);
+
+ //rotatearoundaxis(norm, norm, axis, angle);
+ //rotatearoundaxis(point, point, axis, angle);
+
to->sensor[sensorCount].normal.x = norm[0];
to->sensor[sensorCount].normal.y = norm[1];
@@ -1627,8 +1693,12 @@ 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);
+
+ //rotatearoundaxis(norm, norm, axis, angle);
+ //rotatearoundaxis(point, point, axis, angle);
+
to->sensor[sensorCount].normal.x = norm[0];
to->sensor[sensorCount].normal.y = norm[1];
@@ -1649,6 +1719,20 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
SolveForLighthouse(pos, quat, to, so, 0, 1, 1);
}
+
+ // This code block rotates the lighthouse fixes to accound for any time the tracked object
+ // is oriented other than +z = up
+ // This REALLY DOESN'T WORK!!!
+ //{
+ // for (int lh = 0; lh < 2; lh++)
+ // {
+ // quatrotatevector(&(so->ctx->bsd[lh].Pose.Pos[0]), downQuat, &(so->ctx->bsd[lh].Pose.Pos[0]));
+ // //quatrotateabout(&(so->ctx->bsd[lh].Pose.Rot[0]), &(so->ctx->bsd[lh].Pose.Rot[0]), downQuat);
+ // quatrotateabout(&(so->ctx->bsd[lh].Pose.Rot[0]), downQuat, &(so->ctx->bsd[lh].Pose.Rot[0]));
+ // }
+ //}
+
+
free(to);
//printf( "Full scene data.\n" );
break;