diff --git a/Demos/OpenGL/GL_ShapeDrawer.cpp b/Demos/OpenGL/GL_ShapeDrawer.cpp index 850f70b6d..bfcb5f735 100644 --- a/Demos/OpenGL/GL_ShapeDrawer.cpp +++ b/Demos/OpenGL/GL_ShapeDrawer.cpp @@ -35,7 +35,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btUniformScalingShape.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" - +#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" /// #include "BulletCollision/CollisionShapes/btShapeHull.h" @@ -729,43 +729,41 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons default: { - - if (shape->isConvex()) { - ShapeCache* sc=cache((btConvexShape*)shape); -#if 0 - btConvexShape* convexShape = (btConvexShape*)shape; - if (!shape->getUserPointer()) + const btConvexPolyhedron* poly = shape->isPolyhedral() ? ((btPolyhedralConvexShape*) shape)->getConvexPolyhedron() : 0; + if (poly) { - //create a hull approximation - void* mem = btAlignedAlloc(sizeof(btShapeHull),16); - btShapeHull* hull = new(mem) btShapeHull(convexShape); - - ///cleanup memory - m_shapeHulls.push_back(hull); - - btScalar margin = shape->getMargin(); - hull->buildHull(margin); - convexShape->setUserPointer(hull); - - - // printf("numTriangles = %d\n", hull->numTriangles ()); - // printf("numIndices = %d\n", hull->numIndices ()); - // printf("numVertices = %d\n", hull->numVertices ()); - - - } -#endif - - - - //if (shape->getUserPointer()) + int i; + glBegin (GL_TRIANGLES); + for (i=0;im_faces.size();i++) + { + btVector3 centroid(0,0,0); + int numVerts = poly->m_faces[i].m_indices.size(); + if (numVerts>2) + { + btVector3 v1 = poly->m_vertices[poly->m_faces[i].m_indices[0]]; + for (int v=0;vm_faces[i].m_indices.size()-2;v++) + { + + btVector3 v2 = poly->m_vertices[poly->m_faces[i].m_indices[v+1]]; + btVector3 v3 = poly->m_vertices[poly->m_faces[i].m_indices[v+2]]; + btVector3 normal = (v3-v1).cross(v2-v1); + normal.normalize (); + glNormal3f(normal.getX(),normal.getY(),normal.getZ()); + glVertex3f (v1.x(), v1.y(), v1.z()); + glVertex3f (v2.x(), v2.y(), v2.z()); + glVertex3f (v3.x(), v3.y(), v3.z()); + } + } + } + glEnd (); + } else { + ShapeCache* sc=cache((btConvexShape*)shape); //glutSolidCube(1.0); btShapeHull* hull = &sc->m_shapehull/*(btShapeHull*)shape->getUserPointer()*/; - if (hull->numTriangles () > 0) { int index = 0; diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index d575c427b..dd1f3e249 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -441,15 +441,19 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl //gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); #endif //ZERO_MARGIN - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); - //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); - minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); + minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); #ifdef ZERO_MARGIN - foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f; + foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f; #else - foundSepAxis = gjkPairDetector.getCachedSeparatingDistance()<(min0->getMargin()+min1->getMargin()); + foundSepAxis = gjkPairDetector.getCachedSeparatingDistance()<(min0->getMargin()+min1->getMargin()); #endif + } } if (foundSepAxis) { @@ -486,7 +490,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl btScalar minDist =-1e30f; btScalar maxDist = threshold; - bool foundSepAxis = true; + bool foundSepAxis = false; if (0) { polyhedronB->initializePolyhedralFeatures(); @@ -506,10 +510,15 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); #endif//ZERO_MARGIN - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); - //minDist = gjkPairDetector.getCachedSeparatingDistance(); - //maxDist = threshold; - minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + //minDist = gjkPairDetector.getCachedSeparatingDistance(); + //maxDist = threshold; + minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); + foundSepAxis = true; + } } @@ -544,66 +553,70 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl int i; btVector3 v0,v1; btVector3 sepNormalWorldSpace; + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); - btPlaneSpace1(sepNormalWorldSpace,v0,v1); - - - bool perturbeA = true; - const btScalar angleLimit = 0.125f * SIMD_PI; - btScalar perturbeAngle; - btScalar radiusA = min0->getAngularMotionDisc(); - btScalar radiusB = min1->getAngularMotionDisc(); - if (radiusA < radiusB) + if (l2>SIMD_EPSILON) { - perturbeAngle = gContactBreakingThreshold /radiusA; - perturbeA = true; - } else - { - perturbeAngle = gContactBreakingThreshold / radiusB; - perturbeA = false; - } - if ( perturbeAngle > angleLimit ) - perturbeAngle = angleLimit; - - btTransform unPerturbedTransform; - if (perturbeA) - { - unPerturbedTransform = input.m_transformA; - } else - { - unPerturbedTransform = input.m_transformB; - } - - for ( i=0;iSIMD_EPSILON) - { - btQuaternion perturbeRot(v0,perturbeAngle); - btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); - btQuaternion rotq(sepNormalWorldSpace,iterationAngle); + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); - - if (perturbeA) + btPlaneSpace1(sepNormalWorldSpace,v0,v1); + + + bool perturbeA = true; + const btScalar angleLimit = 0.125f * SIMD_PI; + btScalar perturbeAngle; + btScalar radiusA = min0->getAngularMotionDisc(); + btScalar radiusB = min1->getAngularMotionDisc(); + if (radiusA < radiusB) { - input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis()); - input.m_transformB = body1->getWorldTransform(); -#ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); -#endif //DEBUG_CONTACTS + perturbeAngle = gContactBreakingThreshold /radiusA; + perturbeA = true; } else { - input.m_transformA = body0->getWorldTransform(); - input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis()); -#ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); -#endif + perturbeAngle = gContactBreakingThreshold / radiusB; + perturbeA = false; + } + if ( perturbeAngle > angleLimit ) + perturbeAngle = angleLimit; + + btTransform unPerturbedTransform; + if (perturbeA) + { + unPerturbedTransform = input.m_transformA; + } else + { + unPerturbedTransform = input.m_transformB; } - btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); - gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); + for ( i=0;iSIMD_EPSILON) + { + btQuaternion perturbeRot(v0,perturbeAngle); + btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); + btQuaternion rotq(sepNormalWorldSpace,iterationAngle); + + + if (perturbeA) + { + input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis()); + input.m_transformB = body1->getWorldTransform(); + #ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); + #endif //DEBUG_CONTACTS + } else + { + input.m_transformA = body0->getWorldTransform(); + input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis()); + #ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); + #endif + } + + btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); + gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); + } } - } } diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index fd684c056..bf24246ea 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -64,8 +64,8 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b btAssert(m_manifoldPtr); //order in manifold needs to match -// if (depth > m_manifoldPtr->getContactBreakingThreshold()) - if (depth > m_manifoldPtr->getContactProcessingThreshold()) + if (depth > m_manifoldPtr->getContactBreakingThreshold()) +// if (depth > m_manifoldPtr->getContactProcessingThreshold()) return; bool isSwapped = m_manifoldPtr->getBody0() != m_body0; diff --git a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index a9a887d3e..df1fbe927 100644 --- a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -53,7 +53,7 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures() getVertex(i,newVertex); } - +#if 0 btAlignedObjectArray planeEquations; btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations); @@ -68,11 +68,17 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures() btAlignedObjectArray tmpVertices; btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices); - - btConvexHullComputer conv; conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f); +#else + btConvexHullComputer conv; + conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); + +#endif + + + btAlignedObjectArray faceNormals; int numFaces = conv.faces.size(); faceNormals.resize(numFaces); diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 83d17f2a6..d877f0994 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -197,14 +197,10 @@ public: #endif } + bool validContactDistance(const btManifoldPoint& pt) const { - if (pt.m_lifeTime >1) - { - return pt.m_distance1 <= getContactBreakingThreshold(); - } - return pt.m_distance1 <= getContactProcessingThreshold(); - + return pt.m_distance1 <= getContactBreakingThreshold(); } /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin void refreshContactPoints( const btTransform& trA,const btTransform& trB); diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 1eb3e4105..9b39e6802 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -327,9 +327,6 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) ///perform collision detection performDiscreteCollisionDetection(); - if (getDispatchInfo().m_useContinuous) - addSpeculativeContacts(timeStep); - calculateSimulationIslands(); @@ -951,114 +948,6 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) } } -void btDiscreteDynamicsWorld::addSpeculativeContacts(btScalar timeStep) -{ - BT_PROFILE("addSpeculativeContacts"); - btTransform predictedTrans; - for ( int i=0;isetHitFraction(1.f); - - if (body->isActive() && (!body->isStaticOrKinematicObject())) - { - body->predictIntegratedTransform(timeStep, predictedTrans); - btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - - if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) - { - BT_PROFILE("search speculative contacts"); - if (body->getCollisionShape()->isConvex()) - { - gNumClampedCcdMotions++; - - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); - //btConvexShape* convexShape = static_cast(body->getCollisionShape()); - btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast(body->getCollisionShape()); - - sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; - btTransform modifiedPredictedTrans; - modifiedPredictedTrans = predictedTrans; - modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); - - convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); - if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) - { - btBroadphaseProxy* proxy0 = body->getBroadphaseHandle(); - btBroadphaseProxy* proxy1 = sweepResults.m_hitCollisionObject->getBroadphaseHandle(); - btBroadphasePair* pair = sweepResults.m_pairCache->findPair(proxy0,proxy1); - if (pair) - { - if (pair->m_algorithm) - { - btManifoldArray contacts; - pair->m_algorithm->getAllContactManifolds(contacts); - if (contacts.size()) - { - btManifoldResult result(body,sweepResults.m_hitCollisionObject); - result.setPersistentManifold(contacts[0]); - - btVector3 vec = (modifiedPredictedTrans.getOrigin()-body->getWorldTransform().getOrigin()); - vec*=sweepResults.m_closestHitFraction; - - btScalar lenSqr = vec.length2(); - btScalar depth = 0.f; - btVector3 pointWorld = sweepResults.m_hitPointWorld; - if (lenSqr>SIMD_EPSILON) - { - depth = btSqrt(lenSqr); - pointWorld -= vec; - vec /= depth; - } - - if (contacts[0]->getBody0()==body) - { - result.addContactPoint(sweepResults.m_hitNormalWorld,pointWorld,depth); -#if 0 - debugContacts.push_back(sweepResults.m_hitPointWorld);//sweepResults.m_hitPointWorld); - debugNormals.push_back(sweepResults.m_hitNormalWorld); -#endif - } else - { - //swapped - result.addContactPoint(-sweepResults.m_hitNormalWorld,pointWorld,depth); - //sweepResults.m_hitPointWorld,depth); - -#if 0 - if (1)//firstHit==1) - { - firstHit=0; - debugNormals.push_back(sweepResults.m_hitNormalWorld); - debugContacts.push_back(pointWorld);//sweepResults.m_hitPointWorld); - debugNormals.push_back(sweepResults.m_hitNormalWorld); - debugContacts.push_back(sweepResults.m_hitPointWorld); - } - firstHit--; -#endif - } - } - - } else - { - //no algorithm, use dispatcher to create one - - } - - - } else - { - //add an overlapping pair - //printf("pair missing\n"); - - } - } - } - } - - } - } -} diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 3ba39a1c4..df47c2904 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -62,8 +62,6 @@ protected: virtual void integrateTransforms(btScalar timeStep); - virtual void addSpeculativeContacts(btScalar timeStep); - virtual void calculateSimulationIslands(); virtual void solveConstraints(btContactSolverInfo& solverInfo); diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 96a50a3f0..0fb3560e9 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -329,7 +329,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, { const btScalar scl=(btScalar)0.8; const btScalar alp=(btScalar)1; - const btVector3 col((btScalar)0.7,(btScalar)0.7,(btScalar)0.7); + const btVector3 col((btScalar)0.3,(btScalar)0.3,(btScalar)0.7); for(int i=0;im_tetras.size();++i) { const btSoftBody::Tetra& t=psb->m_tetras[i]; diff --git a/src/LinearMath/btQuickprof.cpp b/src/LinearMath/btQuickprof.cpp index 14534068a..544aee89d 100644 --- a/src/LinearMath/btQuickprof.cpp +++ b/src/LinearMath/btQuickprof.cpp @@ -276,7 +276,8 @@ CProfileNode::CProfileNode( const char * name, CProfileNode * parent ) : RecursionCounter( 0 ), Parent( parent ), Child( NULL ), - Sibling( NULL ) + Sibling( NULL ), + m_userPtr(0) { Reset(); } diff --git a/src/LinearMath/btQuickprof.h b/src/LinearMath/btQuickprof.h index 0da13ab3c..93f3f4a60 100644 --- a/src/LinearMath/btQuickprof.h +++ b/src/LinearMath/btQuickprof.h @@ -82,7 +82,8 @@ public: const char * Get_Name( void ) { return Name; } int Get_Total_Calls( void ) { return TotalCalls; } float Get_Total_Time( void ) { return TotalTime; } - + void* GetUserPointer() const {return m_userPtr;} + void SetUserPointer(void* ptr) { m_userPtr = ptr;} protected: const char * Name; @@ -94,6 +95,7 @@ protected: CProfileNode * Parent; CProfileNode * Child; CProfileNode * Sibling; + void* m_userPtr; }; ///An iterator to navigate through the tree @@ -115,15 +117,20 @@ public: int Get_Current_Total_Calls( void ) { return CurrentChild->Get_Total_Calls(); } float Get_Current_Total_Time( void ) { return CurrentChild->Get_Total_Time(); } + void* Get_Current_UserPointer( void ) { return CurrentChild->GetUserPointer(); } + void Set_Current_UserPointer(void* ptr) {CurrentChild->SetUserPointer(ptr);} // Access the current parent const char * Get_Current_Parent_Name( void ) { return CurrentParent->Get_Name(); } int Get_Current_Parent_Total_Calls( void ) { return CurrentParent->Get_Total_Calls(); } float Get_Current_Parent_Total_Time( void ) { return CurrentParent->Get_Total_Time(); } + + protected: CProfileNode * CurrentParent; CProfileNode * CurrentChild; + CProfileIterator( CProfileNode * start ); friend class CProfileManager;