From b38a727da3ec458a4dc8be1fc1fffb89115b69d3 Mon Sep 17 00:00:00 2001 From: Benoit Bolsee Date: Wed, 6 Jan 2010 08:46:04 +0000 Subject: [PATCH] Bullet: rework softbody raytest patch after approval by Erwin. --- .../CollisionDispatch/btCollisionWorld.cpp | 25 ---- .../CollisionDispatch/btCollisionWorld.h | 2 +- .../btSoftRigidDynamicsWorld.cpp | 138 ++++++++++++++++++ .../BulletSoftBody/btSoftRigidDynamicsWorld.h | 11 ++ 4 files changed, 150 insertions(+), 26 deletions(-) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 7159f552829..5c645f82a45 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -412,31 +412,6 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra // restore collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); } - } else { - if (collisionShape->isSoftBody()) { - btSoftBody* softBody = static_cast(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); - } - } } } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index 87f7137a55b..24343938e5c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -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. diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index a0069b95145..982ec3e5eb3 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -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;igetNumCollisionObjects();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); + } +} diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h index edb848e2481..14220ee7564 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h @@ -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