Blender  V3.3
btSphereBoxCollisionAlgorithm.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 
22 //#include <stdio.h>
23 
25  : btActivatingCollisionAlgorithm(ci, col0Wrap, col1Wrap),
26  m_ownManifold(false),
27  m_manifoldPtr(mf),
28  m_isSwapped(isSwapped)
29 {
30  const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? col1Wrap : col0Wrap;
31  const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? col0Wrap : col1Wrap;
32 
33  if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject()))
34  {
35  m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject());
36  m_ownManifold = true;
37  }
38 }
39 
41 {
42  if (m_ownManifold)
43  {
44  if (m_manifoldPtr)
45  m_dispatcher->releaseManifold(m_manifoldPtr);
46  }
47 }
48 
50 {
51  (void)dispatchInfo;
52  (void)resultOut;
53  if (!m_manifoldPtr)
54  return;
55 
56  const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? body1Wrap : body0Wrap;
57  const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? body0Wrap : body1Wrap;
58 
59  btVector3 pOnBox;
60 
61  btVector3 normalOnSurfaceB;
62  btScalar penetrationDepth;
63  btVector3 sphereCenter = sphereObjWrap->getWorldTransform().getOrigin();
64  const btSphereShape* sphere0 = (const btSphereShape*)sphereObjWrap->getCollisionShape();
65  btScalar radius = sphere0->getRadius();
66  btScalar maxContactDistance = m_manifoldPtr->getContactBreakingThreshold();
67 
68  resultOut->setPersistentManifold(m_manifoldPtr);
69 
70  if (getSphereDistance(boxObjWrap, pOnBox, normalOnSurfaceB, penetrationDepth, sphereCenter, radius, maxContactDistance))
71  {
73  resultOut->addContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth);
74  }
75 
76  if (m_ownManifold)
77  {
78  if (m_manifoldPtr->getNumContacts())
79  {
80  resultOut->refreshContactPoints();
81  }
82  }
83 }
84 
86 {
87  (void)resultOut;
88  (void)dispatchInfo;
89  (void)col0;
90  (void)col1;
91 
92  //not yet
93  return btScalar(1.);
94 }
95 
96 bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance)
97 {
98  const btBoxShape* boxShape = (const btBoxShape*)boxObjWrap->getCollisionShape();
99  btVector3 const& boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
100  btScalar boxMargin = boxShape->getMargin();
101  penetrationDepth = 1.0f;
102 
103  // convert the sphere position to the box's local space
104  btTransform const& m44T = boxObjWrap->getWorldTransform();
105  btVector3 sphereRelPos = m44T.invXform(sphereCenter);
106 
107  // Determine the closest point to the sphere center in the box
108  btVector3 closestPoint = sphereRelPos;
109  closestPoint.setX(btMin(boxHalfExtent.getX(), closestPoint.getX()));
110  closestPoint.setX(btMax(-boxHalfExtent.getX(), closestPoint.getX()));
111  closestPoint.setY(btMin(boxHalfExtent.getY(), closestPoint.getY()));
112  closestPoint.setY(btMax(-boxHalfExtent.getY(), closestPoint.getY()));
113  closestPoint.setZ(btMin(boxHalfExtent.getZ(), closestPoint.getZ()));
114  closestPoint.setZ(btMax(-boxHalfExtent.getZ(), closestPoint.getZ()));
115 
116  btScalar intersectionDist = fRadius + boxMargin;
117  btScalar contactDist = intersectionDist + maxContactDistance;
118  normal = sphereRelPos - closestPoint;
119 
120  //if there is no penetration, we are done
121  btScalar dist2 = normal.length2();
122  if (dist2 > contactDist * contactDist)
123  {
124  return false;
125  }
126 
128 
129  //special case if the sphere center is inside the box
130  if (dist2 <= SIMD_EPSILON)
131  {
132  distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal);
133  }
134  else //compute the penetration details
135  {
136  distance = normal.length();
137  normal /= distance;
138  }
139 
140  pointOnBox = closestPoint + normal * boxMargin;
141  // v3PointOnSphere = sphereRelPos - (normal * fRadius);
142  penetrationDepth = distance - intersectionDist;
143 
144  // transform back in world space
145  btVector3 tmp = m44T(pointOnBox);
146  pointOnBox = tmp;
147  // tmp = m44T(v3PointOnSphere);
148  // v3PointOnSphere = tmp;
149  tmp = m44T.getBasis() * normal;
150  normal = tmp;
151 
152  return true;
153 }
154 
156 {
157  //project the center of the sphere on the closest face of the box
158  btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX();
159  btScalar minDist = faceDist;
160  closestPoint.setX(boxHalfExtent.getX());
161  normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f));
162 
163  faceDist = boxHalfExtent.getX() + sphereRelPos.getX();
164  if (faceDist < minDist)
165  {
166  minDist = faceDist;
167  closestPoint = sphereRelPos;
168  closestPoint.setX(-boxHalfExtent.getX());
169  normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f));
170  }
171 
172  faceDist = boxHalfExtent.getY() - sphereRelPos.getY();
173  if (faceDist < minDist)
174  {
175  minDist = faceDist;
176  closestPoint = sphereRelPos;
177  closestPoint.setY(boxHalfExtent.getY());
178  normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f));
179  }
180 
181  faceDist = boxHalfExtent.getY() + sphereRelPos.getY();
182  if (faceDist < minDist)
183  {
184  minDist = faceDist;
185  closestPoint = sphereRelPos;
186  closestPoint.setY(-boxHalfExtent.getY());
187  normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f));
188  }
189 
190  faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ();
191  if (faceDist < minDist)
192  {
193  minDist = faceDist;
194  closestPoint = sphereRelPos;
195  closestPoint.setZ(boxHalfExtent.getZ());
196  normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f));
197  }
198 
199  faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ();
200  if (faceDist < minDist)
201  {
202  minDist = faceDist;
203  closestPoint = sphereRelPos;
204  closestPoint.setZ(-boxHalfExtent.getZ());
205  normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f));
206  }
207 
208  return minDist;
209 }
btBoxShape(const btVector3 &boxHalfExtents)
Definition: btBoxShape.cpp:17
btCollisionObject
btPersistentManifold * m_manifoldPtr
SIMD_FORCE_INLINE const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
SIMD_FORCE_INLINE const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
btPersistentManifold()
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#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
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
This class is not enabled yet (work-in-progress) to more aggressively activate objects.
virtual void releaseManifold(btPersistentManifold *manifold)=0
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
virtual bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1)=0
btManifoldResult is a helper class to manage contact results.
void setPersistentManifold(btPersistentManifold *manifoldPtr)
SIMD_FORCE_INLINE void refreshContactPoints()
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
btSphereBoxCollisionAlgorithm(btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
bool getSphereDistance(const btCollisionObjectWrapper *boxObjWrap, btVector3 &v3PointOnBox, btVector3 &normal, btScalar &penetrationDepth, const btVector3 &v3SphereCenter, btScalar fRadius, btScalar maxContactDistance)
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
btScalar getSpherePenetration(btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3 &normal)
virtual btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
SyclQueue void void size_t num_bytes void
IconTextureDrawCall normal
T distance(const T &a, const T &b)
SIMD_FORCE_INLINE const btCollisionShape * getCollisionShape() const
SIMD_FORCE_INLINE const btTransform & getWorldTransform() const
SIMD_FORCE_INLINE const btCollisionObject * getCollisionObject() const