summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp')
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp148
1 files changed, 76 insertions, 72 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
index e5bac8438e..7647f67360 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
@@ -18,94 +18,95 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
-
-SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold)
-:m_sphere(sphere),
-m_triangle(triangle),
-m_contactBreakingThreshold(contactBreakingThreshold)
+SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold)
+ : m_sphere(sphere),
+ m_triangle(triangle),
+ m_contactBreakingThreshold(contactBreakingThreshold)
{
-
}
-void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
+void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults)
{
-
(void)debugDraw;
const btTransform& transformA = input.m_transformA;
const btTransform& transformB = input.m_transformB;
- btVector3 point,normal;
+ btVector3 point, normal;
btScalar timeOfImpact = btScalar(1.);
btScalar depth = btScalar(0.);
-// output.m_distance = btScalar(BT_LARGE_FLOAT);
+ // output.m_distance = btScalar(BT_LARGE_FLOAT);
//move sphere into triangle space
- btTransform sphereInTr = transformB.inverseTimes(transformA);
+ btTransform sphereInTr = transformB.inverseTimes(transformA);
- if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold))
+ if (collide(sphereInTr.getOrigin(), point, normal, depth, timeOfImpact, m_contactBreakingThreshold))
{
if (swapResults)
{
- btVector3 normalOnB = transformB.getBasis()*normal;
+ btVector3 normalOnB = transformB.getBasis() * normal;
btVector3 normalOnA = -normalOnB;
- btVector3 pointOnA = transformB*point+normalOnB*depth;
- output.addContactPoint(normalOnA,pointOnA,depth);
- } else
+ btVector3 pointOnA = transformB * point + normalOnB * depth;
+ output.addContactPoint(normalOnA, pointOnA, depth);
+ }
+ else
{
- output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
+ output.addContactPoint(transformB.getBasis() * normal, transformB * point, depth);
}
}
-
}
-
-
// See also geometrictools.com
// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
-btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest);
+btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest);
-btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
+btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest)
+{
btVector3 diff = p - from;
btVector3 v = to - from;
btScalar t = v.dot(diff);
-
- if (t > 0) {
+
+ if (t > 0)
+ {
btScalar dotVV = v.dot(v);
- if (t < dotVV) {
+ if (t < dotVV)
+ {
t /= dotVV;
- diff -= t*v;
- } else {
+ diff -= t * v;
+ }
+ else
+ {
t = 1;
diff -= v;
}
- } else
+ }
+ else
t = 0;
- nearest = from + t*v;
- return diff.dot(diff);
+ nearest = from + t * v;
+ return diff.dot(diff);
}
-bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) {
+bool SphereTriangleDetector::facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal)
+{
btVector3 lp(p);
btVector3 lnormal(normal);
-
+
return pointInTriangle(vertices, lnormal, &lp);
}
-bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
+bool SphereTriangleDetector::collide(const btVector3& sphereCenter, btVector3& point, btVector3& resultNormal, btScalar& depth, btScalar& timeOfImpact, btScalar contactBreakingThreshold)
{
-
const btVector3* vertices = &m_triangle->getVertexPtr(0);
-
+
btScalar radius = m_sphere->getRadius();
btScalar radiusWithThreshold = radius + contactBreakingThreshold;
- btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
+ btVector3 normal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]);
btScalar l2 = normal.length2();
bool hasContact = false;
btVector3 contactPoint;
- if (l2 >= SIMD_EPSILON*SIMD_EPSILON)
+ if (l2 >= SIMD_EPSILON * SIMD_EPSILON)
{
normal /= btSqrt(l2);
@@ -120,54 +121,59 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
}
bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
-
+
// Check for contact / intersection
-
- if (isInsideContactPlane) {
- if (facecontains(sphereCenter, vertices, normal)) {
+
+ if (isInsideContactPlane)
+ {
+ if (facecontains(sphereCenter, vertices, normal))
+ {
// Inside the contact wedge - touches a point on the shell plane
hasContact = true;
- contactPoint = sphereCenter - normal*distanceFromPlane;
+ contactPoint = sphereCenter - normal * distanceFromPlane;
}
- else {
+ else
+ {
// Could be inside one of the contact capsules
- btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
+ btScalar contactCapsuleRadiusSqr = radiusWithThreshold * radiusWithThreshold;
btScalar minDistSqr = contactCapsuleRadiusSqr;
btVector3 nearestOnEdge;
- for (int i = 0; i < m_triangle->getNumEdges(); i++) {
-
+ for (int i = 0; i < m_triangle->getNumEdges(); i++)
+ {
btVector3 pa;
btVector3 pb;
m_triangle->getEdge(i, pa, pb);
btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge);
- if (distanceSqr < minDistSqr) {
+ if (distanceSqr < minDistSqr)
+ {
// Yep, we're inside a capsule, and record the capsule with smallest distance
minDistSqr = distanceSqr;
hasContact = true;
contactPoint = nearestOnEdge;
}
-
}
}
}
}
- if (hasContact) {
+ if (hasContact)
+ {
btVector3 contactToCentre = sphereCenter - contactPoint;
btScalar distanceSqr = contactToCentre.length2();
- if (distanceSqr < radiusWithThreshold*radiusWithThreshold)
+ if (distanceSqr < radiusWithThreshold * radiusWithThreshold)
{
- if (distanceSqr>SIMD_EPSILON)
+ if (distanceSqr > SIMD_EPSILON)
{
btScalar distance = btSqrt(distanceSqr);
resultNormal = contactToCentre;
resultNormal.normalize();
point = contactPoint;
- depth = -(radius-distance);
- } else
+ depth = -(radius - distance);
+ }
+ else
{
resultNormal = normal;
point = contactPoint;
@@ -176,36 +182,34 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
return true;
}
}
-
+
return false;
}
-
-bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
+bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p)
{
const btVector3* p1 = &vertices[0];
const btVector3* p2 = &vertices[1];
const btVector3* p3 = &vertices[2];
- btVector3 edge1( *p2 - *p1 );
- btVector3 edge2( *p3 - *p2 );
- btVector3 edge3( *p1 - *p3 );
+ btVector3 edge1(*p2 - *p1);
+ btVector3 edge2(*p3 - *p2);
+ btVector3 edge3(*p1 - *p3);
+
+ btVector3 p1_to_p(*p - *p1);
+ btVector3 p2_to_p(*p - *p2);
+ btVector3 p3_to_p(*p - *p3);
- btVector3 p1_to_p( *p - *p1 );
- btVector3 p2_to_p( *p - *p2 );
- btVector3 p3_to_p( *p - *p3 );
+ btVector3 edge1_normal(edge1.cross(normal));
+ btVector3 edge2_normal(edge2.cross(normal));
+ btVector3 edge3_normal(edge3.cross(normal));
- btVector3 edge1_normal( edge1.cross(normal));
- btVector3 edge2_normal( edge2.cross(normal));
- btVector3 edge3_normal( edge3.cross(normal));
-
btScalar r1, r2, r3;
- r1 = edge1_normal.dot( p1_to_p );
- r2 = edge2_normal.dot( p2_to_p );
- r3 = edge3_normal.dot( p3_to_p );
- if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
- ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
+ r1 = edge1_normal.dot(p1_to_p);
+ r2 = edge2_normal.dot(p2_to_p);
+ r3 = edge3_normal.dot(p3_to_p);
+ if ((r1 > 0 && r2 > 0 && r3 > 0) ||
+ (r1 <= 0 && r2 <= 0 && r3 <= 0))
return true;
return false;
-
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-21 15:09:16 +0000