diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp index b295601f8a0025ec32043d2f168fa61f09a2bf69..0fb235b9988492064a466c95bbedd7ae3d2b07cd 100644 --- a/src/narrowphase/narrowphase.cpp +++ b/src/narrowphase/narrowphase.cpp @@ -1506,20 +1506,12 @@ bool boxBoxIntersect(const Box& s1, const Transform3f& tf1, if(contact_points) { - Vec3f contact_point; - if (contacts.size()>0) { + if(contacts.size() > 0) + { std::sort(contacts.begin(), contacts.end(), compareContactPoints); *contact_points = contacts[0].point; if(penetration_depth_) *penetration_depth_ = -contacts[0].depth; } - // for(size_t i = 0; i < contacts.size(); ++i) - // { - // contact_point += contacts[i].point; - // } - - // contact_point = contact_point / (FCL_REAL)contacts.size(); - - // *contact_points = contact_point; } return return_code != 0;