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

Bullet Collision Detection & Physics Library

btSphereBoxCollisionAlgorithm.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
24btSphereBoxCollisionAlgorithm::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
40btSphereBoxCollisionAlgorithm::~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}
btBoxShape.h
btCollisionDispatcher.h
btCollisionObjectWrapper.h
btCollisionObject.h
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
btMin
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btSphereBoxCollisionAlgorithm.h
btSphereShape.h
btActivatingCollisionAlgorithm
This class is not enabled yet (work-in-progress) to more aggressively activate objects.
Definition: btActivatingCollisionAlgorithm.h:23
btBoxShape
The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by ...
Definition: btBoxShape.h:28
btBoxShape::getHalfExtentsWithoutMargin
const btVector3 & getHalfExtentsWithoutMargin() const
Definition: btBoxShape.h:42
btCollisionAlgorithm::m_dispatcher
btDispatcher * m_dispatcher
Definition: btCollisionAlgorithm.h:56
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:51
btConvexInternalShape::getMargin
virtual btScalar getMargin() const
Definition: btConvexInternalShape.h:106
btDispatcher::releaseManifold
virtual void releaseManifold(btPersistentManifold *manifold)=0
btDispatcher::needsCollision
virtual bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1)=0
btDispatcher::getNewManifold
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
btManifoldResult
btManifoldResult is a helper class to manage contact results.
Definition: btManifoldResult.h:48
btManifoldResult::setPersistentManifold
void setPersistentManifold(btPersistentManifold *manifoldPtr)
Definition: btManifoldResult.h:77
btManifoldResult::refreshContactPoints
void refreshContactPoints()
Definition: btManifoldResult.h:105
btManifoldResult::addContactPoint
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
Definition: btManifoldResult.cpp:103
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:65
btPersistentManifold::getNumContacts
int getNumContacts() const
Definition: btPersistentManifold.h:120
btPersistentManifold::getContactBreakingThreshold
btScalar getContactBreakingThreshold() const
Definition: btPersistentManifold.cpp:242
btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm
btSphereBoxCollisionAlgorithm(btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
Definition: btSphereBoxCollisionAlgorithm.cpp:24
btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm
virtual ~btSphereBoxCollisionAlgorithm()
Definition: btSphereBoxCollisionAlgorithm.cpp:40
btSphereBoxCollisionAlgorithm::m_manifoldPtr
btPersistentManifold * m_manifoldPtr
Definition: btSphereBoxCollisionAlgorithm.h:32
btSphereBoxCollisionAlgorithm::getSphereDistance
bool getSphereDistance(const btCollisionObjectWrapper *boxObjWrap, btVector3 &v3PointOnBox, btVector3 &normal, btScalar &penetrationDepth, const btVector3 &v3SphereCenter, btScalar fRadius, btScalar maxContactDistance)
Definition: btSphereBoxCollisionAlgorithm.cpp:96
btSphereBoxCollisionAlgorithm::m_isSwapped
bool m_isSwapped
Definition: btSphereBoxCollisionAlgorithm.h:33
btSphereBoxCollisionAlgorithm::processCollision
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
Definition: btSphereBoxCollisionAlgorithm.cpp:49
btSphereBoxCollisionAlgorithm::getSpherePenetration
btScalar getSpherePenetration(btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3 &normal)
Definition: btSphereBoxCollisionAlgorithm.cpp:155
btSphereBoxCollisionAlgorithm::m_ownManifold
bool m_ownManifold
Definition: btSphereBoxCollisionAlgorithm.h:31
btSphereBoxCollisionAlgorithm::calculateTimeOfImpact
virtual btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
Definition: btSphereBoxCollisionAlgorithm.cpp:85
btSphereShape
The btSphereShape implements an implicit sphere, centered around a local origin with radius.
Definition: btSphereShape.h:25
btSphereShape::getRadius
btScalar getRadius() const
Definition: btSphereShape.h:48
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform::invXform
btVector3 invXform(const btVector3 &inVec) const
Definition: btTransform.h:216
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:109
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:114
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
btVector3::setZ
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:571
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btVector3::setY
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:569
btVector3::setX
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:567
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
btCollisionAlgorithmConstructionInfo
Definition: btCollisionAlgorithm.h:33
btCollisionObjectWrapper
Definition: btCollisionObjectWrapper.h:18
btCollisionObjectWrapper::getCollisionShape
const btCollisionShape * getCollisionShape() const
Definition: btCollisionObjectWrapper.h:46
btCollisionObjectWrapper::getCollisionObject
const btCollisionObject * getCollisionObject() const
Definition: btCollisionObjectWrapper.h:45
btCollisionObjectWrapper::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btCollisionObjectWrapper.h:44
btDispatcherInfo
Definition: btDispatcher.h:31