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::btActivatingCollisionAlgorithm
btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo &ci)
Definition btActivatingCollisionAlgorithm.cpp:20
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
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
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