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

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
16 #include "btSphereBoxCollisionAlgorithm.h"
17 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
18 #include "BulletCollision/CollisionShapes/btSphereShape.h"
19 #include "BulletCollision/CollisionShapes/btBoxShape.h"
20 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
21 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
22 //#include <stdio.h>
23
24 btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped)
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
40 btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
41 {
42 if (m_ownManifold)
43 {
44 if (m_manifoldPtr)
45 m_dispatcher->releaseManifold(m_manifoldPtr);
46 }
47 }
48
49 void btSphereBoxCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
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
85 btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
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 }
btDispatcher::releaseManifold
virtual void releaseManifold(btPersistentManifold *manifold)=0
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:523
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:63
btPersistentManifold::getContactBreakingThreshold
btScalar getContactBreakingThreshold() const
Definition: btPersistentManifold.cpp:242
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm
btSphereBoxCollisionAlgorithm(btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
Definition: btSphereBoxCollisionAlgorithm.cpp:24
btBoxShape.h
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btVector3::setZ
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:571
btManifoldResult::setPersistentManifold
void setPersistentManifold(btPersistentManifold *manifoldPtr)
Definition: btManifoldResult.h:77
btPersistentManifold::getNumContacts
int getNumContacts() const
Definition: btPersistentManifold.h:120
btActivatingCollisionAlgorithm
This class is not enabled yet (work-in-progress) to more aggressively activate objects.
Definition: btActivatingCollisionAlgorithm.h:22
btManifoldResult::refreshContactPoints
void refreshContactPoints()
Definition: btManifoldResult.h:105
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btSphereShape
The btSphereShape implements an implicit sphere, centered around a local origin with radius...
Definition: btSphereShape.h:22
btSphereBoxCollisionAlgorithm::processCollision
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
Definition: btSphereBoxCollisionAlgorithm.cpp:49
btManifoldResult
btManifoldResult is a helper class to manage contact results.
Definition: btManifoldResult.h:47
btCollisionObjectWrapper::getCollisionShape
const btCollisionShape * getCollisionShape() const
Definition: btCollisionObjectWrapper.h:40
btCollisionAlgorithm::m_dispatcher
btDispatcher * m_dispatcher
Definition: btCollisionAlgorithm.h:56
btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm
virtual ~btSphereBoxCollisionAlgorithm()
Definition: btSphereBoxCollisionAlgorithm.cpp:40
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
btManifoldResult::addContactPoint
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
Definition: btManifoldResult.cpp:105
btVector3::setX
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:567
btSphereBoxCollisionAlgorithm::getSphereDistance
bool getSphereDistance(const btCollisionObjectWrapper *boxObjWrap, btVector3 &v3PointOnBox, btVector3 &normal, btScalar &penetrationDepth, const btVector3 &v3SphereCenter, btScalar fRadius, btScalar maxContactDistance)
Definition: btSphereBoxCollisionAlgorithm.cpp:96
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btSphereBoxCollisionAlgorithm::getSpherePenetration
btScalar getSpherePenetration(btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3 &normal)
Definition: btSphereBoxCollisionAlgorithm.cpp:155
btSphereBoxCollisionAlgorithm::m_isSwapped
bool m_isSwapped
Definition: btSphereBoxCollisionAlgorithm.h:33
btConvexInternalShape::getMargin
virtual btScalar getMargin() const
Definition: btConvexInternalShape.h:106
btSphereShape::getRadius
btScalar getRadius() const
Definition: btSphereShape.h:48
btCollisionObjectWrapper::getCollisionObject
const btCollisionObject * getCollisionObject() const
Definition: btCollisionObjectWrapper.h:39
btSphereBoxCollisionAlgorithm.h
btVector3::setY
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:569
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:48
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btDispatcher::getNewManifold
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
btCollisionObjectWrapper.h
btTransform::invXform
btVector3 invXform(const btVector3 &inVec) const
Definition: btTransform.h:215
btBoxShape
The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by ...
Definition: btBoxShape.h:26
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
btCollisionDispatcher.h
btCollisionObjectWrapper::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btCollisionObjectWrapper.h:38
btCollisionObject.h
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
btSphereBoxCollisionAlgorithm::m_ownManifold
bool m_ownManifold
Definition: btSphereBoxCollisionAlgorithm.h:31
btBoxShape::getHalfExtentsWithoutMargin
const btVector3 & getHalfExtentsWithoutMargin() const
Definition: btBoxShape.h:42
btDispatcherInfo
Definition: btDispatcher.h:30
btDispatcher::needsCollision
virtual bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1)=0
btSphereBoxCollisionAlgorithm::calculateTimeOfImpact
virtual btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
Definition: btSphereBoxCollisionAlgorithm.cpp:85
btCollisionAlgorithmConstructionInfo
Definition: btCollisionAlgorithm.h:32
btSphereShape.h
btMin
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
btCollisionObjectWrapper
Definition: btCollisionObjectWrapper.h:17
btSphereBoxCollisionAlgorithm::m_manifoldPtr
btPersistentManifold * m_manifoldPtr
Definition: btSphereBoxCollisionAlgorithm.h:32
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257