Bullet Collision Detection & Physics Library
SphereTriangleDetector.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16#include "LinearMath/btScalar.h"
20
21
23:m_sphere(sphere),
24m_triangle(triangle),
25m_contactBreakingThreshold(contactBreakingThreshold)
26{
27
28}
29
30void 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
64btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest);
65
66btScalar 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
87bool 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
94bool 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
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}
btScalar SegmentSqrDistance(const btVector3 &from, const btVector3 &to, const btVector3 &p, btVector3 &nearest)
#define output
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
#define SIMD_EPSILON
Definition: btScalar.h:521
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:30
The btSphereShape implements an implicit sphere, centered around a local origin with radius.
Definition: btSphereShape.h:24
btScalar getRadius() const
Definition: btSphereShape.h:50
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btTransform inverseTimes(const btTransform &t) const
Return the inverse of this transform times the other transform.
Definition: btTransform.h:230
virtual void getEdge(int i, btVector3 &pa, btVector3 &pb) const
virtual int getNumEdges() const
btVector3 & getVertexPtr(int index)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
bool collide(const btVector3 &sphereCenter, btVector3 &point, btVector3 &resultNormal, btScalar &depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
SphereTriangleDetector(btSphereShape *sphere, btTriangleShape *triangle, btScalar contactBreakingThreshold)
bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p)
bool facecontains(const btVector3 &p, const btVector3 *vertices, btVector3 &normal)
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)