diff options
| author | Mike Turvey <mturvey6@gmail.com> | 2017-03-30 22:56:19 -0700 |
|---|---|---|
| committer | Mike Turvey <mturvey6@gmail.com> | 2017-03-30 22:56:19 -0700 |
| commit | c783f758751b9a64ca09b41e6417222a5e6cbff2 (patch) | |
| tree | d98d1ae116ee440fcbf98ef1b5b693b6c1b08f4a | |
| parent | 44ba20dbd8aa5d8a1d7624f4ce674e5e228a76e2 (diff) | |
| download | libsurvive-c783f758751b9a64ca09b41e6417222a5e6cbff2.tar.gz libsurvive-c783f758751b9a64ca09b41e6417222a5e6cbff2.tar.bz2 | |
Checking Smallest Angle
| -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]; |