diff options
| author | CNLohr <charles@cnlohr.com> | 2018-04-25 23:53:09 -0400 |
|---|---|---|
| committer | GitHub <noreply@github.com> | 2018-04-25 23:53:09 -0400 |
| commit | d47ce43f833d7382a66ba492582a497a8deb075f (patch) | |
| tree | b3fc191ff4a0749a915c4134d3bb27049b2e176d /src/poser_charlesrefine.c | |
| parent | 824811185187d2941eff054ec7be1aa2aa6ace50 (diff) | |
| parent | 5d4f2e2d9eb0548d880b130cc15733662fc57158 (diff) | |
| download | libsurvive-d47ce43f833d7382a66ba492582a497a8deb075f.tar.gz libsurvive-d47ce43f833d7382a66ba492582a497a8deb075f.tar.bz2 | |
Merge pull request #130 from abhijeetvhotkar/master
Adding UDP support
Diffstat (limited to 'src/poser_charlesrefine.c')
| -rw-r--r-- | src/poser_charlesrefine.c | 17 |
1 files changed, 15 insertions, 2 deletions
diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 12c2b9c..b35a878 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,7 @@ 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) { + if (validpoints > 1) { LinmathQuat correction; quatcopy(correction, LinmathQuat_Identity); for (i = 0; i < pts; i++) { |