Bullet Collision Detection & Physics Library
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  {
36  m_ownManifold = true;
37  }
38 }
39 
41 {
42  if (m_ownManifold)
43  {
44  if (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 
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  {
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 
127  btScalar distance;
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 
155 btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration(btVector3 const& boxHalfExtent, btVector3 const& sphereRelPos, btVector3& closestPoint, btVector3& normal)
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 }
virtual void releaseManifold(btPersistentManifold *manifold)=0
#define SIMD_EPSILON
Definition: btScalar.h:523
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
btScalar getContactBreakingThreshold() const
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btSphereBoxCollisionAlgorithm(btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:571
void setPersistentManifold(btPersistentManifold *manifoldPtr)
This class is not enabled yet (work-in-progress) to more aggressively activate objects.
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
The btSphereShape implements an implicit sphere, centered around a local origin with radius...
Definition: btSphereShape.h:22
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
btManifoldResult is a helper class to manage contact results.
const btCollisionShape * getCollisionShape() const
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:567
bool getSphereDistance(const btCollisionObjectWrapper *boxObjWrap, btVector3 &v3PointOnBox, btVector3 &normal, btScalar &penetrationDepth, const btVector3 &v3SphereCenter, btScalar fRadius, btScalar maxContactDistance)
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btScalar getSpherePenetration(btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3 &normal)
virtual btScalar getMargin() const
btScalar getRadius() const
Definition: btSphereShape.h:48
const btCollisionObject * getCollisionObject() const
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:569
btCollisionObject can be used to manage collision detection objects.
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
btVector3 invXform(const btVector3 &inVec) const
Definition: btTransform.h:215
The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by ...
Definition: btBoxShape.h:26
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
const btTransform & getWorldTransform() const
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
const btVector3 & getHalfExtentsWithoutMargin() const
Definition: btBoxShape.h:42
virtual bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1)=0
virtual btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257