diff options
author | cnlohr <lohr85@gmail.com> | 2018-04-25 23:31:24 -0400 |
---|---|---|
committer | cnlohr <lohr85@gmail.com> | 2018-04-25 23:31:24 -0400 |
commit | 464b5b045ceb8dab976c0e4232f4112ad0db48bb (patch) | |
tree | 405d5f584719f45bc780e785aeee9eb1809fc9cf /src | |
parent | 35db8cc98e365b2cf01cf5deddee3ff3ec9c4851 (diff) | |
download | libsurvive-464b5b045ceb8dab976c0e4232f4112ad0db48bb.tar.gz libsurvive-464b5b045ceb8dab976c0e4232f4112ad0db48bb.tar.bz2 |
Fix LibSurvive poser for use with a single point (when you're totally strapped)
Diffstat (limited to 'src')
-rw-r--r-- | src/poser_charlesrefine.c | 18 |
1 files changed, 16 insertions, 2 deletions
diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 12c2b9c..5357600 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -166,12 +166,25 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { FLT dist = dot3d(sensorpos_rel_lh, sweep_normal); if ((i = dd->ptsweep) < MAX_PT_PER_SWEEP) { + int repeat = 0; + int k; + + //Detect repeated hits. a rare problem but can happen with lossy sources of pose. + for( k = 0; k < dd->ptsweep; k++ ) + { + if( dd->sensor_ids[k] == sensor_id ) + { + repeat = 1; + i = k; + } + } memcpy(dd->normal_at_errors[i], sweep_normal, sizeof(FLT) * 3); dd->quantity_errors[i] = dist; dd->angles_at_pts[i] = angle; dd->sensor_ids[i] = sensor_id; memcpy(&dd->object_pose_at_hit[i], &dd->InteralPoseUsedForCalc, sizeof(SurvivePose)); - dd->ptsweep++; + if( !repeat ) + dd->ptsweep++; } dd->last_angle_lh_axis[lhid][axis] = inangle; @@ -337,7 +350,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { // Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. // If we were able to determine likliehood of a hit in the sweep instead of afterward // we would actually be able to perform this on a per-hit basis. - if (1) { + printf( ":::%d\n", validpoints ); + if (validpoints > 1) { LinmathQuat correction; quatcopy(correction, LinmathQuat_Identity); for (i = 0; i < pts; i++) { |