From 464b5b045ceb8dab976c0e4232f4112ad0db48bb Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 25 Apr 2018 23:31:24 -0400 Subject: Fix LibSurvive poser for use with a single point (when you're totally strapped) --- src/poser_charlesrefine.c | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) (limited to 'src') 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++) { -- cgit v1.2.3