00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "btSphereBoxCollisionAlgorithm.h"
00017 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
00018 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00019 #include "BulletCollision/CollisionShapes/btBoxShape.h"
00020 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00021 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
00022
00023
00024 btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped)
00025 : btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap),
00026 m_ownManifold(false),
00027 m_manifoldPtr(mf),
00028 m_isSwapped(isSwapped)
00029 {
00030 const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? col1Wrap : col0Wrap;
00031 const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? col0Wrap : col1Wrap;
00032
00033 if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject()))
00034 {
00035 m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject());
00036 m_ownManifold = true;
00037 }
00038 }
00039
00040
00041 btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
00042 {
00043 if (m_ownManifold)
00044 {
00045 if (m_manifoldPtr)
00046 m_dispatcher->releaseManifold(m_manifoldPtr);
00047 }
00048 }
00049
00050
00051
00052 void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
00053 {
00054 (void)dispatchInfo;
00055 (void)resultOut;
00056 if (!m_manifoldPtr)
00057 return;
00058
00059 const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? body1Wrap : body0Wrap;
00060 const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? body0Wrap : body1Wrap;
00061
00062 btVector3 pOnBox;
00063
00064 btVector3 normalOnSurfaceB;
00065 btScalar penetrationDepth;
00066 btVector3 sphereCenter = sphereObjWrap->getWorldTransform().getOrigin();
00067 const btSphereShape* sphere0 = (const btSphereShape*)sphereObjWrap->getCollisionShape();
00068 btScalar radius = sphere0->getRadius();
00069 btScalar maxContactDistance = m_manifoldPtr->getContactBreakingThreshold();
00070
00071 resultOut->setPersistentManifold(m_manifoldPtr);
00072
00073 if (getSphereDistance(boxObjWrap, pOnBox, normalOnSurfaceB, penetrationDepth, sphereCenter, radius, maxContactDistance))
00074 {
00076 resultOut->addContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth);
00077 }
00078
00079 if (m_ownManifold)
00080 {
00081 if (m_manifoldPtr->getNumContacts())
00082 {
00083 resultOut->refreshContactPoints();
00084 }
00085 }
00086
00087 }
00088
00089 btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00090 {
00091 (void)resultOut;
00092 (void)dispatchInfo;
00093 (void)col0;
00094 (void)col1;
00095
00096
00097 return btScalar(1.);
00098 }
00099
00100
00101 bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance )
00102 {
00103 const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape();
00104 btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
00105 btScalar boxMargin = boxShape->getMargin();
00106 penetrationDepth = 1.0f;
00107
00108
00109 btTransform const &m44T = boxObjWrap->getWorldTransform();
00110 btVector3 sphereRelPos = m44T.invXform(sphereCenter);
00111
00112
00113 btVector3 closestPoint = sphereRelPos;
00114 closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) );
00115 closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) );
00116 closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) );
00117 closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) );
00118 closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) );
00119 closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) );
00120
00121 btScalar intersectionDist = fRadius + boxMargin;
00122 btScalar contactDist = intersectionDist + maxContactDistance;
00123 normal = sphereRelPos - closestPoint;
00124
00125
00126 btScalar dist2 = normal.length2();
00127 if (dist2 > contactDist * contactDist)
00128 {
00129 return false;
00130 }
00131
00132 btScalar distance;
00133
00134
00135 if (dist2 <= SIMD_EPSILON)
00136 {
00137 distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal);
00138 }
00139 else
00140 {
00141 distance = normal.length();
00142 normal /= distance;
00143 }
00144
00145 pointOnBox = closestPoint + normal * boxMargin;
00146
00147 penetrationDepth = distance - intersectionDist;
00148
00149
00150 btVector3 tmp = m44T(pointOnBox);
00151 pointOnBox = tmp;
00152
00153
00154 tmp = m44T.getBasis() * normal;
00155 normal = tmp;
00156
00157 return true;
00158 }
00159
00160 btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal )
00161 {
00162
00163 btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX();
00164 btScalar minDist = faceDist;
00165 closestPoint.setX( boxHalfExtent.getX() );
00166 normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f));
00167
00168 faceDist = boxHalfExtent.getX() + sphereRelPos.getX();
00169 if (faceDist < minDist)
00170 {
00171 minDist = faceDist;
00172 closestPoint = sphereRelPos;
00173 closestPoint.setX( -boxHalfExtent.getX() );
00174 normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f));
00175 }
00176
00177 faceDist = boxHalfExtent.getY() - sphereRelPos.getY();
00178 if (faceDist < minDist)
00179 {
00180 minDist = faceDist;
00181 closestPoint = sphereRelPos;
00182 closestPoint.setY( boxHalfExtent.getY() );
00183 normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f));
00184 }
00185
00186 faceDist = boxHalfExtent.getY() + sphereRelPos.getY();
00187 if (faceDist < minDist)
00188 {
00189 minDist = faceDist;
00190 closestPoint = sphereRelPos;
00191 closestPoint.setY( -boxHalfExtent.getY() );
00192 normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f));
00193 }
00194
00195 faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ();
00196 if (faceDist < minDist)
00197 {
00198 minDist = faceDist;
00199 closestPoint = sphereRelPos;
00200 closestPoint.setZ( boxHalfExtent.getZ() );
00201 normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f));
00202 }
00203
00204 faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ();
00205 if (faceDist < minDist)
00206 {
00207 minDist = faceDist;
00208 closestPoint = sphereRelPos;
00209 closestPoint.setZ( -boxHalfExtent.getZ() );
00210 normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f));
00211 }
00212
00213 return minDist;
00214 }