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
41 btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
42 {
43 if (m_ownManifold)
44 {
45 if (m_manifoldPtr)
46 m_dispatcher->releaseManifold(m_manifoldPtr);
47 }
48 }
49
50
51
52 void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
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
71 resultOut->setPersistentManifold(m_manifoldPtr);
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 {
81 if (m_manifoldPtr->getNumContacts())
82 {
83 resultOut->refreshContactPoints();
84 }
85 }
86
87 }
88
89 btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
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 }
btDispatcher::releaseManifold
virtual void releaseManifold(btPersistentManifold *manifold)=0
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:495
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:54
btPersistentManifold::getContactBreakingThreshold
btScalar getContactBreakingThreshold() const
Definition: btPersistentManifold.cpp:240
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:650
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:257
btVector3::setZ
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:581
btManifoldResult::setPersistentManifold
void setPersistentManifold(btPersistentManifold *manifoldPtr)
Definition: btManifoldResult.h:70
btPersistentManifold::getNumContacts
int getNumContacts() const
Definition: btPersistentManifold.h:109
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:99
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:573
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:52
btManifoldResult
btManifoldResult is a helper class to manage contact results.
Definition: btManifoldResult.h:39
btCollisionObjectWrapper::getCollisionShape
const btCollisionShape * getCollisionShape() const
Definition: btCollisionObjectWrapper.h:40
btCollisionAlgorithm::m_dispatcher
btDispatcher * m_dispatcher
Definition: btCollisionAlgorithm.h:60
btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm
virtual ~btSphereBoxCollisionAlgorithm()
Definition: btSphereBoxCollisionAlgorithm.cpp:41
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:575
btManifoldResult::addContactPoint
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
Definition: btManifoldResult.cpp:103
btVector3::setX
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:577
btSphereBoxCollisionAlgorithm::getSphereDistance
bool getSphereDistance(const btCollisionObjectWrapper *boxObjWrap, btVector3 &v3PointOnBox, btVector3 &normal, btScalar &penetrationDepth, const btVector3 &v3SphereCenter, btScalar fRadius, btScalar maxContactDistance)
Definition: btSphereBoxCollisionAlgorithm.cpp:101
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btSphereBoxCollisionAlgorithm::getSpherePenetration
btScalar getSpherePenetration(btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3 &normal)
Definition: btSphereBoxCollisionAlgorithm.cpp:160
btSphereBoxCollisionAlgorithm::m_isSwapped
bool m_isSwapped
Definition: btSphereBoxCollisionAlgorithm.h:33
btConvexInternalShape::getMargin
virtual btScalar getMargin() const
Definition: btConvexInternalShape.h:113
btSphereShape::getRadius
btScalar getRadius() const
Definition: btSphereShape.h:47
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:579
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:49
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
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:223
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:83
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
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:29
btSphereBoxCollisionAlgorithm::m_ownManifold
bool m_ownManifold
Definition: btSphereBoxCollisionAlgorithm.h:31
btBoxShape::getHalfExtentsWithoutMargin
const btVector3 & getHalfExtentsWithoutMargin() const
Definition: btBoxShape.h:44
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:89
btCollisionAlgorithmConstructionInfo
Definition: btCollisionAlgorithm.h:32
btSphereShape.h
btMin
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:23
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:571
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263