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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

SphereTriangleDetector.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
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"
17#include "SphereTriangleDetector.h"
18#include "BulletCollision/CollisionShapes/btTriangleShape.h"
19#include "BulletCollision/CollisionShapes/btSphereShape.h"
20
21SphereTriangleDetector::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}
SegmentSqrDistance
btScalar SegmentSqrDistance(const btVector3 &from, const btVector3 &to, const btVector3 &p, btVector3 &nearest)
Definition: SphereTriangleDetector.cpp:61
SphereTriangleDetector.h
output
#define output
btScalar.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btSphereShape.h
btTriangleShape.h
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:27
btSphereShape
The btSphereShape implements an implicit sphere, centered around a local origin with radius.
Definition: btSphereShape.h:25
btSphereShape::getRadius
btScalar getRadius() const
Definition: btSphereShape.h:48
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:109
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:114
btTransform::inverseTimes
btTransform inverseTimes(const btTransform &t) const
Return the inverse of this transform times the other transform.
Definition: btTransform.h:223
btTriangleShape
Definition: btTriangleShape.h:24
btTriangleShape::getEdge
virtual void getEdge(int i, btVector3 &pa, btVector3 &pb) const
Definition: btTriangleShape.h:54
btTriangleShape::getNumEdges
virtual int getNumEdges() const
Definition: btTriangleShape.h:49
btTriangleShape::getVertexPtr
btVector3 & getVertexPtr(int index)
Definition: btTriangleShape.h:35
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
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
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
SphereTriangleDetector::m_triangle
btTriangleShape * m_triangle
Definition: SphereTriangleDetector.h:40
SphereTriangleDetector::collide
bool collide(const btVector3 &sphereCenter, btVector3 &point, btVector3 &resultNormal, btScalar &depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
Definition: SphereTriangleDetector.cpp:96
SphereTriangleDetector::m_sphere
btSphereShape * m_sphere
Definition: SphereTriangleDetector.h:39
SphereTriangleDetector::SphereTriangleDetector
SphereTriangleDetector(btSphereShape *sphere, btTriangleShape *triangle, btScalar contactBreakingThreshold)
Definition: SphereTriangleDetector.cpp:21
SphereTriangleDetector::pointInTriangle
bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p)
Definition: SphereTriangleDetector.cpp:189
SphereTriangleDetector::facecontains
bool facecontains(const btVector3 &p, const btVector3 *vertices, btVector3 &normal)
Definition: SphereTriangleDetector.cpp:88
SphereTriangleDetector::getClosestPoints
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
Definition: SphereTriangleDetector.cpp:28
SphereTriangleDetector::m_contactBreakingThreshold
btScalar m_contactBreakingThreshold
Definition: SphereTriangleDetector.h:41
btDiscreteCollisionDetectorInterface::ClosestPointInput
Definition: btDiscreteCollisionDetectorInterface.h:40
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformB
btTransform m_transformB
Definition: btDiscreteCollisionDetectorInterface.h:47
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformA
btTransform m_transformA
Definition: btDiscreteCollisionDetectorInterface.h:46
btDiscreteCollisionDetectorInterface::Result
Definition: btDiscreteCollisionDetectorInterface.h:30