From b3fe73dd0729a2527da4f4e3c1c641ca9d91c770 Mon Sep 17 00:00:00 2001
From: EdsterG <eddie_g_89@hotmail.com>
Date: Sat, 29 Nov 2014 19:10:26 -0800
Subject: [PATCH] Changed the point of collision returned by boxBoxIntersect.
The deepest one is returned, instead of the average.
---
src/narrowphase/narrowphase.cpp | 22 ++++++++++++++--------
1 file changed, 14 insertions(+), 8 deletions(-)
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index be133193..6da15abe 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -1249,8 +1249,9 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
pb += ub * beta;
- Vec3f pointInWorld((pa + pb) * 0.5);
- contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth));
+ // Vec3f pointInWorld((pa + pb) * 0.5);
+ // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth));
+ contacts.push_back(ContactPoint(-normal,pb,-*depth));
*return_code = code;
return 1;
@@ -1481,7 +1482,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
return cnum;
}
-
+bool compareContactPoints(const ContactPoint& c1,const ContactPoint& c2) { return c1.depth < c2.depth; } // Accending order, assuming penetration depth is a negative number!
bool boxBoxIntersect(const Box& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
@@ -1502,14 +1503,19 @@ bool boxBoxIntersect(const Box& s1, const Transform3f& tf1,
if(contact_points)
{
Vec3f contact_point;
- for(size_t i = 0; i < contacts.size(); ++i)
- {
- contact_point += contacts[i].point;
+ 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_point = contact_point / (FCL_REAL)contacts.size();
- *contact_points = contact_point;
+ // *contact_points = contact_point;
}
return return_code != 0;
--
GitLab