Bullet: rework softbody raytest patch after approval by Erwin.
authorBenoit Bolsee <benoit.bolsee@online.be>
Wed, 6 Jan 2010 08:46:04 +0000 (08:46 +0000)
committerBenoit Bolsee <benoit.bolsee@online.be>
Wed, 6 Jan 2010 08:46:04 +0000 (08:46 +0000)
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h

index 7159f5528295128b1470fe5ae3409841e254cfb2..5c645f82a45b13d56bab2102b401414de1e6a491 100644 (file)
@@ -412,31 +412,6 @@ void       btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
                                        // restore
                                        collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
                                }
-                       } else {
-                               if (collisionShape->isSoftBody()) {
-                                       btSoftBody* softBody = static_cast<btSoftBody*>(collisionObject);
-                                       btSoftBody::sRayCast softResult;
-                                       if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) 
-                                       {
-                                               btCollisionWorld::LocalShapeInfo shapeInfo;
-                                               shapeInfo.m_shapePart = 0;
-                                               shapeInfo.m_triangleIndex = softResult.index;
-                                               // get the normal
-                                               btVector3 normal = softBody->m_faces[softResult.index].m_normal;
-                                               btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
-                                               if (normal.dot(rayDir) > 0) {
-                                                       // normal always point toward origin of the ray
-                                                       normal = -normal;
-                                               }
-                                               btCollisionWorld::LocalRayResult rayResult
-                                                       (collisionObject,
-                                                        &shapeInfo,
-                                                        normal,
-                                                        softResult.fraction);
-                                               bool    normalInWorldSpace = true;
-                                               resultCallback.addSingleResult(rayResult,normalInWorldSpace);
-                                       }
-                               }
                        }
                }
        }
index 87f7137a55b93af990c7ad2acc5a7839c28b225b..24343938e5c93e35c76206bc05d586f5857a5476 100644 (file)
@@ -354,7 +354,7 @@ public:
 
        /// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback
        /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
-       void    rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; 
+       virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; 
 
        // convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback
        // This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback.
index a0069b9514507983e1188d33a79845de03839689..982ec3e5eb3e59e99f372521ea9324870f21688d 100644 (file)
@@ -22,6 +22,7 @@ subject to the following restrictions:
 #include "btSoftBodyHelpers.h"
 
 
+//#define USE_BRUTEFORCE_RAYBROADPHASE 1
 
 
 
@@ -140,3 +141,140 @@ void      btSoftRigidDynamicsWorld::debugDrawWorld()
                }               
        }       
 }
+
+
+struct btSoftSingleRayCallback : public btBroadphaseRayCallback
+{
+       btVector3       m_rayFromWorld;
+       btVector3       m_rayToWorld;
+       btTransform     m_rayFromTrans;
+       btTransform     m_rayToTrans;
+       btVector3       m_hitNormal;
+
+       const btSoftRigidDynamicsWorld* m_world;
+       btCollisionWorld::RayResultCallback&    m_resultCallback;
+
+       btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftRigidDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
+       :m_rayFromWorld(rayFromWorld),
+       m_rayToWorld(rayToWorld),
+       m_world(world),
+       m_resultCallback(resultCallback)
+       {
+               m_rayFromTrans.setIdentity();
+               m_rayFromTrans.setOrigin(m_rayFromWorld);
+               m_rayToTrans.setIdentity();
+               m_rayToTrans.setOrigin(m_rayToWorld);
+
+               btVector3 rayDir = (rayToWorld-rayFromWorld);
+
+               rayDir.normalize ();
+               ///what about division by zero? --> just set rayDirection[i] to INF/1e30
+               m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
+               m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
+               m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+               m_signs[0] = m_rayDirectionInverse[0] < 0.0;
+               m_signs[1] = m_rayDirectionInverse[1] < 0.0;
+               m_signs[2] = m_rayDirectionInverse[2] < 0.0;
+
+               m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
+
+       }
+
+       
+
+       virtual bool    process(const btBroadphaseProxy* proxy)
+       {
+               ///terminate further ray tests, once the closestHitFraction reached zero
+               if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
+                       return false;
+
+               btCollisionObject*      collisionObject = (btCollisionObject*)proxy->m_clientObject;
+
+               //only perform raycast if filterMask matches
+               if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+               {
+                       //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+                       //btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+#if 0
+#ifdef RECALCULATE_AABB
+                       btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+                       collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+#else
+                       //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
+                       const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
+                       const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
+#endif
+#endif
+                       //btScalar hitLambda = m_resultCallback.m_closestHitFraction;
+                       //culling already done by broadphase
+                       //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
+                       {
+                               m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
+                                       collisionObject,
+                                               collisionObject->getCollisionShape(),
+                                               collisionObject->getWorldTransform(),
+                                               m_resultCallback);
+                       }
+               }
+               return true;
+       }
+};
+
+void   btSoftRigidDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+{
+       BT_PROFILE("rayTest");
+       /// use the broadphase to accelerate the search for objects, based on their aabb
+       /// and for each object with ray-aabb overlap, perform an exact ray test
+       btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
+
+#ifndef USE_BRUTEFORCE_RAYBROADPHASE
+       m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
+#else
+       for (int i=0;i<this->getNumCollisionObjects();i++)
+       {
+               rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
+       }       
+#endif //USE_BRUTEFORCE_RAYBROADPHASE
+
+}
+
+
+void   btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+                                         btCollisionObject* collisionObject,
+                                         const btCollisionShape* collisionShape,
+                                         const btTransform& colObjWorldTransform,
+                                         RayResultCallback& resultCallback)
+{
+       if (collisionShape->isSoftBody()) {
+               btSoftBody* softBody = btSoftBody::upcast(collisionObject);
+               if (softBody) {
+                       btSoftBody::sRayCast softResult;
+                       if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) 
+                       {
+                               if (softResult.fraction<= resultCallback.m_closestHitFraction) 
+                               {
+                                       btCollisionWorld::LocalShapeInfo shapeInfo;
+                                       shapeInfo.m_shapePart = 0;
+                                       shapeInfo.m_triangleIndex = softResult.index;
+                                       // get the normal
+                                       btVector3 normal = softBody->m_faces[softResult.index].m_normal;
+                                       btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
+                                       if (normal.dot(rayDir) > 0) {
+                                               // normal must always point toward origin of the ray
+                                               normal = -normal;
+                                       }
+                                       btCollisionWorld::LocalRayResult rayResult
+                                               (collisionObject,
+                                               &shapeInfo,
+                                               normal,
+                                               softResult.fraction);
+                                       bool    normalInWorldSpace = true;
+                                       resultCallback.addSingleResult(rayResult,normalInWorldSpace);
+                               }
+                       }
+               }
+       } 
+       else {
+               btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback);
+       }
+}
index edb848e24814310519feff320411b8a01c456ed0..14220ee75640b36c87b95c48a4a4035edbbbcc00 100644 (file)
@@ -77,6 +77,17 @@ public:
                return m_softBodies;
        }
 
+       virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; 
+
+       /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
+       /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape.
+       /// This allows more customization.
+       static void     rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+                                         btCollisionObject* collisionObject,
+                                         const btCollisionShape* collisionShape,
+                                         const btTransform& colObjWorldTransform,
+                                         RayResultCallback& resultCallback);
+
 };
 
 #endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H