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 SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold)
22 : m_sphere(sphere),
23 m_triangle(triangle),
24 m_contactBreakingThreshold(contactBreakingThreshold)
25 {
26 }
27
28 void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults)
29 {
30 (void)debugDraw;
31 const btTransform& transformA = input.m_transformA;
32 const btTransform& transformB = input.m_transformB;
33
34 btVector3 point, normal;
35 btScalar timeOfImpact = btScalar(1.);
36 btScalar depth = btScalar(0.);
37 // output.m_distance = btScalar(BT_LARGE_FLOAT);
38 //move sphere into triangle space
39 btTransform sphereInTr = transformB.inverseTimes(transformA);
40
41 if (collide(sphereInTr.getOrigin(), point, normal, depth, timeOfImpact, m_contactBreakingThreshold))
42 {
43 if (swapResults)
44 {
45 btVector3 normalOnB = transformB.getBasis() * normal;
46 btVector3 normalOnA = -normalOnB;
47 btVector3 pointOnA = transformB * point + normalOnB * depth;
48 output.addContactPoint(normalOnA, pointOnA, depth);
49 }
50 else
51 {
52 output.addContactPoint(transformB.getBasis() * normal, transformB * point, depth);
53 }
54 }
55 }
56
57 // See also geometrictools.com
58 // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
59 btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest);
60
61 btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest)
62 {
63 btVector3 diff = p - from;
64 btVector3 v = to - from;
65 btScalar t = v.dot(diff);
66
67 if (t > 0)
68 {
69 btScalar dotVV = v.dot(v);
70 if (t < dotVV)
71 {
72 t /= dotVV;
73 diff -= t * v;
74 }
75 else
76 {
77 t = 1;
78 diff -= v;
79 }
80 }
81 else
82 t = 0;
83
84 nearest = from + t * v;
85 return diff.dot(diff);
86 }
87
88 bool SphereTriangleDetector::facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal)
89 {
90 btVector3 lp(p);
91 btVector3 lnormal(normal);
92
93 return pointInTriangle(vertices, lnormal, &lp);
94 }
95
96 bool SphereTriangleDetector::collide(const btVector3& sphereCenter, btVector3& point, btVector3& resultNormal, btScalar& depth, btScalar& timeOfImpact, btScalar contactBreakingThreshold)
97 {
98 const btVector3* vertices = &m_triangle->getVertexPtr(0);
99
100 btScalar radius = m_sphere->getRadius();
101 btScalar radiusWithThreshold = radius + contactBreakingThreshold;
102
103 btVector3 normal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]);
104
105 btScalar l2 = normal.length2();
106 bool hasContact = false;
107 btVector3 contactPoint;
108
109 if (l2 >= SIMD_EPSILON * SIMD_EPSILON)
110 {
111 normal /= btSqrt(l2);
112
113 btVector3 p1ToCentre = sphereCenter - vertices[0];
114 btScalar distanceFromPlane = p1ToCentre.dot(normal);
115
116 if (distanceFromPlane < btScalar(0.))
117 {
118 //triangle facing the other way
119 distanceFromPlane *= btScalar(-1.);
120 normal *= btScalar(-1.);
121 }
122
123 bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
124
125 // Check for contact / intersection
126
127 if (isInsideContactPlane)
128 {
129 if (facecontains(sphereCenter, vertices, normal))
130 {
131 // Inside the contact wedge - touches a point on the shell plane
132 hasContact = true;
133 contactPoint = sphereCenter - normal * distanceFromPlane;
134 }
135 else
136 {
137 // Could be inside one of the contact capsules
138 btScalar contactCapsuleRadiusSqr = radiusWithThreshold * radiusWithThreshold;
139 btScalar minDistSqr = contactCapsuleRadiusSqr;
140 btVector3 nearestOnEdge;
141 for (int i = 0; i < m_triangle->getNumEdges(); i++)
142 {
143 btVector3 pa;
144 btVector3 pb;
145
146 m_triangle->getEdge(i, pa, pb);
147
148 btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge);
149 if (distanceSqr < minDistSqr)
150 {
151 // Yep, we're inside a capsule, and record the capsule with smallest distance
152 minDistSqr = distanceSqr;
153 hasContact = true;
154 contactPoint = nearestOnEdge;
155 }
156 }
157 }
158 }
159 }
160
161 if (hasContact)
162 {
163 btVector3 contactToCentre = sphereCenter - contactPoint;
164 btScalar distanceSqr = contactToCentre.length2();
165
166 if (distanceSqr < radiusWithThreshold * radiusWithThreshold)
167 {
168 if (distanceSqr > SIMD_EPSILON)
169 {
170 btScalar distance = btSqrt(distanceSqr);
171 resultNormal = contactToCentre;
172 resultNormal.normalize();
173 point = contactPoint;
174 depth = -(radius - distance);
175 }
176 else
177 {
178 resultNormal = normal;
179 point = contactPoint;
180 depth = -radius;
181 }
182 return true;
183 }
184 }
185
186 return false;
187 }
188
189 bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p)
190 {
191 const btVector3* p1 = &vertices[0];
192 const btVector3* p2 = &vertices[1];
193 const btVector3* p3 = &vertices[2];
194
195 btVector3 edge1(*p2 - *p1);
196 btVector3 edge2(*p3 - *p2);
197 btVector3 edge3(*p1 - *p3);
198
199 btVector3 p1_to_p(*p - *p1);
200 btVector3 p2_to_p(*p - *p2);
201 btVector3 p3_to_p(*p - *p3);
202
203 btVector3 edge1_normal(edge1.cross(normal));
204 btVector3 edge2_normal(edge2.cross(normal));
205 btVector3 edge3_normal(edge3.cross(normal));
206
207 btScalar r1, r2, r3;
208 r1 = edge1_normal.dot(p1_to_p);
209 r2 = edge2_normal.dot(p2_to_p);
210 r3 = edge3_normal.dot(p3_to_p);
211 if ((r1 > 0 && r2 > 0 && r3 > 0) ||
212 (r1 <= 0 && r2 <= 0 && r3 <= 0))
213 return true;
214 return false;
215 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btDiscreteCollisionDetectorInterface::Result
Definition: btDiscreteCollisionDetectorInterface.h:30
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
SphereTriangleDetector::getClosestPoints
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
Definition: SphereTriangleDetector.cpp:28
SegmentSqrDistance
btScalar SegmentSqrDistance(const btVector3 &from, const btVector3 &to, const btVector3 &p, btVector3 &nearest)
Definition: SphereTriangleDetector.cpp:61
btScalar.h
SphereTriangleDetector::m_triangle
btTriangleShape * m_triangle
Definition: SphereTriangleDetector.h:40
btTriangleShape
Definition: btTriangleShape.h:24
btTriangleShape.h
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformA
btTransform m_transformA
Definition: btDiscreteCollisionDetectorInterface.h:46
btTriangleShape::getEdge
virtual void getEdge(int i, btVector3 &pa, btVector3 &pb) const
Definition: btTriangleShape.h:54
SphereTriangleDetector::SphereTriangleDetector
SphereTriangleDetector(btSphereShape *sphere, btTriangleShape *triangle, btScalar contactBreakingThreshold)
Definition: SphereTriangleDetector.cpp:21
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:27
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformB
btTransform m_transformB
Definition: btDiscreteCollisionDetectorInterface.h:47
SphereTriangleDetector::collide
bool collide(const btVector3 &sphereCenter, btVector3 &point, btVector3 &resultNormal, btScalar &depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
Definition: SphereTriangleDetector.cpp:96
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btSphereShape
The btSphereShape implements an implicit sphere, centered around a local origin with radius.
Definition: btSphereShape.h:25
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
SphereTriangleDetector::facecontains
bool facecontains(const btVector3 &p, const btVector3 *vertices, btVector3 &normal)
Definition: SphereTriangleDetector.cpp:88
btSphereShape.h
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
SphereTriangleDetector::m_sphere
btSphereShape * m_sphere
Definition: SphereTriangleDetector.h:39
output
#define output
btTransform::inverseTimes
btTransform inverseTimes(const btTransform &t) const
Return the inverse of this transform times the other transform.
Definition: btTransform.h:222
SphereTriangleDetector.h
SphereTriangleDetector::pointInTriangle
bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p)
Definition: SphereTriangleDetector.cpp:189
btTriangleShape::getNumEdges
virtual int getNumEdges() const
Definition: btTriangleShape.h:49
SphereTriangleDetector::m_contactBreakingThreshold
btScalar m_contactBreakingThreshold
Definition: SphereTriangleDetector.h:41
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btTriangleShape::getVertexPtr
btVector3 & getVertexPtr(int index)
Definition: btTriangleShape.h:35
btDiscreteCollisionDetectorInterface::ClosestPointInput
Definition: btDiscreteCollisionDetectorInterface.h:40
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btSphereShape::getRadius
btScalar getRadius() const
Definition: btSphereShape.h:48