Blender  V3.3
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"
20 
22  : m_sphere(sphere),
23  m_triangle(triangle),
24  m_contactBreakingThreshold(contactBreakingThreshold)
25 {
26 }
27 
29 {
30  (void)debugDraw;
31  const btTransform& transformA = input.m_transformA;
32  const btTransform& transformB = input.m_transformB;
33 
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 }
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble t
in reality light always falls off quadratically Particle Retrieve the data of the particle that spawned the object for example to give variation to multiple instances of an object Point Retrieve information about points in a point cloud Retrieve the edges of an object as it appears to Cycles topology will always appear triangulated Convert a blackbody temperature to an RGB value Normal Generate a perturbed normal from an RGB normal map image Typically used for faking highly detailed surfaces Generate an OSL shader from a file or text data block Image Sample an image file as a texture Sky Generate a procedural sky texture Noise Generate fractal Perlin noise Wave Generate procedural bands or rings with noise Voronoi Generate Worley noise based on the distance to random points Typically used to generate textures such as or biological cells Brick Generate a procedural texture producing bricks Texture Retrieve multiple types of texture coordinates nTypically used as inputs for texture nodes Vector Convert a point
btScalar SegmentSqrDistance(const btVector3 &from, const btVector3 &to, const btVector3 &p, btVector3 &nearest)
ATTR_WARN_UNUSED_RESULT const BMVert * v
void debugDraw(btIDebugDraw *debugDrawer)
btActionInterface interface
btScalar m_contactBreakingThreshold
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
#define SIMD_EPSILON
Definition: btScalar.h:543
btSphereShape(btScalar radius)
Definition: btSphereShape.h:29
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTriangleShape()
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
friend double dot(const Vector &lhs, const Vector &rhs)
StackEntry * from
SyclQueue void void size_t num_bytes void
IconTextureDrawCall normal
ccl_global KernelShaderEvalInput ccl_global float * output
ccl_global KernelShaderEvalInput * input
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt=1)
vec_base< T, 3 > cross(const vec_base< T, 3 > &a, const vec_base< T, 3 > &b)
T distance(const T &a, const T &b)
bool collide(const btVector3 &sphereCenter, btVector3 &point, btVector3 &resultNormal, btScalar &depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
SphereTriangleDetector(btSphereShape *sphere, btTriangleShape *triangle, btScalar contactBreakingThreshold)
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)