Bullet Collision Detection & Physics Library: SphereTriangleDetector.cpp Source File

Bullet Collision Detection & Physics Library

SphereTriangleDetector.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "LinearMath/btScalar.h"
17 #include "SphereTriangleDetector.h"
18 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
19 #include "BulletCollision/CollisionShapes/btSphereShape.h"
20
21
22 SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold)
23 :m_sphere(sphere),
24 m_triangle(triangle),
25 m_contactBreakingThreshold(contactBreakingThreshold)
26 {
27
28 }
29
30 void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
31 {
32
33 (void)debugDraw;
34 const btTransform& transformA = input.m_transformA;
35 const btTransform& transformB = input.m_transformB;
36
37 btVector3 point,normal;
38 btScalar timeOfImpact = btScalar(1.);
39 btScalar depth = btScalar(0.);
40 // output.m_distance = btScalar(BT_LARGE_FLOAT);
41 //move sphere into triangle space
42 btTransform sphereInTr = transformB.inverseTimes(transformA);
43
44 if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold))
45 {
46 if (swapResults)
47 {
48 btVector3 normalOnB = transformB.getBasis()*normal;
49 btVector3 normalOnA = -normalOnB;
50 btVector3 pointOnA = transformB*point+normalOnB*depth;
51 output.addContactPoint(normalOnA,pointOnA,depth);
52 } else
53 {
54 output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
55 }
56 }
57
58 }
59
60
61
62 // See also geometrictools.com
63 // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
64 btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest);
65
66 btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
67 btVector3 diff = p - from;
68 btVector3 v = to - from;
69 btScalar t = v.dot(diff);
70
71 if (t > 0) {
72 btScalar dotVV = v.dot(v);
73 if (t < dotVV) {
74 t /= dotVV;
75 diff -= t*v;
76 } else {
77 t = 1;
78 diff -= v;
79 }
80 } else
81 t = 0;
82
83 nearest = from + t*v;
84 return diff.dot(diff);
85 }
86
87 bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) {
88 btVector3 lp(p);
89 btVector3 lnormal(normal);
90
91 return pointInTriangle(vertices, lnormal, &lp);
92 }
93
94 bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
95 {
96
97 const btVector3* vertices = &m_triangle->getVertexPtr(0);
98
99 btScalar radius = m_sphere->getRadius();
100 btScalar radiusWithThreshold = radius + contactBreakingThreshold;
101
102 btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
103
104 btScalar l2 = normal.length2();
105 bool hasContact = false;
106 btVector3 contactPoint;
107
108 if (l2 >= SIMD_EPSILON*SIMD_EPSILON)
109 {
110 normal /= btSqrt(l2);
111
112 btVector3 p1ToCentre = sphereCenter - vertices[0];
113 btScalar distanceFromPlane = p1ToCentre.dot(normal);
114
115 if (distanceFromPlane < btScalar(0.))
116 {
117 //triangle facing the other way
118 distanceFromPlane *= btScalar(-1.);
119 normal *= btScalar(-1.);
120 }
121
122 bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
123
124 // Check for contact / intersection
125
126 if (isInsideContactPlane) {
127 if (facecontains(sphereCenter, vertices, normal)) {
128 // Inside the contact wedge - touches a point on the shell plane
129 hasContact = true;
130 contactPoint = sphereCenter - normal*distanceFromPlane;
131 }
132 else {
133 // Could be inside one of the contact capsules
134 btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
135 btVector3 nearestOnEdge;
136 for (int i = 0; i < m_triangle->getNumEdges(); i++) {
137
138 btVector3 pa;
139 btVector3 pb;
140
141 m_triangle->getEdge(i, pa, pb);
142
143 btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge);
144 if (distanceSqr < contactCapsuleRadiusSqr) {
145 // Yep, we're inside a capsule
146 hasContact = true;
147 contactPoint = nearestOnEdge;
148 }
149
150 }
151 }
152 }
153 }
154
155 if (hasContact) {
156 btVector3 contactToCentre = sphereCenter - contactPoint;
157 btScalar distanceSqr = contactToCentre.length2();
158
159 if (distanceSqr < radiusWithThreshold*radiusWithThreshold)
160 {
161 if (distanceSqr>SIMD_EPSILON)
162 {
163 btScalar distance = btSqrt(distanceSqr);
164 resultNormal = contactToCentre;
165 resultNormal.normalize();
166 point = contactPoint;
167 depth = -(radius-distance);
168 } else
169 {
170 resultNormal = normal;
171 point = contactPoint;
172 depth = -radius;
173 }
174 return true;
175 }
176 }
177
178 return false;
179 }
180
181
182 bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
183 {
184 const btVector3* p1 = &vertices[0];
185 const btVector3* p2 = &vertices[1];
186 const btVector3* p3 = &vertices[2];
187
188 btVector3 edge1( *p2 - *p1 );
189 btVector3 edge2( *p3 - *p2 );
190 btVector3 edge3( *p1 - *p3 );
191
192 btVector3 p1_to_p( *p - *p1 );
193 btVector3 p2_to_p( *p - *p2 );
194 btVector3 p3_to_p( *p - *p3 );
195
196 btVector3 edge1_normal( edge1.cross(normal));
197 btVector3 edge2_normal( edge2.cross(normal));
198 btVector3 edge3_normal( edge3.cross(normal));
199
200 btScalar r1, r2, r3;
201 r1 = edge1_normal.dot( p1_to_p );
202 r2 = edge2_normal.dot( p2_to_p );
203 r3 = edge3_normal.dot( p3_to_p );
204 if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
205 ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
206 return true;
207 return false;
208
209 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:521
SphereTriangleDetector::m_contactBreakingThreshold
btScalar m_contactBreakingThreshold
Definition: SphereTriangleDetector.h:47
btDiscreteCollisionDetectorInterface::Result
Definition: btDiscreteCollisionDetectorInterface.h:31
SphereTriangleDetector.h
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
SphereTriangleDetector::m_triangle
btTriangleShape * m_triangle
Definition: SphereTriangleDetector.h:46
btSphereShape
The btSphereShape implements an implicit sphere, centered around a local origin with radius...
Definition: btSphereShape.h:22
btTriangleShape.h
btTriangleShape
Definition: btTriangleShape.h:22
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformA
btTransform m_transformA
Definition: btDiscreteCollisionDetectorInterface.h:49
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformB
btTransform m_transformB
Definition: btDiscreteCollisionDetectorInterface.h:50
SphereTriangleDetector::facecontains
bool facecontains(const btVector3 &p, const btVector3 *vertices, btVector3 &normal)
Definition: SphereTriangleDetector.cpp:87
SphereTriangleDetector::getClosestPoints
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
Definition: SphereTriangleDetector.cpp:30
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btSphereShape::getRadius
btScalar getRadius() const
Definition: btSphereShape.h:50
btTransform::inverseTimes
btTransform inverseTimes(const btTransform &t) const
Return the inverse of this transform times the other transform.
Definition: btTransform.h:230
btTriangleShape::getVertexPtr
btVector3 & getVertexPtr(int index)
Definition: btTriangleShape.h:37
output
#define output
SphereTriangleDetector::collide
bool collide(const btVector3 &sphereCenter, btVector3 &point, btVector3 &resultNormal, btScalar &depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
Definition: SphereTriangleDetector.cpp:94
SphereTriangleDetector::SphereTriangleDetector
SphereTriangleDetector(btSphereShape *sphere, btTriangleShape *triangle, btScalar contactBreakingThreshold)
Definition: SphereTriangleDetector.cpp:22
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
SphereTriangleDetector::pointInTriangle
bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p)
Definition: SphereTriangleDetector.cpp:182
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btTriangleShape::getEdge
virtual void getEdge(int i, btVector3 &pa, btVector3 &pb) const
Definition: btTriangleShape.h:56
SphereTriangleDetector::m_sphere
btSphereShape * m_sphere
Definition: SphereTriangleDetector.h:45
btDiscreteCollisionDetectorInterface::ClosestPointInput
Definition: btDiscreteCollisionDetectorInterface.h:42
btSphereShape.h
SegmentSqrDistance
btScalar SegmentSqrDistance(const btVector3 &from, const btVector3 &to, const btVector3 &p, btVector3 &nearest)
Definition: SphereTriangleDetector.cpp:66
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btScalar.h
btTriangleShape::getNumEdges
virtual int getNumEdges() const
Definition: btTriangleShape.h:51