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 http://continuousphysics.com/Bullet/
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
22//#include <stdio.h>
23
25: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap),
26m_ownManifold(false),
27m_manifoldPtr(mf),
28m_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();
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
101bool 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
160btScalar 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}
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:29
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
#define SIMD_EPSILON
Definition: btScalar.h:521
This class is not enabled yet (work-in-progress) to more aggressively activate objects.
The btBoxShape is a box primitive around the origin, its sides axis aligned with length specified by ...
Definition: btBoxShape.h:27
const btVector3 & getHalfExtentsWithoutMargin() const
Definition: btBoxShape.h:44
btCollisionObject can be used to manage collision detection objects.
virtual btScalar getMargin() const
virtual void releaseManifold(btPersistentManifold *manifold)=0
virtual bool needsCollision(const btCollisionObject *body0, const btCollisionObject *body1)=0
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
btManifoldResult is a helper class to manage contact results.
void setPersistentManifold(btPersistentManifold *manifoldPtr)
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
btScalar getContactBreakingThreshold() const
btSphereBoxCollisionAlgorithm(btPersistentManifold *mf, const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
bool getSphereDistance(const btCollisionObjectWrapper *boxObjWrap, btVector3 &v3PointOnBox, btVector3 &normal, btScalar &penetrationDepth, const btVector3 &v3SphereCenter, btScalar fRadius, btScalar maxContactDistance)
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
btScalar getSpherePenetration(btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3 &normal)
virtual btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
The btSphereShape implements an implicit sphere, centered around a local origin with radius.
Definition: btSphereShape.h:24
btScalar getRadius() const
Definition: btSphereShape.h:50
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btVector3 invXform(const btVector3 &inVec) const
Definition: btTransform.h:223
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:583
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:581
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:579
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
const btCollisionShape * getCollisionShape() const
const btCollisionObject * getCollisionObject() const
const btTransform & getWorldTransform() const