diff options
| -rw-r--r-- | src/poser_turveytori.c | 9 |
1 files changed, 8 insertions, 1 deletions
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 5d25294..c2d7410 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1129,6 +1129,8 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) Point avgNorm = { 0 }; + FLT smallestAngle = 20.0; + size_t pnaCount = 0; for (unsigned int i = 0; i < obj->numSensors; i++) { @@ -1143,6 +1145,11 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) //pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle); + if (pna[pnaCount].angle < smallestAngle) + { + smallestAngle = pna[pnaCount].angle; + } + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b); @@ -1202,7 +1209,7 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf(" SensorCount(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + printf(" sma(%d) SnsrCnt(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", smallestAngle, obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; |