From d931aae32f84fda09ebf5325c16d3691fbb018d8 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Fri, 11 Aug 2006 04:10:37 +0000 Subject: [PATCH] added basic CCD calculations for Compounds and non-convex (convex-versus-concave). --- .../CompoundCollisionAlgorithm.cpp | 43 ++++++++- .../ConvexConcaveCollisionAlgorithm.cpp | 95 ++++++++++++++----- Bullet/CollisionShapes/CompoundShape.cpp | 4 +- Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp | 89 ++++++++++++++--- .../CcdPhysics/CcdPhysicsEnvironment.cpp | 5 + .../CcdPhysics/SimulationIsland.cpp | 8 +- 6 files changed, 205 insertions(+), 39 deletions(-) diff --git a/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp b/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp index 9c021f63f..0a79d2b7b 100644 --- a/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp +++ b/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp @@ -99,5 +99,46 @@ void CompoundCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,BroadphaseP float CompoundCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo) { - return 1.f; + CollisionObject* colObj = static_cast(m_compoundProxy.m_clientObject); + assert (colObj->m_collisionShape->IsCompound()); + + CompoundShape* compoundShape = static_cast(colObj->m_collisionShape); + + //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps + //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals + //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: + //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 + //then use each overlapping node AABB against Tree0 + //and vise versa. + + float hitFraction = 1.f; + + int numChildren = m_childCollisionAlgorithms.size(); + int i; + for (i=0;iGetChildShape(i); + CollisionObject* colObj = static_cast(m_childProxies[i].m_clientObject); + + //backup + SimdTransform orgTrans = colObj->m_worldTransform; + CollisionShape* orgShape = colObj->m_collisionShape; + + SimdTransform childTrans = compoundShape->GetChildTransform(i); + SimdTransform newChildWorldTrans = orgTrans*childTrans ; + colObj->m_worldTransform = newChildWorldTrans; + + colObj->m_collisionShape = childShape; + float frac = m_childCollisionAlgorithms[i]->CalculateTimeOfImpact(&m_childProxies[i],&m_otherProxy,dispatchInfo); + if (fracm_collisionShape =orgShape; + colObj->m_worldTransform = orgTrans; + } + return hitFraction; + } \ No newline at end of file diff --git a/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp b/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp index f10388230..63ed29888 100644 --- a/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp +++ b/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp @@ -23,7 +23,9 @@ subject to the following restrictions: #include "CollisionDispatch/ManifoldResult.h" #include "NarrowPhaseCollision/RaycastCallback.h" #include "CollisionShapes/TriangleShape.h" +#include "CollisionShapes/SphereShape.h" #include "IDebugDraw.h" +#include "NarrowPhaseCollision/SubSimplexConvexCast.h" ConvexConcaveCollisionAlgorithm::ConvexConcaveCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) : CollisionAlgorithm(ci),m_convex(*proxy0),m_concave(*proxy1), @@ -202,36 +204,84 @@ float ConvexConcaveCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* ,B CollisionObject* convexbody = (CollisionObject* )m_convex.m_clientObject; CollisionObject* triBody = static_cast(m_concave.m_clientObject); - const SimdVector3& from = convexbody->m_worldTransform.getOrigin(); - - SimdVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); + //only perform CCD above a certain treshold, this prevents blocking on the long run + //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... + float squareMot0 = (convexbody->m_interpolationWorldTransform.getOrigin() - convexbody->m_worldTransform.getOrigin()).length2(); + if (squareMot0 < convexbody->m_ccdSquareMotionTreshold) + { + return 1.f; + } + + //const SimdVector3& from = convexbody->m_worldTransform.getOrigin(); + //SimdVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); //todo: only do if the motion exceeds the 'radius' - struct LocalTriangleRaycastCallback : public TriangleRaycastCallback + SimdTransform convexFromLocal = triBody->m_cachedInvertedWorldTransform * convexbody->m_worldTransform; + SimdTransform convexToLocal = triBody->m_cachedInvertedWorldTransform * convexbody->m_interpolationWorldTransform; + + struct LocalTriangleSphereCastCallback : public TriangleCallback { - LocalTriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to) - :TriangleRaycastCallback(from,to) - { + SimdTransform m_ccdSphereFromTrans; + SimdTransform m_ccdSphereToTrans; + SimdTransform m_meshTransform; + + float m_ccdSphereRadius; + float m_hitFraction; + + + LocalTriangleSphereCastCallback(const SimdTransform& from,const SimdTransform& to,float ccdSphereRadius,float hitFraction) + :m_ccdSphereFromTrans(from), + m_ccdSphereToTrans(to), + m_ccdSphereRadius(ccdSphereRadius), + m_hitFraction(hitFraction) + { } - virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) + + virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex) { - //todo: handle ccd here - return 0.f; + //do a swept sphere for now + SimdTransform ident; + ident.setIdentity(); + ConvexCast::CastResult castResult; + castResult.m_fraction = m_hitFraction; + SphereShape pointShape(m_ccdSphereRadius); + TriangleShape triShape(triangle[0],triangle[1],triangle[2]); + VoronoiSimplexSolver simplexSolver; + SubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); + //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); + //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); + //local space? + + if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, + ident,ident,castResult)) + { + if (m_hitFraction > castResult.m_fraction) + m_hitFraction = castResult.m_fraction; + } } + }; - LocalTriangleRaycastCallback raycastCallback(from,to); - - raycastCallback.m_hitFraction = convexbody->m_hitFraction; - - SimdVector3 aabbMin (-1e30f,-1e30f,-1e30f); - SimdVector3 aabbMax (SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + + if (triBody->m_collisionShape->IsConcave()) { + SimdVector3 rayAabbMin = convexFromLocal.getOrigin(); + rayAabbMin.setMin(convexToLocal.getOrigin()); + SimdVector3 rayAabbMax = convexFromLocal.getOrigin(); + rayAabbMax.setMax(convexToLocal.getOrigin()); + rayAabbMin -= SimdVector3(convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius); + rayAabbMax += SimdVector3(convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius); + + float curHitFraction = 1.f; //is this available? + LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, + convexbody->m_ccdSweptShereRadius,curHitFraction); + + raycastCallback.m_hitFraction = convexbody->m_hitFraction; CollisionObject* concavebody = (CollisionObject* )m_concave.m_clientObject; @@ -239,15 +289,16 @@ float ConvexConcaveCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* ,B if (triangleMesh) { - triangleMesh->ProcessAllTriangles(&raycastCallback,aabbMin,aabbMax); + triangleMesh->ProcessAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); } - } + - if (raycastCallback.m_hitFraction < convexbody->m_hitFraction) - { - convexbody->m_hitFraction = raycastCallback.m_hitFraction; - return raycastCallback.m_hitFraction; + if (raycastCallback.m_hitFraction < convexbody->m_hitFraction) + { + convexbody->m_hitFraction = raycastCallback.m_hitFraction; + return raycastCallback.m_hitFraction; + } } return 1.f; diff --git a/Bullet/CollisionShapes/CompoundShape.cpp b/Bullet/CollisionShapes/CompoundShape.cpp index 28fead98b..ecb117013 100644 --- a/Bullet/CollisionShapes/CompoundShape.cpp +++ b/Bullet/CollisionShapes/CompoundShape.cpp @@ -22,7 +22,9 @@ subject to the following restrictions: CompoundShape::CompoundShape() :m_localAabbMin(1e30f,1e30f,1e30f), m_localAabbMax(-1e30f,-1e30f,-1e30f), -m_aabbTree(0) +m_aabbTree(0), +m_collisionMargin(0.f), +m_localScaling(1.f,1.f,1.f) { } diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp index bc47ed14c..826c35e22 100644 --- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp +++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp @@ -29,6 +29,10 @@ subject to the following restrictions: #include "CollisionShapes/CompoundShape.h" #include "CollisionShapes/Simplex1to4Shape.h" #include "CollisionShapes/EmptyShape.h" +#include "CollisionShapes/TriangleMeshShape.h" +#include "CollisionShapes/TriangleIndexVertexArray.h" +#include "CollisionShapes/BvhTriangleMeshShape.h" +#include "CollisionShapes/TriangleMesh.h" #include "Dynamics/RigidBody.h" #include "CollisionDispatch/CollisionDispatcher.h" @@ -94,7 +98,7 @@ bool useCompound = true;//false; #ifdef _DEBUG -const int numObjects = 20; +const int numObjects = 50; #else const int numObjects = 120; #endif @@ -174,6 +178,65 @@ int main(int argc,char** argv) physicsEnvironmentPtr->setDeactivationTime(2.f); physicsEnvironmentPtr->setGravity(0,-10,0);//0,0);//-10,0); + int i; + +//#define USE_TRIMESH_GROUND 1 +#ifdef USE_TRIMESH_GROUND + + +const float TRIANGLE_SIZE=20.f; + + //create a triangle-mesh ground + int vertStride = sizeof(SimdVector3); + int indexStride = 3*sizeof(int); + + const int NUM_VERTS_X = 50; + const int NUM_VERTS_Y = 50; + const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y; + + const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1); + + SimdVector3* gVertices = new SimdVector3[totalVerts]; + int* gIndices = new int[totalTriangles*3]; + + + + for ( i=0;i0) @@ -306,17 +369,21 @@ int main(int argc,char** argv) } - SimdVector3 localInertia; - if (shapePtr[shapeIndex[i]]->GetShapeType() == EMPTY_SHAPE_PROXYTYPE) + SimdVector3 localInertia(0.f,0.f,0.f); + if (shapeIndex[i]) { - //take inertia from first shape - shapePtr[1]->CalculateLocalInertia(shapeProps.m_mass,localInertia); - } else - { - shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia); + if (shapePtr[shapeIndex[i]]->GetShapeType() == EMPTY_SHAPE_PROXYTYPE) + { + //take inertia from first shape + shapePtr[1]->CalculateLocalInertia(shapeProps.m_mass,localInertia); + } else + { + shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia); + } + } ccdObjectCi.m_localInertiaTensor = localInertia; - + ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]]; @@ -326,7 +393,7 @@ int main(int argc,char** argv) physObjects[i]->GetRigidBody()->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS; //Experimental: better estimation of CCD Time of Impact: - //physObjects[i]->GetRigidBody()->m_ccdSweptShereRadius = 0.5*CUBE_HALF_EXTENTS; + physObjects[i]->GetRigidBody()->m_ccdSweptShereRadius = 0.2*CUBE_HALF_EXTENTS; physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]); diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index dc94e809e..44facc69f 100644 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -881,6 +881,11 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) if (!body->IsStatic()) { + if (body->m_hitFraction < 1.f) + { + //set velocity to zero... until we have proper CCD integrated + body->setLinearVelocity(body->getLinearVelocity()*0.5f); + } body->predictIntegratedTransform(timeStep* toi, predictedTrans); body->proceedToTransform( predictedTrans); } diff --git a/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp b/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp index 7cca9f503..60a35e985 100644 --- a/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp @@ -455,10 +455,10 @@ void SimulationIsland::UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface* if (reportMe) { reportMe = false; - //printf("Overflow in AABB, object removed from simulation \n"); - //printf("If you can reproduce this, please email bugs@continuousphysics.com\n"); - //printf("Please include above information, your Platform, version of OS.\n"); - //printf("Thanks.\n"); + printf("Overflow in AABB, object removed from simulation \n"); + printf("If you can reproduce this, please email bugs@continuousphysics.com\n"); + printf("Please include above information, your Platform, version of OS.\n"); + printf("Thanks.\n"); } }