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 
40 
42 {
43  if (m_ownManifold)
44  {
45  if (m_manifoldPtr)
47  }
48 }
49 
50 
51 
53 {
54  (void)dispatchInfo;
55  (void)resultOut;
56  if (!m_manifoldPtr)
57  return;
58 
59  const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? body1Wrap : body0Wrap;
60  const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? body0Wrap : body1Wrap;
61 
62  btVector3 pOnBox;
63 
64  btVector3 normalOnSurfaceB;
65  btScalar penetrationDepth;
66  btVector3 sphereCenter = sphereObjWrap->getWorldTransform().getOrigin();
67  const btSphereShape* sphere0 = (const btSphereShape*)sphereObjWrap->getCollisionShape();
68  btScalar radius = sphere0->getRadius();
69  btScalar maxContactDistance = m_manifoldPtr->getContactBreakingThreshold();
70 
72 
73  if (getSphereDistance(boxObjWrap, pOnBox, normalOnSurfaceB, penetrationDepth, sphereCenter, radius, maxContactDistance))
74  {
76  resultOut->addContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth);
77  }
78 
79  if (m_ownManifold)
80  {
82  {
83  resultOut->refreshContactPoints();
84  }
85  }
86 
87 }
88 
90 {
91  (void)resultOut;
92  (void)dispatchInfo;
93  (void)col0;
94  (void)col1;
95 
96  //not yet
97  return btScalar(1.);
98 }
99 
100 
101 bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance )
102 {
103  const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape();
104  btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
105  btScalar boxMargin = boxShape->getMargin();
106  penetrationDepth = 1.0f;
107 
108  // convert the sphere position to the box's local space
109  btTransform const &m44T = boxObjWrap->getWorldTransform();
110  btVector3 sphereRelPos = m44T.invXform(sphereCenter);
111 
112  // Determine the closest point to the sphere center in the box
113  btVector3 closestPoint = sphereRelPos;
114  closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) );
115  closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) );
116  closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) );
117  closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) );
118  closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) );
119  closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) );
120 
121  btScalar intersectionDist = fRadius + boxMargin;
122  btScalar contactDist = intersectionDist + maxContactDistance;
123  normal = sphereRelPos - closestPoint;
124 
125  //if there is no penetration, we are done
126  btScalar dist2 = normal.length2();
127  if (dist2 > contactDist * contactDist)
128  {
129  return false;
130  }
131 
132  btScalar distance;
133 
134  //special case if the sphere center is inside the box
135  if (dist2 <= SIMD_EPSILON)
136  {
137  distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal);
138  }
139  else //compute the penetration details
140  {
141  distance = normal.length();
142  normal /= distance;
143  }
144 
145  pointOnBox = closestPoint + normal * boxMargin;
146 // v3PointOnSphere = sphereRelPos - (normal * fRadius);
147  penetrationDepth = distance - intersectionDist;
148 
149  // transform back in world space
150  btVector3 tmp = m44T(pointOnBox);
151  pointOnBox = tmp;
152 // tmp = m44T(v3PointOnSphere);
153 // v3PointOnSphere = tmp;
154  tmp = m44T.getBasis() * normal;
155  normal = tmp;
156 
157  return true;
158 }
159 
160 btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal )
161 {
162  //project the center of the sphere on the closest face of the box
163  btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX();
164  btScalar minDist = faceDist;
165  closestPoint.setX( boxHalfExtent.getX() );
166  normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f));
167 
168  faceDist = boxHalfExtent.getX() + sphereRelPos.getX();
169  if (faceDist < minDist)
170  {
171  minDist = faceDist;
172  closestPoint = sphereRelPos;
173  closestPoint.setX( -boxHalfExtent.getX() );
174  normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f));
175  }
176 
177  faceDist = boxHalfExtent.getY() - sphereRelPos.getY();
178  if (faceDist < minDist)
179  {
180  minDist = faceDist;
181  closestPoint = sphereRelPos;
182  closestPoint.setY( boxHalfExtent.getY() );
183  normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f));
184  }
185 
186  faceDist = boxHalfExtent.getY() + sphereRelPos.getY();
187  if (faceDist < minDist)
188  {
189  minDist = faceDist;
190  closestPoint = sphereRelPos;
191  closestPoint.setY( -boxHalfExtent.getY() );
192  normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f));
193  }
194 
195  faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ();
196  if (faceDist < minDist)
197  {
198  minDist = faceDist;
199  closestPoint = sphereRelPos;
200  closestPoint.setZ( boxHalfExtent.getZ() );
201  normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f));
202  }
203 
204  faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ();
205  if (faceDist < minDist)
206  {
207  minDist = faceDist;
208  closestPoint = sphereRelPos;
209  closestPoint.setZ( -boxHalfExtent.getZ() );
210  normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f));
211  }
212 
213  return minDist;
214 }
virtual void releaseManifold(btPersistentManifold *manifold)=0
#define SIMD_EPSILON
Definition: btScalar.h:521
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
btScalar getContactBreakingThreshold() const
btSphereBoxCollisionAlgorithm(btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:583
void setPersistentManifold(btPersistentManifold *manifoldPtr)
This class is not enabled yet (work-in-progress) to more aggressively activate objects.
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 btVector3 & getHalfExtentsWithoutMargin() const
Definition: btBoxShape.h:44
virtual btScalar getMargin() const
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:579
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:117
btScalar getSpherePenetration(btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3 &normal)
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
const btTransform & getWorldTransform() const
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:581
btCollisionObject can be used to manage collision detection objects.
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
const btCollisionShape * getCollisionShape() const
The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by ...
Definition: btBoxShape.h:26
btVector3 invXform(const btVector3 &inVec) const
Definition: btTransform.h:223
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:29
btScalar getRadius() const
Definition: btSphereShape.h:50
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:23
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
const btCollisionObject * getCollisionObject() const