From 50a2694c5bdf6499decfcd286978dabe5cf2d2b4 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Mon, 24 Jul 2006 23:06:59 +0000 Subject: [PATCH] more compounds work, the basics work. now some stackless tree-tree traversal is needed to speedup compound versus compound. --- Bullet/CollisionDispatch/CollisionObject.cpp | 1 + Bullet/CollisionDispatch/CollisionObject.h | 4 ++- .../CompoundCollisionAlgorithm.cpp | 9 +++++ Bullet/CollisionDispatch/ManifoldResult.cpp | 8 +++-- .../SimulationIslandManager.cpp | 3 ++ Bullet/CollisionShapes/CompoundShape.cpp | 1 + Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp | 34 +++++++++++++------ Demos/OpenGL/GLDebugDrawer.cpp | 3 +- Demos/OpenGL/GL_ShapeDrawer.cpp | 14 ++++---- .../CcdPhysics/CcdPhysicsEnvironment.cpp | 22 +++++++++--- .../CcdPhysics/ParallelIslandDispatcher.cpp | 14 ++++++++ .../CcdPhysics/SimulationIsland.cpp | 3 ++ 12 files changed, 92 insertions(+), 24 deletions(-) diff --git a/Bullet/CollisionDispatch/CollisionObject.cpp b/Bullet/CollisionDispatch/CollisionObject.cpp index aec329184..a96707f5e 100644 --- a/Bullet/CollisionDispatch/CollisionObject.cpp +++ b/Bullet/CollisionDispatch/CollisionObject.cpp @@ -25,6 +25,7 @@ CollisionObject::CollisionObject() m_ccdSweptShereRadius(0.f), m_ccdSquareMotionTreshold(0.f) { + m_cachedInvertedWorldTransform.setIdentity(); } diff --git a/Bullet/CollisionDispatch/CollisionObject.h b/Bullet/CollisionDispatch/CollisionObject.h index a6dfeb862..c770574c9 100644 --- a/Bullet/CollisionDispatch/CollisionObject.h +++ b/Bullet/CollisionDispatch/CollisionObject.h @@ -38,7 +38,9 @@ struct CollisionObject //m_interpolationWorldTransform is used for CCD and interpolation //it can be either previous or future (predicted) transform SimdTransform m_interpolationWorldTransform; - + + SimdTransform m_cachedInvertedWorldTransform; + enum CollisionFlags { isStatic = 1, diff --git a/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp b/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp index f42497999..ef27c34c0 100644 --- a/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp +++ b/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp @@ -72,11 +72,20 @@ void CompoundCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,BroadphaseP //temporarily exchange parent CollisionShape with childShape, and recurse CollisionShape* childShape = compoundShape->GetChildShape(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; m_childCollisionAlgorithms[i]->ProcessCollision(&m_childProxies[i],&m_otherProxy,dispatchInfo); //revert back colObj->m_collisionShape =orgShape; + colObj->m_worldTransform = orgTrans; } } diff --git a/Bullet/CollisionDispatch/ManifoldResult.cpp b/Bullet/CollisionDispatch/ManifoldResult.cpp index 15b6512a5..b5bfc18b7 100644 --- a/Bullet/CollisionDispatch/ManifoldResult.cpp +++ b/Bullet/CollisionDispatch/ManifoldResult.cpp @@ -31,8 +31,12 @@ void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const S if (depth > m_manifoldPtr->GetContactBreakingTreshold()) return; - SimdTransform transAInv = m_body0->m_worldTransform.inverse(); - SimdTransform transBInv= m_body1->m_worldTransform.inverse(); + + SimdTransform transAInv = m_body0->m_cachedInvertedWorldTransform; + SimdTransform transBInv= m_body1->m_cachedInvertedWorldTransform; + + //transAInv = m_body0->m_worldTransform.inverse(); + //transBInv= m_body1->m_worldTransform.inverse(); SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth; SimdVector3 localA = transAInv(pointA ); SimdVector3 localB = transBInv(pointInWorld); diff --git a/Bullet/CollisionDispatch/SimulationIslandManager.cpp b/Bullet/CollisionDispatch/SimulationIslandManager.cpp index 6e791fc5c..0850192dc 100644 --- a/Bullet/CollisionDispatch/SimulationIslandManager.cpp +++ b/Bullet/CollisionDispatch/SimulationIslandManager.cpp @@ -113,6 +113,7 @@ void SimulationIslandManager::StoreIslandActivationState(CollisionWorld* colWorl // void SimulationIslandManager::BuildAndProcessIslands(Dispatcher* dispatcher,CollisionObjectArray& collisionObjects, IslandCallback* callback) { + int numBodies = collisionObjects.size(); for (int islandId=0;islandIdm_islandTag1 == islandId) { + if (colObj0->GetActivationState()== ACTIVE_TAG) { allSleeping = false; diff --git a/Bullet/CollisionShapes/CompoundShape.cpp b/Bullet/CollisionShapes/CompoundShape.cpp index 3de0c61db..80539962c 100644 --- a/Bullet/CollisionShapes/CompoundShape.cpp +++ b/Bullet/CollisionShapes/CompoundShape.cpp @@ -77,6 +77,7 @@ void CompoundShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) { //approximation: take the inertia from the aabb for now SimdTransform ident; + ident.setIdentity(); SimdVector3 aabbMin,aabbMax; GetAabb(ident,aabbMin,aabbMax); diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp index 9b60bed20..bc47ed14c 100644 --- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp +++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp @@ -54,7 +54,7 @@ subject to the following restrictions: float deltaTime = 1.f/60.f; float bulletSpeed = 40.f; -bool createConstraint = true; + #ifdef WIN32 #if _MSC_VER >= 1310 //only use SIMD Hull code under Win32 @@ -89,6 +89,9 @@ extern int glutScreenHeight; const int maxProxies = 32766; const int maxOverlap = 65535; +bool createConstraint = true;//false; +bool useCompound = true;//false; + #ifdef _DEBUG const int numObjects = 20; @@ -124,7 +127,7 @@ CollisionShape* shapePtr[numShapes] = #ifdef USE_GROUND_PLANE new StaticPlaneShape(SimdVector3(0,1,0),10), #else - new BoxShape (SimdVector3(450,10,450)), + new BoxShape (SimdVector3(50,10,50)), #endif new BoxShape (SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)), @@ -170,7 +173,7 @@ int main(int argc,char** argv) #endif physicsEnvironmentPtr->setDeactivationTime(2.f); - physicsEnvironmentPtr->setGravity(0,-10,0); + physicsEnvironmentPtr->setGravity(0,-10,0);//0,0);//-10,0); PHY_ShapeProps shapeProps; shapeProps.m_do_anisotropic = false; @@ -205,13 +208,19 @@ int main(int argc,char** argv) shapeIndex[i] = 0; } - CompoundShape* compoundShape = new CompoundShape(); - //shapePtr[1] = compoundShape; + if (useCompound) + { + CompoundShape* compoundShape = new CompoundShape(); + CollisionShape* oldShape = shapePtr[1]; + shapePtr[1] = compoundShape; - SimdTransform ident; - ident.setIdentity(); - - compoundShape->AddChildShape(ident,new BoxShape (SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS))); + SimdTransform ident; + ident.setIdentity(); + ident.setOrigin(SimdVector3(0,0,0)); + compoundShape->AddChildShape(ident,oldShape);// + ident.setOrigin(SimdVector3(0,0,2)); + compoundShape->AddChildShape(ident,new SphereShape(0.9));// + } for (i=0;iGetRigidBody()->GetCollisionShape(),wireColor,getDebugMode()); glPopMatrix(); + ///this block is just experimental code to show some internal issues with replacing shapes on the fly. if (getDebugMode()!=0 && (i>0)) { @@ -668,6 +679,9 @@ void clientDisplay(void) { physicsEnvironmentPtr->UpdateAabbs(deltaTime); + //draw contactpoints + physicsEnvironmentPtr->CallbackTriggers(); + renderme(); diff --git a/Demos/OpenGL/GLDebugDrawer.cpp b/Demos/OpenGL/GLDebugDrawer.cpp index bdeb5c5ed..3522180ee 100644 --- a/Demos/OpenGL/GLDebugDrawer.cpp +++ b/Demos/OpenGL/GLDebugDrawer.cpp @@ -51,7 +51,8 @@ void GLDebugDrawer::DrawContactPoint(const SimdVector3& pointOnB,const SimdVecto glVertex3d(from.getX(), from.getY(), from.getZ()); glVertex3d(to.getX(), to.getY(), to.getZ()); glEnd(); - + + glRasterPos3f(from.x(), from.y(), from.z()); char buf[12]; sprintf(buf," %d",lifeTime); diff --git a/Demos/OpenGL/GL_ShapeDrawer.cpp b/Demos/OpenGL/GL_ShapeDrawer.cpp index 175749ef7..91d5b4c5c 100644 --- a/Demos/OpenGL/GL_ShapeDrawer.cpp +++ b/Demos/OpenGL/GL_ShapeDrawer.cpp @@ -114,7 +114,7 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim if (shape->GetShapeType() == COMPOUND_SHAPE_PROXYTYPE) { const CompoundShape* compoundShape = static_cast(shape); - for (int i=0;iGetNumChildShapes();i++) + for (int i=compoundShape->GetNumChildShapes()-1;i>=0;i--) { SimdTransform childTrans = compoundShape->GetChildTransform(i); const CollisionShape* colShape = compoundShape->GetChildShape(i); @@ -123,16 +123,15 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim DrawOpenGL(childMat,colShape,color,debugMode); } - return; } else { //DrawCoordSystem(); - glPushMatrix(); + //glPushMatrix(); glEnable(GL_COLOR_MATERIAL); glColor3f(color.x(),color.y(), color.z()); - glRasterPos3f(0.0, 0.0, 0.0); + bool useWireframeFallback = true; @@ -268,6 +267,7 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim if (debugMode==IDebugDraw::DBG_DrawFeaturesText) { + glRasterPos3f(0.0, 0.0, 0.0); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->GetExtraDebugInfo()); glColor3f(1.f, 1.f, 1.f); @@ -329,7 +329,7 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim } - glDisable(GL_DEPTH_BUFFER_BIT); + /*glDisable(GL_DEPTH_BUFFER_BIT); if (debugMode==IDebugDraw::DBG_DrawText) { BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->GetName()); @@ -340,7 +340,9 @@ void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const Sim BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->GetExtraDebugInfo()); } glEnable(GL_DEPTH_BUFFER_BIT); - glPopMatrix(); + */ + + // glPopMatrix(); } glPopMatrix(); diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index eec1cca26..e8bfd38d5 100644 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -386,7 +386,8 @@ void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl) assert(shapeinterface); const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform(); - + + body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); SimdPoint3 minAabb,maxAabb; @@ -638,6 +639,9 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) CcdPhysicsController* ctrl = m_controllers[k]; // SimdTransform predictedTrans; RigidBody* body = ctrl->GetRigidBody(); + + body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); + if (body->IsActive()) { if (!body->IsStatic()) @@ -1558,10 +1562,10 @@ void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctr void CcdPhysicsEnvironment::CallbackTriggers() { - /* + CcdPhysicsController* ctrl0=0,*ctrl1=0; - if (m_triggerCallbacks[PHY_OBJECT_RESPONSE]) + if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawContactPoints))) { //walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback int numManifolds = m_collisionWorld->GetDispatcher()->GetNumManifolds(); @@ -1571,6 +1575,16 @@ void CcdPhysicsEnvironment::CallbackTriggers() int numContacts = manifold->GetNumContacts(); if (numContacts) { + if (m_debugDrawer && (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawContactPoints)) + { + for (int j=0;jGetContactPoint(j); + if (m_debugDrawer) + m_debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color); + } + } RigidBody* obj0 = static_cast(manifold->GetBody0()); RigidBody* obj1 = static_cast(manifold->GetBody1()); @@ -1596,7 +1610,7 @@ void CcdPhysicsEnvironment::CallbackTriggers() } -*/ + } diff --git a/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp b/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp index b53ee273c..89a8af1fd 100644 --- a/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp @@ -21,6 +21,8 @@ subject to the following restrictions: #include "CollisionDispatch/ConvexConvexAlgorithm.h" #include "CollisionDispatch/EmptyCollisionAlgorithm.h" #include "CollisionDispatch/ConvexConcaveCollisionAlgorithm.h" +#include "CollisionDispatch/CompoundCollisionAlgorithm.h" + #include "CollisionShapes/CollisionShape.h" #include "CollisionDispatch/CollisionObject.h" #include @@ -211,6 +213,18 @@ CollisionAlgorithm* ParallelIslandDispatcher::InternalFindAlgorithm(BroadphasePr return new ConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0); } + if (body0->m_collisionShape->IsCompound()) + { + return new CompoundCollisionAlgorithm(ci,&proxy0,&proxy1); + } else + { + if (body1->m_collisionShape->IsCompound()) + { + return new CompoundCollisionAlgorithm(ci,&proxy1,&proxy0); + } + } + + //failed to find an algorithm return new EmptyAlgorithm(ci); diff --git a/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp b/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp index 198a0bebe..60a35e985 100644 --- a/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp @@ -46,6 +46,9 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations, CcdPhysicsController* ctrl = m_controllers[k]; // SimdTransform predictedTrans; RigidBody* body = ctrl->GetRigidBody(); + //todo: only do this when necessary, it's used for contact points + body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); + if (body->IsActive()) { if (!body->IsStatic())