diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/CollisionDispatch')
57 files changed, 5438 insertions, 5031 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index 63401780970..7647f673600 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -18,145 +18,162 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" - -SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold) -:m_sphere(sphere), -m_triangle(triangle), -m_contactBreakingThreshold(contactBreakingThreshold) +SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold) + : m_sphere(sphere), + m_triangle(triangle), + m_contactBreakingThreshold(contactBreakingThreshold) { - } -void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults) { - (void)debugDraw; const btTransform& transformA = input.m_transformA; const btTransform& transformB = input.m_transformB; - btVector3 point,normal; + btVector3 point, normal; btScalar timeOfImpact = btScalar(1.); btScalar depth = btScalar(0.); -// output.m_distance = btScalar(BT_LARGE_FLOAT); + // output.m_distance = btScalar(BT_LARGE_FLOAT); //move sphere into triangle space - btTransform sphereInTr = transformB.inverseTimes(transformA); + btTransform sphereInTr = transformB.inverseTimes(transformA); - if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold)) + if (collide(sphereInTr.getOrigin(), point, normal, depth, timeOfImpact, m_contactBreakingThreshold)) { if (swapResults) { - btVector3 normalOnB = transformB.getBasis()*normal; + btVector3 normalOnB = transformB.getBasis() * normal; btVector3 normalOnA = -normalOnB; - btVector3 pointOnA = transformB*point+normalOnB*depth; - output.addContactPoint(normalOnA,pointOnA,depth); - } else + btVector3 pointOnA = transformB * point + normalOnB * depth; + output.addContactPoint(normalOnA, pointOnA, depth); + } + else { - output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth); + output.addContactPoint(transformB.getBasis() * normal, transformB * point, depth); } } - } - - // See also geometrictools.com // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv -btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest); +btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest); -btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) { +btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest) +{ btVector3 diff = p - from; btVector3 v = to - from; btScalar t = v.dot(diff); - - if (t > 0) { + + if (t > 0) + { btScalar dotVV = v.dot(v); - if (t < dotVV) { + if (t < dotVV) + { t /= dotVV; - diff -= t*v; - } else { + diff -= t * v; + } + else + { t = 1; diff -= v; } - } else + } + else t = 0; - nearest = from + t*v; - return diff.dot(diff); + nearest = from + t * v; + return diff.dot(diff); } -bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) { +bool SphereTriangleDetector::facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal) +{ btVector3 lp(p); btVector3 lnormal(normal); - + return pointInTriangle(vertices, lnormal, &lp); } -bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold) +bool SphereTriangleDetector::collide(const btVector3& sphereCenter, btVector3& point, btVector3& resultNormal, btScalar& depth, btScalar& timeOfImpact, btScalar contactBreakingThreshold) { - const btVector3* vertices = &m_triangle->getVertexPtr(0); - + btScalar radius = m_sphere->getRadius(); btScalar radiusWithThreshold = radius + contactBreakingThreshold; - btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); - normal.normalize(); - btVector3 p1ToCentre = sphereCenter - vertices[0]; - btScalar distanceFromPlane = p1ToCentre.dot(normal); - - if (distanceFromPlane < btScalar(0.)) - { - //triangle facing the other way - distanceFromPlane *= btScalar(-1.); - normal *= btScalar(-1.); - } + btVector3 normal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]); - bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; - - // Check for contact / intersection + btScalar l2 = normal.length2(); bool hasContact = false; btVector3 contactPoint; - if (isInsideContactPlane) { - if (facecontains(sphereCenter,vertices,normal)) { - // Inside the contact wedge - touches a point on the shell plane - hasContact = true; - contactPoint = sphereCenter - normal*distanceFromPlane; - } else { - // Could be inside one of the contact capsules - btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold; - btVector3 nearestOnEdge; - for (int i = 0; i < m_triangle->getNumEdges(); i++) { - - btVector3 pa; - btVector3 pb; - - m_triangle->getEdge(i,pa,pb); - - btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge); - if (distanceSqr < contactCapsuleRadiusSqr) { - // Yep, we're inside a capsule - hasContact = true; - contactPoint = nearestOnEdge; + + if (l2 >= SIMD_EPSILON * SIMD_EPSILON) + { + normal /= btSqrt(l2); + + btVector3 p1ToCentre = sphereCenter - vertices[0]; + btScalar distanceFromPlane = p1ToCentre.dot(normal); + + if (distanceFromPlane < btScalar(0.)) + { + //triangle facing the other way + distanceFromPlane *= btScalar(-1.); + normal *= btScalar(-1.); + } + + bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; + + // Check for contact / intersection + + if (isInsideContactPlane) + { + if (facecontains(sphereCenter, vertices, normal)) + { + // Inside the contact wedge - touches a point on the shell plane + hasContact = true; + contactPoint = sphereCenter - normal * distanceFromPlane; + } + else + { + // Could be inside one of the contact capsules + btScalar contactCapsuleRadiusSqr = radiusWithThreshold * radiusWithThreshold; + btScalar minDistSqr = contactCapsuleRadiusSqr; + btVector3 nearestOnEdge; + for (int i = 0; i < m_triangle->getNumEdges(); i++) + { + btVector3 pa; + btVector3 pb; + + m_triangle->getEdge(i, pa, pb); + + btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge); + if (distanceSqr < minDistSqr) + { + // Yep, we're inside a capsule, and record the capsule with smallest distance + minDistSqr = distanceSqr; + hasContact = true; + contactPoint = nearestOnEdge; + } } - } } } - if (hasContact) { + if (hasContact) + { btVector3 contactToCentre = sphereCenter - contactPoint; btScalar distanceSqr = contactToCentre.length2(); - if (distanceSqr < radiusWithThreshold*radiusWithThreshold) + if (distanceSqr < radiusWithThreshold * radiusWithThreshold) { - if (distanceSqr>SIMD_EPSILON) + if (distanceSqr > SIMD_EPSILON) { btScalar distance = btSqrt(distanceSqr); resultNormal = contactToCentre; resultNormal.normalize(); point = contactPoint; - depth = -(radius-distance); - } else + depth = -(radius - distance); + } + else { resultNormal = normal; point = contactPoint; @@ -165,36 +182,34 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po return true; } } - + return false; } - -bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ) +bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p) { const btVector3* p1 = &vertices[0]; const btVector3* p2 = &vertices[1]; const btVector3* p3 = &vertices[2]; - btVector3 edge1( *p2 - *p1 ); - btVector3 edge2( *p3 - *p2 ); - btVector3 edge3( *p1 - *p3 ); + btVector3 edge1(*p2 - *p1); + btVector3 edge2(*p3 - *p2); + btVector3 edge3(*p1 - *p3); - btVector3 p1_to_p( *p - *p1 ); - btVector3 p2_to_p( *p - *p2 ); - btVector3 p3_to_p( *p - *p3 ); + btVector3 p1_to_p(*p - *p1); + btVector3 p2_to_p(*p - *p2); + btVector3 p3_to_p(*p - *p3); + + btVector3 edge1_normal(edge1.cross(normal)); + btVector3 edge2_normal(edge2.cross(normal)); + btVector3 edge3_normal(edge3.cross(normal)); - btVector3 edge1_normal( edge1.cross(normal)); - btVector3 edge2_normal( edge2.cross(normal)); - btVector3 edge3_normal( edge3.cross(normal)); - btScalar r1, r2, r3; - r1 = edge1_normal.dot( p1_to_p ); - r2 = edge2_normal.dot( p2_to_p ); - r3 = edge3_normal.dot( p3_to_p ); - if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || - ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) + r1 = edge1_normal.dot(p1_to_p); + r2 = edge2_normal.dot(p2_to_p); + r3 = edge3_normal.dot(p3_to_p); + if ((r1 > 0 && r2 > 0 && r3 > 0) || + (r1 <= 0 && r2 <= 0 && r3 <= 0)) return true; return false; - } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index 22953af43fd..d47e47530d4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -18,34 +18,26 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" - - class btSphereShape; class btTriangleShape; - - /// sphere-triangle to match the btDiscreteCollisionDetectorInterface struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface { - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false); - SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle, btScalar contactBreakingThreshold); + SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold); - virtual ~SphereTriangleDetector() {}; + virtual ~SphereTriangleDetector(){}; - bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold); + bool collide(const btVector3& sphereCenter, btVector3& point, btVector3& resultNormal, btScalar& depth, btScalar& timeOfImpact, btScalar contactBreakingThreshold); private: - - - bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ); - bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal); + bool pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p); + bool facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal); btSphereShape* m_sphere; btTriangleShape* m_triangle; - btScalar m_contactBreakingThreshold; - + btScalar m_contactBreakingThreshold; }; -#endif //BT_SPHERE_TRIANGLE_DETECTOR_H - +#endif //BT_SPHERE_TRIANGLE_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp index 57f14649353..ac5de45d271 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp @@ -17,31 +17,31 @@ subject to the following restrictions: #include "btCollisionDispatcher.h" #include "btCollisionObject.h" -btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci) -:btCollisionAlgorithm(ci) +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btCollisionAlgorithm(ci) //, //m_colObj0(0), //m_colObj1(0) { } -btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* ) -:btCollisionAlgorithm(ci) +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper*, const btCollisionObjectWrapper*) + : btCollisionAlgorithm(ci) //, //m_colObj0(0), //m_colObj1(0) { -// if (ci.m_dispatcher1->needsCollision(colObj0,colObj1)) -// { -// m_colObj0 = colObj0; -// m_colObj1 = colObj1; -// -// m_colObj0->activate(); -// m_colObj1->activate(); -// } + // if (ci.m_dispatcher1->needsCollision(colObj0,colObj1)) + // { + // m_colObj0 = colObj0; + // m_colObj1 = colObj1; + // + // m_colObj0->activate(); + // m_colObj1->activate(); + // } } btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm() { -// m_colObj0->activate(); -// m_colObj1->activate(); + // m_colObj0->activate(); + // m_colObj1->activate(); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h index 489812b9663..862060571b5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h @@ -21,16 +21,15 @@ subject to the following restrictions: ///This class is not enabled yet (work-in-progress) to more aggressively activate objects. class btActivatingCollisionAlgorithm : public btCollisionAlgorithm { -// btCollisionObject* m_colObj0; -// btCollisionObject* m_colObj1; + // btCollisionObject* m_colObj0; + // btCollisionObject* m_colObj1; -public: - - btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci); +protected: + btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci); - btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); +public: virtual ~btActivatingCollisionAlgorithm(); - }; -#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H +#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp index c1da9f36cfb..6873a95d90a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp @@ -26,61 +26,55 @@ subject to the following restrictions: #define USE_PERSISTENT_CONTACTS 1 -btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap) -: btActivatingCollisionAlgorithm(ci,obj0Wrap,obj1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf) +btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap) + : btActivatingCollisionAlgorithm(ci, obj0Wrap, obj1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf) { - if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject())) + if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(), obj1Wrap->getCollisionObject())) { - m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(), obj1Wrap->getCollisionObject()); m_ownManifold = true; } } btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm() { - if (m_ownManifold) { if (m_manifoldPtr) m_dispatcher->releaseManifold(m_manifoldPtr); } - } - -void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB); +void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB); //#include <stdio.h> -void btBox2dBox2dCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btBox2dBox2dCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { if (!m_manifoldPtr) return; - const btBox2dShape* box0 = (const btBox2dShape*)body0Wrap->getCollisionShape(); const btBox2dShape* box1 = (const btBox2dShape*)body1Wrap->getCollisionShape(); resultOut->setPersistentManifold(m_manifoldPtr); - b2CollidePolygons(resultOut,box0,body0Wrap->getWorldTransform(),box1,body1Wrap->getWorldTransform()); + b2CollidePolygons(resultOut, box0, body0Wrap->getWorldTransform(), box1, body1Wrap->getWorldTransform()); // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added if (m_ownManifold) { resultOut->refreshContactPoints(); } - } -btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +btScalar btBox2dBox2dCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/) { //not yet return 1.f; } - struct ClipVertex { btVector3 v; @@ -89,16 +83,16 @@ struct ClipVertex //b2ContactID id; }; -#define b2Dot(a,b) (a).dot(b) -#define b2Mul(a,b) (a)*(b) -#define b2MulT(a,b) (a).transpose()*(b) -#define b2Cross(a,b) (a).cross(b) -#define btCrossS(a,s) btVector3(s * a.getY(), -s * a.getX(),0.f) +#define b2Dot(a, b) (a).dot(b) +#define b2Mul(a, b) (a) * (b) +#define b2MulT(a, b) (a).transpose() * (b) +#define b2Cross(a, b) (a).cross(b) +#define btCrossS(a, s) btVector3(s* a.getY(), -s* a.getX(), 0.f) -int b2_maxManifoldPoints =2; +int b2_maxManifoldPoints = 2; static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], - const btVector3& normal, btScalar offset) + const btVector3& normal, btScalar offset) { // Start with no output points int numOut = 0; @@ -133,7 +127,7 @@ static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], // Find the separation between poly1 and poly2 for a give edge normal on poly1. static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1, int edge1, - const btBox2dShape* poly2, const btTransform& xf2) + const btBox2dShape* poly2, const btTransform& xf2) { const btVector3* vertices1 = poly1->getVertices(); const btVector3* normals1 = poly1->getNormals(); @@ -151,8 +145,8 @@ static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1 int index = 0; btScalar minDot = BT_LARGE_FLOAT; - if( count2 > 0 ) - index = (int) normal1.minDot( vertices2, count2, minDot); + if (count2 > 0) + index = (int)normal1.minDot(vertices2, count2, minDot); btVector3 v1 = b2Mul(xf1, vertices1[edge1]); btVector3 v2 = b2Mul(xf2, vertices2[index]); @@ -162,8 +156,8 @@ static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1 // Find the max separation between poly1 and poly2 using edge normals from poly1. static btScalar FindMaxSeparation(int* edgeIndex, - const btBox2dShape* poly1, const btTransform& xf1, - const btBox2dShape* poly2, const btTransform& xf2) + const btBox2dShape* poly1, const btTransform& xf1, + const btBox2dShape* poly2, const btTransform& xf2) { int count1 = poly1->getVertexCount(); const btVector3* normals1 = poly1->getNormals(); @@ -175,8 +169,8 @@ static btScalar FindMaxSeparation(int* edgeIndex, // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; btScalar maxDot; - if( count1 > 0 ) - edge = (int) dLocal1.maxDot( normals1, count1, maxDot); + if (count1 > 0) + edge = (int)dLocal1.maxDot(normals1, count1, maxDot); // Get the separation for the edge normal. btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); @@ -224,7 +218,7 @@ static btScalar FindMaxSeparation(int* edgeIndex, } // Perform a local search for the best edge normal. - for ( ; ; ) + for (;;) { if (increment == -1) edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; @@ -285,14 +279,14 @@ static void FindIncidentEdge(ClipVertex c[2], int i2 = i1 + 1 < count2 ? i1 + 1 : 0; c[0].v = b2Mul(xf2, vertices2[i1]); -// c[0].id.features.referenceEdge = (unsigned char)edge1; -// c[0].id.features.incidentEdge = (unsigned char)i1; -// c[0].id.features.incidentVertex = 0; + // c[0].id.features.referenceEdge = (unsigned char)edge1; + // c[0].id.features.incidentEdge = (unsigned char)i1; + // c[0].id.features.incidentVertex = 0; c[1].v = b2Mul(xf2, vertices2[i2]); -// c[1].id.features.referenceEdge = (unsigned char)edge1; -// c[1].id.features.incidentEdge = (unsigned char)i2; -// c[1].id.features.incidentVertex = 1; + // c[1].id.features.referenceEdge = (unsigned char)edge1; + // c[1].id.features.incidentEdge = (unsigned char)i2; + // c[1].id.features.incidentVertex = 1; } // Find edge normal of max separation on A - return if separating axis is found @@ -303,10 +297,9 @@ static void FindIncidentEdge(ClipVertex c[2], // The normal points from 1 to 2 void b2CollidePolygons(btManifoldResult* manifold, - const btBox2dShape* polyA, const btTransform& xfA, - const btBox2dShape* polyB, const btTransform& xfB) + const btBox2dShape* polyA, const btTransform& xfA, + const btBox2dShape* polyB, const btTransform& xfB) { - int edgeA = 0; btScalar separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB); if (separationA > 0.0f) @@ -317,10 +310,10 @@ void b2CollidePolygons(btManifoldResult* manifold, if (separationB > 0.0f) return; - const btBox2dShape* poly1; // reference poly - const btBox2dShape* poly2; // incident poly + const btBox2dShape* poly1; // reference poly + const btBox2dShape* poly2; // incident poly btTransform xf1, xf2; - int edge1; // reference edge + int edge1; // reference edge unsigned char flip; const btScalar k_relativeTol = 0.98f; const btScalar k_absoluteTol = 0.001f; @@ -352,14 +345,13 @@ void b2CollidePolygons(btManifoldResult* manifold, const btVector3* vertices1 = poly1->getVertices(); btVector3 v11 = vertices1[edge1]; - btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0]; + btVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0]; //btVector3 dv = v12 - v11; btVector3 sideNormal = b2Mul(xf1.getBasis(), v12 - v11); sideNormal.normalize(); btVector3 frontNormal = btCrossS(sideNormal, 1.0f); - - + v11 = b2Mul(xf1, v11); v12 = b2Mul(xf1, v12); @@ -369,13 +361,12 @@ void b2CollidePolygons(btManifoldResult* manifold, // Clip incident edge against extruded edge1 side edges. ClipVertex clipPoints1[2]; - clipPoints1[0].v.setValue(0,0,0); - clipPoints1[1].v.setValue(0,0,0); + clipPoints1[0].v.setValue(0, 0, 0); + clipPoints1[1].v.setValue(0, 0, 0); ClipVertex clipPoints2[2]; - clipPoints2[0].v.setValue(0,0,0); - clipPoints2[1].v.setValue(0,0,0); - + clipPoints2[0].v.setValue(0, 0, 0); + clipPoints2[1].v.setValue(0, 0, 0); int np; @@ -386,7 +377,7 @@ void b2CollidePolygons(btManifoldResult* manifold, return; // Clip to negative box side 1 - np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2); + np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2); if (np < 2) { @@ -403,19 +394,18 @@ void b2CollidePolygons(btManifoldResult* manifold, if (separation <= 0.0f) { - //b2ManifoldPoint* cp = manifold->points + pointCount; //btScalar separation = separation; //cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v); //cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v); - manifold->addContactPoint(-manifoldNormal,clipPoints2[i].v,separation); + manifold->addContactPoint(-manifoldNormal, clipPoints2[i].v, separation); -// cp->id = clipPoints2[i].id; -// cp->id.features.flip = flip; + // cp->id = clipPoints2[i].id; + // cp->id.features.flip = flip; ++pointCount; } } -// manifold->pointCount = pointCount;} + // manifold->pointCount = pointCount;} } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h index 6ea6e89bda6..3b66d1fd0b9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h @@ -26,22 +26,22 @@ class btPersistentManifold; ///box-box collision detection class btBox2dBox2dCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + public: btBox2dBox2dCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btActivatingCollisionAlgorithm(ci) {} - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); virtual ~btBox2dBox2dCollisionAlgorithm(); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { @@ -49,18 +49,15 @@ public: } } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { int bbsize = sizeof(btBox2dBox2dCollisionAlgorithm); void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); - return new(ptr) btBox2dBox2dCollisionAlgorithm(0,ci,body0Wrap,body1Wrap); + return new (ptr) btBox2dBox2dCollisionAlgorithm(0, ci, body0Wrap, body1Wrap); } }; - }; -#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H - +#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp index ac68968f590..7a391e059a7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp @@ -21,14 +21,14 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" #define USE_PERSISTENT_CONTACTS 1 -btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf) +btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf) { - if (!m_manifoldPtr && m_dispatcher->needsCollision(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject())) + if (!m_manifoldPtr && m_dispatcher->needsCollision(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject())) { - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } } @@ -42,30 +42,27 @@ btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm() } } -void btBoxBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btBoxBoxCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { if (!m_manifoldPtr) return; - const btBoxShape* box0 = (btBoxShape*)body0Wrap->getCollisionShape(); const btBoxShape* box1 = (btBoxShape*)body1Wrap->getCollisionShape(); - - /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); -#ifndef USE_PERSISTENT_CONTACTS +#ifndef USE_PERSISTENT_CONTACTS m_manifoldPtr->clearManifold(); -#endif //USE_PERSISTENT_CONTACTS +#endif //USE_PERSISTENT_CONTACTS btDiscreteCollisionDetectorInterface::ClosestPointInput input; input.m_maximumDistanceSquared = BT_LARGE_FLOAT; input.m_transformA = body0Wrap->getWorldTransform(); input.m_transformB = body1Wrap->getWorldTransform(); - btBoxBoxDetector detector(box0,box1); - detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + btBoxBoxDetector detector(box0, box1); + detector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); #ifdef USE_PERSISTENT_CONTACTS // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added @@ -73,11 +70,10 @@ void btBoxBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrappe { resultOut->refreshContactPoints(); } -#endif //USE_PERSISTENT_CONTACTS - +#endif //USE_PERSISTENT_CONTACTS } -btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/, btCollisionObject* /*body1*/, const btDispatcherInfo& /*dispatchInfo*/, btManifoldResult* /*resultOut*/) { //not yet return 1.f; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h index 59808df5a9d..eb210657659 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h @@ -26,22 +26,22 @@ class btPersistentManifold; ///box-box collision detection class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + public: btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btActivatingCollisionAlgorithm(ci) {} - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btBoxBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); virtual ~btBoxBoxCollisionAlgorithm(); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { @@ -49,18 +49,15 @@ public: } } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { int bbsize = sizeof(btBoxBoxCollisionAlgorithm); void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); - return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap); + return new (ptr) btBoxBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap); } }; - }; -#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H - +#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp index 7043bde34f5..202039956e7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -24,14 +24,12 @@ subject to the following restrictions: #include <float.h> #include <string.h> -btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2) -: m_box1(box1), -m_box2(box2) +btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1, const btBoxShape* box2) + : m_box1(box1), + m_box2(box2) { - } - // given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and // generate contact points. this returns 0 if there is no contact otherwise // it returns the number of contacts generated. @@ -48,67 +46,66 @@ m_box2(box2) // collision functions. this function only fills in the position and depth // fields. struct dContactGeom; -#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) +#define dDOTpq(a, b, p, q) ((a)[0] * (b)[0] + (a)[p] * (b)[q] + (a)[2 * (p)] * (b)[2 * (q)]) #define dInfinity FLT_MAX - /*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); } PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); } PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); } */ -static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } -static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); } -static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); } -static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); } -#define dMULTIPLYOP1_331(A,op,B,C) \ -{\ - (A)[0] op dDOT41((B),(C)); \ - (A)[1] op dDOT41((B+1),(C)); \ - (A)[2] op dDOT41((B+2),(C)); \ -} +static btScalar dDOT(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 1); } +static btScalar dDOT44(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 4); } +static btScalar dDOT41(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 4, 1); } +static btScalar dDOT14(const btScalar* a, const btScalar* b) { return dDOTpq(a, b, 1, 4); } +#define dMULTIPLYOP1_331(A, op, B, C) \ + { \ + (A)[0] op dDOT41((B), (C)); \ + (A)[1] op dDOT41((B + 1), (C)); \ + (A)[2] op dDOT41((B + 2), (C)); \ + } -#define dMULTIPLYOP0_331(A,op,B,C) \ -{ \ - (A)[0] op dDOT((B),(C)); \ - (A)[1] op dDOT((B+4),(C)); \ - (A)[2] op dDOT((B+8),(C)); \ -} +#define dMULTIPLYOP0_331(A, op, B, C) \ + { \ + (A)[0] op dDOT((B), (C)); \ + (A)[1] op dDOT((B + 4), (C)); \ + (A)[2] op dDOT((B + 8), (C)); \ + } -#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C) -#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) +#define dMULTIPLY1_331(A, B, C) dMULTIPLYOP1_331(A, =, B, C) +#define dMULTIPLY0_331(A, B, C) dMULTIPLYOP0_331(A, =, B, C) -typedef btScalar dMatrix3[4*3]; +typedef btScalar dMatrix3[4 * 3]; -void dLineClosestApproach (const btVector3& pa, const btVector3& ua, - const btVector3& pb, const btVector3& ub, - btScalar *alpha, btScalar *beta); -void dLineClosestApproach (const btVector3& pa, const btVector3& ua, - const btVector3& pb, const btVector3& ub, - btScalar *alpha, btScalar *beta) +void dLineClosestApproach(const btVector3& pa, const btVector3& ua, + const btVector3& pb, const btVector3& ub, + btScalar* alpha, btScalar* beta); +void dLineClosestApproach(const btVector3& pa, const btVector3& ua, + const btVector3& pb, const btVector3& ub, + btScalar* alpha, btScalar* beta) { - btVector3 p; - p[0] = pb[0] - pa[0]; - p[1] = pb[1] - pa[1]; - p[2] = pb[2] - pa[2]; - btScalar uaub = dDOT(ua,ub); - btScalar q1 = dDOT(ua,p); - btScalar q2 = -dDOT(ub,p); - btScalar d = 1-uaub*uaub; - if (d <= btScalar(0.0001f)) { - // @@@ this needs to be made more robust - *alpha = 0; - *beta = 0; - } - else { - d = 1.f/d; - *alpha = (q1 + uaub*q2)*d; - *beta = (uaub*q1 + q2)*d; - } + btVector3 p; + p[0] = pb[0] - pa[0]; + p[1] = pb[1] - pa[1]; + p[2] = pb[2] - pa[2]; + btScalar uaub = dDOT(ua, ub); + btScalar q1 = dDOT(ua, p); + btScalar q2 = -dDOT(ub, p); + btScalar d = 1 - uaub * uaub; + if (d <= btScalar(0.0001f)) + { + // @@@ this needs to be made more robust + *alpha = 0; + *beta = 0; + } + else + { + d = 1.f / d; + *alpha = (q1 + uaub * q2) * d; + *beta = (uaub * q1 + q2) * d; + } } - - // find all the intersection points between the 2D rectangle with vertices // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). @@ -117,60 +114,66 @@ void dLineClosestApproach (const btVector3& pa, const btVector3& ua, // the number of intersection points is returned by the function (this will // be in the range 0 to 8). -static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16]) +static int intersectRectQuad2(btScalar h[2], btScalar p[8], btScalar ret[16]) { - // q (and r) contain nq (and nr) coordinate points for the current (and - // chopped) polygons - int nq=4,nr=0; - btScalar buffer[16]; - btScalar *q = p; - btScalar *r = ret; - for (int dir=0; dir <= 1; dir++) { - // direction notation: xy[0] = x axis, xy[1] = y axis - for (int sign=-1; sign <= 1; sign += 2) { - // chop q along the line xy[dir] = sign*h[dir] - btScalar *pq = q; - btScalar *pr = r; - nr = 0; - for (int i=nq; i > 0; i--) { - // go through all points in q and all lines between adjacent points - if (sign*pq[dir] < h[dir]) { - // this point is inside the chopping line - pr[0] = pq[0]; - pr[1] = pq[1]; - pr += 2; - nr++; - if (nr & 8) { - q = r; - goto done; - } - } - btScalar *nextq = (i > 1) ? pq+2 : q; - if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) { - // this line crosses the chopping line - pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / - (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); - pr[dir] = sign*h[dir]; - pr += 2; - nr++; - if (nr & 8) { - q = r; - goto done; - } + // q (and r) contain nq (and nr) coordinate points for the current (and + // chopped) polygons + int nq = 4, nr = 0; + btScalar buffer[16]; + btScalar* q = p; + btScalar* r = ret; + for (int dir = 0; dir <= 1; dir++) + { + // direction notation: xy[0] = x axis, xy[1] = y axis + for (int sign = -1; sign <= 1; sign += 2) + { + // chop q along the line xy[dir] = sign*h[dir] + btScalar* pq = q; + btScalar* pr = r; + nr = 0; + for (int i = nq; i > 0; i--) + { + // go through all points in q and all lines between adjacent points + if (sign * pq[dir] < h[dir]) + { + // this point is inside the chopping line + pr[0] = pq[0]; + pr[1] = pq[1]; + pr += 2; + nr++; + if (nr & 8) + { + q = r; + goto done; + } + } + btScalar* nextq = (i > 1) ? pq + 2 : q; + if ((sign * pq[dir] < h[dir]) ^ (sign * nextq[dir] < h[dir])) + { + // this line crosses the chopping line + pr[1 - dir] = pq[1 - dir] + (nextq[1 - dir] - pq[1 - dir]) / + (nextq[dir] - pq[dir]) * (sign * h[dir] - pq[dir]); + pr[dir] = sign * h[dir]; + pr += 2; + nr++; + if (nr & 8) + { + q = r; + goto done; + } + } + pq += 2; + } + q = r; + r = (q == ret) ? buffer : ret; + nq = nr; + } } - pq += 2; - } - q = r; - r = (q==ret) ? buffer : ret; - nq = nr; - } - } - done: - if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar)); - return nr; +done: + if (q != ret) memcpy(ret, q, nr * 2 * sizeof(btScalar)); + return nr; } - #define M__PI 3.14159265f // given n points in the plane (array p, of size 2*n), generate m points that @@ -181,538 +184,584 @@ static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16]) // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be // in the range [0..n-1]. -void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]); -void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) +void cullPoints2(int n, btScalar p[], int m, int i0, int iret[]); +void cullPoints2(int n, btScalar p[], int m, int i0, int iret[]) { - // compute the centroid of the polygon in cx,cy - int i,j; - btScalar a,cx,cy,q; - if (n==1) { - cx = p[0]; - cy = p[1]; - } - else if (n==2) { - cx = btScalar(0.5)*(p[0] + p[2]); - cy = btScalar(0.5)*(p[1] + p[3]); - } - else { - a = 0; - cx = 0; - cy = 0; - for (i=0; i<(n-1); i++) { - q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; - a += q; - cx += q*(p[i*2]+p[i*2+2]); - cy += q*(p[i*2+1]+p[i*2+3]); - } - q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; - if (btFabs(a+q) > SIMD_EPSILON) + // compute the centroid of the polygon in cx,cy + int i, j; + btScalar a, cx, cy, q; + if (n == 1) { - a = 1.f/(btScalar(3.0)*(a+q)); - } else + cx = p[0]; + cy = p[1]; + } + else if (n == 2) { - a=BT_LARGE_FLOAT; + cx = btScalar(0.5) * (p[0] + p[2]); + cy = btScalar(0.5) * (p[1] + p[3]); } - cx = a*(cx + q*(p[n*2-2]+p[0])); - cy = a*(cy + q*(p[n*2-1]+p[1])); - } - - // compute the angle of each point w.r.t. the centroid - btScalar A[8]; - for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx); - - // search for points that have angles closest to A[i0] + i*(2*pi/m). - int avail[8]; - for (i=0; i<n; i++) avail[i] = 1; - avail[i0] = 0; - iret[0] = i0; - iret++; - for (j=1; j<m; j++) { - a = btScalar(j)*(2*M__PI/m) + A[i0]; - if (a > M__PI) a -= 2*M__PI; - btScalar maxdiff=1e9,diff; - - *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 - - for (i=0; i<n; i++) { - if (avail[i]) { - diff = btFabs (A[i]-a); - if (diff > M__PI) diff = 2*M__PI - diff; - if (diff < maxdiff) { - maxdiff = diff; - *iret = i; + else + { + a = 0; + cx = 0; + cy = 0; + for (i = 0; i < (n - 1); i++) + { + q = p[i * 2] * p[i * 2 + 3] - p[i * 2 + 2] * p[i * 2 + 1]; + a += q; + cx += q * (p[i * 2] + p[i * 2 + 2]); + cy += q * (p[i * 2 + 1] + p[i * 2 + 3]); + } + q = p[n * 2 - 2] * p[1] - p[0] * p[n * 2 - 1]; + if (btFabs(a + q) > SIMD_EPSILON) + { + a = 1.f / (btScalar(3.0) * (a + q)); + } + else + { + a = BT_LARGE_FLOAT; + } + cx = a * (cx + q * (p[n * 2 - 2] + p[0])); + cy = a * (cy + q * (p[n * 2 - 1] + p[1])); } - } - } -#if defined(DEBUG) || defined (_DEBUG) - btAssert (*iret != i0); // ensure iret got set + + // compute the angle of each point w.r.t. the centroid + btScalar A[8]; + for (i = 0; i < n; i++) A[i] = btAtan2(p[i * 2 + 1] - cy, p[i * 2] - cx); + + // search for points that have angles closest to A[i0] + i*(2*pi/m). + int avail[8]; + for (i = 0; i < n; i++) avail[i] = 1; + avail[i0] = 0; + iret[0] = i0; + iret++; + for (j = 1; j < m; j++) + { + a = btScalar(j) * (2 * M__PI / m) + A[i0]; + if (a > M__PI) a -= 2 * M__PI; + btScalar maxdiff = 1e9, diff; + + *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 + + for (i = 0; i < n; i++) + { + if (avail[i]) + { + diff = btFabs(A[i] - a); + if (diff > M__PI) diff = 2 * M__PI - diff; + if (diff < maxdiff) + { + maxdiff = diff; + *iret = i; + } + } + } +#if defined(DEBUG) || defined(_DEBUG) + btAssert(*iret != i0); // ensure iret got set #endif - avail[*iret] = 0; - iret++; - } + avail[*iret] = 0; + iret++; + } } +int dBoxBox2(const btVector3& p1, const dMatrix3 R1, + const btVector3& side1, const btVector3& p2, + const dMatrix3 R2, const btVector3& side2, + btVector3& normal, btScalar* depth, int* return_code, + int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output); +int dBoxBox2(const btVector3& p1, const dMatrix3 R1, + const btVector3& side1, const btVector3& p2, + const dMatrix3 R2, const btVector3& side2, + btVector3& normal, btScalar* depth, int* return_code, + int maxc, dContactGeom* /*contact*/, int /*skip*/, btDiscreteCollisionDetectorInterface::Result& output) +{ + const btScalar fudge_factor = btScalar(1.05); + btVector3 p, pp, normalC(0.f, 0.f, 0.f); + const btScalar* normalR = 0; + btScalar A[3], B[3], R11, R12, R13, R21, R22, R23, R31, R32, R33, + Q11, Q12, Q13, Q21, Q22, Q23, Q31, Q32, Q33, s, s2, l; + int i, j, invert_normal, code; + + // get vector from centers of box 1 to box 2, relative to box 1 + p = p2 - p1; + dMULTIPLY1_331(pp, R1, p); // get pp = p relative to body 1 + + // get side lengths / 2 + A[0] = side1[0] * btScalar(0.5); + A[1] = side1[1] * btScalar(0.5); + A[2] = side1[2] * btScalar(0.5); + B[0] = side2[0] * btScalar(0.5); + B[1] = side2[1] * btScalar(0.5); + B[2] = side2[2] * btScalar(0.5); + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 + R11 = dDOT44(R1 + 0, R2 + 0); + R12 = dDOT44(R1 + 0, R2 + 1); + R13 = dDOT44(R1 + 0, R2 + 2); + R21 = dDOT44(R1 + 1, R2 + 0); + R22 = dDOT44(R1 + 1, R2 + 1); + R23 = dDOT44(R1 + 1, R2 + 2); + R31 = dDOT44(R1 + 2, R2 + 0); + R32 = dDOT44(R1 + 2, R2 + 1); + R33 = dDOT44(R1 + 2, R2 + 2); + + Q11 = btFabs(R11); + Q12 = btFabs(R12); + Q13 = btFabs(R13); + Q21 = btFabs(R21); + Q22 = btFabs(R22); + Q23 = btFabs(R23); + Q31 = btFabs(R31); + Q32 = btFabs(R32); + Q33 = btFabs(R33); + + // for all 15 possible separating axes: + // * see if the axis separates the boxes. if so, return 0. + // * find the depth of the penetration along the separating axis (s2) + // * if this is the largest depth so far, record it. + // the normal vector will be set to the separating axis with the smallest + // depth. note: normalR is set to point to a column of R1 or R2 if that is + // the smallest depth normal so far. otherwise normalR is 0 and normalC is + // set to a vector relative to body 1. invert_normal is 1 if the sign of + // the normal should be flipped. + +#define TST(expr1, expr2, norm, cc) \ + s2 = btFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) \ + { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } + s = -dInfinity; + invert_normal = 0; + code = 0; -int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, - const btVector3& side1, const btVector3& p2, - const dMatrix3 R2, const btVector3& side2, - btVector3& normal, btScalar *depth, int *return_code, - int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output); -int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, - const btVector3& side1, const btVector3& p2, - const dMatrix3 R2, const btVector3& side2, - btVector3& normal, btScalar *depth, int *return_code, - int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output) -{ - const btScalar fudge_factor = btScalar(1.05); - btVector3 p,pp,normalC(0.f,0.f,0.f); - const btScalar *normalR = 0; - btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, - Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l; - int i,j,invert_normal,code; - - // get vector from centers of box 1 to box 2, relative to box 1 - p = p2 - p1; - dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 - - // get side lengths / 2 - A[0] = side1[0]*btScalar(0.5); - A[1] = side1[1]*btScalar(0.5); - A[2] = side1[2]*btScalar(0.5); - B[0] = side2[0]*btScalar(0.5); - B[1] = side2[1]*btScalar(0.5); - B[2] = side2[2]*btScalar(0.5); - - // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 - R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); - R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); - R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); - - Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13); - Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23); - Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33); - - // for all 15 possible separating axes: - // * see if the axis separates the boxes. if so, return 0. - // * find the depth of the penetration along the separating axis (s2) - // * if this is the largest depth so far, record it. - // the normal vector will be set to the separating axis with the smallest - // depth. note: normalR is set to point to a column of R1 or R2 if that is - // the smallest depth normal so far. otherwise normalR is 0 and normalC is - // set to a vector relative to body 1. invert_normal is 1 if the sign of - // the normal should be flipped. - -#define TST(expr1,expr2,norm,cc) \ - s2 = btFabs(expr1) - (expr2); \ - if (s2 > 0) return 0; \ - if (s2 > s) { \ - s = s2; \ - normalR = norm; \ - invert_normal = ((expr1) < 0); \ - code = (cc); \ - } - - s = -dInfinity; - invert_normal = 0; - code = 0; - - // separating axis = u1,u2,u3 - TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); - TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); - TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); - - // separating axis = v1,v2,v3 - TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); - TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); - TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); - - // note: cross product axes need to be scaled when s is computed. - // normal (n1,n2,n3) is relative to box 1. + // separating axis = u1,u2,u3 + TST(pp[0], (A[0] + B[0] * Q11 + B[1] * Q12 + B[2] * Q13), R1 + 0, 1); + TST(pp[1], (A[1] + B[0] * Q21 + B[1] * Q22 + B[2] * Q23), R1 + 1, 2); + TST(pp[2], (A[2] + B[0] * Q31 + B[1] * Q32 + B[2] * Q33), R1 + 2, 3); + + // separating axis = v1,v2,v3 + TST(dDOT41(R2 + 0, p), (A[0] * Q11 + A[1] * Q21 + A[2] * Q31 + B[0]), R2 + 0, 4); + TST(dDOT41(R2 + 1, p), (A[0] * Q12 + A[1] * Q22 + A[2] * Q32 + B[1]), R2 + 1, 5); + TST(dDOT41(R2 + 2, p), (A[0] * Q13 + A[1] * Q23 + A[2] * Q33 + B[2]), R2 + 2, 6); + + // note: cross product axes need to be scaled when s is computed. + // normal (n1,n2,n3) is relative to box 1. #undef TST -#define TST(expr1,expr2,n1,n2,n3,cc) \ - s2 = btFabs(expr1) - (expr2); \ - if (s2 > SIMD_EPSILON) return 0; \ - l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ - if (l > SIMD_EPSILON) { \ - s2 /= l; \ - if (s2*fudge_factor > s) { \ - s = s2; \ - normalR = 0; \ - normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ - invert_normal = ((expr1) < 0); \ - code = (cc); \ - } \ - } - - btScalar fudge2 (1.0e-5f); - - Q11 += fudge2; - Q12 += fudge2; - Q13 += fudge2; - - Q21 += fudge2; - Q22 += fudge2; - Q23 += fudge2; - - Q31 += fudge2; - Q32 += fudge2; - Q33 += fudge2; - - // separating axis = u1 x (v1,v2,v3) - TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); - TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); - TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); - - // separating axis = u2 x (v1,v2,v3) - TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); - TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); - TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); - - // separating axis = u3 x (v1,v2,v3) - TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); - TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); - TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); +#define TST(expr1, expr2, n1, n2, n3, cc) \ + s2 = btFabs(expr1) - (expr2); \ + if (s2 > SIMD_EPSILON) return 0; \ + l = btSqrt((n1) * (n1) + (n2) * (n2) + (n3) * (n3)); \ + if (l > SIMD_EPSILON) \ + { \ + s2 /= l; \ + if (s2 * fudge_factor > s) \ + { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1) / l; \ + normalC[1] = (n2) / l; \ + normalC[2] = (n3) / l; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } \ + } + + btScalar fudge2(1.0e-5f); + + Q11 += fudge2; + Q12 += fudge2; + Q13 += fudge2; + + Q21 += fudge2; + Q22 += fudge2; + Q23 += fudge2; + + Q31 += fudge2; + Q32 += fudge2; + Q33 += fudge2; + + // separating axis = u1 x (v1,v2,v3) + TST(pp[2] * R21 - pp[1] * R31, (A[1] * Q31 + A[2] * Q21 + B[1] * Q13 + B[2] * Q12), 0, -R31, R21, 7); + TST(pp[2] * R22 - pp[1] * R32, (A[1] * Q32 + A[2] * Q22 + B[0] * Q13 + B[2] * Q11), 0, -R32, R22, 8); + TST(pp[2] * R23 - pp[1] * R33, (A[1] * Q33 + A[2] * Q23 + B[0] * Q12 + B[1] * Q11), 0, -R33, R23, 9); + + // separating axis = u2 x (v1,v2,v3) + TST(pp[0] * R31 - pp[2] * R11, (A[0] * Q31 + A[2] * Q11 + B[1] * Q23 + B[2] * Q22), R31, 0, -R11, 10); + TST(pp[0] * R32 - pp[2] * R12, (A[0] * Q32 + A[2] * Q12 + B[0] * Q23 + B[2] * Q21), R32, 0, -R12, 11); + TST(pp[0] * R33 - pp[2] * R13, (A[0] * Q33 + A[2] * Q13 + B[0] * Q22 + B[1] * Q21), R33, 0, -R13, 12); + + // separating axis = u3 x (v1,v2,v3) + TST(pp[1] * R11 - pp[0] * R21, (A[0] * Q21 + A[1] * Q11 + B[1] * Q33 + B[2] * Q32), -R21, R11, 0, 13); + TST(pp[1] * R12 - pp[0] * R22, (A[0] * Q22 + A[1] * Q12 + B[0] * Q33 + B[2] * Q31), -R22, R12, 0, 14); + TST(pp[1] * R13 - pp[0] * R23, (A[0] * Q23 + A[1] * Q13 + B[0] * Q32 + B[1] * Q31), -R23, R13, 0, 15); #undef TST - if (!code) return 0; - - // if we get to this point, the boxes interpenetrate. compute the normal - // in global coordinates. - if (normalR) { - normal[0] = normalR[0]; - normal[1] = normalR[4]; - normal[2] = normalR[8]; - } - else { - dMULTIPLY0_331 (normal,R1,normalC); - } - if (invert_normal) { - normal[0] = -normal[0]; - normal[1] = -normal[1]; - normal[2] = -normal[2]; - } - *depth = -s; - - // compute contact point(s) - - if (code > 6) { - // an edge from box 1 touches an edge from box 2. - // find a point pa on the intersecting edge of box 1 - btVector3 pa; - btScalar sign; - for (i=0; i<3; i++) pa[i] = p1[i]; - for (j=0; j<3; j++) { - sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0); - for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; - } - - // find a point pb on the intersecting edge of box 2 - btVector3 pb; - for (i=0; i<3; i++) pb[i] = p2[i]; - for (j=0; j<3; j++) { - sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0); - for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; - } - - btScalar alpha,beta; - btVector3 ua,ub; - for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; - for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; - - dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); - for (i=0; i<3; i++) pa[i] += ua[i]*alpha; - for (i=0; i<3; i++) pb[i] += ub[i]*beta; + if (!code) return 0; + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if (normalR) + { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { - - //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]); - //contact[0].depth = *depth; - btVector3 pointInWorld; + dMULTIPLY0_331(normal, R1, normalC); + } + if (invert_normal) + { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + *depth = -s; + + // compute contact point(s) + + if (code > 6) + { + // an edge from box 1 touches an edge from box 2. + // find a point pa on the intersecting edge of box 1 + btVector3 pa; + btScalar sign; + for (i = 0; i < 3; i++) pa[i] = p1[i]; + for (j = 0; j < 3; j++) + { + sign = (dDOT14(normal, R1 + j) > 0) ? btScalar(1.0) : btScalar(-1.0); + for (i = 0; i < 3; i++) pa[i] += sign * A[j] * R1[i * 4 + j]; + } + + // find a point pb on the intersecting edge of box 2 + btVector3 pb; + for (i = 0; i < 3; i++) pb[i] = p2[i]; + for (j = 0; j < 3; j++) + { + sign = (dDOT14(normal, R2 + j) > 0) ? btScalar(-1.0) : btScalar(1.0); + for (i = 0; i < 3; i++) pb[i] += sign * B[j] * R2[i * 4 + j]; + } + + btScalar alpha, beta; + btVector3 ua, ub; + for (i = 0; i < 3; i++) ua[i] = R1[((code)-7) / 3 + i * 4]; + for (i = 0; i < 3; i++) ub[i] = R2[((code)-7) % 3 + i * 4]; + + dLineClosestApproach(pa, ua, pb, ub, &alpha, &beta); + for (i = 0; i < 3; i++) pa[i] += ua[i] * alpha; + for (i = 0; i < 3; i++) pb[i] += ub[i] * beta; + + { + //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]); + //contact[0].depth = *depth; + btVector3 pointInWorld; #ifdef USE_CENTER_POINT - for (i=0; i<3; i++) - pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5); - output.addContactPoint(-normal,pointInWorld,-*depth); + for (i = 0; i < 3; i++) + pointInWorld[i] = (pa[i] + pb[i]) * btScalar(0.5); + output.addContactPoint(-normal, pointInWorld, -*depth); #else - output.addContactPoint(-normal,pb,-*depth); + output.addContactPoint(-normal, pb, -*depth); -#endif // - *return_code = code; +#endif // + *return_code = code; + } + return 1; + } + + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). define face 'a' to be the reference + // face (i.e. the normal vector is perpendicular to this) and face 'b' to be + // the incident face (the closest face of the other box). + + const btScalar *Ra, *Rb, *pa, *pb, *Sa, *Sb; + if (code <= 3) + { + Ra = R1; + Rb = R2; + pa = p1; + pb = p2; + Sa = A; + Sb = B; + } + else + { + Ra = R2; + Rb = R1; + pa = p2; + pb = p1; + Sa = B; + Sb = A; + } + + // nr = normal vector of reference face dotted with axes of incident box. + // anr = absolute values of nr. + btVector3 normal2, nr, anr; + if (code <= 3) + { + normal2[0] = normal[0]; + normal2[1] = normal[1]; + normal2[2] = normal[2]; + } + else + { + normal2[0] = -normal[0]; + normal2[1] = -normal[1]; + normal2[2] = -normal[2]; } - return 1; - } - - // okay, we have a face-something intersection (because the separating - // axis is perpendicular to a face). define face 'a' to be the reference - // face (i.e. the normal vector is perpendicular to this) and face 'b' to be - // the incident face (the closest face of the other box). - - const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb; - if (code <= 3) { - Ra = R1; - Rb = R2; - pa = p1; - pb = p2; - Sa = A; - Sb = B; - } - else { - Ra = R2; - Rb = R1; - pa = p2; - pb = p1; - Sa = B; - Sb = A; - } - - // nr = normal vector of reference face dotted with axes of incident box. - // anr = absolute values of nr. - btVector3 normal2,nr,anr; - if (code <= 3) { - normal2[0] = normal[0]; - normal2[1] = normal[1]; - normal2[2] = normal[2]; - } - else { - normal2[0] = -normal[0]; - normal2[1] = -normal[1]; - normal2[2] = -normal[2]; - } - dMULTIPLY1_331 (nr,Rb,normal2); - anr[0] = btFabs (nr[0]); - anr[1] = btFabs (nr[1]); - anr[2] = btFabs (nr[2]); - - // find the largest compontent of anr: this corresponds to the normal - // for the indident face. the other axis numbers of the indicent face - // are stored in a1,a2. - int lanr,a1,a2; - if (anr[1] > anr[0]) { - if (anr[1] > anr[2]) { - a1 = 0; - lanr = 1; - a2 = 2; - } - else { - a1 = 0; - a2 = 1; - lanr = 2; - } - } - else { - if (anr[0] > anr[2]) { - lanr = 0; - a1 = 1; - a2 = 2; - } - else { - a1 = 0; - a2 = 1; - lanr = 2; - } - } - - // compute center point of incident face, in reference-face coordinates - btVector3 center; - if (nr[lanr] < 0) { - for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; - } - else { - for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; - } - - // find the normal and non-normal axis numbers of the reference box - int codeN,code1,code2; - if (code <= 3) codeN = code-1; else codeN = code-4; - if (codeN==0) { - code1 = 1; - code2 = 2; - } - else if (codeN==1) { - code1 = 0; - code2 = 2; - } - else { - code1 = 0; - code2 = 1; - } - - // find the four corners of the incident face, in reference-face coordinates - btScalar quad[8]; // 2D coordinate of incident face (x,y pairs) - btScalar c1,c2,m11,m12,m21,m22; - c1 = dDOT14 (center,Ra+code1); - c2 = dDOT14 (center,Ra+code2); - // optimize this? - we have already computed this data above, but it is not - // stored in an easy-to-index format. for now it's quicker just to recompute - // the four dot products. - m11 = dDOT44 (Ra+code1,Rb+a1); - m12 = dDOT44 (Ra+code1,Rb+a2); - m21 = dDOT44 (Ra+code2,Rb+a1); - m22 = dDOT44 (Ra+code2,Rb+a2); - { - btScalar k1 = m11*Sb[a1]; - btScalar k2 = m21*Sb[a1]; - btScalar k3 = m12*Sb[a2]; - btScalar k4 = m22*Sb[a2]; - quad[0] = c1 - k1 - k3; - quad[1] = c2 - k2 - k4; - quad[2] = c1 - k1 + k3; - quad[3] = c2 - k2 + k4; - quad[4] = c1 + k1 + k3; - quad[5] = c2 + k2 + k4; - quad[6] = c1 + k1 - k3; - quad[7] = c2 + k2 - k4; - } - - // find the size of the reference face - btScalar rect[2]; - rect[0] = Sa[code1]; - rect[1] = Sa[code2]; - - // intersect the incident and reference faces - btScalar ret[16]; - int n = intersectRectQuad2 (rect,quad,ret); - if (n < 1) return 0; // this should never happen - - // convert the intersection points into reference-face coordinates, - // and compute the contact position and depth for each point. only keep - // those points that have a positive (penetrating) depth. delete points in - // the 'ret' array as necessary so that 'point' and 'ret' correspond. - btScalar point[3*8]; // penetrating contact points - btScalar dep[8]; // depths for those points - btScalar det1 = 1.f/(m11*m22 - m12*m21); - m11 *= det1; - m12 *= det1; - m21 *= det1; - m22 *= det1; - int cnum = 0; // number of penetrating contact points found - for (j=0; j < n; j++) { - btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); - btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); - for (i=0; i<3; i++) point[cnum*3+i] = - center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; - dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); - if (dep[cnum] >= 0) { - ret[cnum*2] = ret[j*2]; - ret[cnum*2+1] = ret[j*2+1]; - cnum++; - } - } - if (cnum < 1) return 0; // this should never happen - - // we can't generate more contacts than we actually have - if (maxc > cnum) maxc = cnum; - if (maxc < 1) maxc = 1; - - if (cnum <= maxc) { - - if (code<4) - { - // we have less contacts than we need, so we use them all - for (j=0; j < cnum; j++) + dMULTIPLY1_331(nr, Rb, normal2); + anr[0] = btFabs(nr[0]); + anr[1] = btFabs(nr[1]); + anr[2] = btFabs(nr[2]); + + // find the largest compontent of anr: this corresponds to the normal + // for the indident face. the other axis numbers of the indicent face + // are stored in a1,a2. + int lanr, a1, a2; + if (anr[1] > anr[0]) { - btVector3 pointInWorld; - for (i=0; i<3; i++) - pointInWorld[i] = point[j*3+i] + pa[i]; - output.addContactPoint(-normal,pointInWorld,-dep[j]); - - } - } else - { - // we have less contacts than we need, so we use them all - for (j=0; j < cnum; j++) + if (anr[1] > anr[2]) { - btVector3 pointInWorld; - for (i=0; i<3; i++) - pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j]; + a1 = 0; + lanr = 1; + a2 = 2; + } + else + { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + else + { + if (anr[0] > anr[2]) + { + lanr = 0; + a1 = 1; + a2 = 2; + } + else + { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + + // compute center point of incident face, in reference-face coordinates + btVector3 center; + if (nr[lanr] < 0) + { + for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i * 4 + lanr]; + } + else + { + for (i = 0; i < 3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i * 4 + lanr]; + } + + // find the normal and non-normal axis numbers of the reference box + int codeN, code1, code2; + if (code <= 3) + codeN = code - 1; + else + codeN = code - 4; + if (codeN == 0) + { + code1 = 1; + code2 = 2; + } + else if (codeN == 1) + { + code1 = 0; + code2 = 2; + } + else + { + code1 = 0; + code2 = 1; + } + + // find the four corners of the incident face, in reference-face coordinates + btScalar quad[8]; // 2D coordinate of incident face (x,y pairs) + btScalar c1, c2, m11, m12, m21, m22; + c1 = dDOT14(center, Ra + code1); + c2 = dDOT14(center, Ra + code2); + // optimize this? - we have already computed this data above, but it is not + // stored in an easy-to-index format. for now it's quicker just to recompute + // the four dot products. + m11 = dDOT44(Ra + code1, Rb + a1); + m12 = dDOT44(Ra + code1, Rb + a2); + m21 = dDOT44(Ra + code2, Rb + a1); + m22 = dDOT44(Ra + code2, Rb + a2); + { + btScalar k1 = m11 * Sb[a1]; + btScalar k2 = m21 * Sb[a1]; + btScalar k3 = m12 * Sb[a2]; + btScalar k4 = m22 * Sb[a2]; + quad[0] = c1 - k1 - k3; + quad[1] = c2 - k2 - k4; + quad[2] = c1 - k1 + k3; + quad[3] = c2 - k2 + k4; + quad[4] = c1 + k1 + k3; + quad[5] = c2 + k2 + k4; + quad[6] = c1 + k1 - k3; + quad[7] = c2 + k2 - k4; + } + + // find the size of the reference face + btScalar rect[2]; + rect[0] = Sa[code1]; + rect[1] = Sa[code2]; + + // intersect the incident and reference faces + btScalar ret[16]; + int n = intersectRectQuad2(rect, quad, ret); + if (n < 1) return 0; // this should never happen + + // convert the intersection points into reference-face coordinates, + // and compute the contact position and depth for each point. only keep + // those points that have a positive (penetrating) depth. delete points in + // the 'ret' array as necessary so that 'point' and 'ret' correspond. + btScalar point[3 * 8]; // penetrating contact points + btScalar dep[8]; // depths for those points + btScalar det1 = 1.f / (m11 * m22 - m12 * m21); + m11 *= det1; + m12 *= det1; + m21 *= det1; + m22 *= det1; + int cnum = 0; // number of penetrating contact points found + for (j = 0; j < n; j++) + { + btScalar k1 = m22 * (ret[j * 2] - c1) - m12 * (ret[j * 2 + 1] - c2); + btScalar k2 = -m21 * (ret[j * 2] - c1) + m11 * (ret[j * 2 + 1] - c2); + for (i = 0; i < 3; i++) point[cnum * 3 + i] = + center[i] + k1 * Rb[i * 4 + a1] + k2 * Rb[i * 4 + a2]; + dep[cnum] = Sa[codeN] - dDOT(normal2, point + cnum * 3); + if (dep[cnum] >= 0) + { + ret[cnum * 2] = ret[j * 2]; + ret[cnum * 2 + 1] = ret[j * 2 + 1]; + cnum++; + } + } + if (cnum < 1) return 0; // this should never happen + + // we can't generate more contacts than we actually have + if (maxc > cnum) maxc = cnum; + if (maxc < 1) maxc = 1; + + if (cnum <= maxc) + { + if (code < 4) + { + // we have less contacts than we need, so we use them all + for (j = 0; j < cnum; j++) + { + btVector3 pointInWorld; + for (i = 0; i < 3; i++) + pointInWorld[i] = point[j * 3 + i] + pa[i]; + output.addContactPoint(-normal, pointInWorld, -dep[j]); + } + } + else + { + // we have less contacts than we need, so we use them all + for (j = 0; j < cnum; j++) + { + btVector3 pointInWorld; + for (i = 0; i < 3; i++) + pointInWorld[i] = point[j * 3 + i] + pa[i] - normal[i] * dep[j]; //pointInWorld[i] = point[j*3+i] + pa[i]; - output.addContactPoint(-normal,pointInWorld,-dep[j]); + output.addContactPoint(-normal, pointInWorld, -dep[j]); + } } - } - } - else { - // we have more contacts than are wanted, some of them must be culled. - // find the deepest point, it is always the first contact. - int i1 = 0; - btScalar maxdepth = dep[0]; - for (i=1; i<cnum; i++) { - if (dep[i] > maxdepth) { - maxdepth = dep[i]; - i1 = i; - } - } - - int iret[8]; - cullPoints2 (cnum,ret,maxc,i1,iret); - - for (j=0; j < maxc; j++) { -// dContactGeom *con = CONTACT(contact,skip*j); - // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; - // con->depth = dep[iret[j]]; - - btVector3 posInWorld; - for (i=0; i<3; i++) - posInWorld[i] = point[iret[j]*3+i] + pa[i]; - if (code<4) - { - output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); - } else + } + else + { + // we have more contacts than are wanted, some of them must be culled. + // find the deepest point, it is always the first contact. + int i1 = 0; + btScalar maxdepth = dep[0]; + for (i = 1; i < cnum; i++) { - output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]); + if (dep[i] > maxdepth) + { + maxdepth = dep[i]; + i1 = i; + } } - } - cnum = maxc; - } - *return_code = code; - return cnum; + int iret[8]; + cullPoints2(cnum, ret, maxc, i1, iret); + + for (j = 0; j < maxc; j++) + { + // dContactGeom *con = CONTACT(contact,skip*j); + // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; + // con->depth = dep[iret[j]]; + + btVector3 posInWorld; + for (i = 0; i < 3; i++) + posInWorld[i] = point[iret[j] * 3 + i] + pa[i]; + if (code < 4) + { + output.addContactPoint(-normal, posInWorld, -dep[iret[j]]); + } + else + { + output.addContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]); + } + } + cnum = maxc; + } + + *return_code = code; + return cnum; } -void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/) +void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* /*debugDraw*/, bool /*swapResults*/) { - const btTransform& transformA = input.m_transformA; const btTransform& transformB = input.m_transformB; - + int skip = 0; - dContactGeom *contact = 0; + dContactGeom* contact = 0; dMatrix3 R1; dMatrix3 R2; - for (int j=0;j<3;j++) + for (int j = 0; j < 3; j++) { - R1[0+4*j] = transformA.getBasis()[j].x(); - R2[0+4*j] = transformB.getBasis()[j].x(); - - R1[1+4*j] = transformA.getBasis()[j].y(); - R2[1+4*j] = transformB.getBasis()[j].y(); - + R1[0 + 4 * j] = transformA.getBasis()[j].x(); + R2[0 + 4 * j] = transformB.getBasis()[j].x(); - R1[2+4*j] = transformA.getBasis()[j].z(); - R2[2+4*j] = transformB.getBasis()[j].z(); + R1[1 + 4 * j] = transformA.getBasis()[j].y(); + R2[1 + 4 * j] = transformB.getBasis()[j].y(); + R1[2 + 4 * j] = transformA.getBasis()[j].z(); + R2[2 + 4 * j] = transformB.getBasis()[j].z(); } - - btVector3 normal; btScalar depth; int return_code; int maxc = 4; - - dBoxBox2 (transformA.getOrigin(), - R1, - 2.f*m_box1->getHalfExtentsWithMargin(), - transformB.getOrigin(), - R2, - 2.f*m_box2->getHalfExtentsWithMargin(), - normal, &depth, &return_code, - maxc, contact, skip, - output - ); - + dBoxBox2(transformA.getOrigin(), + R1, + 2.f * m_box1->getHalfExtentsWithMargin(), + transformB.getOrigin(), + R2, + 2.f * m_box2->getHalfExtentsWithMargin(), + normal, &depth, &return_code, + maxc, contact, skip, + output); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h index 39243777051..9f7d988fc11 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h @@ -19,11 +19,9 @@ subject to the following restrictions: #ifndef BT_BOX_BOX_DETECTOR_H #define BT_BOX_BOX_DETECTOR_H - class btBoxShape; #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" - /// btBoxBoxDetector wraps the ODE box-box collision detector /// re-distributed under the Zlib license with permission from Russell L. Smith struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface @@ -32,13 +30,11 @@ struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface const btBoxShape* m_box2; public: + btBoxBoxDetector(const btBoxShape* box1, const btBoxShape* box2); - btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2); - - virtual ~btBoxBoxDetector() {}; - - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + virtual ~btBoxBoxDetector(){}; + virtual void getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults = false); }; -#endif //BT_BOX_BOX_DETECTOR_H +#endif //BT_BOX_BOX_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h index 66949849448..d6e15f555bf 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -23,11 +23,9 @@ class btPoolAllocator; ///btCollisionConfiguration allows to configure Bullet collision detection ///stack allocator size, default collision algorithms and persistent manifold pool size ///@todo: describe the meaning -class btCollisionConfiguration +class btCollisionConfiguration { - public: - virtual ~btCollisionConfiguration() { } @@ -37,10 +35,9 @@ public: virtual btPoolAllocator* getCollisionAlgorithmPool() = 0; + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0; - virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0; - + virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0; }; -#endif //BT_COLLISION_CONFIGURATION - +#endif //BT_COLLISION_CONFIGURATION diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h index 62ee66c4e9a..bd812849393 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -26,20 +26,18 @@ struct btCollisionAlgorithmConstructionInfo; struct btCollisionAlgorithmCreateFunc { bool m_swapped; - + btCollisionAlgorithmCreateFunc() - :m_swapped(false) + : m_swapped(false) { } virtual ~btCollisionAlgorithmCreateFunc(){}; - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& , const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo&, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - (void)body0Wrap; (void)body1Wrap; return 0; } }; -#endif //BT_COLLISION_CREATE_FUNC - +#endif //BT_COLLISION_CREATE_FUNC diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 3b6913c0e1e..25b2b1ea464 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -13,10 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btCollisionDispatcher.h" - +#include "LinearMath/btQuickprof.h" #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" @@ -27,82 +25,73 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -int gNumManifold = 0; - #ifdef BT_DEBUG #include <stdio.h> #endif - -btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): -m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD), - m_collisionConfiguration(collisionConfiguration) +btCollisionDispatcher::btCollisionDispatcher(btCollisionConfiguration* collisionConfiguration) : m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD), + m_collisionConfiguration(collisionConfiguration) { int i; setNearCallback(defaultNearCallback); - + m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool(); m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool(); - for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++) + for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++) { - for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++) + for (int j = 0; j < MAX_BROADPHASE_COLLISION_TYPES; j++) { - m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j); - btAssert(m_doubleDispatch[i][j]); + m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i, j); + btAssert(m_doubleDispatchContactPoints[i][j]); + m_doubleDispatchClosestPoints[i][j] = m_collisionConfiguration->getClosestPointsAlgorithmCreateFunc(i, j); } } - - } +void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc) +{ + m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc; +} -void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) +void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc) { - m_doubleDispatch[proxyType0][proxyType1] = createFunc; + m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc; } btCollisionDispatcher::~btCollisionDispatcher() { } -btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1) -{ - gNumManifold++; - +btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1) +{ //btAssert(gNumManifold < 65535); - - //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance) - - btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? - btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold) , body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold)) - : gContactBreakingThreshold ; - - btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold()); - - void* mem = 0; - - if (m_persistentManifoldPoolAllocator->getFreeCount()) - { - mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); - } else + + btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold), body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold)) + : gContactBreakingThreshold; + + btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold()); + + void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); + if (NULL == mem) { //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert. - if ((m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION)==0) + if ((m_dispatcherFlags & CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION) == 0) { - mem = btAlignedAlloc(sizeof(btPersistentManifold),16); - } else + mem = btAlignedAlloc(sizeof(btPersistentManifold), 16); + } + else { btAssert(0); //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration return 0; } } - btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold); + btPersistentManifold* manifold = new (mem) btPersistentManifold(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold); manifold->m_index1a = m_manifoldsPtr.size(); m_manifoldsPtr.push_back(manifold); @@ -114,18 +103,14 @@ void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold) manifold->clearManifold(); } - void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) { - - gNumManifold--; - //printf("releaseManifold: gNumManifold %d\n",gNumManifold); clearManifold(manifold); int findIndex = manifold->m_index1a; btAssert(findIndex < m_manifoldsPtr.size()); - m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1); + m_manifoldsPtr.swap(findIndex, m_manifoldsPtr.size() - 1); m_manifoldsPtr[findIndex]->m_index1a = findIndex; m_manifoldsPtr.pop_back(); @@ -133,42 +118,44 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) if (m_persistentManifoldPoolAllocator->validPtr(manifold)) { m_persistentManifoldPoolAllocator->freeMemory(manifold); - } else + } + else { btAlignedFree(manifold); } - } - - -btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold) +btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType) { - btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = this; ci.m_manifold = sharedManifold; - btCollisionAlgorithm* algo = m_doubleDispatch[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0Wrap,body1Wrap); + btCollisionAlgorithm* algo = 0; + if (algoType == BT_CONTACT_POINT_ALGORITHMS) + { + algo = m_doubleDispatchContactPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap); + } + else + { + algo = m_doubleDispatchClosestPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap); + } return algo; } - - - -bool btCollisionDispatcher::needsResponse(const btCollisionObject* body0,const btCollisionObject* body1) +bool btCollisionDispatcher::needsResponse(const btCollisionObject* body0, const btCollisionObject* body1) { //here you can do filtering - bool hasResponse = + bool hasResponse = (body0->hasContactResponse() && body1->hasContactResponse()); //no response between two static/kinematic bodies: hasResponse = hasResponse && - ((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject())); + ((!body0->isStaticOrKinematicObject()) || (!body1->isStaticOrKinematicObject())); return hasResponse; } -bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const btCollisionObject* body1) +bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1) { btAssert(body0); btAssert(body1); @@ -185,31 +172,27 @@ bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); } } -#endif //BT_DEBUG +#endif //BT_DEBUG if ((!body0->isActive()) && (!body1->isActive())) needsCollision = false; else if ((!body0->checkCollideWith(body1)) || (!body1->checkCollideWith(body0))) needsCollision = false; - - return needsCollision ; + return needsCollision; } - - ///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc) ///this is useful for the collision dispatcher. class btCollisionPairCallback : public btOverlapCallback { const btDispatcherInfo& m_dispatchInfo; - btCollisionDispatcher* m_dispatcher; + btCollisionDispatcher* m_dispatcher; public: - - btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher* dispatcher) - :m_dispatchInfo(dispatchInfo), - m_dispatcher(dispatcher) + btCollisionPairCallback(const btDispatcherInfo& dispatchInfo, btCollisionDispatcher* dispatcher) + : m_dispatchInfo(dispatchInfo), + m_dispatcher(dispatcher) { } @@ -221,85 +204,76 @@ public: } */ - virtual ~btCollisionPairCallback() {} - - virtual bool processOverlap(btBroadphasePair& pair) + virtual bool processOverlap(btBroadphasePair& pair) { - (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo); - + (*m_dispatcher->getNearCallback())(pair, *m_dispatcher, m_dispatchInfo); return false; } }; - - -void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) +void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher) { //m_blockedForChanges = true; - btCollisionPairCallback collisionCallback(dispatchInfo,this); + btCollisionPairCallback collisionCallback(dispatchInfo, this); - pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); + { + BT_PROFILE("processAllOverlappingPairs"); + pairCache->processAllOverlappingPairs(&collisionCallback, dispatcher, dispatchInfo); + } //m_blockedForChanges = false; - } - - - //by default, Bullet will use this near callback void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo) { - btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; + btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; + + if (dispatcher.needsCollision(colObj0, colObj1)) + { + btCollisionObjectWrapper obj0Wrap(0, colObj0->getCollisionShape(), colObj0, colObj0->getWorldTransform(), -1, -1); + btCollisionObjectWrapper obj1Wrap(0, colObj1->getCollisionShape(), colObj1, colObj1->getWorldTransform(), -1, -1); - if (dispatcher.needsCollision(colObj0,colObj1)) + //dispatcher will keep algorithms persistent in the collision pair + if (!collisionPair.m_algorithm) { - btCollisionObjectWrapper obj0Wrap(0,colObj0->getCollisionShape(),colObj0,colObj0->getWorldTransform(),-1,-1); - btCollisionObjectWrapper obj1Wrap(0,colObj1->getCollisionShape(),colObj1,colObj1->getWorldTransform(),-1,-1); + collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap, &obj1Wrap, 0, BT_CONTACT_POINT_ALGORITHMS); + } + if (collisionPair.m_algorithm) + { + btManifoldResult contactPointResult(&obj0Wrap, &obj1Wrap); - //dispatcher will keep algorithms persistent in the collision pair - if (!collisionPair.m_algorithm) + if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) { - collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap); - } + //discrete collision detection query - if (collisionPair.m_algorithm) + collisionPair.m_algorithm->processCollision(&obj0Wrap, &obj1Wrap, dispatchInfo, &contactPointResult); + } + else { - btManifoldResult contactPointResult(&obj0Wrap,&obj1Wrap); - - if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) - { - //discrete collision detection query - - collisionPair.m_algorithm->processCollision(&obj0Wrap,&obj1Wrap,dispatchInfo,&contactPointResult); - } else - { - //continuous collision detection query, time of impact (toi) - btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult); - if (dispatchInfo.m_timeOfImpact > toi) - dispatchInfo.m_timeOfImpact = toi; - - } + //continuous collision detection query, time of impact (toi) + btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0, colObj1, dispatchInfo, &contactPointResult); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; } } - + } } - void* btCollisionDispatcher::allocateCollisionAlgorithm(int size) { - if (m_collisionAlgorithmPoolAllocator->getFreeCount()) + void* mem = m_collisionAlgorithmPoolAllocator->allocate(size); + if (NULL == mem) { - return m_collisionAlgorithmPoolAllocator->allocate(size); + //warn user for overflow? + return btAlignedAlloc(static_cast<size_t>(size), 16); } - - //warn user for overflow? - return btAlignedAlloc(static_cast<size_t>(size), 16); + return mem; } void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) @@ -307,7 +281,8 @@ void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) if (m_collisionAlgorithmPoolAllocator->validPtr(ptr)) { m_collisionAlgorithmPoolAllocator->freeMemory(ptr); - } else + } + else { btAlignedFree(ptr); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index 92696ee5429..04309670cfc 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -37,33 +37,28 @@ class btCollisionDispatcher; ///user can override this nearcallback for collision filtering and more finegrained control over collision detection typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); - ///btCollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs. ///Time of Impact, Closest Points and Penetration Depth. class btCollisionDispatcher : public btDispatcher { - protected: + int m_dispatcherFlags; - int m_dispatcherFlags; + btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr; - btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr; + btNearCallback m_nearCallback; - btManifoldResult m_defaultManifoldResult; + btPoolAllocator* m_collisionAlgorithmPoolAllocator; - btNearCallback m_nearCallback; - - btPoolAllocator* m_collisionAlgorithmPoolAllocator; + btPoolAllocator* m_persistentManifoldPoolAllocator; - btPoolAllocator* m_persistentManifoldPoolAllocator; + btCollisionAlgorithmCreateFunc* m_doubleDispatchContactPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; - btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; - - btCollisionConfiguration* m_collisionConfiguration; + btCollisionAlgorithmCreateFunc* m_doubleDispatchClosestPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; + btCollisionConfiguration* m_collisionConfiguration; public: - enum DispatcherFlags { CD_STATIC_STATIC_REPORTED = 1, @@ -71,101 +66,104 @@ public: CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION = 4 }; - int getDispatcherFlags() const + int getDispatcherFlags() const { return m_dispatcherFlags; } - void setDispatcherFlags(int flags) + void setDispatcherFlags(int flags) { m_dispatcherFlags = flags; } ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions - void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); + void registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); - int getNumManifolds() const - { - return int( m_manifoldsPtr.size()); + void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); + + int getNumManifolds() const + { + return int(m_manifoldsPtr.size()); } - btPersistentManifold** getInternalManifoldPointer() + btPersistentManifold** getInternalManifoldPointer() { - return m_manifoldsPtr.size()? &m_manifoldsPtr[0] : 0; + return m_manifoldsPtr.size() ? &m_manifoldsPtr[0] : 0; } - btPersistentManifold* getManifoldByIndexInternal(int index) + btPersistentManifold* getManifoldByIndexInternal(int index) { + btAssert(index>=0); + btAssert(index<m_manifoldsPtr.size()); return m_manifoldsPtr[index]; } - const btPersistentManifold* getManifoldByIndexInternal(int index) const + const btPersistentManifold* getManifoldByIndexInternal(int index) const { + btAssert(index>=0); + btAssert(index<m_manifoldsPtr.size()); return m_manifoldsPtr[index]; } - btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration); + btCollisionDispatcher(btCollisionConfiguration* collisionConfiguration); virtual ~btCollisionDispatcher(); - virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1); - - virtual void releaseManifold(btPersistentManifold* manifold); + virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0, const btCollisionObject* b1); + virtual void releaseManifold(btPersistentManifold* manifold); virtual void clearManifold(btPersistentManifold* manifold); - btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold = 0); - - virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1); - - virtual bool needsResponse(const btCollisionObject* body0,const btCollisionObject* body1); - - virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ; + btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType); + + virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1); + + virtual bool needsResponse(const btCollisionObject* body0, const btCollisionObject* body1); - void setNearCallback(btNearCallback nearCallback) + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher); + + void setNearCallback(btNearCallback nearCallback) { - m_nearCallback = nearCallback; + m_nearCallback = nearCallback; } - btNearCallback getNearCallback() const + btNearCallback getNearCallback() const { return m_nearCallback; } //by default, Bullet will use this near callback - static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); + static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); - virtual void* allocateCollisionAlgorithm(int size); + virtual void* allocateCollisionAlgorithm(int size); - virtual void freeCollisionAlgorithm(void* ptr); + virtual void freeCollisionAlgorithm(void* ptr); - btCollisionConfiguration* getCollisionConfiguration() + btCollisionConfiguration* getCollisionConfiguration() { return m_collisionConfiguration; } - const btCollisionConfiguration* getCollisionConfiguration() const + const btCollisionConfiguration* getCollisionConfiguration() const { return m_collisionConfiguration; } - void setCollisionConfiguration(btCollisionConfiguration* config) + void setCollisionConfiguration(btCollisionConfiguration* config) { m_collisionConfiguration = config; } - virtual btPoolAllocator* getInternalManifoldPool() + virtual btPoolAllocator* getInternalManifoldPool() { return m_persistentManifoldPoolAllocator; } - virtual const btPoolAllocator* getInternalManifoldPool() const + virtual const btPoolAllocator* getInternalManifoldPool() const { return m_persistentManifoldPoolAllocator; } - }; -#endif //BT_COLLISION__DISPATCHER_H - +#endif //BT_COLLISION__DISPATCHER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp new file mode 100644 index 00000000000..89bc8d920ef --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp @@ -0,0 +1,162 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btCollisionDispatcherMt.h" +#include "LinearMath/btQuickprof.h" + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" + +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "LinearMath/btPoolAllocator.h" +#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + +btCollisionDispatcherMt::btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize) + : btCollisionDispatcher(config) +{ + m_batchManifoldsPtr.resize(btGetTaskScheduler()->getNumThreads()); + m_batchUpdating = false; + m_grainSize = grainSize; // iterations per task +} + +btPersistentManifold* btCollisionDispatcherMt::getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1) +{ + //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance) + + btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold), body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold)) + : gContactBreakingThreshold; + + btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold()); + + void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); + if (NULL == mem) + { + //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert. + if ((m_dispatcherFlags & CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION) == 0) + { + mem = btAlignedAlloc(sizeof(btPersistentManifold), 16); + } + else + { + btAssert(0); + //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration + return 0; + } + } + btPersistentManifold* manifold = new (mem) btPersistentManifold(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold); + if (!m_batchUpdating) + { + // batch updater will update manifold pointers array after finishing, so + // only need to update array when not batch-updating + //btAssert( !btThreadsAreRunning() ); + manifold->m_index1a = m_manifoldsPtr.size(); + m_manifoldsPtr.push_back(manifold); + } + else + { + m_batchManifoldsPtr[btGetCurrentThreadIndex()].push_back(manifold); + } + + return manifold; +} + +void btCollisionDispatcherMt::releaseManifold(btPersistentManifold* manifold) +{ + clearManifold(manifold); + //btAssert( !btThreadsAreRunning() ); + if (!m_batchUpdating) + { + // batch updater will update manifold pointers array after finishing, so + // only need to update array when not batch-updating + int findIndex = manifold->m_index1a; + btAssert(findIndex < m_manifoldsPtr.size()); + m_manifoldsPtr.swap(findIndex, m_manifoldsPtr.size() - 1); + m_manifoldsPtr[findIndex]->m_index1a = findIndex; + m_manifoldsPtr.pop_back(); + } + + manifold->~btPersistentManifold(); + if (m_persistentManifoldPoolAllocator->validPtr(manifold)) + { + m_persistentManifoldPoolAllocator->freeMemory(manifold); + } + else + { + btAlignedFree(manifold); + } +} + +struct CollisionDispatcherUpdater : public btIParallelForBody +{ + btBroadphasePair* mPairArray; + btNearCallback mCallback; + btCollisionDispatcher* mDispatcher; + const btDispatcherInfo* mInfo; + + CollisionDispatcherUpdater() + { + mPairArray = NULL; + mCallback = NULL; + mDispatcher = NULL; + mInfo = NULL; + } + void forLoop(int iBegin, int iEnd) const + { + for (int i = iBegin; i < iEnd; ++i) + { + btBroadphasePair* pair = &mPairArray[i]; + mCallback(*pair, *mDispatcher, *mInfo); + } + } +}; + +void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) +{ + const int pairCount = pairCache->getNumOverlappingPairs(); + if (pairCount == 0) + { + return; + } + CollisionDispatcherUpdater updater; + updater.mCallback = getNearCallback(); + updater.mPairArray = pairCache->getOverlappingPairArrayPtr(); + updater.mDispatcher = this; + updater.mInfo = &info; + + m_batchUpdating = true; + btParallelFor(0, pairCount, m_grainSize, updater); + m_batchUpdating = false; + + // merge new manifolds, if any + for (int i = 0; i < m_batchManifoldsPtr.size(); ++i) + { + btAlignedObjectArray<btPersistentManifold*>& batchManifoldsPtr = m_batchManifoldsPtr[i]; + + for (int j = 0; j < batchManifoldsPtr.size(); ++j) + { + m_manifoldsPtr.push_back(batchManifoldsPtr[j]); + } + + batchManifoldsPtr.resizeNoInitialize(0); + } + + // update the indices (used when releasing manifolds) + for (int i = 0; i < m_manifoldsPtr.size(); ++i) + { + m_manifoldsPtr[i]->m_index1a = i; + } +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h new file mode 100644 index 00000000000..1155de2cfe6 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h @@ -0,0 +1,38 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_COLLISION_DISPATCHER_MT_H +#define BT_COLLISION_DISPATCHER_MT_H + +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "LinearMath/btThreads.h" + +class btCollisionDispatcherMt : public btCollisionDispatcher +{ +public: + btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize = 40); + + virtual btPersistentManifold* getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1) BT_OVERRIDE; + virtual void releaseManifold(btPersistentManifold* manifold) BT_OVERRIDE; + + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) BT_OVERRIDE; + +protected: + btAlignedObjectArray<btAlignedObjectArray<btPersistentManifold*> > m_batchManifoldsPtr; + bool m_batchUpdating; + int m_grainSize; +}; + +#endif //BT_COLLISION_DISPATCHER_MT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 395df3a550f..b48d9301d74 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -13,36 +13,45 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btCollisionObject.h" #include "LinearMath/btSerializer.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" btCollisionObject::btCollisionObject() - : m_anisotropicFriction(1.f,1.f,1.f), - m_hasAnisotropicFriction(false), - m_contactProcessingThreshold(BT_LARGE_FLOAT), - m_broadphaseHandle(0), - m_collisionShape(0), - m_extensionPointer(0), - m_rootCollisionShape(0), - m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT), - m_islandTag1(-1), - m_companionId(-1), - m_activationState1(1), - m_deactivationTime(btScalar(0.)), - m_friction(btScalar(0.5)), - m_restitution(btScalar(0.)), - m_rollingFriction(0.0f), - m_internalType(CO_COLLISION_OBJECT), - m_userObjectPointer(0), - m_userIndex(-1), - m_hitFraction(btScalar(1.)), - m_ccdSweptSphereRadius(btScalar(0.)), - m_ccdMotionThreshold(btScalar(0.)), - m_checkCollideWith(false), - m_updateRevision(0) + : m_interpolationLinearVelocity(0.f, 0.f, 0.f), + m_interpolationAngularVelocity(0.f, 0.f, 0.f), + m_anisotropicFriction(1.f, 1.f, 1.f), + m_hasAnisotropicFriction(false), + m_contactProcessingThreshold(BT_LARGE_FLOAT), + m_broadphaseHandle(0), + m_collisionShape(0), + m_extensionPointer(0), + m_rootCollisionShape(0), + m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT), + m_islandTag1(-1), + m_companionId(-1), + m_worldArrayIndex(-1), + m_activationState1(1), + m_deactivationTime(btScalar(0.)), + m_friction(btScalar(0.5)), + m_restitution(btScalar(0.)), + m_rollingFriction(0.0f), + m_spinningFriction(0.f), + m_contactDamping(.1), + m_contactStiffness(BT_LARGE_FLOAT), + m_internalType(CO_COLLISION_OBJECT), + m_userObjectPointer(0), + m_userIndex2(-1), + m_userIndex(-1), + m_userIndex3(-1), + m_hitFraction(btScalar(1.)), + m_ccdSweptSphereRadius(btScalar(0.)), + m_ccdMotionThreshold(btScalar(0.)), + m_checkCollideWith(false), + m_updateRevision(0) { m_worldTransform.setIdentity(); + m_interpolationWorldTransform.setIdentity(); } btCollisionObject::~btCollisionObject() @@ -50,8 +59,8 @@ btCollisionObject::~btCollisionObject() } void btCollisionObject::setActivationState(int newState) const -{ - if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION)) +{ + if ((m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION)) m_activationState1 = newState; } @@ -62,7 +71,7 @@ void btCollisionObject::forceActivationState(int newState) const void btCollisionObject::activate(bool forceActivation) const { - if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT))) + if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT))) { setActivationState(ACTIVE_TAG); m_deactivationTime = btScalar(0.); @@ -71,7 +80,6 @@ void btCollisionObject::activate(bool forceActivation) const const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* serializer) const { - btCollisionObjectData* dataOut = (btCollisionObjectData*)dataBuffer; m_worldTransform.serialize(dataOut->m_worldTransform); @@ -83,7 +91,7 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali dataOut->m_contactProcessingThreshold = m_contactProcessingThreshold; dataOut->m_broadphaseHandle = 0; dataOut->m_collisionShape = serializer->getUniquePointer(m_collisionShape); - dataOut->m_rootCollisionShape = 0;//@todo + dataOut->m_rootCollisionShape = 0; //@todo dataOut->m_collisionFlags = m_collisionFlags; dataOut->m_islandTag1 = m_islandTag1; dataOut->m_companionId = m_companionId; @@ -91,10 +99,12 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali dataOut->m_deactivationTime = m_deactivationTime; dataOut->m_friction = m_friction; dataOut->m_rollingFriction = m_rollingFriction; + dataOut->m_contactDamping = m_contactDamping; + dataOut->m_contactStiffness = m_contactStiffness; dataOut->m_restitution = m_restitution; dataOut->m_internalType = m_internalType; - - char* name = (char*) serializer->findNameForPointer(this); + + char* name = (char*)serializer->findNameForPointer(this); dataOut->m_name = (char*)serializer->getUniquePointer(name); if (dataOut->m_name) { @@ -104,15 +114,25 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius; dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold; dataOut->m_checkCollideWith = m_checkCollideWith; - + if (m_broadphaseHandle) + { + dataOut->m_collisionFilterGroup = m_broadphaseHandle->m_collisionFilterGroup; + dataOut->m_collisionFilterMask = m_broadphaseHandle->m_collisionFilterMask; + dataOut->m_uniqueId = m_broadphaseHandle->m_uniqueId; + } + else + { + dataOut->m_collisionFilterGroup = 0; + dataOut->m_collisionFilterMask = 0; + dataOut->m_uniqueId = -1; + } return btCollisionObjectDataName; } - void btCollisionObject::serializeSingleObject(class btSerializer* serializer) const { int len = calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_COLLISIONOBJECT_CODE,(void*)this); + serializer->finalizeChunk(chunk, structType, BT_COLLISIONOBJECT_CODE, (void*)this); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index c68402418f7..3a1c271af5b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -25,8 +25,8 @@ subject to the following restrictions: #define DISABLE_DEACTIVATION 4 #define DISABLE_SIMULATION 5 -struct btBroadphaseProxy; -class btCollisionShape; +struct btBroadphaseProxy; +class btCollisionShape; struct btCollisionShapeData; #include "LinearMath/btMotionState.h" #include "LinearMath/btAlignedAllocator.h" @@ -42,110 +42,120 @@ typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray; #define btCollisionObjectDataName "btCollisionObjectFloatData" #endif - -/// btCollisionObject can be used to manage collision detection objects. +/// btCollisionObject can be used to manage collision detection objects. /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. /// They can be added to the btCollisionWorld. -ATTRIBUTE_ALIGNED16(class) btCollisionObject +ATTRIBUTE_ALIGNED16(class) +btCollisionObject { - protected: - - btTransform m_worldTransform; + btTransform m_worldTransform; ///m_interpolationWorldTransform is used for CCD and interpolation ///it can be either previous or future (predicted) transform - btTransform m_interpolationWorldTransform; - //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) + btTransform m_interpolationWorldTransform; + //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) //without destroying the continuous interpolated motion (which uses this interpolation velocities) - btVector3 m_interpolationLinearVelocity; - btVector3 m_interpolationAngularVelocity; - - btVector3 m_anisotropicFriction; - int m_hasAnisotropicFriction; - btScalar m_contactProcessingThreshold; - - btBroadphaseProxy* m_broadphaseHandle; - btCollisionShape* m_collisionShape; + btVector3 m_interpolationLinearVelocity; + btVector3 m_interpolationAngularVelocity; + + btVector3 m_anisotropicFriction; + int m_hasAnisotropicFriction; + btScalar m_contactProcessingThreshold; + + btBroadphaseProxy* m_broadphaseHandle; + btCollisionShape* m_collisionShape; ///m_extensionPointer is used by some internal low-level Bullet extensions. - void* m_extensionPointer; - + void* m_extensionPointer; + ///m_rootCollisionShape is temporarily used to store the original collision shape ///The m_collisionShape might be temporarily replaced by a child collision shape during collision detection purposes ///If it is NULL, the m_collisionShape is not temporarily replaced. - btCollisionShape* m_rootCollisionShape; + btCollisionShape* m_rootCollisionShape; - int m_collisionFlags; + int m_collisionFlags; - int m_islandTag1; - int m_companionId; + int m_islandTag1; + int m_companionId; + int m_worldArrayIndex; // index of object in world's collisionObjects array - mutable int m_activationState1; - mutable btScalar m_deactivationTime; + mutable int m_activationState1; + mutable btScalar m_deactivationTime; - btScalar m_friction; - btScalar m_restitution; - btScalar m_rollingFriction; + btScalar m_friction; + btScalar m_restitution; + btScalar m_rollingFriction; //torsional friction orthogonal to contact normal (useful to stop spheres rolling forever) + btScalar m_spinningFriction; // torsional friction around the contact normal (useful for grasping) + btScalar m_contactDamping; + btScalar m_contactStiffness; ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc. ///do not assign your own m_internalType unless you write a new dynamics object class. - int m_internalType; + int m_internalType; ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer - void* m_userObjectPointer; - - int m_userIndex; + void* m_userObjectPointer; + + int m_userIndex2; + + int m_userIndex; + + int m_userIndex3; ///time of impact calculation - btScalar m_hitFraction; - + btScalar m_hitFraction; + ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - btScalar m_ccdSweptSphereRadius; + btScalar m_ccdSweptSphereRadius; /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold - btScalar m_ccdMotionThreshold; - + btScalar m_ccdMotionThreshold; + /// If some object should have elaborate collision filtering by sub-classes - int m_checkCollideWith; + int m_checkCollideWith; btAlignedObjectArray<const btCollisionObject*> m_objectsWithoutCollisionCheck; ///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation. - int m_updateRevision; + int m_updateRevision; + btVector3 m_customDebugColorRGB; public: - BT_DECLARE_ALIGNED_ALLOCATOR(); enum CollisionFlags { - CF_STATIC_OBJECT= 1, - CF_KINEMATIC_OBJECT= 2, + CF_STATIC_OBJECT = 1, + CF_KINEMATIC_OBJECT = 2, CF_NO_CONTACT_RESPONSE = 4, - CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) + CF_CUSTOM_MATERIAL_CALLBACK = 8, //this allows per-triangle material (friction/restitution) CF_CHARACTER_OBJECT = 16, - CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing - CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing + CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing + CF_DISABLE_SPU_COLLISION_PROCESSING = 64, //disable parallel/SPU processing + CF_HAS_CONTACT_STIFFNESS_DAMPING = 128, + CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256, + CF_HAS_FRICTION_ANCHOR = 512, + CF_HAS_COLLISION_SOUND_TRIGGER = 1024 }; - enum CollisionObjectTypes + enum CollisionObjectTypes { - CO_COLLISION_OBJECT =1, - CO_RIGID_BODY=2, + CO_COLLISION_OBJECT = 1, + CO_RIGID_BODY = 2, ///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter ///It is useful for collision sensors, explosion objects, character controller etc. - CO_GHOST_OBJECT=4, - CO_SOFT_BODY=8, - CO_HF_FLUID=16, - CO_USER_TYPE=32, - CO_FEATHERSTONE_LINK=64 + CO_GHOST_OBJECT = 4, + CO_SOFT_BODY = 8, + CO_HF_FLUID = 16, + CO_USER_TYPE = 32, + CO_FEATHERSTONE_LINK = 64 }; enum AnisotropicFrictionFlags { - CF_ANISOTROPIC_FRICTION_DISABLED=0, + CF_ANISOTROPIC_FRICTION_DISABLED = 0, CF_ANISOTROPIC_FRICTION = 1, CF_ANISOTROPIC_ROLLING_FRICTION = 2 }; @@ -153,76 +163,77 @@ public: SIMD_FORCE_INLINE bool mergesSimulationIslands() const { ///static objects, kinematic and object without contact response don't merge islands - return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0); + return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE)) == 0); } const btVector3& getAnisotropicFriction() const { return m_anisotropicFriction; } - void setAnisotropicFriction(const btVector3& anisotropicFriction, int frictionMode = CF_ANISOTROPIC_FRICTION) + void setAnisotropicFriction(const btVector3& anisotropicFriction, int frictionMode = CF_ANISOTROPIC_FRICTION) { m_anisotropicFriction = anisotropicFriction; - bool isUnity = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f); - m_hasAnisotropicFriction = isUnity?frictionMode : 0; + bool isUnity = (anisotropicFriction[0] != 1.f) || (anisotropicFriction[1] != 1.f) || (anisotropicFriction[2] != 1.f); + m_hasAnisotropicFriction = isUnity ? frictionMode : 0; } - bool hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const + bool hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const { - return (m_hasAnisotropicFriction&frictionMode)!=0; + return (m_hasAnisotropicFriction & frictionMode) != 0; } ///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default. ///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges - void setContactProcessingThreshold( btScalar contactProcessingThreshold) + void setContactProcessingThreshold(btScalar contactProcessingThreshold) { m_contactProcessingThreshold = contactProcessingThreshold; } - btScalar getContactProcessingThreshold() const + btScalar getContactProcessingThreshold() const { return m_contactProcessingThreshold; } - SIMD_FORCE_INLINE bool isStaticObject() const { + SIMD_FORCE_INLINE bool isStaticObject() const + { return (m_collisionFlags & CF_STATIC_OBJECT) != 0; } - SIMD_FORCE_INLINE bool isKinematicObject() const + SIMD_FORCE_INLINE bool isKinematicObject() const { return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0; } - SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const + SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const { - return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ; + return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0; } - SIMD_FORCE_INLINE bool hasContactResponse() const { - return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0; + SIMD_FORCE_INLINE bool hasContactResponse() const + { + return (m_collisionFlags & CF_NO_CONTACT_RESPONSE) == 0; } - btCollisionObject(); virtual ~btCollisionObject(); - virtual void setCollisionShape(btCollisionShape* collisionShape) + virtual void setCollisionShape(btCollisionShape * collisionShape) { m_updateRevision++; m_collisionShape = collisionShape; m_rootCollisionShape = collisionShape; } - SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const + SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_collisionShape; } - SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() + SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() { return m_collisionShape; } - void setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck) + void setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck) { if (ignoreCollisionCheck) { @@ -240,7 +251,17 @@ public: m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0; } - virtual bool checkCollideWithOverride(const btCollisionObject* co) const + int getNumObjectsWithoutCollision() const + { + return m_objectsWithoutCollisionCheck.size(); + } + + const btCollisionObject* getObjectWithoutCollision(int index) + { + return m_objectsWithoutCollisionCheck[index]; + } + + virtual bool checkCollideWithOverride(const btCollisionObject* co) const { int index = m_objectsWithoutCollisionCheck.findLinearSearch(co); if (index < m_objectsWithoutCollisionCheck.size()) @@ -250,249 +271,325 @@ public: return true; } - - - - ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions. + ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions. ///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead. - void* internalGetExtensionPointer() const + void* internalGetExtensionPointer() const { return m_extensionPointer; } ///Avoid using this internal API call, the extension pointer is used by some Bullet extensions ///If you need to store your own user pointer, use 'setUserPointer/getUserPointer' instead. - void internalSetExtensionPointer(void* pointer) + void internalSetExtensionPointer(void* pointer) { m_extensionPointer = pointer; } - SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1;} - + SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1; } + void setActivationState(int newState) const; - void setDeactivationTime(btScalar time) + void setDeactivationTime(btScalar time) { m_deactivationTime = time; } - btScalar getDeactivationTime() const + btScalar getDeactivationTime() const { return m_deactivationTime; } void forceActivationState(int newState) const; - void activate(bool forceActivation = false) const; + void activate(bool forceActivation = false) const; SIMD_FORCE_INLINE bool isActive() const { return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); } - void setRestitution(btScalar rest) + void setRestitution(btScalar rest) { m_updateRevision++; m_restitution = rest; } - btScalar getRestitution() const + btScalar getRestitution() const { return m_restitution; } - void setFriction(btScalar frict) + void setFriction(btScalar frict) { m_updateRevision++; m_friction = frict; } - btScalar getFriction() const + btScalar getFriction() const { return m_friction; } - void setRollingFriction(btScalar frict) + void setRollingFriction(btScalar frict) { m_updateRevision++; m_rollingFriction = frict; } - btScalar getRollingFriction() const + btScalar getRollingFriction() const { return m_rollingFriction; } + void setSpinningFriction(btScalar frict) + { + m_updateRevision++; + m_spinningFriction = frict; + } + btScalar getSpinningFriction() const + { + return m_spinningFriction; + } + void setContactStiffnessAndDamping(btScalar stiffness, btScalar damping) + { + m_updateRevision++; + m_contactStiffness = stiffness; + m_contactDamping = damping; + m_collisionFlags |= CF_HAS_CONTACT_STIFFNESS_DAMPING; + + //avoid divisions by zero... + if (m_contactStiffness < SIMD_EPSILON) + { + m_contactStiffness = SIMD_EPSILON; + } + } + + btScalar getContactStiffness() const + { + return m_contactStiffness; + } + + btScalar getContactDamping() const + { + return m_contactDamping; + } ///reserved for Bullet internal usage - int getInternalType() const + int getInternalType() const { return m_internalType; } - btTransform& getWorldTransform() + btTransform& getWorldTransform() { return m_worldTransform; } - const btTransform& getWorldTransform() const + const btTransform& getWorldTransform() const { return m_worldTransform; } - void setWorldTransform(const btTransform& worldTrans) + void setWorldTransform(const btTransform& worldTrans) { m_updateRevision++; m_worldTransform = worldTrans; } - - SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle() + SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle() { return m_broadphaseHandle; } - SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const + SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const { return m_broadphaseHandle; } - void setBroadphaseHandle(btBroadphaseProxy* handle) + void setBroadphaseHandle(btBroadphaseProxy * handle) { m_broadphaseHandle = handle; } - - const btTransform& getInterpolationWorldTransform() const + const btTransform& getInterpolationWorldTransform() const { return m_interpolationWorldTransform; } - btTransform& getInterpolationWorldTransform() + btTransform& getInterpolationWorldTransform() { return m_interpolationWorldTransform; } - void setInterpolationWorldTransform(const btTransform& trans) + void setInterpolationWorldTransform(const btTransform& trans) { m_updateRevision++; m_interpolationWorldTransform = trans; } - void setInterpolationLinearVelocity(const btVector3& linvel) + void setInterpolationLinearVelocity(const btVector3& linvel) { m_updateRevision++; m_interpolationLinearVelocity = linvel; } - void setInterpolationAngularVelocity(const btVector3& angvel) + void setInterpolationAngularVelocity(const btVector3& angvel) { m_updateRevision++; m_interpolationAngularVelocity = angvel; } - const btVector3& getInterpolationLinearVelocity() const + const btVector3& getInterpolationLinearVelocity() const { return m_interpolationLinearVelocity; } - const btVector3& getInterpolationAngularVelocity() const + const btVector3& getInterpolationAngularVelocity() const { return m_interpolationAngularVelocity; } SIMD_FORCE_INLINE int getIslandTag() const { - return m_islandTag1; + return m_islandTag1; } - void setIslandTag(int tag) + void setIslandTag(int tag) { m_islandTag1 = tag; } SIMD_FORCE_INLINE int getCompanionId() const { - return m_companionId; + return m_companionId; } - void setCompanionId(int id) + void setCompanionId(int id) { m_companionId = id; } - SIMD_FORCE_INLINE btScalar getHitFraction() const + SIMD_FORCE_INLINE int getWorldArrayIndex() const { - return m_hitFraction; + return m_worldArrayIndex; } - void setHitFraction(btScalar hitFraction) + // only should be called by CollisionWorld + void setWorldArrayIndex(int ix) + { + m_worldArrayIndex = ix; + } + + SIMD_FORCE_INLINE btScalar getHitFraction() const + { + return m_hitFraction; + } + + void setHitFraction(btScalar hitFraction) { m_hitFraction = hitFraction; } - - SIMD_FORCE_INLINE int getCollisionFlags() const + SIMD_FORCE_INLINE int getCollisionFlags() const { return m_collisionFlags; } - void setCollisionFlags(int flags) + void setCollisionFlags(int flags) { m_collisionFlags = flags; } - + ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - btScalar getCcdSweptSphereRadius() const + btScalar getCcdSweptSphereRadius() const { return m_ccdSweptSphereRadius; } ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - void setCcdSweptSphereRadius(btScalar radius) + void setCcdSweptSphereRadius(btScalar radius) { m_ccdSweptSphereRadius = radius; } - btScalar getCcdMotionThreshold() const + btScalar getCcdMotionThreshold() const { return m_ccdMotionThreshold; } - btScalar getCcdSquareMotionThreshold() const + btScalar getCcdSquareMotionThreshold() const { - return m_ccdMotionThreshold*m_ccdMotionThreshold; + return m_ccdMotionThreshold * m_ccdMotionThreshold; } - - /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold - void setCcdMotionThreshold(btScalar ccdMotionThreshold) + void setCcdMotionThreshold(btScalar ccdMotionThreshold) { m_ccdMotionThreshold = ccdMotionThreshold; } ///users can point to their objects, userPointer is not used by Bullet - void* getUserPointer() const + void* getUserPointer() const { return m_userObjectPointer; } - int getUserIndex() const + int getUserIndex() const { return m_userIndex; } + + int getUserIndex2() const + { + return m_userIndex2; + } + + int getUserIndex3() const + { + return m_userIndex3; + } + ///users can point to their objects, userPointer is not used by Bullet - void setUserPointer(void* userPointer) + void setUserPointer(void* userPointer) { m_userObjectPointer = userPointer; } ///users can point to their objects, userPointer is not used by Bullet - void setUserIndex(int index) + void setUserIndex(int index) { m_userIndex = index; } - int getUpdateRevisionInternal() const + void setUserIndex2(int index) + { + m_userIndex2 = index; + } + + void setUserIndex3(int index) + { + m_userIndex3 = index; + } + + int getUpdateRevisionInternal() const { return m_updateRevision; } + void setCustomDebugColor(const btVector3& colorRGB) + { + m_customDebugColorRGB = colorRGB; + m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; + } + + void removeCustomDebugColor() + { + m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; + } + + bool getCustomDebugColor(btVector3 & colorRGB) const + { + bool hasCustomColor = (0 != (m_collisionFlags & CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR)); + if (hasCustomColor) + { + colorRGB = m_customDebugColorRGB; + } + return hasCustomColor; + } inline bool checkCollideWith(const btCollisionObject* co) const { @@ -502,15 +599,16 @@ public: return true; } - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; - - virtual void serializeSingleObject(class btSerializer* serializer) const; + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; + virtual void serializeSingleObject(class btSerializer * serializer) const; }; +// clang-format off + ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btCollisionObjectDoubleData { @@ -528,11 +626,12 @@ struct btCollisionObjectDoubleData double m_deactivationTime; double m_friction; double m_rollingFriction; + double m_contactDamping; + double m_contactStiffness; double m_restitution; double m_hitFraction; double m_ccdSweptSphereRadius; double m_ccdMotionThreshold; - int m_hasAnisotropicFriction; int m_collisionFlags; int m_islandTag1; @@ -540,8 +639,9 @@ struct btCollisionObjectDoubleData int m_activationState1; int m_internalType; int m_checkCollideWith; - - char m_padding[4]; + int m_collisionFilterGroup; + int m_collisionFilterMask; + int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 @@ -561,12 +661,12 @@ struct btCollisionObjectFloatData float m_deactivationTime; float m_friction; float m_rollingFriction; - + float m_contactDamping; + float m_contactStiffness; float m_restitution; float m_hitFraction; float m_ccdSweptSphereRadius; float m_ccdMotionThreshold; - int m_hasAnisotropicFriction; int m_collisionFlags; int m_islandTag1; @@ -574,16 +674,15 @@ struct btCollisionObjectFloatData int m_activationState1; int m_internalType; int m_checkCollideWith; - char m_padding[4]; + int m_collisionFilterGroup; + int m_collisionFilterMask; + int m_uniqueId; }; +// clang-format on - - -SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const +SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const { return sizeof(btCollisionObjectData); } - - -#endif //BT_COLLISION_OBJECT_H +#endif //BT_COLLISION_OBJECT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h index 952440b7dee..56341b7d292 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h @@ -1,25 +1,25 @@ #ifndef BT_COLLISION_OBJECT_WRAPPER_H #define BT_COLLISION_OBJECT_WRAPPER_H -///btCollisionObjectWrapperis an internal data structure. +///btCollisionObjectWrapperis an internal data structure. ///Most users can ignore this and use btCollisionObject and btCollisionShape instead class btCollisionShape; class btCollisionObject; class btTransform; -#include "LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition +#include "LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition #define BT_DECLARE_STACK_ONLY_OBJECT \ - private: \ - void* operator new(size_t size); \ - void operator delete(void*); +private: \ + void* operator new(size_t size); \ + void operator delete(void*); struct btCollisionObjectWrapper; struct btCollisionObjectWrapper { -BT_DECLARE_STACK_ONLY_OBJECT + BT_DECLARE_STACK_ONLY_OBJECT private: - btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed. + btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed. btCollisionObjectWrapper* operator=(const btCollisionObjectWrapper&); public: @@ -27,17 +27,23 @@ public: const btCollisionShape* m_shape; const btCollisionObject* m_collisionObject; const btTransform& m_worldTransform; - int m_partId; - int m_index; + const btTransform* m_preTransform; + int m_partId; + int m_index; btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, int partId, int index) - : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), - m_partId(partId), m_index(index) - {} + : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_preTransform(NULL), m_partId(partId), m_index(index) + { + } + + btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, const btTransform& preTransform, int partId, int index) + : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_preTransform(&preTransform), m_partId(partId), m_index(index) + { + } SIMD_FORCE_INLINE const btTransform& getWorldTransform() const { return m_worldTransform; } SIMD_FORCE_INLINE const btCollisionObject* getCollisionObject() const { return m_collisionObject; } SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_shape; } }; -#endif //BT_COLLISION_OBJECT_WRAPPER_H +#endif //BT_COLLISION_OBJECT_WRAPPER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index c505ed5d508..71184f36acd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -19,8 +19,10 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting -#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" //for raycasting #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" @@ -37,7 +39,6 @@ subject to the following restrictions: //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION - //#define USE_BRUTEFORCE_RAYBROADPHASE 1 //RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation' or 'updateAabbs' before using a rayTest //#define RECALCULATE_AABB_RAYCAST 1 @@ -47,7 +48,6 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" - ///for debug drawing //for debug rendering @@ -64,25 +64,21 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" - - -btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration) -:m_dispatcher1(dispatcher), -m_broadphasePairCache(pairCache), -m_debugDrawer(0), -m_forceUpdateAllAabbs(true) +btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration) + : m_dispatcher1(dispatcher), + m_broadphasePairCache(pairCache), + m_debugDrawer(0), + m_forceUpdateAllAabbs(true) { } - btCollisionWorld::~btCollisionWorld() { - //clean up remaining objects int i; - for (i=0;i<m_collisionObjects.size();i++) + for (i = 0; i < m_collisionObjects.size(); i++) { - btCollisionObject* collisionObject= m_collisionObjects[i]; + btCollisionObject* collisionObject = m_collisionObjects[i]; btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); if (bp) @@ -90,73 +86,83 @@ btCollisionWorld::~btCollisionWorld() // // only clear the cached algorithms // - getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); - getBroadphase()->destroyProxy(bp,m_dispatcher1); + getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp, m_dispatcher1); + getBroadphase()->destroyProxy(bp, m_dispatcher1); collisionObject->setBroadphaseHandle(0); } } - - } - - - - - - - - - -void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) +void btCollisionWorld::refreshBroadphaseProxy(btCollisionObject* collisionObject) { + if (collisionObject->getBroadphaseHandle()) + { + int collisionFilterGroup = collisionObject->getBroadphaseHandle()->m_collisionFilterGroup; + int collisionFilterMask = collisionObject->getBroadphaseHandle()->m_collisionFilterMask; + + getBroadphase()->destroyProxy(collisionObject->getBroadphaseHandle(), getDispatcher()); + + //calculate new AABB + btTransform trans = collisionObject->getWorldTransform(); + + btVector3 minAabb; + btVector3 maxAabb; + collisionObject->getCollisionShape()->getAabb(trans, minAabb, maxAabb); + + int type = collisionObject->getCollisionShape()->getShapeType(); + collisionObject->setBroadphaseHandle(getBroadphase()->createProxy( + minAabb, + maxAabb, + type, + collisionObject, + collisionFilterGroup, + collisionFilterMask, + m_dispatcher1)); + } +} +void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) +{ btAssert(collisionObject); //check that the object isn't already added - btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); + btAssert(m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); + btAssert(collisionObject->getWorldArrayIndex() == -1); // do not add the same object to more than one collision world + collisionObject->setWorldArrayIndex(m_collisionObjects.size()); m_collisionObjects.push_back(collisionObject); //calculate new AABB btTransform trans = collisionObject->getWorldTransform(); - btVector3 minAabb; - btVector3 maxAabb; - collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb); + btVector3 minAabb; + btVector3 maxAabb; + collisionObject->getCollisionShape()->getAabb(trans, minAabb, maxAabb); int type = collisionObject->getCollisionShape()->getShapeType(); - collisionObject->setBroadphaseHandle( getBroadphase()->createProxy( + collisionObject->setBroadphaseHandle(getBroadphase()->createProxy( minAabb, maxAabb, type, collisionObject, collisionFilterGroup, collisionFilterMask, - m_dispatcher1,0 - )) ; - - - - - + m_dispatcher1)); } - - -void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) +void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) { - btVector3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + btVector3 minAabb, maxAabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb); //need to increase the aabb for contact thresholds - btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + btVector3 contactThreshold(gContactBreakingThreshold, gContactBreakingThreshold, gContactBreakingThreshold); minAabb -= contactThreshold; maxAabb += contactThreshold; - if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) + if (getDispatchInfo().m_useContinuous && colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) { - btVector3 minAabb2,maxAabb2; - colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); + btVector3 minAabb2, maxAabb2; + colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(), minAabb2, maxAabb2); minAabb2 -= contactThreshold; maxAabb2 += contactThreshold; minAabb.setMin(minAabb2); @@ -166,10 +172,11 @@ void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; //moving objects should be moderately sized, probably something wrong if not - if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) + if (colObj->isStaticObject() || ((maxAabb - minAabb).length2() < btScalar(1e12))) { - bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); - } else + bp->setAabb(colObj->getBroadphaseHandle(), minAabb, maxAabb, m_dispatcher1); + } + else { //something went wrong, investigate //this assert is unwanted in 3D modelers (danger of loosing work) @@ -187,14 +194,15 @@ void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) } } -void btCollisionWorld::updateAabbs() +void btCollisionWorld::updateAabbs() { BT_PROFILE("updateAabbs"); btTransform predictedTrans; - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; + btAssert(colObj->getWorldArrayIndex() == i); //only update aabb of active objects if (m_forceUpdateAllAabbs || colObj->isActive()) @@ -204,14 +212,13 @@ void btCollisionWorld::updateAabbs() } } - -void btCollisionWorld::computeOverlappingPairs() +void btCollisionWorld::computeOverlappingPairs() { BT_PROFILE("calculateOverlappingPairs"); m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); } -void btCollisionWorld::performDiscreteCollisionDetection() +void btCollisionWorld::performDiscreteCollisionDetection() { BT_PROFILE("performDiscreteCollisionDetection"); @@ -225,53 +232,61 @@ void btCollisionWorld::performDiscreteCollisionDetection() { BT_PROFILE("dispatchAllCollisionPairs"); if (dispatcher) - dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); + dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(), dispatchInfo, m_dispatcher1); } - } - - -void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) +void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) { - - //bool removeFromBroadphase = false; { - btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); if (bp) { // // only clear the cached algorithms // - getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); - getBroadphase()->destroyProxy(bp,m_dispatcher1); + getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp, m_dispatcher1); + getBroadphase()->destroyProxy(bp, m_dispatcher1); collisionObject->setBroadphaseHandle(0); } } - - //swapremove - m_collisionObjects.remove(collisionObject); - + int iObj = collisionObject->getWorldArrayIndex(); + // btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously? + if (iObj >= 0 && iObj < m_collisionObjects.size()) + { + btAssert(collisionObject == m_collisionObjects[iObj]); + m_collisionObjects.swap(iObj, m_collisionObjects.size() - 1); + m_collisionObjects.pop_back(); + if (iObj < m_collisionObjects.size()) + { + m_collisionObjects[iObj]->setWorldArrayIndex(iObj); + } + } + else + { + // slow linear search + //swapremove + m_collisionObjects.remove(collisionObject); + } + collisionObject->setWorldArrayIndex(-1); } - -void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) +void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) { - btCollisionObjectWrapper colObWrap(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1); - btCollisionWorld::rayTestSingleInternal(rayFromTrans,rayToTrans,&colObWrap,resultCallback); + btCollisionObjectWrapper colObWrap(0, collisionShape, collisionObject, colObjWorldTransform, -1, -1); + btCollisionWorld::rayTestSingleInternal(rayFromTrans, rayToTrans, &colObWrap, resultCallback); } -void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans, - const btCollisionObjectWrapper* collisionObjectWrap, - RayResultCallback& resultCallback) +void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, const btTransform& rayToTrans, + const btCollisionObjectWrapper* collisionObjectWrap, + RayResultCallback& resultCallback) { btSphereShape pointShape(btScalar(0.0)); pointShape.setMargin(0.f); @@ -285,12 +300,12 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con btConvexCast::CastResult castResult; castResult.m_fraction = resultCallback.m_closestHitFraction; - btConvexShape* convexShape = (btConvexShape*) collisionShape; - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast subSimplexConvexCaster(castShape,convexShape,&simplexSolver); - - btGjkConvexCast gjkConvexCaster(castShape,convexShape,&simplexSolver); - + btConvexShape* convexShape = (btConvexShape*)collisionShape; + btVoronoiSimplexSolver simplexSolver; + btSubsimplexConvexCast subSimplexConvexCaster(castShape, convexShape, &simplexSolver); + + btGjkConvexCast gjkConvexCaster(castShape, convexShape, &simplexSolver); + //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); btConvexCast* convexCasterPtr = 0; @@ -299,10 +314,10 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con convexCasterPtr = &gjkConvexCaster; else convexCasterPtr = &subSimplexConvexCaster; - + btConvexCast& convexCaster = *convexCasterPtr; - if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + if (convexCaster.calcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) @@ -313,81 +328,105 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con #ifdef USE_SUBSIMPLEX_CONVEX_CAST //rotate normal into worldspace castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal; -#endif //USE_SUBSIMPLEX_CONVEX_CAST +#endif //USE_SUBSIMPLEX_CONVEX_CAST castResult.m_normal.normalize(); - btCollisionWorld::LocalRayResult localRayResult - ( + btCollisionWorld::LocalRayResult localRayResult( collisionObjectWrap->getCollisionObject(), 0, castResult.m_normal, - castResult.m_fraction - ); + castResult.m_fraction); bool normalInWorldSpace = true; resultCallback.addSingleResult(localRayResult, normalInWorldSpace); - } } } - } else { + } + else + { if (collisionShape->isConcave()) { - //ConvexCast::CastResult - struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + { + btCollisionWorld::RayResultCallback* m_resultCallback; + const btCollisionObject* m_collisionObject; + const btConcaveShape* m_triangleMesh; + + btTransform m_colObjWorldTransform; + + BridgeTriangleRaycastCallback(const btVector3& from, const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject, const btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform) : //@BP Mod + btTriangleRaycastCallback(from, to, resultCallback->m_flags), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh), + m_colObjWorldTransform(colObjWorldTransform) { - btCollisionWorld::RayResultCallback* m_resultCallback; - const btCollisionObject* m_collisionObject; - const btConcaveShape* m_triangleMesh; - - btTransform m_colObjWorldTransform; - - BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,const btConcaveShape* triangleMesh,const btTransform& colObjWorldTransform): - //@BP Mod - btTriangleRaycastCallback(from,to, resultCallback->m_flags), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh), - m_colObjWorldTransform(colObjWorldTransform) - { - } - - - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = partId; - shapeInfo.m_triangleIndex = triangleIndex; + } - btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; - btCollisionWorld::LocalRayResult rayResult - (m_collisionObject, - &shapeInfo, - hitNormalWorld, - hitFraction); + btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; - bool normalInWorldSpace = true; - return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); - } + btCollisionWorld::LocalRayResult rayResult(m_collisionObject, + &shapeInfo, + hitNormalWorld, + hitFraction); - }; + bool normalInWorldSpace = true; + return m_resultCallback->addSingleResult(rayResult, normalInWorldSpace); + } + }; btTransform worldTocollisionObject = colObjWorldTransform.inverse(); btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); // BT_PROFILE("rayTestConcave"); - if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) + if (collisionShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { ///optimized version for btBvhTriangleMeshShape btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; - - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),triangleMesh,colObjWorldTransform); + + BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), triangleMesh, colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; - triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); + triangleMesh->performRaycast(&rcb, rayFromLocal, rayToLocal); + } + else if (collisionShape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + ///optimized version for btScaledBvhTriangleMeshShape + btScaledBvhTriangleMeshShape* scaledTriangleMesh = (btScaledBvhTriangleMeshShape*)collisionShape; + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)scaledTriangleMesh->getChildShape(); + + //scale the ray positions + btVector3 scale = scaledTriangleMesh->getLocalScaling(); + btVector3 rayFromLocalScaled = rayFromLocal / scale; + btVector3 rayToLocalScaled = rayToLocal / scale; + + //perform raycast in the underlying btBvhTriangleMeshShape + BridgeTriangleRaycastCallback rcb(rayFromLocalScaled, rayToLocalScaled, &resultCallback, collisionObjectWrap->getCollisionObject(), triangleMesh, colObjWorldTransform); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + triangleMesh->performRaycast(&rcb, rayFromLocalScaled, rayToLocalScaled); + } + else if (((resultCallback.m_flags&btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator)==0) + && collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE + ) + { + ///optimized version for btHeightfieldTerrainShape + btHeightfieldTerrainShape* heightField = (btHeightfieldTerrainShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), heightField, colObjWorldTransform); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + heightField->performRaycast(&rcb, rayFromLocal, rayToLocal); } else { @@ -404,45 +443,40 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback { btCollisionWorld::RayResultCallback* m_resultCallback; - const btCollisionObject* m_collisionObject; - btConcaveShape* m_triangleMesh; + const btCollisionObject* m_collisionObject; + btConcaveShape* m_triangleMesh; btTransform m_colObjWorldTransform; - BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform): - //@BP Mod - btTriangleRaycastCallback(from,to, resultCallback->m_flags), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh), - m_colObjWorldTransform(colObjWorldTransform) + BridgeTriangleRaycastCallback(const btVector3& from, const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject, btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform) : //@BP Mod + btTriangleRaycastCallback(from, to, resultCallback->m_flags), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh), + m_colObjWorldTransform(colObjWorldTransform) { } - - virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex) { - btCollisionWorld::LocalShapeInfo shapeInfo; + btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; - btCollisionWorld::LocalRayResult rayResult - (m_collisionObject, - &shapeInfo, - hitNormalWorld, - hitFraction); + btCollisionWorld::LocalRayResult rayResult(m_collisionObject, + &shapeInfo, + hitNormalWorld, + hitFraction); - bool normalInWorldSpace = true; - return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); + bool normalInWorldSpace = true; + return m_resultCallback->addSingleResult(rayResult, normalInWorldSpace); } - }; - - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform); + BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), concaveShape, colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 rayAabbMinLocal = rayFromLocal; @@ -450,9 +484,11 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con btVector3 rayAabbMaxLocal = rayFromLocal; rayAabbMaxLocal.setMax(rayToLocal); - concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); + concaveShape->processAllTriangles(&rcb, rayAabbMinLocal, rayAabbMaxLocal); } - } else { + } + else + { // BT_PROFILE("rayTestCompound"); if (collisionShape->isCompound()) { @@ -460,10 +496,10 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con { RayResultCallback* m_userCallback; int m_i; - - LocalInfoAdder2 (int i, RayResultCallback *user) + + LocalInfoAdder2(int i, RayResultCallback* user) : m_userCallback(user), m_i(i) - { + { m_closestHitFraction = m_userCallback->m_closestHitFraction; m_flags = m_userCallback->m_flags; } @@ -472,7 +508,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con return m_userCallback->needsCollision(p); } - virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b) + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& r, bool b) { btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = -1; @@ -485,7 +521,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con return result; } }; - + struct RayTester : btDbvt::ICollide { const btCollisionObject* m_collisionObject; @@ -494,33 +530,29 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con const btTransform& m_rayFromTrans; const btTransform& m_rayToTrans; RayResultCallback& m_resultCallback; - + RayTester(const btCollisionObject* collisionObject, - const btCompoundShape* compoundShape, - const btTransform& colObjWorldTransform, - const btTransform& rayFromTrans, - const btTransform& rayToTrans, - RayResultCallback& resultCallback): - m_collisionObject(collisionObject), - m_compoundShape(compoundShape), - m_colObjWorldTransform(colObjWorldTransform), - m_rayFromTrans(rayFromTrans), - m_rayToTrans(rayToTrans), - m_resultCallback(resultCallback) + const btCompoundShape* compoundShape, + const btTransform& colObjWorldTransform, + const btTransform& rayFromTrans, + const btTransform& rayToTrans, + RayResultCallback& resultCallback) : m_collisionObject(collisionObject), + m_compoundShape(compoundShape), + m_colObjWorldTransform(colObjWorldTransform), + m_rayFromTrans(rayFromTrans), + m_rayToTrans(rayToTrans), + m_resultCallback(resultCallback) { - } - + void ProcessLeaf(int i) { const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i); const btTransform& childTrans = m_compoundShape->getChildTransform(i); btTransform childWorldTrans = m_colObjWorldTransform * childTrans; - - btCollisionObjectWrapper tmpOb(0,childCollisionShape,m_collisionObject,childWorldTrans,-1,i); - // replace collision shape so that callback can determine the triangle - + btCollisionObjectWrapper tmpOb(0, childCollisionShape, m_collisionObject, childWorldTrans, -1, i); + // replace collision shape so that callback can determine the triangle LocalInfoAdder2 my_cb(i, &m_resultCallback); @@ -529,19 +561,17 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con m_rayToTrans, &tmpOb, my_cb); - } - + void Process(const btDbvtNode* leaf) { ProcessLeaf(leaf->dataAsInt); } }; - + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); const btDbvt* dbvt = compoundShape->getDynamicAabbTree(); - RayTester rayCB( collisionObjectWrap->getCollisionObject(), compoundShape, @@ -549,39 +579,39 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con rayFromTrans, rayToTrans, resultCallback); -#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION +#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION if (dbvt) { btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin(); btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin(); - btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB); + btDbvt::rayTest(dbvt->m_root, localRayFrom, localRayTo, rayCB); } else -#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION +#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION { for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i) { rayCB.ProcessLeaf(i); - } + } } } } } } -void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - ConvexResultCallback& resultCallback, btScalar allowedPenetration) +void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback, btScalar allowedPenetration) { - btCollisionObjectWrapper tmpOb(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1); - btCollisionWorld::objectQuerySingleInternal(castShape,convexFromTrans,convexToTrans,&tmpOb,resultCallback,allowedPenetration); + btCollisionObjectWrapper tmpOb(0, collisionShape, collisionObject, colObjWorldTransform, -1, -1); + btCollisionWorld::objectQuerySingleInternal(castShape, convexFromTrans, convexToTrans, &tmpOb, resultCallback, allowedPenetration); } -void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, - const btCollisionObjectWrapper* colObjWrap, - ConvexResultCallback& resultCallback, btScalar allowedPenetration) +void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, + const btCollisionObjectWrapper* colObjWrap, + ConvexResultCallback& resultCallback, btScalar allowedPenetration) { const btCollisionShape* collisionShape = colObjWrap->getCollisionShape(); const btTransform& colObjWorldTransform = colObjWrap->getWorldTransform(); @@ -591,21 +621,19 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, //BT_PROFILE("convexSweepConvex"); btConvexCast::CastResult castResult; castResult.m_allowedPenetration = allowedPenetration; - castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//?? + castResult.m_fraction = resultCallback.m_closestHitFraction; //btScalar(1.);//?? - btConvexShape* convexShape = (btConvexShape*) collisionShape; - btVoronoiSimplexSolver simplexSolver; - btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; + btConvexShape* convexShape = (btConvexShape*)collisionShape; + btVoronoiSimplexSolver simplexSolver; + btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; - btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver); + btContinuousConvexCollision convexCaster1(castShape, convexShape, &simplexSolver, &gjkEpaPenetrationSolver); //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver); //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver); btConvexCast* castPtr = &convexCaster1; - - - if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + if (castPtr->calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) @@ -613,25 +641,24 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, if (castResult.m_fraction < resultCallback.m_closestHitFraction) { castResult.m_normal.normalize(); - btCollisionWorld::LocalConvexResult localConvexResult - ( + btCollisionWorld::LocalConvexResult localConvexResult( colObjWrap->getCollisionObject(), 0, castResult.m_normal, castResult.m_hitPoint, - castResult.m_fraction - ); + castResult.m_fraction); bool normalInWorldSpace = true; resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); - } } } - } else { + } + else + { if (collisionShape->isConcave()) { - if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) + if (collisionShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { //BT_PROFILE("convexSweepbtBvhTriangleMesh"); btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; @@ -645,62 +672,57 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback { btCollisionWorld::ConvexResultCallback* m_resultCallback; - const btCollisionObject* m_collisionObject; - btTriangleMeshShape* m_triangleMesh; - - BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, - btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld): - btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) + const btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; + + BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from, const btTransform& to, + btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject, btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld) : btTriangleConvexcastCallback(castShape, from, to, triangleToWorld, triangleMesh->getMargin()), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) { } - - virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) { - btCollisionWorld::LocalShapeInfo shapeInfo; + btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; if (hitFraction <= m_resultCallback->m_closestHitFraction) { + btCollisionWorld::LocalConvexResult convexResult(m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); - btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitPointLocal, - hitFraction); - - bool normalInWorldSpace = true; + bool normalInWorldSpace = true; - - return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); + return m_resultCallback->addSingleResult(convexResult, normalInWorldSpace); } return hitFraction; } - }; - BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),triangleMesh, colObjWorldTransform); + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans, convexToTrans, &resultCallback, colObjWrap->getCollisionObject(), triangleMesh, colObjWorldTransform); tccb.m_hitFraction = resultCallback.m_closestHitFraction; tccb.m_allowedPenetration = allowedPenetration; btVector3 boxMinLocal, boxMaxLocal; castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); - triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal); - } else + triangleMesh->performConvexcast(&tccb, convexFromLocal, convexToLocal, boxMinLocal, boxMaxLocal); + } + else { - if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE) + if (collisionShape->getShapeType() == STATIC_PLANE_PROXYTYPE) { btConvexCast::CastResult castResult; castResult.m_allowedPenetration = allowedPenetration; castResult.m_fraction = resultCallback.m_closestHitFraction; - btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape; - btContinuousConvexCollision convexCaster1(castShape,planeShape); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*)collisionShape; + btContinuousConvexCollision convexCaster1(castShape, planeShape); btConvexCast* castPtr = &convexCaster1; - if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + if (castPtr->calcTimeOfImpact(convexFromTrans, convexToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) @@ -708,22 +730,20 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, if (castResult.m_fraction < resultCallback.m_closestHitFraction) { castResult.m_normal.normalize(); - btCollisionWorld::LocalConvexResult localConvexResult - ( + btCollisionWorld::LocalConvexResult localConvexResult( colObjWrap->getCollisionObject(), 0, castResult.m_normal, castResult.m_hitPoint, - castResult.m_fraction - ); + castResult.m_fraction); bool normalInWorldSpace = true; resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); } } } - - } else + } + else { //BT_PROFILE("convexSweepConcave"); btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; @@ -737,44 +757,39 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback { btCollisionWorld::ConvexResultCallback* m_resultCallback; - const btCollisionObject* m_collisionObject; - btConcaveShape* m_triangleMesh; - - BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, - btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): - btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) + const btCollisionObject* m_collisionObject; + btConcaveShape* m_triangleMesh; + + BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from, const btTransform& to, + btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject, btConcaveShape* triangleMesh, const btTransform& triangleToWorld) : btTriangleConvexcastCallback(castShape, from, to, triangleToWorld, triangleMesh->getMargin()), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) { } - - virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) { - btCollisionWorld::LocalShapeInfo shapeInfo; + btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; if (hitFraction <= m_resultCallback->m_closestHitFraction) { + btCollisionWorld::LocalConvexResult convexResult(m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); - btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitPointLocal, - hitFraction); - - bool normalInWorldSpace = true; + bool normalInWorldSpace = true; - return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); + return m_resultCallback->addSingleResult(convexResult, normalInWorldSpace); } return hitFraction; } - }; - BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),concaveShape, colObjWorldTransform); + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans, convexToTrans, &resultCallback, colObjWrap->getCollisionObject(), concaveShape, colObjWorldTransform); tccb.m_hitFraction = resultCallback.m_closestHitFraction; tccb.m_allowedPenetration = allowedPenetration; btVector3 boxMinLocal, boxMaxLocal; @@ -786,27 +801,56 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, rayAabbMaxLocal.setMax(convexToLocal); rayAabbMinLocal += boxMinLocal; rayAabbMaxLocal += boxMaxLocal; - concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); + concaveShape->processAllTriangles(&tccb, rayAabbMinLocal, rayAabbMaxLocal); } } - } else { - ///@todo : use AABB tree or other BVH acceleration structure! + } + else + { if (collisionShape->isCompound()) { - BT_PROFILE("convexSweepCompound"); - const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); - int i=0; - for (i=0;i<compoundShape->getNumChildShapes();i++) + struct btCompoundLeafCallback : btDbvt::ICollide { - btTransform childTrans = compoundShape->getChildTransform(i); - const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); - btTransform childWorldTrans = colObjWorldTransform * childTrans; - - struct LocalInfoAdder : public ConvexResultCallback { - ConvexResultCallback* m_userCallback; + btCompoundLeafCallback( + const btCollisionObjectWrapper* colObjWrap, + const btConvexShape* castShape, + const btTransform& convexFromTrans, + const btTransform& convexToTrans, + btScalar allowedPenetration, + const btCompoundShape* compoundShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback) + : m_colObjWrap(colObjWrap), + m_castShape(castShape), + m_convexFromTrans(convexFromTrans), + m_convexToTrans(convexToTrans), + m_allowedPenetration(allowedPenetration), + m_compoundShape(compoundShape), + m_colObjWorldTransform(colObjWorldTransform), + m_resultCallback(resultCallback) + { + } + + const btCollisionObjectWrapper* m_colObjWrap; + const btConvexShape* m_castShape; + const btTransform& m_convexFromTrans; + const btTransform& m_convexToTrans; + btScalar m_allowedPenetration; + const btCompoundShape* m_compoundShape; + const btTransform& m_colObjWorldTransform; + ConvexResultCallback& m_resultCallback; + + public: + void ProcessChild(int index, const btTransform& childTrans, const btCollisionShape* childCollisionShape) + { + btTransform childWorldTrans = m_colObjWorldTransform * childTrans; + + struct LocalInfoAdder : public ConvexResultCallback + { + ConvexResultCallback* m_userCallback; int m_i; - LocalInfoAdder (int i, ConvexResultCallback *user) + LocalInfoAdder(int i, ConvexResultCallback* user) : m_userCallback(user), m_i(i) { m_closestHitFraction = m_userCallback->m_closestHitFraction; @@ -815,60 +859,99 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, { return m_userCallback->needsCollision(p); } - virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = -1; - shapeInfo.m_triangleIndex = m_i; - if (r.m_localShapeInfo == NULL) - r.m_localShapeInfo = &shapeInfo; - const btScalar result = m_userCallback->addSingleResult(r, b); - m_closestHitFraction = m_userCallback->m_closestHitFraction; - return result; - - } - }; - - LocalInfoAdder my_cb(i, &resultCallback); - - btCollisionObjectWrapper tmpObj(colObjWrap,childCollisionShape,colObjWrap->getCollisionObject(),childWorldTrans,-1,i); - - objectQuerySingleInternal(castShape, convexFromTrans,convexToTrans, - &tmpObj,my_cb, allowedPenetration); - + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& r, bool b) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = -1; + shapeInfo.m_triangleIndex = m_i; + if (r.m_localShapeInfo == NULL) + r.m_localShapeInfo = &shapeInfo; + const btScalar result = m_userCallback->addSingleResult(r, b); + m_closestHitFraction = m_userCallback->m_closestHitFraction; + return result; + } + }; + + LocalInfoAdder my_cb(index, &m_resultCallback); + + btCollisionObjectWrapper tmpObj(m_colObjWrap, childCollisionShape, m_colObjWrap->getCollisionObject(), childWorldTrans, -1, index); + + objectQuerySingleInternal(m_castShape, m_convexFromTrans, m_convexToTrans, &tmpObj, my_cb, m_allowedPenetration); + } + + void Process(const btDbvtNode* leaf) + { + // Processing leaf node + int index = leaf->dataAsInt; + + btTransform childTrans = m_compoundShape->getChildTransform(index); + const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(index); + + ProcessChild(index, childTrans, childCollisionShape); + } + }; + + BT_PROFILE("convexSweepCompound"); + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); + + btVector3 fromLocalAabbMin, fromLocalAabbMax; + btVector3 toLocalAabbMin, toLocalAabbMax; + + castShape->getAabb(colObjWorldTransform.inverse() * convexFromTrans, fromLocalAabbMin, fromLocalAabbMax); + castShape->getAabb(colObjWorldTransform.inverse() * convexToTrans, toLocalAabbMin, toLocalAabbMax); + + fromLocalAabbMin.setMin(toLocalAabbMin); + fromLocalAabbMax.setMax(toLocalAabbMax); + + btCompoundLeafCallback callback(colObjWrap, castShape, convexFromTrans, convexToTrans, + allowedPenetration, compoundShape, colObjWorldTransform, resultCallback); + + const btDbvt* tree = compoundShape->getDynamicAabbTree(); + if (tree) + { + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(fromLocalAabbMin, fromLocalAabbMax); + tree->collideTV(tree->m_root, bounds, callback); + } + else + { + int i; + for (i = 0; i < compoundShape->getNumChildShapes(); i++) + { + const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); + btTransform childTrans = compoundShape->getChildTransform(i); + callback.ProcessChild(i, childTrans, childCollisionShape); + } } } } } } - struct btSingleRayCallback : public btBroadphaseRayCallback { - - btVector3 m_rayFromWorld; - btVector3 m_rayToWorld; - btTransform m_rayFromTrans; - btTransform m_rayToTrans; - btVector3 m_hitNormal; - - const btCollisionWorld* m_world; - btCollisionWorld::RayResultCallback& m_resultCallback; - - btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld), - m_world(world), - m_resultCallback(resultCallback) + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; + + const btCollisionWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; + + btSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btCollisionWorld* 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); + btVector3 rayDir = (rayToWorld - rayFromWorld); - rayDir.normalize (); + rayDir.normalize(); ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; @@ -877,22 +960,19 @@ struct btSingleRayCallback : public btBroadphaseRayCallback 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); - + m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); } - - - virtual bool process(const btBroadphaseProxy* proxy) + 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; + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; @@ -910,57 +990,53 @@ struct btSingleRayCallback : public btBroadphaseRayCallback //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); + m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback); } } return true; } }; -void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const +void btCollisionWorld::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 - btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); + btSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); #ifndef USE_BRUTEFORCE_RAYBROADPHASE - m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); + m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); #else - for (int i=0;i<this->getNumCollisionObjects();i++) + for (int i = 0; i < this->getNumCollisionObjects(); i++) { rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); - } -#endif //USE_BRUTEFORCE_RAYBROADPHASE - + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE } - struct btSingleSweepCallback : public btBroadphaseRayCallback { - - btTransform m_convexFromTrans; - btTransform m_convexToTrans; - btVector3 m_hitNormal; - const btCollisionWorld* m_world; - btCollisionWorld::ConvexResultCallback& m_resultCallback; - btScalar m_allowedCcdPenetration; + btTransform m_convexFromTrans; + btTransform m_convexToTrans; + btVector3 m_hitNormal; + const btCollisionWorld* m_world; + btCollisionWorld::ConvexResultCallback& m_resultCallback; + btScalar m_allowedCcdPenetration; const btConvexShape* m_castShape; - - btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration) - :m_convexFromTrans(convexFromTrans), - m_convexToTrans(convexToTrans), - m_world(world), - m_resultCallback(resultCallback), - m_allowedCcdPenetration(allowedPenetration), - m_castShape(castShape) + btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, const btCollisionWorld* world, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedPenetration) + : m_convexFromTrans(convexFromTrans), + m_convexToTrans(convexToTrans), + m_world(world), + m_resultCallback(resultCallback), + m_allowedCcdPenetration(allowedPenetration), + m_castShape(castShape) { - btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin()); + btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin() - m_convexFromTrans.getOrigin()); btVector3 rayDir = unnormalizedRayDir.normalized(); ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; @@ -971,109 +1047,102 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback m_signs[2] = m_rayDirectionInverse[2] < 0.0; m_lambda_max = rayDir.dot(unnormalizedRayDir); - } - virtual bool process(const btBroadphaseProxy* proxy) + virtual bool process(const btBroadphaseProxy* proxy) { ///terminate further convex sweep tests, once the closestHitFraction reached zero if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) return false; - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback, - m_allowedCcdPenetration); + m_world->objectQuerySingle(m_castShape, m_convexFromTrans, m_convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback, + m_allowedCcdPenetration); } return true; } }; - - -void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const +void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const { - BT_PROFILE("convexSweepTest"); /// 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 /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical - - - btTransform convexFromTrans,convexToTrans; + btTransform convexFromTrans, convexToTrans; convexFromTrans = convexFromWorld; convexToTrans = convexToWorld; btVector3 castShapeAabbMin, castShapeAabbMax; /* Compute AABB that encompasses angular movement */ { btVector3 linVel, angVel; - btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0f, linVel, angVel); + btTransformUtil::calculateVelocity(convexFromTrans, convexToTrans, 1.0f, linVel, angVel); btVector3 zeroLinVel; - zeroLinVel.setValue(0,0,0); + zeroLinVel.setValue(0, 0, 0); btTransform R; - R.setIdentity (); - R.setRotation (convexFromTrans.getRotation()); - castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax); + R.setIdentity(); + R.setRotation(convexFromTrans.getRotation()); + castShape->calculateTemporalAabb(R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax); } #ifndef USE_BRUTEFORCE_RAYBROADPHASE - btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration); + btSingleSweepCallback convexCB(castShape, convexFromWorld, convexToWorld, this, resultCallback, allowedCcdPenetration); - m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax); + m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(), convexToTrans.getOrigin(), convexCB, castShapeAabbMin, castShapeAabbMax); #else /// go over all objects, and if the ray intersects their aabb + cast shape aabb, // do a ray-shape query using convexCaster (CCD) int i; - for (i=0;i<m_collisionObjects.size();i++) + for (i = 0; i < m_collisionObjects.size(); i++) { - btCollisionObject* collisionObject= m_collisionObjects[i]; + btCollisionObject* collisionObject = m_collisionObjects[i]; //only perform raycast if filterMask matches - if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + if (resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); - btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 collisionObjectAabbMin, collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax); + AabbExpand(collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing btVector3 hitNormal; - if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + if (btRayAabb(convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) { - objectQuerySingle(castShape, convexFromTrans,convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback, - allowedCcdPenetration); + objectQuerySingle(castShape, convexFromTrans, convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + allowedCcdPenetration); } } } -#endif //USE_BRUTEFORCE_RAYBROADPHASE +#endif //USE_BRUTEFORCE_RAYBROADPHASE } - - struct btBridgedManifoldResult : public btManifoldResult { + btCollisionWorld::ContactResultCallback& m_resultCallback; - btCollisionWorld::ContactResultCallback& m_resultCallback; - - btBridgedManifoldResult( const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap,btCollisionWorld::ContactResultCallback& resultCallback ) - :btManifoldResult(obj0Wrap,obj1Wrap), - m_resultCallback(resultCallback) + btBridgedManifoldResult(const btCollisionObjectWrapper* obj0Wrap, const btCollisionObjectWrapper* obj1Wrap, btCollisionWorld::ContactResultCallback& resultCallback) + : btManifoldResult(obj0Wrap, obj1Wrap), + m_resultCallback(resultCallback) { } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); btVector3 pointA = pointInWorld + normalOnBInWorld * depth; @@ -1081,78 +1150,74 @@ struct btBridgedManifoldResult : public btManifoldResult btVector3 localB; if (isSwapped) { - localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA); localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } else + } + else { - localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA); localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); } - - btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + + btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth); newPt.m_positionWorldOnA = pointA; newPt.m_positionWorldOnB = pointInWorld; - - //BP mod, store contact triangles. + + //BP mod, store contact triangles. if (isSwapped) { newPt.m_partId0 = m_partId1; newPt.m_partId1 = m_partId0; - newPt.m_index0 = m_index1; - newPt.m_index1 = m_index0; - } else + newPt.m_index0 = m_index1; + newPt.m_index1 = m_index0; + } + else { newPt.m_partId0 = m_partId0; newPt.m_partId1 = m_partId1; - newPt.m_index0 = m_index0; - newPt.m_index1 = m_index1; + newPt.m_index0 = m_index0; + newPt.m_index1 = m_index1; } //experimental feature info, for per-triangle material etc. - const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap; - const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap; - m_resultCallback.addSingleResult(newPt,obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1); - + const btCollisionObjectWrapper* obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap; + m_resultCallback.addSingleResult(newPt, obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1); } - }; - - struct btSingleContactCallback : public btBroadphaseAabbCallback { - btCollisionObject* m_collisionObject; - btCollisionWorld* m_world; - btCollisionWorld::ContactResultCallback& m_resultCallback; - - - btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback) - :m_collisionObject(collisionObject), - m_world(world), - m_resultCallback(resultCallback) + btCollisionWorld* m_world; + btCollisionWorld::ContactResultCallback& m_resultCallback; + + btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world, btCollisionWorld::ContactResultCallback& resultCallback) + : m_collisionObject(collisionObject), + m_world(world), + m_resultCallback(resultCallback) { } - virtual bool process(const btBroadphaseProxy* proxy) + virtual bool process(const btBroadphaseProxy* proxy) { - btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; if (collisionObject == m_collisionObject) return true; //only perform raycast if filterMask matches - if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { - btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1); - btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1); + btCollisionObjectWrapper ob0(0, m_collisionObject->getCollisionShape(), m_collisionObject, m_collisionObject->getWorldTransform(), -1, -1); + btCollisionObjectWrapper ob1(0, collisionObject->getCollisionShape(), collisionObject, collisionObject->getWorldTransform(), -1, -1); - btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1); + btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0, &ob1, 0, BT_CLOSEST_POINT_ALGORITHMS); if (algorithm) { - btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback); + btBridgedManifoldResult contactPointResult(&ob0, &ob1, m_resultCallback); //discrete collision detection query - - algorithm->processCollision(&ob0,&ob1, m_world->getDispatchInfo(),&contactPointResult); + + algorithm->processCollision(&ob0, &ob1, m_world->getDispatchInfo(), &contactPointResult); algorithm->~btCollisionAlgorithm(); m_world->getDispatcher()->freeCollisionAlgorithm(algorithm); @@ -1162,294 +1227,271 @@ struct btSingleContactCallback : public btBroadphaseAabbCallback } }; - ///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback. ///it reports one or more contact points for every overlapping object (including the one with deepest penetration) -void btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback) +void btCollisionWorld::contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback) { - btVector3 aabbMin,aabbMax; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax); - btSingleContactCallback contactCB(colObj,this,resultCallback); - - m_broadphasePairCache->aabbTest(aabbMin,aabbMax,contactCB); -} + btVector3 aabbMin, aabbMax; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), aabbMin, aabbMax); + btSingleContactCallback contactCB(colObj, this, resultCallback); + m_broadphasePairCache->aabbTest(aabbMin, aabbMax, contactCB); +} ///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected. ///it reports one or more contact points (including the one with deepest penetration) -void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback) +void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback) { - btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1); - btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1); + btCollisionObjectWrapper obA(0, colObjA->getCollisionShape(), colObjA, colObjA->getWorldTransform(), -1, -1); + btCollisionObjectWrapper obB(0, colObjB->getCollisionShape(), colObjB, colObjB->getWorldTransform(), -1, -1); - btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB); + btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA, &obB, 0, BT_CLOSEST_POINT_ALGORITHMS); if (algorithm) { - btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback); + btBridgedManifoldResult contactPointResult(&obA, &obB, resultCallback); + contactPointResult.m_closestPointDistanceThreshold = resultCallback.m_closestDistanceThreshold; //discrete collision detection query - algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult); + algorithm->processCollision(&obA, &obB, getDispatchInfo(), &contactPointResult); algorithm->~btCollisionAlgorithm(); getDispatcher()->freeCollisionAlgorithm(algorithm); } - } - - - class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback { - btIDebugDraw* m_debugDrawer; - btVector3 m_color; - btTransform m_worldTrans; + btIDebugDraw* m_debugDrawer; + btVector3 m_color; + btTransform m_worldTrans; public: + DebugDrawcallback(btIDebugDraw* debugDrawer, const btTransform& worldTrans, const btVector3& color) : m_debugDrawer(debugDrawer), + m_color(color), + m_worldTrans(worldTrans) + { + } - DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) : - m_debugDrawer(debugDrawer), - m_color(color), - m_worldTrans(worldTrans) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - processTriangle(triangle,partId,triangleIndex); - } - - virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex) - { - (void)partId; - (void)triangleIndex; - - btVector3 wv0,wv1,wv2; - wv0 = m_worldTrans*triangle[0]; - wv1 = m_worldTrans*triangle[1]; - wv2 = m_worldTrans*triangle[2]; - btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.); - - if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals ) - { - btVector3 normal = (wv1-wv0).cross(wv2-wv0); - normal.normalize(); - btVector3 normalColor(1,1,0); - m_debugDrawer->drawLine(center,center+normal,normalColor); - } - m_debugDrawer->drawLine(wv0,wv1,m_color); - m_debugDrawer->drawLine(wv1,wv2,m_color); - m_debugDrawer->drawLine(wv2,wv0,m_color); - } -}; + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + processTriangle(triangle, partId, triangleIndex); + } + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + (void)partId; + (void)triangleIndex; + btVector3 wv0, wv1, wv2; + wv0 = m_worldTrans * triangle[0]; + wv1 = m_worldTrans * triangle[1]; + wv2 = m_worldTrans * triangle[2]; + btVector3 center = (wv0 + wv1 + wv2) * btScalar(1. / 3.); + + if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals) + { + btVector3 normal = (wv1 - wv0).cross(wv2 - wv0); + normal.normalize(); + btVector3 normalColor(1, 1, 0); + m_debugDrawer->drawLine(center, center + normal, normalColor); + } + m_debugDrawer->drawLine(wv0, wv1, m_color); + m_debugDrawer->drawLine(wv1, wv2, m_color); + m_debugDrawer->drawLine(wv2, wv0, m_color); + } +}; void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) { // Draw a small simplex at the center of the object if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames) { - getDebugDrawer()->drawTransform(worldTransform,1); + getDebugDrawer()->drawTransform(worldTransform, .1); } if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) { const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape); - for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) + for (int i = compoundShape->getNumChildShapes() - 1; i >= 0; i--) { btTransform childTrans = compoundShape->getChildTransform(i); const btCollisionShape* colShape = compoundShape->getChildShape(i); - debugDrawObject(worldTransform*childTrans,colShape,color); + debugDrawObject(worldTransform * childTrans, colShape, color); } - - } else + } + else { + switch (shape->getShapeType()) + { + case BOX_SHAPE_PROXYTYPE: + { + const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); + btVector3 halfExtents = boxShape->getHalfExtentsWithMargin(); + getDebugDrawer()->drawBox(-halfExtents, halfExtents, worldTransform, color); + break; + } - switch (shape->getShapeType()) - { - - case BOX_SHAPE_PROXYTYPE: - { - const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); - btVector3 halfExtents = boxShape->getHalfExtentsWithMargin(); - getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color); - break; - } - - case SPHERE_SHAPE_PROXYTYPE: - { - const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); - btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin - - getDebugDrawer()->drawSphere(radius, worldTransform, color); - break; - } - case MULTI_SPHERE_SHAPE_PROXYTYPE: - { - const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); - - btTransform childTransform; - childTransform.setIdentity(); - - for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) - { - childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); - getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color); - } - - break; - } - case CAPSULE_SHAPE_PROXYTYPE: - { - const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape); - - btScalar radius = capsuleShape->getRadius(); - btScalar halfHeight = capsuleShape->getHalfHeight(); - - int upAxis = capsuleShape->getUpAxis(); - getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color); - break; - } - case CONE_SHAPE_PROXYTYPE: - { - const btConeShape* coneShape = static_cast<const btConeShape*>(shape); - btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); - btScalar height = coneShape->getHeight();//+coneShape->getMargin(); - - int upAxis= coneShape->getConeUpIndex(); - getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color); - break; - - } - case CYLINDER_SHAPE_PROXYTYPE: - { - const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); - int upAxis = cylinder->getUpAxis(); - btScalar radius = cylinder->getRadius(); - btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; - getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color); - break; - } - - case STATIC_PLANE_PROXYTYPE: - { - const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); - btScalar planeConst = staticPlaneShape->getPlaneConstant(); - const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); - getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color); - break; - - } - default: - { - - /// for polyhedral shapes - if (shape->isPolyhedral()) - { - btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; - - int i; - if (polyshape->getConvexPolyhedron()) - { - const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron(); - for (i=0;i<poly->m_faces.size();i++) - { - btVector3 centroid(0,0,0); - int numVerts = poly->m_faces[i].m_indices.size(); - if (numVerts) - { - int lastV = poly->m_faces[i].m_indices[numVerts-1]; - for (int v=0;v<poly->m_faces[i].m_indices.size();v++) - { - int curVert = poly->m_faces[i].m_indices[v]; - centroid+=poly->m_vertices[curVert]; - getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color); - lastV = curVert; - } - } - centroid*= btScalar(1.f)/btScalar(numVerts); - if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals) - { - btVector3 normalColor(1,1,0); - btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]); - getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor); - } - - } - - - } else - { - for (i=0;i<polyshape->getNumEdges();i++) - { - btVector3 a,b; - polyshape->getEdge(i,a,b); - btVector3 wa = worldTransform * a; - btVector3 wb = worldTransform * b; - getDebugDrawer()->drawLine(wa,wb,color); - } - } - - - } - - if (shape->isConcave()) - { - btConcaveShape* concaveMesh = (btConcaveShape*) shape; - - ///@todo pass camera, for some culling? no -> we are not a graphics lib - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); - - } - - if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) - { - btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; - //todo: pass camera for some culling - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - //DebugDrawcallback drawCallback; - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); - } - - - - } - + case SPHERE_SHAPE_PROXYTYPE: + { + const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); + btScalar radius = sphereShape->getMargin(); //radius doesn't include the margin, so draw with margin + + getDebugDrawer()->drawSphere(radius, worldTransform, color); + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); + + btTransform childTransform; + childTransform.setIdentity(); + + for (int i = multiSphereShape->getSphereCount() - 1; i >= 0; i--) + { + childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); + getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform * childTransform, color); + } + + break; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape); + + btScalar radius = capsuleShape->getRadius(); + btScalar halfHeight = capsuleShape->getHalfHeight(); + + int upAxis = capsuleShape->getUpAxis(); + getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color); + break; + } + case CONE_SHAPE_PROXYTYPE: + { + const btConeShape* coneShape = static_cast<const btConeShape*>(shape); + btScalar radius = coneShape->getRadius(); //+coneShape->getMargin(); + btScalar height = coneShape->getHeight(); //+coneShape->getMargin(); + + int upAxis = coneShape->getConeUpIndex(); + getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color); + break; + } + case CYLINDER_SHAPE_PROXYTYPE: + { + const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); + int upAxis = cylinder->getUpAxis(); + btScalar radius = cylinder->getRadius(); + btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; + getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color); + break; + } + + case STATIC_PLANE_PROXYTYPE: + { + const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); + btScalar planeConst = staticPlaneShape->getPlaneConstant(); + const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); + getDebugDrawer()->drawPlane(planeNormal, planeConst, worldTransform, color); + break; + } + default: + { + /// for polyhedral shapes + if (shape->isPolyhedral()) + { + btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*)shape; + + int i; + if (polyshape->getConvexPolyhedron()) + { + const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron(); + for (i = 0; i < poly->m_faces.size(); i++) + { + btVector3 centroid(0, 0, 0); + int numVerts = poly->m_faces[i].m_indices.size(); + if (numVerts) + { + int lastV = poly->m_faces[i].m_indices[numVerts - 1]; + for (int v = 0; v < poly->m_faces[i].m_indices.size(); v++) + { + int curVert = poly->m_faces[i].m_indices[v]; + centroid += poly->m_vertices[curVert]; + getDebugDrawer()->drawLine(worldTransform * poly->m_vertices[lastV], worldTransform * poly->m_vertices[curVert], color); + lastV = curVert; + } + } + centroid *= btScalar(1.f) / btScalar(numVerts); + if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals) + { + btVector3 normalColor(1, 1, 0); + btVector3 faceNormal(poly->m_faces[i].m_plane[0], poly->m_faces[i].m_plane[1], poly->m_faces[i].m_plane[2]); + getDebugDrawer()->drawLine(worldTransform * centroid, worldTransform * (centroid + faceNormal), normalColor); + } + } + } + else + { + for (i = 0; i < polyshape->getNumEdges(); i++) + { + btVector3 a, b; + polyshape->getEdge(i, a, b); + btVector3 wa = worldTransform * a; + btVector3 wb = worldTransform * b; + getDebugDrawer()->drawLine(wa, wb, color); + } + } + } + + if (shape->isConcave()) + { + btConcaveShape* concaveMesh = (btConcaveShape*)shape; + + ///@todo pass camera, for some culling? no -> we are not a graphics lib + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + + DebugDrawcallback drawCallback(getDebugDrawer(), worldTransform, color); + concaveMesh->processAllTriangles(&drawCallback, aabbMin, aabbMax); + } + + if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + { + btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*)shape; + //todo: pass camera for some culling + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + //DebugDrawcallback drawCallback; + DebugDrawcallback drawCallback(getDebugDrawer(), worldTransform, color); + convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback, aabbMin, aabbMax); + } + } } } } - -void btCollisionWorld::debugDrawWorld() +void btCollisionWorld::debugDrawWorld() { if (getDebugDrawer()) { + getDebugDrawer()->clearLines(); + btIDebugDraw::DefaultColors defaultColors = getDebugDrawer()->getDefaultColors(); - if ( getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) + if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) { - - if (getDispatcher()) { int numManifolds = getDispatcher()->getNumManifolds(); - - for (int i=0;i<numManifolds;i++) + + for (int i = 0; i < numManifolds; i++) { btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i); //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0()); //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1()); int numContacts = contactManifold->getNumContacts(); - for (int j=0;j<numContacts;j++) + for (int j = 0; j < numContacts; j++) { btManifoldPoint& cp = contactManifold->getContactPoint(j); - getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),defaultColors.m_contactPoint); + getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB, cp.m_normalWorldOnB, cp.getDistance(), cp.getLifeTime(), defaultColors.m_contactPoint); } } } @@ -1459,56 +1501,63 @@ void btCollisionWorld::debugDrawWorld() { int i; - for ( i=0;i<m_collisionObjects.size();i++) + for (i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; - if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0) + if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT) == 0) { if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)) { - btVector3 color(btScalar(0.4),btScalar(0.4),btScalar(0.4)); + btVector3 color(btScalar(0.4), btScalar(0.4), btScalar(0.4)); - switch(colObj->getActivationState()) + switch (colObj->getActivationState()) { - case ACTIVE_TAG: - color = defaultColors.m_activeObject; break; - case ISLAND_SLEEPING: - color = defaultColors.m_deactivatedObject;break; - case WANTS_DEACTIVATION: - color = defaultColors.m_wantsDeactivationObject;break; - case DISABLE_DEACTIVATION: - color = defaultColors.m_disabledDeactivationObject;break; - case DISABLE_SIMULATION: - color = defaultColors.m_disabledSimulationObject;break; - default: + case ACTIVE_TAG: + color = defaultColors.m_activeObject; + break; + case ISLAND_SLEEPING: + color = defaultColors.m_deactivatedObject; + break; + case WANTS_DEACTIVATION: + color = defaultColors.m_wantsDeactivationObject; + break; + case DISABLE_DEACTIVATION: + color = defaultColors.m_disabledDeactivationObject; + break; + case DISABLE_SIMULATION: + color = defaultColors.m_disabledSimulationObject; + break; + default: { - color = btVector3(btScalar(.3),btScalar(0.3),btScalar(0.3)); + color = btVector3(btScalar(.3), btScalar(0.3), btScalar(0.3)); } }; - debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); + colObj->getCustomDebugColor(color); + + debugDrawObject(colObj->getWorldTransform(), colObj->getCollisionShape(), color); } if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { - btVector3 minAabb,maxAabb; + btVector3 minAabb, maxAabb; btVector3 colorvec = defaultColors.m_aabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb); + btVector3 contactThreshold(gContactBreakingThreshold, gContactBreakingThreshold, gContactBreakingThreshold); minAabb -= contactThreshold; maxAabb += contactThreshold; - btVector3 minAabb2,maxAabb2; + btVector3 minAabb2, maxAabb2; - if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) + if (getDispatchInfo().m_useContinuous && colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) { - colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); + colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(), minAabb2, maxAabb2); minAabb2 -= contactThreshold; maxAabb2 += contactThreshold; minAabb.setMin(minAabb2); maxAabb.setMax(maxAabb2); } - m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); + m_debugDrawer->drawAabb(minAabb, maxAabb, colorvec); } } } @@ -1516,45 +1565,63 @@ void btCollisionWorld::debugDrawWorld() } } - -void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) +void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) { int i; ///keep track of shapes already serialized - btHashMap<btHashPtr,btCollisionShape*> serializedShapes; + btHashMap<btHashPtr, btCollisionShape*> serializedShapes; - for (i=0;i<m_collisionObjects.size();i++) + for (i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btCollisionShape* shape = colObj->getCollisionShape(); if (!serializedShapes.find(shape)) { - serializedShapes.insert(shape,shape); + serializedShapes.insert(shape, shape); shape->serializeSingleShape(serializer); } } //serialize all collision objects - for (i=0;i<m_collisionObjects.size();i++) + for (i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; - if ((colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)) + if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) { colObj->serializeSingleObject(serializer); } } } - -void btCollisionWorld::serialize(btSerializer* serializer) +void btCollisionWorld::serializeContactManifolds(btSerializer* serializer) { + if (serializer->getSerializationFlags() & BT_SERIALIZE_CONTACT_MANIFOLDS) + { + int numManifolds = getDispatcher()->getNumManifolds(); + for (int i = 0; i < numManifolds; i++) + { + const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i]; + //don't serialize empty manifolds, they just take space + //(may have to do it anyway if it destroys determinism) + if (manifold->getNumContacts() == 0) + continue; + + btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1); + const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk, structType, BT_CONTACTMANIFOLD_CODE, (void*)manifold); + } + } +} +void btCollisionWorld::serialize(btSerializer* serializer) +{ serializer->startSerialization(); - + serializeCollisionObjects(serializer); - + + serializeContactManifolds(serializer); + serializer->finishSerialization(); } - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index ec40c969f51..fd0e5b9bbdd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -13,9 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - /** - * @page Bullet Documentation + * @mainpage Bullet Documentation * * @section intro_sec Introduction * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ). @@ -66,8 +65,6 @@ subject to the following restrictions: * For up-to-data information and copyright and contributors list check out the Bullet_User_Manual.pdf * */ - - #ifndef BT_COLLISION_WORLD_H #define BT_COLLISION_WORLD_H @@ -87,144 +84,138 @@ class btSerializer; ///CollisionWorld is interface and container for the collision detection class btCollisionWorld { - - protected: + btAlignedObjectArray<btCollisionObject*> m_collisionObjects; - btAlignedObjectArray<btCollisionObject*> m_collisionObjects; - - btDispatcher* m_dispatcher1; + btDispatcher* m_dispatcher1; - btDispatcherInfo m_dispatchInfo; + btDispatcherInfo m_dispatchInfo; - btBroadphaseInterface* m_broadphasePairCache; + btBroadphaseInterface* m_broadphasePairCache; - btIDebugDraw* m_debugDrawer; + btIDebugDraw* m_debugDrawer; ///m_forceUpdateAllAabbs can be set to false as an optimization to only update active object AABBs ///it is true by default, because it is error-prone (setting the position of static objects wouldn't update their AABB) bool m_forceUpdateAllAabbs; - void serializeCollisionObjects(btSerializer* serializer); + void serializeCollisionObjects(btSerializer* serializer); -public: + void serializeContactManifolds(btSerializer* serializer); +public: //this constructor doesn't own the dispatcher and paircache/broadphase - btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration); + btCollisionWorld(btDispatcher* dispatcher, btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration); virtual ~btCollisionWorld(); - void setBroadphase(btBroadphaseInterface* pairCache) + void setBroadphase(btBroadphaseInterface* pairCache) { m_broadphasePairCache = pairCache; } - const btBroadphaseInterface* getBroadphase() const + const btBroadphaseInterface* getBroadphase() const { return m_broadphasePairCache; } - btBroadphaseInterface* getBroadphase() + btBroadphaseInterface* getBroadphase() { return m_broadphasePairCache; } - btOverlappingPairCache* getPairCache() + btOverlappingPairCache* getPairCache() { return m_broadphasePairCache->getOverlappingPairCache(); } - - btDispatcher* getDispatcher() + btDispatcher* getDispatcher() { return m_dispatcher1; } - const btDispatcher* getDispatcher() const + const btDispatcher* getDispatcher() const { return m_dispatcher1; } - void updateSingleAabb(btCollisionObject* colObj); + void updateSingleAabb(btCollisionObject* colObj); - virtual void updateAabbs(); + virtual void updateAabbs(); ///the computeOverlappingPairs is usually already called by performDiscreteCollisionDetection (or stepSimulation) ///it can be useful to use if you perform ray tests without collision detection/simulation - virtual void computeOverlappingPairs(); + virtual void computeOverlappingPairs(); - - virtual void setDebugDrawer(btIDebugDraw* debugDrawer) + virtual void setDebugDrawer(btIDebugDraw* debugDrawer) { - m_debugDrawer = debugDrawer; + m_debugDrawer = debugDrawer; } - virtual btIDebugDraw* getDebugDrawer() + virtual btIDebugDraw* getDebugDrawer() { return m_debugDrawer; } - virtual void debugDrawWorld(); + virtual void debugDrawWorld(); virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); - ///LocalShapeInfo gives extra information for complex shapes ///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart - struct LocalShapeInfo + struct LocalShapeInfo { - int m_shapePart; - int m_triangleIndex; - + int m_shapePart; + int m_triangleIndex; + //const btCollisionShape* m_shapeTemp; //const btTransform* m_shapeLocalTransform; }; - struct LocalRayResult + struct LocalRayResult { - LocalRayResult(const btCollisionObject* collisionObject, - LocalShapeInfo* localShapeInfo, - const btVector3& hitNormalLocal, - btScalar hitFraction) - :m_collisionObject(collisionObject), - m_localShapeInfo(localShapeInfo), - m_hitNormalLocal(hitNormalLocal), - m_hitFraction(hitFraction) + LocalRayResult(const btCollisionObject* collisionObject, + LocalShapeInfo* localShapeInfo, + const btVector3& hitNormalLocal, + btScalar hitFraction) + : m_collisionObject(collisionObject), + m_localShapeInfo(localShapeInfo), + m_hitNormalLocal(hitNormalLocal), + m_hitFraction(hitFraction) { } - const btCollisionObject* m_collisionObject; - LocalShapeInfo* m_localShapeInfo; - btVector3 m_hitNormalLocal; - btScalar m_hitFraction; - + const btCollisionObject* m_collisionObject; + LocalShapeInfo* m_localShapeInfo; + btVector3 m_hitNormalLocal; + btScalar m_hitFraction; }; ///RayResultCallback is used to report new raycast results - struct RayResultCallback + struct RayResultCallback { - btScalar m_closestHitFraction; - const btCollisionObject* m_collisionObject; - short int m_collisionFilterGroup; - short int m_collisionFilterMask; + btScalar m_closestHitFraction; + const btCollisionObject* m_collisionObject; + int m_collisionFilterGroup; + int m_collisionFilterMask; //@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback.h. Apply any of the EFlags defined there on m_flags here to invoke. unsigned int m_flags; virtual ~RayResultCallback() { } - bool hasHit() const + bool hasHit() const { return (m_collisionObject != 0); } RayResultCallback() - :m_closestHitFraction(btScalar(1.)), - m_collisionObject(0), - m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter), - //@BP Mod - m_flags(0) + : m_closestHitFraction(btScalar(1.)), + m_collisionObject(0), + m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter), + //@BP Mod + m_flags(0) { } @@ -235,62 +226,62 @@ public: return collides; } - - virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) = 0; + virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) = 0; }; - struct ClosestRayResultCallback : public RayResultCallback + struct ClosestRayResultCallback : public RayResultCallback { - ClosestRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld) + ClosestRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld) + : m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld) { } - btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction - btVector3 m_rayToWorld; + btVector3 m_rayFromWorld; //used to calculate hitPointWorld from hitFraction + btVector3 m_rayToWorld; + + btVector3 m_hitNormalWorld; + btVector3 m_hitPointWorld; - btVector3 m_hitNormalWorld; - btVector3 m_hitPointWorld; - - virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) + virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) { //caller already does the filter on the m_closestHitFraction btAssert(rayResult.m_hitFraction <= m_closestHitFraction); - + m_closestHitFraction = rayResult.m_hitFraction; m_collisionObject = rayResult.m_collisionObject; if (normalInWorldSpace) { m_hitNormalWorld = rayResult.m_hitNormalLocal; - } else + } + else { ///need to transform normal into worldspace - m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; + m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal; } - m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction); + m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction); return rayResult.m_hitFraction; } }; - struct AllHitsRayResultCallback : public RayResultCallback + struct AllHitsRayResultCallback : public RayResultCallback { - AllHitsRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) - :m_rayFromWorld(rayFromWorld), - m_rayToWorld(rayToWorld) + AllHitsRayResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld) + : m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld) { } - btAlignedObjectArray<const btCollisionObject*> m_collisionObjects; + btAlignedObjectArray<const btCollisionObject*> m_collisionObjects; - btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction - btVector3 m_rayToWorld; + btVector3 m_rayFromWorld; //used to calculate hitPointWorld from hitFraction + btVector3 m_rayToWorld; - btAlignedObjectArray<btVector3> m_hitNormalWorld; - btAlignedObjectArray<btVector3> m_hitPointWorld; + btAlignedObjectArray<btVector3> m_hitNormalWorld; + btAlignedObjectArray<btVector3> m_hitPointWorld; btAlignedObjectArray<btScalar> m_hitFractions; - - virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) + + virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) { m_collisionObject = rayResult.m_collisionObject; m_collisionObjects.push_back(rayResult.m_collisionObject); @@ -298,69 +289,66 @@ public: if (normalInWorldSpace) { hitNormalWorld = rayResult.m_hitNormalLocal; - } else + } + else { ///need to transform normal into worldspace - hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; + hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal; } m_hitNormalWorld.push_back(hitNormalWorld); btVector3 hitPointWorld; - hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction); + hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction); m_hitPointWorld.push_back(hitPointWorld); m_hitFractions.push_back(rayResult.m_hitFraction); return m_closestHitFraction; } }; - struct LocalConvexResult { - LocalConvexResult(const btCollisionObject* hitCollisionObject, - LocalShapeInfo* localShapeInfo, - const btVector3& hitNormalLocal, - const btVector3& hitPointLocal, - btScalar hitFraction - ) - :m_hitCollisionObject(hitCollisionObject), - m_localShapeInfo(localShapeInfo), - m_hitNormalLocal(hitNormalLocal), - m_hitPointLocal(hitPointLocal), - m_hitFraction(hitFraction) + LocalConvexResult(const btCollisionObject* hitCollisionObject, + LocalShapeInfo* localShapeInfo, + const btVector3& hitNormalLocal, + const btVector3& hitPointLocal, + btScalar hitFraction) + : m_hitCollisionObject(hitCollisionObject), + m_localShapeInfo(localShapeInfo), + m_hitNormalLocal(hitNormalLocal), + m_hitPointLocal(hitPointLocal), + m_hitFraction(hitFraction) { } - const btCollisionObject* m_hitCollisionObject; - LocalShapeInfo* m_localShapeInfo; - btVector3 m_hitNormalLocal; - btVector3 m_hitPointLocal; - btScalar m_hitFraction; + const btCollisionObject* m_hitCollisionObject; + LocalShapeInfo* m_localShapeInfo; + btVector3 m_hitNormalLocal; + btVector3 m_hitPointLocal; + btScalar m_hitFraction; }; ///RayResultCallback is used to report new raycast results - struct ConvexResultCallback + struct ConvexResultCallback { - btScalar m_closestHitFraction; - short int m_collisionFilterGroup; - short int m_collisionFilterMask; - + btScalar m_closestHitFraction; + int m_collisionFilterGroup; + int m_collisionFilterMask; + ConvexResultCallback() - :m_closestHitFraction(btScalar(1.)), - m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter) + : m_closestHitFraction(btScalar(1.)), + m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter) { } virtual ~ConvexResultCallback() { } - - bool hasHit() const + + bool hasHit() const { return (m_closestHitFraction < btScalar(1.)); } - - virtual bool needsCollision(btBroadphaseProxy* proxy0) const { bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; @@ -368,39 +356,40 @@ public: return collides; } - virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) = 0; + virtual btScalar addSingleResult(LocalConvexResult& convexResult, bool normalInWorldSpace) = 0; }; - struct ClosestConvexResultCallback : public ConvexResultCallback + struct ClosestConvexResultCallback : public ConvexResultCallback { - ClosestConvexResultCallback(const btVector3& convexFromWorld,const btVector3& convexToWorld) - :m_convexFromWorld(convexFromWorld), - m_convexToWorld(convexToWorld), - m_hitCollisionObject(0) + ClosestConvexResultCallback(const btVector3& convexFromWorld, const btVector3& convexToWorld) + : m_convexFromWorld(convexFromWorld), + m_convexToWorld(convexToWorld), + m_hitCollisionObject(0) { } - btVector3 m_convexFromWorld;//used to calculate hitPointWorld from hitFraction - btVector3 m_convexToWorld; + btVector3 m_convexFromWorld; //used to calculate hitPointWorld from hitFraction + btVector3 m_convexToWorld; - btVector3 m_hitNormalWorld; - btVector3 m_hitPointWorld; - const btCollisionObject* m_hitCollisionObject; - - virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) + btVector3 m_hitNormalWorld; + btVector3 m_hitPointWorld; + const btCollisionObject* m_hitCollisionObject; + + virtual btScalar addSingleResult(LocalConvexResult& convexResult, bool normalInWorldSpace) { -//caller already does the filter on the m_closestHitFraction + //caller already does the filter on the m_closestHitFraction btAssert(convexResult.m_hitFraction <= m_closestHitFraction); - + m_closestHitFraction = convexResult.m_hitFraction; m_hitCollisionObject = convexResult.m_hitCollisionObject; if (normalInWorldSpace) { m_hitNormalWorld = convexResult.m_hitNormalLocal; - } else + } + else { ///need to transform normal into worldspace - m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal; + m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis() * convexResult.m_hitNormalLocal; } m_hitPointWorld = convexResult.m_hitPointLocal; return convexResult.m_hitFraction; @@ -408,21 +397,23 @@ public: }; ///ContactResultCallback is used to report contact points - struct ContactResultCallback + struct ContactResultCallback { - short int m_collisionFilterGroup; - short int m_collisionFilterMask; - + int m_collisionFilterGroup; + int m_collisionFilterMask; + btScalar m_closestDistanceThreshold; + ContactResultCallback() - :m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter) + : m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter), + m_closestDistanceThreshold(0) { } virtual ~ContactResultCallback() { } - + virtual bool needsCollision(btBroadphaseProxy* proxy0) const { bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; @@ -430,58 +421,57 @@ public: return collides; } - virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) = 0; + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) = 0; }; - - - int getNumCollisionObjects() const + int getNumCollisionObjects() const { return int(m_collisionObjects.size()); } /// 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. - virtual 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. - void convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = btScalar(0.)) const; + void convexSweepTest(const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = btScalar(0.)) const; ///contactTest performs a discrete collision test between colObj against all objects in the btCollisionWorld, and calls the resultCallback. ///it reports one or more contact points for every overlapping object (including the one with deepest penetration) - void contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback); + void contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback); ///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected. ///it reports one or more contact points (including the one with deepest penetration) - void contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback); - + void contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback); /// 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); + static void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback); - static void rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans, - const btCollisionObjectWrapper* collisionObjectWrap, - RayResultCallback& resultCallback); + static void rayTestSingleInternal(const btTransform& rayFromTrans, const btTransform& rayToTrans, + const btCollisionObjectWrapper* collisionObjectWrap, + RayResultCallback& resultCallback); /// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest. - static void objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - ConvexResultCallback& resultCallback, btScalar allowedPenetration); + static void objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback, btScalar allowedPenetration); + + static void objectQuerySingleInternal(const btConvexShape* castShape, const btTransform& convexFromTrans, const btTransform& convexToTrans, + const btCollisionObjectWrapper* colObjWrap, + ConvexResultCallback& resultCallback, btScalar allowedPenetration); - static void objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, - const btCollisionObjectWrapper* colObjWrap, - ConvexResultCallback& resultCallback, btScalar allowedPenetration); + virtual void addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); - virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); + virtual void refreshBroadphaseProxy(btCollisionObject* collisionObject); btCollisionObjectArray& getCollisionObjectArray() { @@ -493,10 +483,9 @@ public: return m_collisionObjects; } + virtual void removeCollisionObject(btCollisionObject* collisionObject); - virtual void removeCollisionObject(btCollisionObject* collisionObject); - - virtual void performDiscreteCollisionDetection(); + virtual void performDiscreteCollisionDetection(); btDispatcherInfo& getDispatchInfo() { @@ -507,20 +496,18 @@ public: { return m_dispatchInfo; } - - bool getForceUpdateAllAabbs() const + + bool getForceUpdateAllAabbs() const { return m_forceUpdateAllAabbs; } - void setForceUpdateAllAabbs( bool forceUpdateAllAabbs) + void setForceUpdateAllAabbs(bool forceUpdateAllAabbs) { m_forceUpdateAllAabbs = forceUpdateAllAabbs; } ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (Bullet/Demos/SerializeDemo) - virtual void serialize(btSerializer* serializer); - + virtual void serialize(btSerializer* serializer); }; - -#endif //BT_COLLISION_WORLD_H +#endif //BT_COLLISION_WORLD_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp index 57eb81703e7..e56e73dcf5b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp @@ -15,269 +15,251 @@ subject to the following restrictions: #include "btCollisionWorldImporter.h" #include "btBulletCollisionCommon.h" -#include "LinearMath/btSerializer.h" //for btBulletSerializedArrays definition +#include "LinearMath/btSerializer.h" //for btBulletSerializedArrays definition #ifdef SUPPORT_GIMPACT_SHAPE_IMPORT #include "BulletCollision/Gimpact/btGImpactShape.h" -#endif //SUPPORT_GIMPACT_SHAPE_IMPORT +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT btCollisionWorldImporter::btCollisionWorldImporter(btCollisionWorld* world) -:m_collisionWorld(world), -m_verboseMode(0) + : m_collisionWorld(world), + m_verboseMode(0) { - } btCollisionWorldImporter::~btCollisionWorldImporter() { } - - - - -bool btCollisionWorldImporter::convertAllObjects( btBulletSerializedArrays* arrays) +bool btCollisionWorldImporter::convertAllObjects(btBulletSerializedArrays* arrays) { - m_shapeMap.clear(); m_bodyMap.clear(); int i; - for (i=0;i<arrays->m_bvhsDouble.size();i++) + for (i = 0; i < arrays->m_bvhsDouble.size(); i++) { btOptimizedBvh* bvh = createOptimizedBvh(); btQuantizedBvhDoubleData* bvhData = arrays->m_bvhsDouble[i]; bvh->deSerializeDouble(*bvhData); - m_bvhMap.insert(arrays->m_bvhsDouble[i],bvh); + m_bvhMap.insert(arrays->m_bvhsDouble[i], bvh); } - for (i=0;i<arrays->m_bvhsFloat.size();i++) - { - btOptimizedBvh* bvh = createOptimizedBvh(); - btQuantizedBvhFloatData* bvhData = arrays->m_bvhsFloat[i]; + for (i = 0; i < arrays->m_bvhsFloat.size(); i++) + { + btOptimizedBvh* bvh = createOptimizedBvh(); + btQuantizedBvhFloatData* bvhData = arrays->m_bvhsFloat[i]; bvh->deSerializeFloat(*bvhData); - m_bvhMap.insert(arrays->m_bvhsFloat[i],bvh); + m_bvhMap.insert(arrays->m_bvhsFloat[i], bvh); } - - - - - for (i=0;i<arrays->m_colShapeData.size();i++) + for (i = 0; i < arrays->m_colShapeData.size(); i++) { btCollisionShapeData* shapeData = arrays->m_colShapeData[i]; btCollisionShape* shape = convertCollisionShape(shapeData); if (shape) { - // printf("shapeMap.insert(%x,%x)\n",shapeData,shape); - m_shapeMap.insert(shapeData,shape); + // printf("shapeMap.insert(%x,%x)\n",shapeData,shape); + m_shapeMap.insert(shapeData, shape); } - if (shape&& shapeData->m_name) + if (shape && shapeData->m_name) { char* newname = duplicateName(shapeData->m_name); - m_objectNameMap.insert(shape,newname); - m_nameShapeMap.insert(newname,shape); + m_objectNameMap.insert(shape, newname); + m_nameShapeMap.insert(newname, shape); } } - - for (i=0;i<arrays->m_collisionObjectDataDouble.size();i++) + for (i = 0; i < arrays->m_collisionObjectDataDouble.size(); i++) { - btCollisionObjectDoubleData* colObjData = arrays->m_collisionObjectDataDouble[i]; - btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); - if (shapePtr && *shapePtr) - { - btTransform startTransform; - colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; - startTransform.deSerializeDouble(colObjData->m_worldTransform); - - btCollisionShape* shape = (btCollisionShape*)*shapePtr; - btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); - body->setFriction(btScalar(colObjData->m_friction)); - body->setRestitution(btScalar(colObjData->m_restitution)); + btCollisionObjectDoubleData* colObjData = arrays->m_collisionObjectDataDouble[i]; + btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); + if (shapePtr && *shapePtr) + { + btTransform startTransform; + colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; + startTransform.deSerializeDouble(colObjData->m_worldTransform); + + btCollisionShape* shape = (btCollisionShape*)*shapePtr; + btCollisionObject* body = createCollisionObject(startTransform, shape, colObjData->m_name); + body->setFriction(btScalar(colObjData->m_friction)); + body->setRestitution(btScalar(colObjData->m_restitution)); #ifdef USE_INTERNAL_EDGE_UTILITY - if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) - { - btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; - if (trimesh->getTriangleInfoMap()) - { - body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); - } - } -#endif //USE_INTERNAL_EDGE_UTILITY - m_bodyMap.insert(colObjData,body); - } else - { - printf("error: no shape found\n"); - } + if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; + if (trimesh->getTriangleInfoMap()) + { + body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + } + } +#endif //USE_INTERNAL_EDGE_UTILITY + m_bodyMap.insert(colObjData, body); + } + else + { + printf("error: no shape found\n"); + } } - for (i=0;i<arrays->m_collisionObjectDataFloat.size();i++) + for (i = 0; i < arrays->m_collisionObjectDataFloat.size(); i++) { - btCollisionObjectFloatData* colObjData = arrays->m_collisionObjectDataFloat[i]; - btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); - if (shapePtr && *shapePtr) - { - btTransform startTransform; - colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; - startTransform.deSerializeFloat(colObjData->m_worldTransform); + btCollisionObjectFloatData* colObjData = arrays->m_collisionObjectDataFloat[i]; + btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); + if (shapePtr && *shapePtr) + { + btTransform startTransform; + colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; + startTransform.deSerializeFloat(colObjData->m_worldTransform); - btCollisionShape* shape = (btCollisionShape*)*shapePtr; - btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); + btCollisionShape* shape = (btCollisionShape*)*shapePtr; + btCollisionObject* body = createCollisionObject(startTransform, shape, colObjData->m_name); #ifdef USE_INTERNAL_EDGE_UTILITY - if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) - { - btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; - if (trimesh->getTriangleInfoMap()) - { - body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); - } - } -#endif //USE_INTERNAL_EDGE_UTILITY - m_bodyMap.insert(colObjData,body); - } else - { - printf("error: no shape found\n"); - } - } + if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; + if (trimesh->getTriangleInfoMap()) + { + body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + } + } +#endif //USE_INTERNAL_EDGE_UTILITY + m_bodyMap.insert(colObjData, body); + } + else + { + printf("error: no shape found\n"); + } + } return true; } - - void btCollisionWorldImporter::deleteAllData() { int i; - for (i=0;i<m_allocatedCollisionObjects.size();i++) + for (i = 0; i < m_allocatedCollisionObjects.size(); i++) { - if(m_collisionWorld) + if (m_collisionWorld) m_collisionWorld->removeCollisionObject(m_allocatedCollisionObjects[i]); delete m_allocatedCollisionObjects[i]; } m_allocatedCollisionObjects.clear(); - - for (i=0;i<m_allocatedCollisionShapes.size();i++) + for (i = 0; i < m_allocatedCollisionShapes.size(); i++) { delete m_allocatedCollisionShapes[i]; } m_allocatedCollisionShapes.clear(); - - for (i=0;i<m_allocatedBvhs.size();i++) + for (i = 0; i < m_allocatedBvhs.size(); i++) { delete m_allocatedBvhs[i]; } m_allocatedBvhs.clear(); - for (i=0;i<m_allocatedTriangleInfoMaps.size();i++) + for (i = 0; i < m_allocatedTriangleInfoMaps.size(); i++) { delete m_allocatedTriangleInfoMaps[i]; } m_allocatedTriangleInfoMaps.clear(); - for (i=0;i<m_allocatedTriangleIndexArrays.size();i++) + for (i = 0; i < m_allocatedTriangleIndexArrays.size(); i++) { delete m_allocatedTriangleIndexArrays[i]; } m_allocatedTriangleIndexArrays.clear(); - for (i=0;i<m_allocatedNames.size();i++) + for (i = 0; i < m_allocatedNames.size(); i++) { delete[] m_allocatedNames[i]; } m_allocatedNames.clear(); - for (i=0;i<m_allocatedbtStridingMeshInterfaceDatas.size();i++) + for (i = 0; i < m_allocatedbtStridingMeshInterfaceDatas.size(); i++) { btStridingMeshInterfaceData* curData = m_allocatedbtStridingMeshInterfaceDatas[i]; - for(int a = 0;a < curData->m_numMeshParts;a++) + for (int a = 0; a < curData->m_numMeshParts; a++) { btMeshPartData* curPart = &curData->m_meshPartsPtr[a]; - if(curPart->m_vertices3f) - delete [] curPart->m_vertices3f; + if (curPart->m_vertices3f) + delete[] curPart->m_vertices3f; - if(curPart->m_vertices3d) - delete [] curPart->m_vertices3d; + if (curPart->m_vertices3d) + delete[] curPart->m_vertices3d; - if(curPart->m_indices32) - delete [] curPart->m_indices32; + if (curPart->m_indices32) + delete[] curPart->m_indices32; - if(curPart->m_3indices16) - delete [] curPart->m_3indices16; + if (curPart->m_3indices16) + delete[] curPart->m_3indices16; - if(curPart->m_indices16) - delete [] curPart->m_indices16; + if (curPart->m_indices16) + delete[] curPart->m_indices16; if (curPart->m_3indices8) - delete [] curPart->m_3indices8; - + delete[] curPart->m_3indices8; } - delete [] curData->m_meshPartsPtr; + delete[] curData->m_meshPartsPtr; delete curData; } m_allocatedbtStridingMeshInterfaceDatas.clear(); - for (i=0;i<m_indexArrays.size();i++) + for (i = 0; i < m_indexArrays.size(); i++) { btAlignedFree(m_indexArrays[i]); } - m_indexArrays.clear(); + m_indexArrays.clear(); - for (i=0;i<m_shortIndexArrays.size();i++) + for (i = 0; i < m_shortIndexArrays.size(); i++) { btAlignedFree(m_shortIndexArrays[i]); } - m_shortIndexArrays.clear(); + m_shortIndexArrays.clear(); - for (i=0;i<m_charIndexArrays.size();i++) + for (i = 0; i < m_charIndexArrays.size(); i++) { btAlignedFree(m_charIndexArrays[i]); } - m_charIndexArrays.clear(); + m_charIndexArrays.clear(); - for (i=0;i<m_floatVertexArrays.size();i++) + for (i = 0; i < m_floatVertexArrays.size(); i++) { btAlignedFree(m_floatVertexArrays[i]); } - m_floatVertexArrays.clear(); + m_floatVertexArrays.clear(); - for (i=0;i<m_doubleVertexArrays.size();i++) + for (i = 0; i < m_doubleVertexArrays.size(); i++) { btAlignedFree(m_doubleVertexArrays[i]); } - m_doubleVertexArrays.clear(); - - + m_doubleVertexArrays.clear(); } - - -btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionShapeData* shapeData ) +btCollisionShape* btCollisionWorldImporter::convertCollisionShape(btCollisionShapeData* shapeData) { btCollisionShape* shape = 0; switch (shapeData->m_shapeType) - { - case STATIC_PLANE_PROXYTYPE: + { + case STATIC_PLANE_PROXYTYPE: { btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)shapeData; - btVector3 planeNormal,localScaling; + btVector3 planeNormal, localScaling; planeNormal.deSerializeFloat(planeData->m_planeNormal); localScaling.deSerializeFloat(planeData->m_localScaling); - shape = createPlaneShape(planeNormal,planeData->m_planeConstant); + shape = createPlaneShape(planeNormal, planeData->m_planeConstant); shape->setLocalScaling(localScaling); break; } - case SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE: + case SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE: { - btScaledTriangleMeshShapeData* scaledMesh = (btScaledTriangleMeshShapeData*) shapeData; - btCollisionShapeData* colShapeData = (btCollisionShapeData*) &scaledMesh->m_trimeshShapeData; + btScaledTriangleMeshShapeData* scaledMesh = (btScaledTriangleMeshShapeData*)shapeData; + btCollisionShapeData* colShapeData = (btCollisionShapeData*)&scaledMesh->m_trimeshShapeData; colShapeData->m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; btCollisionShape* childShape = convertCollisionShape(colShapeData); btBvhTriangleMeshShape* meshShape = (btBvhTriangleMeshShape*)childShape; @@ -288,15 +270,14 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS break; } #ifdef SUPPORT_GIMPACT_SHAPE_IMPORT - case GIMPACT_SHAPE_PROXYTYPE: + case GIMPACT_SHAPE_PROXYTYPE: { - btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*) shapeData; + btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*)shapeData; if (gimpactData->m_gimpactSubType == CONST_GIMPACT_TRIMESH_SHAPE) { btStridingMeshInterfaceData* interfaceData = createStridingMeshInterfaceData(&gimpactData->m_meshInterface); btTriangleIndexVertexArray* meshInterface = createMeshInterface(*interfaceData); - btGImpactMeshShape* gimpactShape = createGimpactShape(meshInterface); btVector3 localScaling; localScaling.deSerializeFloat(gimpactData->m_localScaling); @@ -304,47 +285,45 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS gimpactShape->setMargin(btScalar(gimpactData->m_collisionMargin)); gimpactShape->updateBound(); shape = gimpactShape; - } else + } + else { printf("unsupported gimpact sub type\n"); } break; } -#endif //SUPPORT_GIMPACT_SHAPE_IMPORT - //The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API - //so deal with this +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT + //The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API + //so deal with this case CAPSULE_SHAPE_PROXYTYPE: { btCapsuleShapeData* capData = (btCapsuleShapeData*)shapeData; - switch (capData->m_upAxis) { - case 0: + case 0: { - shape = createCapsuleShapeX(1,1); + shape = createCapsuleShapeX(1, 1); break; } - case 1: + case 1: { - shape = createCapsuleShapeY(1,1); + shape = createCapsuleShapeY(1, 1); break; } - case 2: + case 2: { - shape = createCapsuleShapeZ(1,1); + shape = createCapsuleShapeZ(1, 1); break; } - default: + default: { printf("error: wrong up axis for btCapsuleShape\n"); } - - }; if (shape) { - btCapsuleShape* cap = (btCapsuleShape*) shape; + btCapsuleShape* cap = (btCapsuleShape*)shape; cap->deSerializeFloat(capData); } break; @@ -355,163 +334,156 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS case SPHERE_SHAPE_PROXYTYPE: case MULTI_SPHERE_SHAPE_PROXYTYPE: case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData; + btVector3 implicitShapeDimensions; + implicitShapeDimensions.deSerializeFloat(bsd->m_implicitShapeDimensions); + btVector3 localScaling; + localScaling.deSerializeFloat(bsd->m_localScaling); + btVector3 margin(bsd->m_collisionMargin, bsd->m_collisionMargin, bsd->m_collisionMargin); + switch (shapeData->m_shapeType) { - btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData; - btVector3 implicitShapeDimensions; - implicitShapeDimensions.deSerializeFloat(bsd->m_implicitShapeDimensions); - btVector3 localScaling; - localScaling.deSerializeFloat(bsd->m_localScaling); - btVector3 margin(bsd->m_collisionMargin,bsd->m_collisionMargin,bsd->m_collisionMargin); - switch (shapeData->m_shapeType) + case BOX_SHAPE_PROXYTYPE: { - case BOX_SHAPE_PROXYTYPE: - { - btBoxShape* box= (btBoxShape*)createBoxShape(implicitShapeDimensions/localScaling+margin); - //box->initializePolyhedralFeatures(); - shape = box; + btBoxShape* box = (btBoxShape*)createBoxShape(implicitShapeDimensions / localScaling + margin); + //box->initializePolyhedralFeatures(); + shape = box; + break; + } + case SPHERE_SHAPE_PROXYTYPE: + { + shape = createSphereShape(implicitShapeDimensions.getX()); + break; + } + + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShapeData* cylData = (btCylinderShapeData*)shapeData; + btVector3 halfExtents = implicitShapeDimensions + margin; + switch (cylData->m_upAxis) + { + case 0: + { + shape = createCylinderShapeX(halfExtents.getY(), halfExtents.getX()); break; } - case SPHERE_SHAPE_PROXYTYPE: + case 1: { - shape = createSphereShape(implicitShapeDimensions.getX()); + shape = createCylinderShapeY(halfExtents.getX(), halfExtents.getY()); break; } - - case CYLINDER_SHAPE_PROXYTYPE: + case 2: { - btCylinderShapeData* cylData = (btCylinderShapeData*) shapeData; - btVector3 halfExtents = implicitShapeDimensions+margin; - switch (cylData->m_upAxis) - { - case 0: - { - shape = createCylinderShapeX(halfExtents.getY(),halfExtents.getX()); - break; - } - case 1: - { - shape = createCylinderShapeY(halfExtents.getX(),halfExtents.getY()); - break; - } - case 2: - { - shape = createCylinderShapeZ(halfExtents.getX(),halfExtents.getZ()); - break; - } - default: - { - printf("unknown Cylinder up axis\n"); - } - - }; - - - + shape = createCylinderShapeZ(halfExtents.getX(), halfExtents.getZ()); break; } - case CONE_SHAPE_PROXYTYPE: + default: { - btConeShapeData* conData = (btConeShapeData*) shapeData; - btVector3 halfExtents = implicitShapeDimensions;//+margin; - switch (conData->m_upIndex) - { - case 0: - { - shape = createConeShapeX(halfExtents.getY(),halfExtents.getX()); - break; - } - case 1: - { - shape = createConeShapeY(halfExtents.getX(),halfExtents.getY()); - break; - } - case 2: - { - shape = createConeShapeZ(halfExtents.getX(),halfExtents.getZ()); - break; - } - default: - { - printf("unknown Cone up axis\n"); - } - - }; - - + printf("unknown Cylinder up axis\n"); + } + }; + break; + } + case CONE_SHAPE_PROXYTYPE: + { + btConeShapeData* conData = (btConeShapeData*)shapeData; + btVector3 halfExtents = implicitShapeDimensions; //+margin; + switch (conData->m_upIndex) + { + case 0: + { + shape = createConeShapeX(halfExtents.getY(), halfExtents.getX()); break; } - case MULTI_SPHERE_SHAPE_PROXYTYPE: + case 1: { - btMultiSphereShapeData* mss = (btMultiSphereShapeData*)bsd; - int numSpheres = mss->m_localPositionArraySize; - - btAlignedObjectArray<btVector3> tmpPos; - btAlignedObjectArray<btScalar> radii; - radii.resize(numSpheres); - tmpPos.resize(numSpheres); - int i; - for ( i=0;i<numSpheres;i++) - { - tmpPos[i].deSerializeFloat(mss->m_localPositionArrayPtr[i].m_pos); - radii[i] = mss->m_localPositionArrayPtr[i].m_radius; - } - shape = createMultiSphereShape(&tmpPos[0],&radii[0],numSpheres); + shape = createConeShapeY(halfExtents.getX(), halfExtents.getY()); break; } - case CONVEX_HULL_SHAPE_PROXYTYPE: + case 2: { - // int sz = sizeof(btConvexHullShapeData); - // int sz2 = sizeof(btConvexInternalShapeData); - // int sz3 = sizeof(btCollisionShapeData); - btConvexHullShapeData* convexData = (btConvexHullShapeData*)bsd; - int numPoints = convexData->m_numUnscaledPoints; - - btAlignedObjectArray<btVector3> tmpPoints; - tmpPoints.resize(numPoints); - int i; - for ( i=0;i<numPoints;i++) - { -#ifdef BT_USE_DOUBLE_PRECISION - if (convexData->m_unscaledPointsDoublePtr) - tmpPoints[i].deSerialize(convexData->m_unscaledPointsDoublePtr[i]); - if (convexData->m_unscaledPointsFloatPtr) - tmpPoints[i].deSerializeFloat(convexData->m_unscaledPointsFloatPtr[i]); -#else - if (convexData->m_unscaledPointsFloatPtr) - tmpPoints[i].deSerialize(convexData->m_unscaledPointsFloatPtr[i]); - if (convexData->m_unscaledPointsDoublePtr) - tmpPoints[i].deSerializeDouble(convexData->m_unscaledPointsDoublePtr[i]); -#endif //BT_USE_DOUBLE_PRECISION - } - btConvexHullShape* hullShape = createConvexHullShape(); - for (i=0;i<numPoints;i++) - { - hullShape->addPoint(tmpPoints[i]); - } - hullShape->setMargin(bsd->m_collisionMargin); - //hullShape->initializePolyhedralFeatures(); - shape = hullShape; + shape = createConeShapeZ(halfExtents.getX(), halfExtents.getZ()); break; } - default: + default: { - printf("error: cannot create shape type (%d)\n",shapeData->m_shapeType); + printf("unknown Cone up axis\n"); } - } + }; - if (shape) + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + btMultiSphereShapeData* mss = (btMultiSphereShapeData*)bsd; + int numSpheres = mss->m_localPositionArraySize; + + btAlignedObjectArray<btVector3> tmpPos; + btAlignedObjectArray<btScalar> radii; + radii.resize(numSpheres); + tmpPos.resize(numSpheres); + int i; + for (i = 0; i < numSpheres; i++) + { + tmpPos[i].deSerializeFloat(mss->m_localPositionArrayPtr[i].m_pos); + radii[i] = mss->m_localPositionArrayPtr[i].m_radius; + } + shape = createMultiSphereShape(&tmpPos[0], &radii[0], numSpheres); + break; + } + case CONVEX_HULL_SHAPE_PROXYTYPE: { - shape->setMargin(bsd->m_collisionMargin); + // int sz = sizeof(btConvexHullShapeData); + // int sz2 = sizeof(btConvexInternalShapeData); + // int sz3 = sizeof(btCollisionShapeData); + btConvexHullShapeData* convexData = (btConvexHullShapeData*)bsd; + int numPoints = convexData->m_numUnscaledPoints; + + btAlignedObjectArray<btVector3> tmpPoints; + tmpPoints.resize(numPoints); + int i; + for (i = 0; i < numPoints; i++) + { +#ifdef BT_USE_DOUBLE_PRECISION + if (convexData->m_unscaledPointsDoublePtr) + tmpPoints[i].deSerialize(convexData->m_unscaledPointsDoublePtr[i]); + if (convexData->m_unscaledPointsFloatPtr) + tmpPoints[i].deSerializeFloat(convexData->m_unscaledPointsFloatPtr[i]); +#else + if (convexData->m_unscaledPointsFloatPtr) + tmpPoints[i].deSerialize(convexData->m_unscaledPointsFloatPtr[i]); + if (convexData->m_unscaledPointsDoublePtr) + tmpPoints[i].deSerializeDouble(convexData->m_unscaledPointsDoublePtr[i]); +#endif //BT_USE_DOUBLE_PRECISION + } + btConvexHullShape* hullShape = createConvexHullShape(); + for (i = 0; i < numPoints; i++) + { + hullShape->addPoint(tmpPoints[i]); + } + hullShape->setMargin(bsd->m_collisionMargin); + //hullShape->initializePolyhedralFeatures(); + shape = hullShape; + break; + } + default: + { + printf("error: cannot create shape type (%d)\n", shapeData->m_shapeType); + } + } - btVector3 localScaling; - localScaling.deSerializeFloat(bsd->m_localScaling); - shape->setLocalScaling(localScaling); + if (shape) + { + shape->setMargin(bsd->m_collisionMargin); - } - break; + btVector3 localScaling; + localScaling.deSerializeFloat(bsd->m_localScaling); + shape->setLocalScaling(localScaling); } + break; + } case TRIANGLE_MESH_SHAPE_PROXYTYPE: { btTriangleMeshShapeData* trimesh = (btTriangleMeshShapeData*)shapeData; @@ -522,10 +494,10 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS return 0; } - btVector3 scaling; scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling); + btVector3 scaling; + scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling); meshInterface->setScaling(scaling); - btOptimizedBvh* bvh = 0; #if 1 if (trimesh->m_quantizedFloatBvh) @@ -534,7 +506,8 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS if (bvhPtr && *bvhPtr) { bvh = *bvhPtr; - } else + } + else { bvh = createOptimizedBvh(); bvh->deSerializeFloat(*trimesh->m_quantizedFloatBvh); @@ -546,7 +519,8 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS if (bvhPtr && *bvhPtr) { bvh = *bvhPtr; - } else + } + else { bvh = createOptimizedBvh(); bvh->deSerializeDouble(*trimesh->m_quantizedDoubleBvh); @@ -554,8 +528,7 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS } #endif - - btBvhTriangleMeshShape* trimeshShape = createBvhTriangleMeshShape(meshInterface,bvh); + btBvhTriangleMeshShape* trimeshShape = createBvhTriangleMeshShape(meshInterface, bvh); trimeshShape->setMargin(trimesh->m_collisionMargin); shape = trimeshShape; @@ -567,67 +540,66 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS #ifdef USE_INTERNAL_EDGE_UTILITY gContactAddedCallback = btAdjustInternalEdgeContactsCallback; -#endif //USE_INTERNAL_EDGE_UTILITY - +#endif //USE_INTERNAL_EDGE_UTILITY } //printf("trimesh->m_collisionMargin=%f\n",trimesh->m_collisionMargin); break; } case COMPOUND_SHAPE_PROXYTYPE: + { + btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData; + btCompoundShape* compoundShape = createCompoundShape(); + + //btCompoundShapeChildData* childShapeDataArray = &compoundData->m_childShapePtr[0]; + + btAlignedObjectArray<btCollisionShape*> childShapes; + for (int i = 0; i < compoundData->m_numChildShapes; i++) { - btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData; - btCompoundShape* compoundShape = createCompoundShape(); + //btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i]; + btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape; - btAlignedObjectArray<btCollisionShape*> childShapes; - for (int i=0;i<compoundData->m_numChildShapes;i++) + btCollisionShape* childShape = convertCollisionShape(cd); + if (childShape) + { + btTransform localTransform; + localTransform.deSerializeFloat(compoundData->m_childShapePtr[i].m_transform); + compoundShape->addChildShape(localTransform, childShape); + } + else { - btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape; - - btCollisionShape* childShape = convertCollisionShape(cd); - if (childShape) - { - btTransform localTransform; - localTransform.deSerializeFloat(compoundData->m_childShapePtr[i].m_transform); - compoundShape->addChildShape(localTransform,childShape); - } else - { #ifdef _DEBUG - printf("error: couldn't create childShape for compoundShape\n"); + printf("error: couldn't create childShape for compoundShape\n"); #endif - } - } - shape = compoundShape; - - break; } + shape = compoundShape; + + break; + } case SOFTBODY_SHAPE_PROXYTYPE: - { - return 0; - } + { + return 0; + } default: - { + { #ifdef _DEBUG - printf("unsupported shape type (%d)\n",shapeData->m_shapeType); + printf("unsupported shape type (%d)\n", shapeData->m_shapeType); #endif - } } + } - return shape; - + return shape; } - - char* btCollisionWorldImporter::duplicateName(const char* name) { if (name) { int l = (int)strlen(name); - char* newName = new char[l+1]; - memcpy(newName,name,l); + char* newName = new char[l + 1]; + memcpy(newName, name, l); newName[l] = 0; m_allocatedNames.push_back(newName); return newName; @@ -635,53 +607,43 @@ char* btCollisionWorldImporter::duplicateName(const char* name) return 0; } - - - - - - - - - - -btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStridingMeshInterfaceData& meshData) +btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStridingMeshInterfaceData& meshData) { btTriangleIndexVertexArray* meshInterface = createTriangleMeshContainer(); - for (int i=0;i<meshData.m_numMeshParts;i++) + for (int i = 0; i < meshData.m_numMeshParts; i++) { btIndexedMesh meshPart; meshPart.m_numTriangles = meshData.m_meshPartsPtr[i].m_numTriangles; meshPart.m_numVertices = meshData.m_meshPartsPtr[i].m_numVertices; - if (meshData.m_meshPartsPtr[i].m_indices32) { meshPart.m_indexType = PHY_INTEGER; - meshPart.m_triangleIndexStride = 3*sizeof(int); - int* indexArray = (int*)btAlignedAlloc(sizeof(int)*3*meshPart.m_numTriangles,16); + meshPart.m_triangleIndexStride = 3 * sizeof(int); + int* indexArray = (int*)btAlignedAlloc(sizeof(int) * 3 * meshPart.m_numTriangles, 16); m_indexArrays.push_back(indexArray); - for (int j=0;j<3*meshPart.m_numTriangles;j++) + for (int j = 0; j < 3 * meshPart.m_numTriangles; j++) { indexArray[j] = meshData.m_meshPartsPtr[i].m_indices32[j].m_value; } meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; - } else + } + else { if (meshData.m_meshPartsPtr[i].m_3indices16) { meshPart.m_indexType = PHY_SHORT; - meshPart.m_triangleIndexStride = sizeof(short int)*3;//sizeof(btShortIntIndexTripletData); + meshPart.m_triangleIndexStride = sizeof(short int) * 3; //sizeof(btShortIntIndexTripletData); - short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int)*3*meshPart.m_numTriangles,16); + short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int) * 3 * meshPart.m_numTriangles, 16); m_shortIndexArrays.push_back(indexArray); - for (int j=0;j<meshPart.m_numTriangles;j++) + for (int j = 0; j < meshPart.m_numTriangles; j++) { - indexArray[3*j] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[0]; - indexArray[3*j+1] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[1]; - indexArray[3*j+2] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[2]; + indexArray[3 * j] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[0]; + indexArray[3 * j + 1] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[1]; + indexArray[3 * j + 2] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[2]; } meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; @@ -689,10 +651,10 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri if (meshData.m_meshPartsPtr[i].m_indices16) { meshPart.m_indexType = PHY_SHORT; - meshPart.m_triangleIndexStride = 3*sizeof(short int); - short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int)*3*meshPart.m_numTriangles,16); + meshPart.m_triangleIndexStride = 3 * sizeof(short int); + short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int) * 3 * meshPart.m_numTriangles, 16); m_shortIndexArrays.push_back(indexArray); - for (int j=0;j<3*meshPart.m_numTriangles;j++) + for (int j = 0; j < 3 * meshPart.m_numTriangles; j++) { indexArray[j] = meshData.m_meshPartsPtr[i].m_indices16[j].m_value; } @@ -703,16 +665,16 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri if (meshData.m_meshPartsPtr[i].m_3indices8) { meshPart.m_indexType = PHY_UCHAR; - meshPart.m_triangleIndexStride = sizeof(unsigned char)*3; + meshPart.m_triangleIndexStride = sizeof(unsigned char) * 3; - unsigned char* indexArray = (unsigned char*)btAlignedAlloc(sizeof(unsigned char)*3*meshPart.m_numTriangles,16); + unsigned char* indexArray = (unsigned char*)btAlignedAlloc(sizeof(unsigned char) * 3 * meshPart.m_numTriangles, 16); m_charIndexArrays.push_back(indexArray); - for (int j=0;j<meshPart.m_numTriangles;j++) + for (int j = 0; j < meshPart.m_numTriangles; j++) { - indexArray[3*j] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[0]; - indexArray[3*j+1] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[1]; - indexArray[3*j+2] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[2]; + indexArray[3 * j] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[0]; + indexArray[3 * j + 1] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[1]; + indexArray[3 * j + 2] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[2]; } meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; @@ -723,10 +685,10 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri { meshPart.m_vertexType = PHY_FLOAT; meshPart.m_vertexStride = sizeof(btVector3FloatData); - btVector3FloatData* vertices = (btVector3FloatData*) btAlignedAlloc(sizeof(btVector3FloatData)*meshPart.m_numVertices,16); + btVector3FloatData* vertices = (btVector3FloatData*)btAlignedAlloc(sizeof(btVector3FloatData) * meshPart.m_numVertices, 16); m_floatVertexArrays.push_back(vertices); - for (int j=0;j<meshPart.m_numVertices;j++) + for (int j = 0; j < meshPart.m_numVertices; j++) { vertices[j].m_floats[0] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[0]; vertices[j].m_floats[1] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[1]; @@ -734,16 +696,16 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri vertices[j].m_floats[3] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[3]; } meshPart.m_vertexBase = (const unsigned char*)vertices; - } else + } + else { meshPart.m_vertexType = PHY_DOUBLE; meshPart.m_vertexStride = sizeof(btVector3DoubleData); - - btVector3DoubleData* vertices = (btVector3DoubleData*) btAlignedAlloc(sizeof(btVector3DoubleData)*meshPart.m_numVertices,16); + btVector3DoubleData* vertices = (btVector3DoubleData*)btAlignedAlloc(sizeof(btVector3DoubleData) * meshPart.m_numVertices, 16); m_doubleVertexArrays.push_back(vertices); - for (int j=0;j<meshPart.m_numVertices;j++) + for (int j = 0; j < meshPart.m_numVertices; j++) { vertices[j].m_floats[0] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[0]; vertices[j].m_floats[1] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[1]; @@ -755,14 +717,13 @@ btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStri if (meshPart.m_triangleIndexBase && meshPart.m_vertexBase) { - meshInterface->addIndexedMesh(meshPart,meshPart.m_indexType); + meshInterface->addIndexedMesh(meshPart, meshPart.m_indexType); } } return meshInterface; } - btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData) { //create a new btStridingMeshInterfaceData that is an exact copy of shapedata and store it in the WorldImporter @@ -772,7 +733,7 @@ btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfa newData->m_numMeshParts = interfaceData->m_numMeshParts; newData->m_meshPartsPtr = new btMeshPartData[newData->m_numMeshParts]; - for(int i = 0;i < newData->m_numMeshParts;i++) + for (int i = 0; i < newData->m_numMeshParts; i++) { btMeshPartData* curPart = &interfaceData->m_meshPartsPtr[i]; btMeshPartData* curNewPart = &newData->m_meshPartsPtr[i]; @@ -780,18 +741,18 @@ btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfa curNewPart->m_numTriangles = curPart->m_numTriangles; curNewPart->m_numVertices = curPart->m_numVertices; - if(curPart->m_vertices3f) + if (curPart->m_vertices3f) { curNewPart->m_vertices3f = new btVector3FloatData[curNewPart->m_numVertices]; - memcpy(curNewPart->m_vertices3f,curPart->m_vertices3f,sizeof(btVector3FloatData) * curNewPart->m_numVertices); + memcpy(curNewPart->m_vertices3f, curPart->m_vertices3f, sizeof(btVector3FloatData) * curNewPart->m_numVertices); } else curNewPart->m_vertices3f = NULL; - if(curPart->m_vertices3d) + if (curPart->m_vertices3d) { curNewPart->m_vertices3d = new btVector3DoubleData[curNewPart->m_numVertices]; - memcpy(curNewPart->m_vertices3d,curPart->m_vertices3d,sizeof(btVector3DoubleData) * curNewPart->m_numVertices); + memcpy(curNewPart->m_vertices3d, curPart->m_vertices3d, sizeof(btVector3DoubleData) * curNewPart->m_numVertices); } else curNewPart->m_vertices3d = NULL; @@ -799,63 +760,60 @@ btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfa int numIndices = curNewPart->m_numTriangles * 3; ///the m_3indices8 was not initialized in some Bullet versions, this can cause crashes at loading time ///we catch it by only dealing with m_3indices8 if none of the other indices are initialized - bool uninitialized3indices8Workaround =false; + bool uninitialized3indices8Workaround = false; - if(curPart->m_indices32) + if (curPart->m_indices32) { - uninitialized3indices8Workaround=true; + uninitialized3indices8Workaround = true; curNewPart->m_indices32 = new btIntIndexData[numIndices]; - memcpy(curNewPart->m_indices32,curPart->m_indices32,sizeof(btIntIndexData) * numIndices); + memcpy(curNewPart->m_indices32, curPart->m_indices32, sizeof(btIntIndexData) * numIndices); } else curNewPart->m_indices32 = NULL; - if(curPart->m_3indices16) + if (curPart->m_3indices16) { - uninitialized3indices8Workaround=true; + uninitialized3indices8Workaround = true; curNewPart->m_3indices16 = new btShortIntIndexTripletData[curNewPart->m_numTriangles]; - memcpy(curNewPart->m_3indices16,curPart->m_3indices16,sizeof(btShortIntIndexTripletData) * curNewPart->m_numTriangles); + memcpy(curNewPart->m_3indices16, curPart->m_3indices16, sizeof(btShortIntIndexTripletData) * curNewPart->m_numTriangles); } else curNewPart->m_3indices16 = NULL; - if(curPart->m_indices16) + if (curPart->m_indices16) { - uninitialized3indices8Workaround=true; + uninitialized3indices8Workaround = true; curNewPart->m_indices16 = new btShortIntIndexData[numIndices]; - memcpy(curNewPart->m_indices16,curPart->m_indices16,sizeof(btShortIntIndexData) * numIndices); + memcpy(curNewPart->m_indices16, curPart->m_indices16, sizeof(btShortIntIndexData) * numIndices); } else curNewPart->m_indices16 = NULL; - if(!uninitialized3indices8Workaround && curPart->m_3indices8) + if (!uninitialized3indices8Workaround && curPart->m_3indices8) { curNewPart->m_3indices8 = new btCharIndexTripletData[curNewPart->m_numTriangles]; - memcpy(curNewPart->m_3indices8,curPart->m_3indices8,sizeof(btCharIndexTripletData) * curNewPart->m_numTriangles); + memcpy(curNewPart->m_3indices8, curPart->m_3indices8, sizeof(btCharIndexTripletData) * curNewPart->m_numTriangles); } else curNewPart->m_3indices8 = NULL; - } m_allocatedbtStridingMeshInterfaceDatas.push_back(newData); - return(newData); + return (newData); } #ifdef USE_INTERNAL_EDGE_UTILITY -extern ContactAddedCallback gContactAddedCallback; +extern ContactAddedCallback gContactAddedCallback; -static bool btAdjustInternalEdgeContactsCallback(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1) +static bool btAdjustInternalEdgeContactsCallback(btManifoldPoint& cp, const btCollisionObject* colObj0, int partId0, int index0, const btCollisionObject* colObj1, int partId1, int index1) { - - btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1); - //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_BACKFACE_MODE); - //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_DOUBLE_SIDED+BT_TRIANGLE_CONCAVE_DOUBLE_SIDED); + btAdjustInternalEdgeContacts(cp, colObj1, colObj0, partId1, index1); + //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_BACKFACE_MODE); + //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_DOUBLE_SIDED+BT_TRIANGLE_CONCAVE_DOUBLE_SIDED); return true; } -#endif //USE_INTERNAL_EDGE_UTILITY - +#endif //USE_INTERNAL_EDGE_UTILITY /* btRigidBody* btWorldImporter::createRigidBody(bool isDynamic, btScalar mass, const btTransform& startTransform,btCollisionShape* shape,const char* bodyName) @@ -894,29 +852,27 @@ btCollisionObject* btCollisionWorldImporter::getCollisionObjectByName(const char return 0; } -btCollisionObject* btCollisionWorldImporter::createCollisionObject(const btTransform& startTransform,btCollisionShape* shape, const char* bodyName) +btCollisionObject* btCollisionWorldImporter::createCollisionObject(const btTransform& startTransform, btCollisionShape* shape, const char* bodyName) { btCollisionObject* colObj = new btCollisionObject(); colObj->setWorldTransform(startTransform); colObj->setCollisionShape(shape); - m_collisionWorld->addCollisionObject(colObj);//todo: flags etc + m_collisionWorld->addCollisionObject(colObj); //todo: flags etc if (bodyName) { char* newname = duplicateName(bodyName); - m_objectNameMap.insert(colObj,newname); - m_nameColObjMap.insert(newname,colObj); + m_objectNameMap.insert(colObj, newname); + m_nameColObjMap.insert(newname, colObj); } m_allocatedCollisionObjects.push_back(colObj); return colObj; } - - -btCollisionShape* btCollisionWorldImporter::createPlaneShape(const btVector3& planeNormal,btScalar planeConstant) +btCollisionShape* btCollisionWorldImporter::createPlaneShape(const btVector3& planeNormal, btScalar planeConstant) { - btStaticPlaneShape* shape = new btStaticPlaneShape(planeNormal,planeConstant); + btStaticPlaneShape* shape = new btStaticPlaneShape(planeNormal, planeConstant); m_allocatedCollisionShapes.push_back(shape); return shape; } @@ -933,85 +889,83 @@ btCollisionShape* btCollisionWorldImporter::createSphereShape(btScalar radius) return shape; } - btCollisionShape* btCollisionWorldImporter::createCapsuleShapeX(btScalar radius, btScalar height) { - btCapsuleShapeX* shape = new btCapsuleShapeX(radius,height); + btCapsuleShapeX* shape = new btCapsuleShapeX(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } btCollisionShape* btCollisionWorldImporter::createCapsuleShapeY(btScalar radius, btScalar height) { - btCapsuleShape* shape = new btCapsuleShape(radius,height); + btCapsuleShape* shape = new btCapsuleShape(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } btCollisionShape* btCollisionWorldImporter::createCapsuleShapeZ(btScalar radius, btScalar height) { - btCapsuleShapeZ* shape = new btCapsuleShapeZ(radius,height); + btCapsuleShapeZ* shape = new btCapsuleShapeZ(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createCylinderShapeX(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createCylinderShapeX(btScalar radius, btScalar height) { - btCylinderShapeX* shape = new btCylinderShapeX(btVector3(height,radius,radius)); + btCylinderShapeX* shape = new btCylinderShapeX(btVector3(height, radius, radius)); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createCylinderShapeY(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createCylinderShapeY(btScalar radius, btScalar height) { - btCylinderShape* shape = new btCylinderShape(btVector3(radius,height,radius)); + btCylinderShape* shape = new btCylinderShape(btVector3(radius, height, radius)); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createCylinderShapeZ(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createCylinderShapeZ(btScalar radius, btScalar height) { - btCylinderShapeZ* shape = new btCylinderShapeZ(btVector3(radius,radius,height)); + btCylinderShapeZ* shape = new btCylinderShapeZ(btVector3(radius, radius, height)); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createConeShapeX(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createConeShapeX(btScalar radius, btScalar height) { - btConeShapeX* shape = new btConeShapeX(radius,height); + btConeShapeX* shape = new btConeShapeX(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createConeShapeY(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createConeShapeY(btScalar radius, btScalar height) { - btConeShape* shape = new btConeShape(radius,height); + btConeShape* shape = new btConeShape(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } -btCollisionShape* btCollisionWorldImporter::createConeShapeZ(btScalar radius,btScalar height) +btCollisionShape* btCollisionWorldImporter::createConeShapeZ(btScalar radius, btScalar height) { - btConeShapeZ* shape = new btConeShapeZ(radius,height); + btConeShapeZ* shape = new btConeShapeZ(radius, height); m_allocatedCollisionShapes.push_back(shape); return shape; } -btTriangleIndexVertexArray* btCollisionWorldImporter::createTriangleMeshContainer() +btTriangleIndexVertexArray* btCollisionWorldImporter::createTriangleMeshContainer() { btTriangleIndexVertexArray* in = new btTriangleIndexVertexArray(); m_allocatedTriangleIndexArrays.push_back(in); return in; } -btOptimizedBvh* btCollisionWorldImporter::createOptimizedBvh() +btOptimizedBvh* btCollisionWorldImporter::createOptimizedBvh() { btOptimizedBvh* bvh = new btOptimizedBvh(); m_allocatedBvhs.push_back(bvh); return bvh; } - btTriangleInfoMap* btCollisionWorldImporter::createTriangleInfoMap() { btTriangleInfoMap* tim = new btTriangleInfoMap(); @@ -1023,16 +977,15 @@ btBvhTriangleMeshShape* btCollisionWorldImporter::createBvhTriangleMeshShape(btS { if (bvh) { - btBvhTriangleMeshShape* bvhTriMesh = new btBvhTriangleMeshShape(trimesh,bvh->isQuantized(), false); + btBvhTriangleMeshShape* bvhTriMesh = new btBvhTriangleMeshShape(trimesh, bvh->isQuantized(), false); bvhTriMesh->setOptimizedBvh(bvh); m_allocatedCollisionShapes.push_back(bvhTriMesh); return bvhTriMesh; } - btBvhTriangleMeshShape* ts = new btBvhTriangleMeshShape(trimesh,true); + btBvhTriangleMeshShape* ts = new btBvhTriangleMeshShape(trimesh, true); m_allocatedCollisionShapes.push_back(ts); return ts; - } btCollisionShape* btCollisionWorldImporter::createConvexTriangleMeshShape(btStridingMeshInterface* trimesh) { @@ -1044,9 +997,8 @@ btGImpactMeshShape* btCollisionWorldImporter::createGimpactShape(btStridingMeshI btGImpactMeshShape* shape = new btGImpactMeshShape(trimesh); m_allocatedCollisionShapes.push_back(shape); return shape; - } -#endif //SUPPORT_GIMPACT_SHAPE_IMPORT +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT btConvexHullShape* btCollisionWorldImporter::createConvexHullShape() { @@ -1062,25 +1014,22 @@ btCompoundShape* btCollisionWorldImporter::createCompoundShape() return shape; } - -btScaledBvhTriangleMeshShape* btCollisionWorldImporter::createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScaling) +btScaledBvhTriangleMeshShape* btCollisionWorldImporter::createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape, const btVector3& localScaling) { - btScaledBvhTriangleMeshShape* shape = new btScaledBvhTriangleMeshShape(meshShape,localScaling); + btScaledBvhTriangleMeshShape* shape = new btScaledBvhTriangleMeshShape(meshShape, localScaling); m_allocatedCollisionShapes.push_back(shape); return shape; } -btMultiSphereShape* btCollisionWorldImporter::createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres) +btMultiSphereShape* btCollisionWorldImporter::createMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres) { btMultiSphereShape* shape = new btMultiSphereShape(positions, radi, numSpheres); m_allocatedCollisionShapes.push_back(shape); return shape; } - - - // query for data -int btCollisionWorldImporter::getNumCollisionShapes() const +// query for data +int btCollisionWorldImporter::getNumCollisionShapes() const { return m_allocatedCollisionShapes.size(); } @@ -1093,23 +1042,21 @@ btCollisionShape* btCollisionWorldImporter::getCollisionShapeByIndex(int index) btCollisionShape* btCollisionWorldImporter::getCollisionShapeByName(const char* name) { btCollisionShape** shapePtr = m_nameShapeMap.find(name); - if (shapePtr&& *shapePtr) + if (shapePtr && *shapePtr) { return *shapePtr; } return 0; } - -const char* btCollisionWorldImporter::getNameForPointer(const void* ptr) const +const char* btCollisionWorldImporter::getNameForPointer(const void* ptr) const { - const char*const * namePtr = m_objectNameMap.find(ptr); + const char* const* namePtr = m_objectNameMap.find(ptr); if (namePtr && *namePtr) return *namePtr; return 0; } - int btCollisionWorldImporter::getNumRigidBodies() const { return m_allocatedRigidBodies.size(); @@ -1120,12 +1067,11 @@ btCollisionObject* btCollisionWorldImporter::getRigidBodyByIndex(int index) cons return m_allocatedRigidBodies[index]; } - int btCollisionWorldImporter::getNumBvhs() const { return m_allocatedBvhs.size(); } - btOptimizedBvh* btCollisionWorldImporter::getBvhByIndex(int index) const +btOptimizedBvh* btCollisionWorldImporter::getBvhByIndex(int index) const { return m_allocatedBvhs[index]; } @@ -1139,5 +1085,3 @@ btTriangleInfoMap* btCollisionWorldImporter::getTriangleInfoMapByIndex(int index { return m_allocatedTriangleInfoMaps[index]; } - - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h index 9a6d16fbea7..5e8bc95341d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_COLLISION_WORLD_IMPORTER_H #define BT_COLLISION_WORLD_IMPORTER_H @@ -26,7 +25,6 @@ class btCollisionShape; class btCollisionObject; struct btBulletSerializedArrays; - struct ConstraintInput; class btCollisionWorld; struct btCollisionShapeData; @@ -46,9 +44,6 @@ class btSliderConstraint; class btGearConstraint; struct btContactSolverInfo; - - - class btCollisionWorldImporter { protected: @@ -56,60 +51,53 @@ protected: int m_verboseMode; - btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes; + btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes; btAlignedObjectArray<btCollisionObject*> m_allocatedRigidBodies; - btAlignedObjectArray<btOptimizedBvh*> m_allocatedBvhs; + btAlignedObjectArray<btOptimizedBvh*> m_allocatedBvhs; btAlignedObjectArray<btTriangleInfoMap*> m_allocatedTriangleInfoMaps; btAlignedObjectArray<btTriangleIndexVertexArray*> m_allocatedTriangleIndexArrays; btAlignedObjectArray<btStridingMeshInterfaceData*> m_allocatedbtStridingMeshInterfaceDatas; btAlignedObjectArray<btCollisionObject*> m_allocatedCollisionObjects; + btAlignedObjectArray<char*> m_allocatedNames; - btAlignedObjectArray<char*> m_allocatedNames; + btAlignedObjectArray<int*> m_indexArrays; + btAlignedObjectArray<short int*> m_shortIndexArrays; + btAlignedObjectArray<unsigned char*> m_charIndexArrays; - btAlignedObjectArray<int*> m_indexArrays; - btAlignedObjectArray<short int*> m_shortIndexArrays; - btAlignedObjectArray<unsigned char*> m_charIndexArrays; + btAlignedObjectArray<btVector3FloatData*> m_floatVertexArrays; + btAlignedObjectArray<btVector3DoubleData*> m_doubleVertexArrays; - btAlignedObjectArray<btVector3FloatData*> m_floatVertexArrays; - btAlignedObjectArray<btVector3DoubleData*> m_doubleVertexArrays; + btHashMap<btHashPtr, btOptimizedBvh*> m_bvhMap; + btHashMap<btHashPtr, btTriangleInfoMap*> m_timMap; + btHashMap<btHashString, btCollisionShape*> m_nameShapeMap; + btHashMap<btHashString, btCollisionObject*> m_nameColObjMap; - btHashMap<btHashPtr,btOptimizedBvh*> m_bvhMap; - btHashMap<btHashPtr,btTriangleInfoMap*> m_timMap; - - btHashMap<btHashString,btCollisionShape*> m_nameShapeMap; - btHashMap<btHashString,btCollisionObject*> m_nameColObjMap; - - btHashMap<btHashPtr,const char*> m_objectNameMap; - - btHashMap<btHashPtr,btCollisionShape*> m_shapeMap; - btHashMap<btHashPtr,btCollisionObject*> m_bodyMap; + btHashMap<btHashPtr, const char*> m_objectNameMap; + btHashMap<btHashPtr, btCollisionShape*> m_shapeMap; + btHashMap<btHashPtr, btCollisionObject*> m_bodyMap; //methods + char* duplicateName(const char* name); - - char* duplicateName(const char* name); - - btCollisionShape* convertCollisionShape( btCollisionShapeData* shapeData ); - + btCollisionShape* convertCollisionShape(btCollisionShapeData* shapeData); public: - btCollisionWorldImporter(btCollisionWorld* world); virtual ~btCollisionWorldImporter(); - bool convertAllObjects( btBulletSerializedArrays* arrays); + bool convertAllObjects(btBulletSerializedArrays* arrays); - ///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load. + ///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load. ///make sure you don't use the dynamics world containing objects after you call this method virtual void deleteAllData(); - void setVerboseMode(int verboseMode) + void setVerboseMode(int verboseMode) { m_verboseMode = verboseMode; } @@ -119,15 +107,14 @@ public: return m_verboseMode; } - // query for data - int getNumCollisionShapes() const; + // query for data + int getNumCollisionShapes() const; btCollisionShape* getCollisionShapeByIndex(int index); int getNumRigidBodies() const; btCollisionObject* getRigidBodyByIndex(int index) const; - int getNumConstraints() const; int getNumBvhs() const; - btOptimizedBvh* getBvhByIndex(int index) const; + btOptimizedBvh* getBvhByIndex(int index) const; int getNumTriangleInfoMaps() const; btTriangleInfoMap* getTriangleInfoMapByIndex(int index) const; @@ -135,56 +122,48 @@ public: btCollisionShape* getCollisionShapeByName(const char* name); btCollisionObject* getCollisionObjectByName(const char* name); - - const char* getNameForPointer(const void* ptr) const; + const char* getNameForPointer(const void* ptr) const; ///those virtuals are called by load and can be overridden by the user - - //bodies - virtual btCollisionObject* createCollisionObject( const btTransform& startTransform, btCollisionShape* shape,const char* bodyName); + virtual btCollisionObject* createCollisionObject(const btTransform& startTransform, btCollisionShape* shape, const char* bodyName); ///shapes - virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal,btScalar planeConstant); + virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal, btScalar planeConstant); virtual btCollisionShape* createBoxShape(const btVector3& halfExtents); virtual btCollisionShape* createSphereShape(btScalar radius); virtual btCollisionShape* createCapsuleShapeX(btScalar radius, btScalar height); virtual btCollisionShape* createCapsuleShapeY(btScalar radius, btScalar height); virtual btCollisionShape* createCapsuleShapeZ(btScalar radius, btScalar height); - virtual btCollisionShape* createCylinderShapeX(btScalar radius,btScalar height); - virtual btCollisionShape* createCylinderShapeY(btScalar radius,btScalar height); - virtual btCollisionShape* createCylinderShapeZ(btScalar radius,btScalar height); - virtual btCollisionShape* createConeShapeX(btScalar radius,btScalar height); - virtual btCollisionShape* createConeShapeY(btScalar radius,btScalar height); - virtual btCollisionShape* createConeShapeZ(btScalar radius,btScalar height); - virtual class btTriangleIndexVertexArray* createTriangleMeshContainer(); - virtual btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh); + virtual btCollisionShape* createCylinderShapeX(btScalar radius, btScalar height); + virtual btCollisionShape* createCylinderShapeY(btScalar radius, btScalar height); + virtual btCollisionShape* createCylinderShapeZ(btScalar radius, btScalar height); + virtual btCollisionShape* createConeShapeX(btScalar radius, btScalar height); + virtual btCollisionShape* createConeShapeY(btScalar radius, btScalar height); + virtual btCollisionShape* createConeShapeZ(btScalar radius, btScalar height); + virtual class btTriangleIndexVertexArray* createTriangleMeshContainer(); + virtual btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh); virtual btCollisionShape* createConvexTriangleMeshShape(btStridingMeshInterface* trimesh); #ifdef SUPPORT_GIMPACT_SHAPE_IMPORT virtual btGImpactMeshShape* createGimpactShape(btStridingMeshInterface* trimesh); -#endif //SUPPORT_GIMPACT_SHAPE_IMPORT +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT virtual btStridingMeshInterfaceData* createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData); virtual class btConvexHullShape* createConvexHullShape(); virtual class btCompoundShape* createCompoundShape(); - virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScalingbtBvhTriangleMeshShape); + virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape, const btVector3& localScalingbtBvhTriangleMeshShape); - virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres); + virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions, const btScalar* radi, int numSpheres); virtual btTriangleIndexVertexArray* createMeshInterface(btStridingMeshInterfaceData& meshData); ///acceleration and connectivity structures - virtual btOptimizedBvh* createOptimizedBvh(); + virtual btOptimizedBvh* createOptimizedBvh(); virtual btTriangleInfoMap* createTriangleInfoMap(); - - - - }; - -#endif //BT_WORLD_IMPORTER_H +#endif //BT_WORLD_IMPORTER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 589a7f55f2b..b5f4a3c869f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -25,56 +25,58 @@ subject to the following restrictions: btShapePairCallback gCompoundChildShapePairCallback = 0; -btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_isSwapped(isSwapped), -m_sharedManifold(ci.m_manifold) +btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_isSwapped(isSwapped), + m_sharedManifold(ci.m_manifold) { m_ownsManifold = false; - const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; - btAssert (colObjWrap->getCollisionShape()->isCompound()); - + const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + btAssert(colObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape()); m_compoundShapeRevision = compoundShape->getUpdateRevision(); - - - preallocateChildAlgorithms(body0Wrap,body1Wrap); + + preallocateChildAlgorithms(body0Wrap, body1Wrap); } -void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) +void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap; - btAssert (colObjWrap->getCollisionShape()->isCompound()); - + const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap; + btAssert(colObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape()); int numChildren = compoundShape->getNumChildShapes(); int i; - + m_childCollisionAlgorithms.resize(numChildren); - for (i=0;i<numChildren;i++) + for (i = 0; i < numChildren; i++) { if (compoundShape->getDynamicAabbTree()) { m_childCollisionAlgorithms[i] = 0; - } else + } + else { - const btCollisionShape* childShape = compoundShape->getChildShape(i); - btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully) - m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold); + btCollisionObjectWrapper childWrap(colObjWrap, childShape, colObjWrap->getCollisionObject(), colObjWrap->getWorldTransform(), -1, i); //wrong child trans, but unused (hopefully) + m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap, otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + + btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsContact; + btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsClosestPoints; } } } -void btCompoundCollisionAlgorithm::removeChildAlgorithms() +void btCompoundCollisionAlgorithm::removeChildAlgorithms() { int numChildren = m_childCollisionAlgorithms.size(); int i; - for (i=0;i<numChildren;i++) + for (i = 0; i < numChildren; i++) { if (m_childCollisionAlgorithms[i]) { @@ -89,66 +91,79 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() removeChildAlgorithms(); } - - - -struct btCompoundLeafCallback : btDbvt::ICollide +struct btCompoundLeafCallback : btDbvt::ICollide { - public: - const btCollisionObjectWrapper* m_compoundColObjWrap; const btCollisionObjectWrapper* m_otherObjWrap; btDispatcher* m_dispatcher; const btDispatcherInfo& m_dispatchInfo; - btManifoldResult* m_resultOut; - btCollisionAlgorithm** m_childCollisionAlgorithms; - btPersistentManifold* m_sharedManifold; - - btCompoundLeafCallback (const btCollisionObjectWrapper* compoundObjWrap,const btCollisionObjectWrapper* otherObjWrap,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut,btCollisionAlgorithm** childCollisionAlgorithms,btPersistentManifold* sharedManifold) - :m_compoundColObjWrap(compoundObjWrap),m_otherObjWrap(otherObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), - m_childCollisionAlgorithms(childCollisionAlgorithms), - m_sharedManifold(sharedManifold) - { + btManifoldResult* m_resultOut; + btCollisionAlgorithm** m_childCollisionAlgorithms; + btPersistentManifold* m_sharedManifold; + btCompoundLeafCallback(const btCollisionObjectWrapper* compoundObjWrap, const btCollisionObjectWrapper* otherObjWrap, btDispatcher* dispatcher, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut, btCollisionAlgorithm** childCollisionAlgorithms, btPersistentManifold* sharedManifold) + : m_compoundColObjWrap(compoundObjWrap), m_otherObjWrap(otherObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithms(childCollisionAlgorithms), m_sharedManifold(sharedManifold) + { } - - void ProcessChildShape(const btCollisionShape* childShape,int index) + void ProcessChildShape(const btCollisionShape* childShape, int index) { - btAssert(index>=0); + btAssert(index >= 0); const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape()); - btAssert(index<compoundShape->getNumChildShapes()); + btAssert(index < compoundShape->getNumChildShapes()); + if (gCompoundChildShapePairCallback) + { + if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape)) + return; + } //backup - btTransform orgTrans = m_compoundColObjWrap->getWorldTransform(); - + btTransform orgTrans = m_compoundColObjWrap->getWorldTransform(); + const btTransform& childTrans = compoundShape->getChildTransform(index); - btTransform newChildWorldTrans = orgTrans*childTrans ; + btTransform newChildWorldTrans = orgTrans * childTrans; //perform an AABB check first - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); - m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); + btVector3 aabbMin0, aabbMax0; + childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0); - if (gCompoundChildShapePairCallback) - { - if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape)) - return; - } + btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold); + aabbMin0 -= extendAabb; + aabbMax0 += extendAabb; - if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) - { + btVector3 aabbMin1, aabbMax1; + m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1); - btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index); + if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) + { + btTransform preTransform = childTrans; + if (this->m_compoundColObjWrap->m_preTransform) + { + preTransform = preTransform *(*(this->m_compoundColObjWrap->m_preTransform)); + } + btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, preTransform, -1, index); + + btCollisionAlgorithm* algo = 0; + bool allocatedAlgorithm = false; - //the contactpoint is still projected back using the original inverted worldtrans - if (!m_childCollisionAlgorithms[index]) - m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold); + if (m_resultOut->m_closestPointDistanceThreshold > 0) + { + algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); + allocatedAlgorithm = true; + } + else + { + //the contactpoint is still projected back using the original inverted worldtrans + if (!m_childCollisionAlgorithms[index]) + { + m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + } + algo = m_childCollisionAlgorithms[index]; + } - const btCollisionObjectWrapper* tmpWrap = 0; ///detect swapping case @@ -156,16 +171,16 @@ public: { tmpWrap = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&compoundWrap); - m_resultOut->setShapeIdentifiersA(-1,index); - } else + m_resultOut->setShapeIdentifiersA(-1, index); + } + else { tmpWrap = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&compoundWrap); - m_resultOut->setShapeIdentifiersB(-1,index); + m_resultOut->setShapeIdentifiersB(-1, index); } - - m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut); + algo->processCollision(&compoundWrap, m_otherObjWrap, m_dispatchInfo, m_resultOut); #if 0 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) @@ -179,14 +194,19 @@ public: if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject()) { m_resultOut->setBody0Wrap(tmpWrap); - } else + } + else { m_resultOut->setBody1Wrap(tmpWrap); } - + if (allocatedAlgorithm) + { + algo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algo); + } } } - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { int index = leaf->dataAsInt; @@ -203,22 +223,16 @@ public: } #endif - ProcessChildShape(childShape,index); - + ProcessChildShape(childShape, index); } }; - - - - - -void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap; + const btCollisionObjectWrapper* colObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* otherObjWrap = m_isSwapped ? body0Wrap : body1Wrap; - btAssert (colObjWrap->getCollisionShape()->isCompound()); + btAssert(colObjWrap->getCollisionShape()->isCompound()); const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape()); ///btCompoundShape might have changed: @@ -227,36 +241,36 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap { ///clear and update all removeChildAlgorithms(); - - preallocateChildAlgorithms(body0Wrap,body1Wrap); + + preallocateChildAlgorithms(body0Wrap, body1Wrap); m_compoundShapeRevision = compoundShape->getUpdateRevision(); } - if (m_childCollisionAlgorithms.size()==0) + if (m_childCollisionAlgorithms.size() == 0) return; - + const btDbvt* tree = compoundShape->getDynamicAabbTree(); //use a dynamic aabb tree to cull potential child-overlaps - btCompoundLeafCallback callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold); + btCompoundLeafCallback callback(colObjWrap, otherObjWrap, m_dispatcher, dispatchInfo, resultOut, &m_childCollisionAlgorithms[0], m_sharedManifold); ///we need to refresh all contact manifolds ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm { int i; - btManifoldArray manifoldArray; - for (i=0;i<m_childCollisionAlgorithms.size();i++) + manifoldArray.resize(0); + for (i = 0; i < m_childCollisionAlgorithms.size(); i++) { if (m_childCollisionAlgorithms[i]) { m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray); - for (int m=0;m<manifoldArray.size();m++) + for (int m = 0; m < manifoldArray.size(); m++) { if (manifoldArray[m]->getNumContacts()) { resultOut->setPersistentManifold(manifoldArray[m]); resultOut->refreshContactPoints(); - resultOut->setPersistentManifold(0);//??necessary? + resultOut->setPersistentManifold(0); //??necessary? } } manifoldArray.resize(0); @@ -266,54 +280,56 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap if (tree) { - - btVector3 localAabbMin,localAabbMax; + btVector3 localAabbMin, localAabbMax; btTransform otherInCompoundSpace; otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform(); - otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax); + otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace, localAabbMin, localAabbMax); + btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold); + localAabbMin -= extraExtends; + localAabbMax += extraExtends; - const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(localAabbMin, localAabbMax); //process all children, that overlap with the given AABB bounds - tree->collideTV(tree->m_root,bounds,callback); - - } else + tree->collideTVNoStackAlloc(tree->m_root, bounds, stack2, callback); + } + else { //iterate over all children, perform an AABB check inside ProcessChildShape int numChildren = m_childCollisionAlgorithms.size(); int i; - for (i=0;i<numChildren;i++) + for (i = 0; i < numChildren; i++) { - callback.ProcessChildShape(compoundShape->getChildShape(i),i); + callback.ProcessChildShape(compoundShape->getChildShape(i), i); } } { - //iterate over all children, perform an AABB check inside ProcessChildShape + //iterate over all children, perform an AABB check inside ProcessChildShape int numChildren = m_childCollisionAlgorithms.size(); int i; - btManifoldArray manifoldArray; - const btCollisionShape* childShape = 0; - btTransform orgTrans; - - btTransform newChildWorldTrans; - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - - for (i=0;i<numChildren;i++) + manifoldArray.resize(0); + const btCollisionShape* childShape = 0; + btTransform orgTrans; + + btTransform newChildWorldTrans; + btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1; + + for (i = 0; i < numChildren; i++) { if (m_childCollisionAlgorithms[i]) { childShape = compoundShape->getChildShape(i); - //if not longer overlapping, remove the algorithm + //if not longer overlapping, remove the algorithm orgTrans = colObjWrap->getWorldTransform(); - + const btTransform& childTrans = compoundShape->getChildTransform(i); - newChildWorldTrans = orgTrans*childTrans ; + newChildWorldTrans = orgTrans * childTrans; //perform an AABB check first - childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); - otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); + childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0); + otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1); - if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) { m_childCollisionAlgorithms[i]->~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); @@ -324,15 +340,15 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap } } -btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { btAssert(0); //needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures - btCollisionObject* colObj = m_isSwapped? body1 : body0; - btCollisionObject* otherObj = m_isSwapped? body0 : body1; + btCollisionObject* colObj = m_isSwapped ? body1 : body0; + btCollisionObject* otherObj = m_isSwapped ? body0 : body1; + + btAssert(colObj->getCollisionShape()->isCompound()); - btAssert (colObj->getCollisionShape()->isCompound()); - btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape()); //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps @@ -346,33 +362,29 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* int numChildren = m_childCollisionAlgorithms.size(); int i; - btTransform orgTrans; - btScalar frac; - for (i=0;i<numChildren;i++) + btTransform orgTrans; + btScalar frac; + for (i = 0; i < numChildren; i++) { //btCollisionShape* childShape = compoundShape->getChildShape(i); //backup - orgTrans = colObj->getWorldTransform(); - + orgTrans = colObj->getWorldTransform(); + const btTransform& childTrans = compoundShape->getChildTransform(i); //btTransform newChildWorldTrans = orgTrans*childTrans ; - colObj->setWorldTransform( orgTrans*childTrans ); + colObj->setWorldTransform(orgTrans * childTrans); //btCollisionShape* tmpShape = colObj->getCollisionShape(); //colObj->internalSetTemporaryCollisionShape( childShape ); - frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); - if (frac<hitFraction) + frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut); + if (frac < hitFraction) { hitFraction = frac; } //revert back //colObj->internalSetTemporaryCollisionShape( tmpShape); - colObj->setWorldTransform( orgTrans); + colObj->setWorldTransform(orgTrans); } return hitFraction; - } - - - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index 7d792c18d34..4ea5e77185b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -26,6 +26,7 @@ class btDispatcher; #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "btCollisionCreateFunc.h" #include "LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/BroadphaseCollision/btDbvt.h" class btDispatcher; class btCollisionObject; @@ -34,67 +35,65 @@ typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCol extern btShapePairCallback gCompoundChildShapePairCallback; /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes -class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm +class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm { + btNodeStack stack2; + btManifoldArray manifoldArray; + protected: btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms; bool m_isSwapped; - class btPersistentManifold* m_sharedManifold; - bool m_ownsManifold; + class btPersistentManifold* m_sharedManifold; + bool m_ownsManifold; + int m_compoundShapeRevision; //to keep track of changes, so that childAlgorithm array can be updated - int m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated - - void removeChildAlgorithms(); - - void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + void removeChildAlgorithms(); -public: + void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); - btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); +public: + btCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); virtual ~btCompoundCollisionAlgorithm(); - btCollisionAlgorithm* getChildAlgorithm (int n) const + btCollisionAlgorithm* getChildAlgorithm(int n) const { return m_childCollisionAlgorithms[n]; } + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { int i; - for (i=0;i<m_childCollisionAlgorithms.size();i++) + for (i = 0; i < m_childCollisionAlgorithms.size(); i++) { if (m_childCollisionAlgorithms[i]) m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray); } } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm)); - return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + return new (mem) btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, false); } }; - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm)); - return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + return new (mem) btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, true); } }; - }; -#endif //BT_COMPOUND_COLLISION_ALGORITHM_H +#endif //BT_COMPOUND_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp index 1d64d84b87b..044b60dbb1d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -15,6 +15,7 @@ subject to the following restrictions: */ #include "btCompoundCompoundCollisionAlgorithm.h" +#include "LinearMath/btQuickprof.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/BroadphaseCollision/btDbvt.h" @@ -23,32 +24,30 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btManifoldResult.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" +//USE_LOCAL_STACK will avoid most (often all) dynamic memory allocations due to resizing in processCollision and MycollideTT +#define USE_LOCAL_STACK 1 btShapePairCallback gCompoundCompoundChildShapePairCallback = 0; -btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped) +btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) + : btCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, isSwapped) { - - void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16); - m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache(); + void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache), 16); + m_childCollisionAlgorithmCache = new (ptr) btHashedSimplePairCache(); const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; - btAssert (col0ObjWrap->getCollisionShape()->isCompound()); + btAssert(col0ObjWrap->getCollisionShape()->isCompound()); const btCollisionObjectWrapper* col1ObjWrap = body1Wrap; - btAssert (col1ObjWrap->getCollisionShape()->isCompound()); - + btAssert(col1ObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape()); m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape()); m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); - - } - btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm() { removeChildAlgorithms(); @@ -56,32 +55,30 @@ btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm() btAlignedFree(m_childCollisionAlgorithmCache); } -void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) { int i; btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - for (i=0;i<pairs.size();i++) + for (i = 0; i < pairs.size(); i++) { if (pairs[i].m_userPointer) { - ((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray); } } } - -void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() +void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() { btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); int numChildren = pairs.size(); int i; - for (i=0;i<numChildren;i++) + for (i = 0; i < numChildren; i++) { if (pairs[i].m_userPointer) { - btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer; + btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer; algo->~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(algo); } @@ -89,106 +86,106 @@ void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() m_childCollisionAlgorithmCache->removeAllPairs(); } -struct btCompoundCompoundLeafCallback : btDbvt::ICollide +struct btCompoundCompoundLeafCallback : btDbvt::ICollide { int m_numOverlapPairs; - const btCollisionObjectWrapper* m_compound0ColObjWrap; const btCollisionObjectWrapper* m_compound1ColObjWrap; btDispatcher* m_dispatcher; const btDispatcherInfo& m_dispatchInfo; - btManifoldResult* m_resultOut; - - - class btHashedSimplePairCache* m_childCollisionAlgorithmCache; - - btPersistentManifold* m_sharedManifold; - - btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap, - const btCollisionObjectWrapper* compound0ObjWrap, - btDispatcher* dispatcher, - const btDispatcherInfo& dispatchInfo, - btManifoldResult* resultOut, - btHashedSimplePairCache* childAlgorithmsCache, - btPersistentManifold* sharedManifold) - :m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), - m_childCollisionAlgorithmCache(childAlgorithmsCache), - m_sharedManifold(sharedManifold) - { + btManifoldResult* m_resultOut; - } + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; + btPersistentManifold* m_sharedManifold; + btCompoundCompoundLeafCallback(const btCollisionObjectWrapper* compound1ObjWrap, + const btCollisionObjectWrapper* compound0ObjWrap, + btDispatcher* dispatcher, + const btDispatcherInfo& dispatchInfo, + btManifoldResult* resultOut, + btHashedSimplePairCache* childAlgorithmsCache, + btPersistentManifold* sharedManifold) + : m_numOverlapPairs(0), m_compound0ColObjWrap(compound1ObjWrap), m_compound1ColObjWrap(compound0ObjWrap), m_dispatcher(dispatcher), m_dispatchInfo(dispatchInfo), m_resultOut(resultOut), m_childCollisionAlgorithmCache(childAlgorithmsCache), m_sharedManifold(sharedManifold) + { + } - - void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1) + void Process(const btDbvtNode* leaf0, const btDbvtNode* leaf1) { + BT_PROFILE("btCompoundCompoundLeafCallback::Process"); m_numOverlapPairs++; - int childIndex0 = leaf0->dataAsInt; int childIndex1 = leaf1->dataAsInt; - - - btAssert(childIndex0>=0); - btAssert(childIndex1>=0); + btAssert(childIndex0 >= 0); + btAssert(childIndex1 >= 0); const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape()); - btAssert(childIndex0<compoundShape0->getNumChildShapes()); + btAssert(childIndex0 < compoundShape0->getNumChildShapes()); const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape()); - btAssert(childIndex1<compoundShape1->getNumChildShapes()); + btAssert(childIndex1 < compoundShape1->getNumChildShapes()); const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0); const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1); //backup - btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform(); + btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform(); const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0); - btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ; - - btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform(); + btTransform newChildWorldTrans0 = orgTrans0 * childTrans0; + + btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform(); const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1); - btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ; - + btTransform newChildWorldTrans1 = orgTrans1 * childTrans1; //perform an AABB check first - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); - childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); - + btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1; + childShape0->getAabb(newChildWorldTrans0, aabbMin0, aabbMax0); + childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1); + + btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold); + + aabbMin0 -= thresholdVec; + aabbMax0 += thresholdVec; + if (gCompoundCompoundChildShapePairCallback) { - if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1)) + if (!gCompoundCompoundChildShapePairCallback(childShape0, childShape1)) return; } - if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) { - btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0); - btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1); - - - btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1); + btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap, childShape0, m_compound0ColObjWrap->getCollisionObject(), newChildWorldTrans0, -1, childIndex0); + btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap, childShape1, m_compound1ColObjWrap->getCollisionObject(), newChildWorldTrans1, -1, childIndex1); + btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0, childIndex1); + bool removePair = false; btCollisionAlgorithm* colAlgo = 0; - - if (pair) + if (m_resultOut->m_closestPointDistanceThreshold > 0) { - colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; - - } else + colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS); + removePair = true; + } + else { - colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold); - pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1); - btAssert(pair); - pair->m_userPointer = colAlgo; + if (pair) + { + colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; + } + else + { + colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1); + btAssert(pair); + pair->m_userPointer = colAlgo; + } } btAssert(colAlgo); - + const btCollisionObjectWrapper* tmpWrap0 = 0; const btCollisionObjectWrapper* tmpWrap1 = 0; @@ -198,94 +195,100 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide m_resultOut->setBody0Wrap(&compoundWrap0); m_resultOut->setBody1Wrap(&compoundWrap1); - m_resultOut->setShapeIdentifiersA(-1,childIndex0); - m_resultOut->setShapeIdentifiersB(-1,childIndex1); + m_resultOut->setShapeIdentifiersA(-1, childIndex0); + m_resultOut->setShapeIdentifiersB(-1, childIndex1); + colAlgo->processCollision(&compoundWrap0, &compoundWrap1, m_dispatchInfo, m_resultOut); - colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut); - m_resultOut->setBody0Wrap(tmpWrap0); m_resultOut->setBody1Wrap(tmpWrap1); - - + if (removePair) + { + colAlgo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(colAlgo); + } } } }; - -static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, const btTransform& xform) +static DBVT_INLINE bool MyIntersect(const btDbvtAabbMm& a, + const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold) { - btVector3 newmin,newmax; - btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax); - btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax); - return Intersect(a,newb); + btVector3 newmin, newmax; + btTransformAabb(b.Mins(), b.Maxs(), 0.f, xform, newmin, newmax); + newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold); + newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold); + btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin, newmax); + return Intersect(a, newb); } - -static inline void MycollideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - btCompoundCompoundLeafCallback* callback) +static inline void MycollideTT(const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold) { - - if(root0&&root1) + if (root0 && root1) + { + int depth = 1; + int treshold = btDbvt::DOUBLE_STACKSIZE - 4; + btAlignedObjectArray<btDbvt::sStkNN> stkStack; +#ifdef USE_LOCAL_STACK + ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]); + stkStack.initializeFromBuffer(&localStack, btDbvt::DOUBLE_STACKSIZE, btDbvt::DOUBLE_STACKSIZE); +#else + stkStack.resize(btDbvt::DOUBLE_STACKSIZE); +#endif + stkStack[0] = btDbvt::sStkNN(root0, root1); + do { - int depth=1; - int treshold=btDbvt::DOUBLE_STACKSIZE-4; - btAlignedObjectArray<btDbvt::sStkNN> stkStack; - stkStack.resize(btDbvt::DOUBLE_STACKSIZE); - stkStack[0]=btDbvt::sStkNN(root0,root1); - do { - btDbvt::sStkNN p=stkStack[--depth]; - if(MyIntersect(p.a->volume,p.b->volume,xform)) + btDbvt::sStkNN p = stkStack[--depth]; + if (MyIntersect(p.a->volume, p.b->volume, xform, distanceThreshold)) + { + if (depth > treshold) { - if(depth>treshold) + stkStack.resize(stkStack.size() * 2); + treshold = stkStack.size() - 4; + } + if (p.a->isinternal()) + { + if (p.b->isinternal()) + { + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[0]); + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[0]); + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b->childs[1]); + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b->childs[1]); + } + else { - stkStack.resize(stkStack.size()*2); - treshold=stkStack.size()-4; + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[0], p.b); + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b); } - if(p.a->isinternal()) + } + else + { + if (p.b->isinternal()) { - if(p.b->isinternal()) - { - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b); - stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b); - } + stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[0]); + stkStack[depth++] = btDbvt::sStkNN(p.a, p.b->childs[1]); } else { - if(p.b->isinternal()) - { - stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]); - stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]); - } - else - { - callback->Process(p.a,p.b); - } + callback->Process(p.a, p.b); } } - } while(depth); - } + } + } while (depth); + } } -void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btCompoundCompoundCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; - const btCollisionObjectWrapper* col1ObjWrap= body1Wrap; + const btCollisionObjectWrapper* col1ObjWrap = body1Wrap; - btAssert (col0ObjWrap->getCollisionShape()->isCompound()); - btAssert (col1ObjWrap->getCollisionShape()->isCompound()); + btAssert(col0ObjWrap->getCollisionShape()->isCompound()); + btAssert(col1ObjWrap->getCollisionShape()->isCompound()); const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape()); const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape()); @@ -293,7 +296,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb const btDbvt* tree1 = compoundShape1->getDynamicAabbTree(); if (!tree0 || !tree1) { - return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut); + return btCompoundCollisionAlgorithm::processCollision(body0Wrap, body1Wrap, dispatchInfo, resultOut); } ///btCompoundShape might have changed: ////make sure the internal child collision algorithm caches are still valid @@ -303,24 +306,26 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb removeChildAlgorithms(); m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); - } - ///we need to refresh all contact manifolds ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm { int i; btManifoldArray manifoldArray; +#ifdef USE_LOCAL_STACK + btPersistentManifold localManifolds[4]; + manifoldArray.initializeFromBuffer(&localManifolds, 0, 4); +#endif btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - for (i=0;i<pairs.size();i++) + for (i = 0; i < pairs.size(); i++) { if (pairs[i].m_userPointer) { - btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer; + btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer; algo->getAllContactManifolds(manifoldArray); - for (int m=0;m<manifoldArray.size();m++) + for (int m = 0; m < manifoldArray.size(); m++) { if (manifoldArray[m]->getNumContacts()) { @@ -334,93 +339,75 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb } } + btCompoundCompoundLeafCallback callback(col0ObjWrap, col1ObjWrap, this->m_dispatcher, dispatchInfo, resultOut, this->m_childCollisionAlgorithmCache, m_sharedManifold); - - - btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold); - - - const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform(); - MycollideTT(tree0->m_root,tree1->m_root,xform,&callback); + const btTransform xform = col0ObjWrap->getWorldTransform().inverse() * col1ObjWrap->getWorldTransform(); + MycollideTT(tree0->m_root, tree1->m_root, xform, &callback, resultOut->m_closestPointDistanceThreshold); //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs); //remove non-overlapping child pairs { - btAssert(m_removePairs.size()==0); + btAssert(m_removePairs.size() == 0); //iterate over all children, perform an AABB check inside ProcessChildShape btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); - + int i; - btManifoldArray manifoldArray; - - - - - - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - - for (i=0;i<pairs.size();i++) + btManifoldArray manifoldArray; + + btVector3 aabbMin0, aabbMax0, aabbMin1, aabbMax1; + + for (i = 0; i < pairs.size(); i++) { if (pairs[i].m_userPointer) { btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer; { - btTransform orgTrans0; const btCollisionShape* childShape0 = 0; - - btTransform newChildWorldTrans0; - btTransform orgInterpolationTrans0; + + btTransform newChildWorldTrans0; childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA); - orgTrans0 = col0ObjWrap->getWorldTransform(); - orgInterpolationTrans0 = col0ObjWrap->getWorldTransform(); const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA); - newChildWorldTrans0 = orgTrans0*childTrans0 ; - childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); + newChildWorldTrans0 = col0ObjWrap->getWorldTransform() * childTrans0; + childShape0->getAabb(newChildWorldTrans0, aabbMin0, aabbMax0); } - + btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold); + aabbMin0 -= thresholdVec; + aabbMax0 += thresholdVec; { - btTransform orgInterpolationTrans1; const btCollisionShape* childShape1 = 0; - btTransform orgTrans1; - btTransform newChildWorldTrans1; + btTransform newChildWorldTrans1; childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB); - orgTrans1 = col1ObjWrap->getWorldTransform(); - orgInterpolationTrans1 = col1ObjWrap->getWorldTransform(); const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB); - newChildWorldTrans1 = orgTrans1*childTrans1 ; - childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); + newChildWorldTrans1 = col1ObjWrap->getWorldTransform() * childTrans1; + childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1); } - - - if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + aabbMin1 -= thresholdVec; + aabbMax1 += thresholdVec; + + if (!TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) { algo->~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(algo); - m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB)); + m_removePairs.push_back(btSimplePair(pairs[i].m_indexA, pairs[i].m_indexB)); } } } - for (int i=0;i<m_removePairs.size();i++) + for (int i = 0; i < m_removePairs.size(); i++) { - m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB); + m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA, m_removePairs[i].m_indexB); } m_removePairs.clear(); } - } -btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { btAssert(0); return 0.f; - } - - - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h index 06a762f209d..a940d840e0c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h @@ -33,57 +33,50 @@ class btDispatcher; class btCollisionObject; class btCollisionShape; -typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1); + extern btShapePairCallback gCompoundCompoundChildShapePairCallback; /// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes -class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm +class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm { - - class btHashedSimplePairCache* m_childCollisionAlgorithmCache; + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; btSimplePairArray m_removePairs; + int m_compoundShapeRevision0; //to keep track of changes, so that childAlgorithm array can be updated + int m_compoundShapeRevision1; - int m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated - int m_compoundShapeRevision1; - - void removeChildAlgorithms(); - -// void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + void removeChildAlgorithms(); -public: + // void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); - btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); +public: + btCompoundCompoundCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); virtual ~btCompoundCompoundCollisionAlgorithm(); - + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void getAllContactManifolds(btManifoldArray& manifoldArray); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray); - - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); - return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + return new (mem) btCompoundCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, false); } }; - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); - return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + return new (mem) btCompoundCompoundCollisionAlgorithm(ci, body0Wrap, body1Wrap, true); } }; - }; -#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H +#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp index 1cb3d2e7a14..9087f843985 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp @@ -22,7 +22,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btCapsuleShape.h" - #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" @@ -34,8 +33,6 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" - - #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -45,31 +42,28 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) +btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) { m_simplexSolver = simplexSolver; m_pdSolver = pdSolver; } -btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() -{ +btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() +{ } -btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_simplexSolver(simplexSolver), -m_pdSolver(pdSolver), -m_ownManifold (false), -m_manifoldPtr(mf), -m_lowLevelOfDetail(false) +btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_simplexSolver(simplexSolver), + m_pdSolver(pdSolver), + m_ownManifold(false), + m_manifoldPtr(mf), + m_lowLevelOfDetail(false) { (void)body0Wrap; (void)body1Wrap; } - - - btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm() { if (m_ownManifold) @@ -79,26 +73,22 @@ btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm() } } -void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel) +void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel) { m_lowLevelOfDetail = useLowLevel; } - - extern btScalar gContactBreakingThreshold; - // // Convex-Convex collision algorithm // -void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvex2dConvex2dAlgorithm ::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - if (!m_manifoldPtr) { //swapped? - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } resultOut->setPersistentManifold(m_manifoldPtr); @@ -106,49 +96,41 @@ void btConvex2dConvex2dAlgorithm ::processCollision (const btCollisionObjectWrap //comment-out next line to test multi-contact generation //resultOut->getPersistentManifold()->clearManifold(); - const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape()); const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape()); - btVector3 normalOnB; - btVector3 pointOnBWorld; + btVector3 normalOnB; + btVector3 pointOnBWorld; { - - btGjkPairDetector::ClosestPointInput input; - btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); + btGjkPairDetector gjkPairDetector(min0, min1, m_simplexSolver, m_pdSolver); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.setMinkowskiA(min0); gjkPairDetector.setMinkowskiB(min1); { input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); - input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; + input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared; } input.m_transformA = body0Wrap->getWorldTransform(); input.m_transformB = body1Wrap->getWorldTransform(); - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); - btVector3 v0,v1; + btVector3 v0, v1; btVector3 sepNormalWorldSpace; - } if (m_ownManifold) { resultOut->refreshContactPoints(); } - } - - - -btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; @@ -158,7 +140,6 @@ btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* c ///col0->m_worldTransform, btScalar resultFraction = btScalar(1.); - btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); @@ -166,77 +147,65 @@ btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* c squareMot1 < col1->getCcdSquareMotionThreshold()) return resultFraction; - //An adhoc way of testing the Continuous Collision Detection algorithms //One object is approximated as a sphere, to simplify things //Starting in penetration should report no time of impact //For proper CCD, better accuracy and handling of 'allowed' penetration should be added //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) - /// Convex0 against sphere for Convex1 { btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape()); - btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); + btGjkConvexCast ccd1(convex0, &sphere1, &voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(), + col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result)) { - //store result.m_fraction in both bodies - if (col0->getHitFraction()> result.m_fraction) - col0->setHitFraction( result.m_fraction ); + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction(result.m_fraction); if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); + col1->setHitFraction(result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; - } - - - - } /// Sphere (for convex0) against Convex1 { btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape()); - btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); + btGjkConvexCast ccd1(&sphere0, convex1, &voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(), + col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result)) { - //store result.m_fraction in both bodies - if (col0->getHitFraction() > result.m_fraction) - col0->setHitFraction( result.m_fraction); + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction(result.m_fraction); if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); + col1->setHitFraction(result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; - } } return resultFraction; - } - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h index 24d13367786..9fca463fbe4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h @@ -23,70 +23,61 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil +#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil class btConvexPenetrationDepthSolver; - ///The convex2dConvex2dAlgorithm collision algorithm support 2d collision detection for btConvex2dShape ///Currently it requires the btMinkowskiPenetrationDepthSolver, it has support for 2d penetration depth computation class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm { - btSimplexSolverInterface* m_simplexSolver; + btSimplexSolverInterface* m_simplexSolver; btConvexPenetrationDepthSolver* m_pdSolver; - - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_lowLevelOfDetail; - -public: - - btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_lowLevelOfDetail; +public: + btConvex2dConvex2dAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); virtual ~btConvex2dConvex2dAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { ///should we use m_ownManifold to avoid adding duplicates? if (m_manifoldPtr && m_ownManifold) manifoldArray.push_back(m_manifoldPtr); } + void setLowLevelOfDetail(bool useLowLevel); - void setLowLevelOfDetail(bool useLowLevel); - - - const btPersistentManifold* getManifold() + const btPersistentManifold* getManifold() { return m_manifoldPtr; } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - - btConvexPenetrationDepthSolver* m_pdSolver; - btSimplexSolverInterface* m_simplexSolver; + btConvexPenetrationDepthSolver* m_pdSolver; + btSimplexSolverInterface* m_simplexSolver; int m_numPerturbationIterations; int m_minimumPointsPerturbationThreshold; - CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); - + CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); + virtual ~CreateFunc(); - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm)); - return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + return new (mem) btConvex2dConvex2dAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_simplexSolver, m_pdSolver, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold); } }; - - }; -#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H +#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 912a5285568..e50f85e2bbe 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btConvexConcaveCollisionAlgorithm.h" +#include "LinearMath/btQuickprof.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -26,11 +26,12 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" +#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h" -btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_isSwapped(isSwapped), -m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped) +btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_btConvexTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped), + m_isSwapped(isSwapped) { } @@ -38,7 +39,7 @@ btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm() { } -void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_btConvexTriangleCallback.m_manifoldPtr) { @@ -46,55 +47,46 @@ void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& } } - -btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped): - m_dispatcher(dispatcher), - m_dispatchInfoPtr(0) +btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher), + m_dispatchInfoPtr(0) { - m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap; - m_triBodyWrap = isSwapped? body0Wrap:body1Wrap; - - // - // create the manifold from the dispatcher 'manifold pool' - // - m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject()); + m_convexBodyWrap = isSwapped ? body1Wrap : body0Wrap; + m_triBodyWrap = isSwapped ? body0Wrap : body1Wrap; + + // + // create the manifold from the dispatcher 'manifold pool' + // + m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(), m_triBodyWrap->getCollisionObject()); - clearCache(); + clearCache(); } btConvexTriangleCallback::~btConvexTriangleCallback() { clearCache(); - m_dispatcher->releaseManifold( m_manifoldPtr ); - + m_dispatcher->releaseManifold(m_manifoldPtr); } - -void btConvexTriangleCallback::clearCache() +void btConvexTriangleCallback::clearCache() { m_dispatcher->clearManifold(m_manifoldPtr); } - -void btConvexTriangleCallback::processTriangle(btVector3* triangle,int -partId, int triangleIndex) +void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex) { + BT_PROFILE("btConvexTriangleCallback::processTriangle"); if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax)) { return; } - //just for debugging purposes - //printf("triangle %d",m_triangleCount++); - - + //just for debugging purposes + //printf("triangle %d",m_triangleCount++); btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = m_dispatcher; - - #if 0 ///debug drawing of the overlapping triangles @@ -108,52 +100,55 @@ partId, int triangleIndex) m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); } #endif - + if (m_convexBodyWrap->getCollisionShape()->isConvex()) { - btTriangleShape tm(triangle[0],triangle[1],triangle[2]); + btTriangleShape tm(triangle[0], triangle[1], triangle[2]); tm.setMargin(m_collisionMarginTriangle); - - - btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform? - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr); + btCollisionObjectWrapper triObWrap(m_triBodyWrap, &tm, m_triBodyWrap->getCollisionObject(), m_triBodyWrap->getWorldTransform(), partId, triangleIndex); //correct transform? + btCollisionAlgorithm* colAlgo = 0; + + if (m_resultOut->m_closestPointDistanceThreshold > 0) + { + colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); + } + else + { + colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS); + } const btCollisionObjectWrapper* tmpWrap = 0; if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) { tmpWrap = m_resultOut->getBody0Wrap(); m_resultOut->setBody0Wrap(&triObWrap); - m_resultOut->setShapeIdentifiersA(partId,triangleIndex); + m_resultOut->setShapeIdentifiersA(partId, triangleIndex); } else { tmpWrap = m_resultOut->getBody1Wrap(); m_resultOut->setBody1Wrap(&triObWrap); - m_resultOut->setShapeIdentifiersB(partId,triangleIndex); + m_resultOut->setShapeIdentifiersB(partId, triangleIndex); } - - colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut); + + colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut); if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) { m_resultOut->setBody0Wrap(tmpWrap); - } else + } + else { m_resultOut->setBody1Wrap(tmpWrap); } - - colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); } - } - - -void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut) +void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut) { m_convexBodyWrap = convexBodyWrap; m_triBodyWrap = triBodyWrap; @@ -167,65 +162,120 @@ void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform(); const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape()); //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); - convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); - btScalar extraMargin = collisionMarginTriangle; - btVector3 extra(extraMargin,extraMargin,extraMargin); + convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax); + btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold; + + btVector3 extra(extraMargin, extraMargin, extraMargin); m_aabbMax += extra; m_aabbMin -= extra; - } void btConvexConcaveCollisionAlgorithm::clearCache() { m_btConvexTriangleCallback.clearCache(); - } -void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - - + BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision"); + const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap; const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap; if (triBodyWrap->getCollisionShape()->isConcave()) { + if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE) + { + btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape(); + if (convexBodyWrap->getCollisionShape()->isConvex()) + { + btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape(); + btAlignedObjectArray<btVector3> queryVertices; + + if (convex->isPolyhedral()) + { + btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; + for (int v = 0; v < poly->getNumVertices(); v++) + { + btVector3 vtx; + poly->getVertex(v, vtx); + queryVertices.push_back(vtx); + } + } + btScalar maxDist = SIMD_EPSILON; + + if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) + { + queryVertices.push_back(btVector3(0, 0, 0)); + btSphereShape* sphere = (btSphereShape*)convex; + maxDist = sphere->getRadius() + SIMD_EPSILON; + } + if (queryVertices.size()) + { + resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); + //m_btConvexTriangleCallback.m_manifoldPtr->clearManifold(); + + btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; + for (int v = 0; v < queryVertices.size(); v++) + { + const btVector3& vtx = queryVertices[v]; + btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform() * vtx; + btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace); + + btVector3 normalLocal; + btScalar dist; + if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal)) + { + if (dist <= maxDist) + { + normalLocal.safeNormalize(); + btVector3 normal = triBodyWrap->getWorldTransform().getBasis() * normalLocal; + + if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) + { + btSphereShape* sphere = (btSphereShape*)convex; + dist -= sphere->getRadius(); + vtxWorldSpace -= sphere->getRadius() * normal; + } + resultOut->addContactPoint(normal, vtxWorldSpace - normal * dist, dist); + } + } + } + resultOut->refreshContactPoints(); + } + } + } + else + { + const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triBodyWrap->getCollisionShape()); + if (convexBodyWrap->getCollisionShape()->isConvex()) + { + btScalar collisionMarginTriangle = concaveShape->getMargin(); - - const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape()); - - if (convexBodyWrap->getCollisionShape()->isConvex()) - { - btScalar collisionMarginTriangle = concaveShape->getMargin(); - - resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); - m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut); + resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); + m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, convexBodyWrap, triBodyWrap, resultOut); - m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject()); + m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(), triBodyWrap->getCollisionObject()); - concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax()); - - resultOut->refreshContactPoints(); + concaveShape->processAllTriangles(&m_btConvexTriangleCallback, m_btConvexTriangleCallback.getAabbMin(), m_btConvexTriangleCallback.getAabbMax()); - m_btConvexTriangleCallback.clearWrapperData(); - + resultOut->refreshContactPoints(); + + m_btConvexTriangleCallback.clearWrapperData(); + } } - } - } - -btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; btCollisionObject* convexbody = m_isSwapped ? body1 : body0; btCollisionObject* triBody = m_isSwapped ? body0 : body1; - //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) //only perform CCD above a certain threshold, this prevents blocking on the long run @@ -244,27 +294,26 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); - struct LocalTriangleSphereCastCallback : public btTriangleCallback + struct LocalTriangleSphereCastCallback : public btTriangleCallback { btTransform m_ccdSphereFromTrans; btTransform m_ccdSphereToTrans; - btTransform m_meshTransform; + btTransform m_meshTransform; - btScalar m_ccdSphereRadius; - btScalar m_hitFraction; - + btScalar m_ccdSphereRadius; + btScalar m_hitFraction; - LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) - :m_ccdSphereFromTrans(from), - m_ccdSphereToTrans(to), - m_ccdSphereRadius(ccdSphereRadius), - m_hitFraction(hitFraction) - { + LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction) + : m_ccdSphereFromTrans(from), + m_ccdSphereToTrans(to), + m_ccdSphereRadius(ccdSphereRadius), + m_hitFraction(hitFraction) + { } - - + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { + BT_PROFILE("processTriangle"); (void)partId; (void)triangleIndex; //do a swept sphere for now @@ -272,29 +321,23 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj ident.setIdentity(); btConvexCast::CastResult castResult; castResult.m_fraction = m_hitFraction; - btSphereShape pointShape(m_ccdSphereRadius); - btTriangleShape triShape(triangle[0],triangle[1],triangle[2]); - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); + btSphereShape pointShape(m_ccdSphereRadius); + btTriangleShape triShape(triangle[0], triangle[1], triangle[2]); + btVoronoiSimplexSolver simplexSolver; + btSubsimplexConvexCast 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 (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans, + ident, ident, castResult)) { if (m_hitFraction > castResult.m_fraction) m_hitFraction = castResult.m_fraction; } - } - }; - - - - if (triBody->getCollisionShape()->isConcave()) { btVector3 rayAabbMin = convexFromLocal.getOrigin(); @@ -302,33 +345,30 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj btVector3 rayAabbMax = convexFromLocal.getOrigin(); rayAabbMax.setMax(convexToLocal.getOrigin()); btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); - rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); + rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0); + rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0); - btScalar curHitFraction = btScalar(1.); //is this available? - LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, - convexbody->getCcdSweptSphereRadius(),curHitFraction); + btScalar curHitFraction = btScalar(1.); //is this available? + LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal, + convexbody->getCcdSweptSphereRadius(), curHitFraction); raycastCallback.m_hitFraction = convexbody->getHitFraction(); btCollisionObject* concavebody = triBody; - btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); - + btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape(); + if (triangleMesh) { - triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); + triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax); } - - if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) { - convexbody->setHitFraction( raycastCallback.m_hitFraction); + convexbody->setHitFraction(raycastCallback.m_hitFraction); return raycastCallback.m_hitFraction; } } return btScalar(1.); - } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h index e90d06eb191..b72e4029811 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -26,38 +26,40 @@ class btDispatcher; #include "btCollisionCreateFunc.h" ///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called. -class btConvexTriangleCallback : public btTriangleCallback +ATTRIBUTE_ALIGNED16(class) +btConvexTriangleCallback : public btTriangleCallback { + btVector3 m_aabbMin; + btVector3 m_aabbMax; + const btCollisionObjectWrapper* m_convexBodyWrap; const btCollisionObjectWrapper* m_triBodyWrap; - btVector3 m_aabbMin; - btVector3 m_aabbMax ; - - btManifoldResult* m_resultOut; - btDispatcher* m_dispatcher; + btDispatcher* m_dispatcher; const btDispatcherInfo* m_dispatchInfoPtr; btScalar m_collisionMarginTriangle; - + public: -int m_triangleCount; - - btPersistentManifold* m_manifoldPtr; + BT_DECLARE_ALIGNED_ALLOCATOR(); + + int m_triangleCount; - btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + btPersistentManifold* m_manifoldPtr; - void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut); + btConvexTriangleCallback(btDispatcher * dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); - void clearWrapperData() + void setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut); + + void clearWrapperData() { m_convexBodyWrap = 0; m_triBodyWrap = 0; } virtual ~btConvexTriangleCallback(); - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - + virtual void processTriangle(btVector3 * triangle, int partId, int triangleIndex); + void clearCache(); SIMD_FORCE_INLINE const btVector3& getAabbMin() const @@ -68,54 +70,48 @@ int m_triangleCount; { return m_aabbMax; } - }; - - - /// btConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes. -class btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm +ATTRIBUTE_ALIGNED16(class) +btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm { - - bool m_isSwapped; - btConvexTriangleCallback m_btConvexTriangleCallback; - + bool m_isSwapped; public: + BT_DECLARE_ALIGNED_ALLOCATOR(); - btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); virtual ~btConvexConcaveCollisionAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); + + btScalar calculateTimeOfImpact(btCollisionObject * body0, btCollisionObject * body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void getAllContactManifolds(btManifoldArray & manifoldArray); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray); - - void clearCache(); + void clearCache(); - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm)); - return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + return new (mem) btConvexConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, false); } }; - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + struct SwappedCreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm)); - return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + return new (mem) btConvexConcaveCollisionAlgorithm(ci, body0Wrap, body1Wrap, true); } }; - }; -#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H +#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 7f2722aa463..b48d97f2b28 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: ///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance ///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums ///with reproduction case -//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1 +//#define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1 //#define ZERO_MARGIN #include "btConvexConvexAlgorithm.h" @@ -28,8 +28,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btCapsuleShape.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h" - - +#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -42,8 +41,6 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" - - #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -56,8 +53,6 @@ subject to the following restrictions: /////////// - - static SIMD_FORCE_INLINE void segmentsClosestPoints( btVector3& ptsVector, btVector3& offsetA, @@ -65,43 +60,49 @@ static SIMD_FORCE_INLINE void segmentsClosestPoints( btScalar& tA, btScalar& tB, const btVector3& translation, const btVector3& dirA, btScalar hlenA, - const btVector3& dirB, btScalar hlenB ) + const btVector3& dirB, btScalar hlenB) { // compute the parameters of the closest points on each line segment - btScalar dirA_dot_dirB = btDot(dirA,dirB); - btScalar dirA_dot_trans = btDot(dirA,translation); - btScalar dirB_dot_trans = btDot(dirB,translation); + btScalar dirA_dot_dirB = btDot(dirA, dirB); + btScalar dirA_dot_trans = btDot(dirA, translation); + btScalar dirB_dot_trans = btDot(dirB, translation); btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; - if ( denom == 0.0f ) { + if (denom == 0.0f) + { tA = 0.0f; - } else { - tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; - if ( tA < -hlenA ) + } + else + { + tA = (dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB) / denom; + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } tB = tA * dirA_dot_dirB - dirB_dot_trans; - if ( tB < -hlenB ) { + if (tB < -hlenB) + { tB = -hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; - } else if ( tB > hlenB ) { + } + else if (tB > hlenB) + { tB = hlenB; tA = tB * dirA_dot_dirB + dirA_dot_trans; - if ( tA < -hlenA ) + if (tA < -hlenA) tA = -hlenA; - else if ( tA > hlenA ) + else if (tA > hlenA) tA = hlenA; } @@ -113,19 +114,18 @@ static SIMD_FORCE_INLINE void segmentsClosestPoints( ptsVector = translation - offsetA + offsetB; } - static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance( btVector3& normalOnB, btVector3& pointOnB, btScalar capsuleLengthA, - btScalar capsuleRadiusA, + btScalar capsuleRadiusA, btScalar capsuleLengthB, - btScalar capsuleRadiusB, + btScalar capsuleRadiusB, int capsuleAxisA, int capsuleAxisB, const btTransform& transformA, const btTransform& transformB, - btScalar distanceThreshold ) + btScalar distanceThreshold) { btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA); btVector3 translationA = transformA.getOrigin(); @@ -138,80 +138,66 @@ static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance( // compute the closest points of the capsule line segments - btVector3 ptsVector; // the vector between the closest points - - btVector3 offsetA, offsetB; // offsets from segment centers to their closest points - btScalar tA, tB; // parameters on line segment + btVector3 ptsVector; // the vector between the closest points + + btVector3 offsetA, offsetB; // offsets from segment centers to their closest points + btScalar tA, tB; // parameters on line segment - segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation, - directionA, capsuleLengthA, directionB, capsuleLengthB ); + segmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB, translation, + directionA, capsuleLengthA, directionB, capsuleLengthB); btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB; - if ( distance > distanceThreshold ) + if (distance > distanceThreshold) return distance; btScalar lenSqr = ptsVector.length2(); - if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON)) + if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON)) { //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA' btVector3 q; - btPlaneSpace1(directionA,normalOnB,q); - } else + btPlaneSpace1(directionA, normalOnB, q); + } + else { // compute the contact normal - normalOnB = ptsVector*-btRecipSqrt(lenSqr); + normalOnB = ptsVector * -btRecipSqrt(lenSqr); } - pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB; + pointOnB = transformB.getOrigin() + offsetB + normalOnB * capsuleRadiusB; return distance; } - - - - - - ////////// - - - - -btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) +btConvexConvexAlgorithm::CreateFunc::CreateFunc(btConvexPenetrationDepthSolver* pdSolver) { m_numPerturbationIterations = 0; m_minimumPointsPerturbationThreshold = 3; - m_simplexSolver = simplexSolver; m_pdSolver = pdSolver; } -btConvexConvexAlgorithm::CreateFunc::~CreateFunc() -{ +btConvexConvexAlgorithm::CreateFunc::~CreateFunc() +{ } -btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_simplexSolver(simplexSolver), -m_pdSolver(pdSolver), -m_ownManifold (false), -m_manifoldPtr(mf), -m_lowLevelOfDetail(false), +btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_pdSolver(pdSolver), + m_ownManifold(false), + m_manifoldPtr(mf), + m_lowLevelOfDetail(false), #ifdef USE_SEPDISTANCE_UTIL2 -m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(), - (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()), + m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(), + (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()), #endif -m_numPerturbationIterations(numPerturbationIterations), -m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) + m_numPerturbationIterations(numPerturbationIterations), + m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) { (void)body0Wrap; (void)body1Wrap; } - - - btConvexConvexAlgorithm::~btConvexConvexAlgorithm() { if (m_ownManifold) @@ -221,339 +207,466 @@ btConvexConvexAlgorithm::~btConvexConvexAlgorithm() } } -void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel) +void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel) { m_lowLevelOfDetail = useLowLevel; } - struct btPerturbedContactResult : public btManifoldResult { btManifoldResult* m_originalManifoldResult; btTransform m_transformA; btTransform m_transformB; - btTransform m_unPerturbedTransform; - bool m_perturbA; - btIDebugDraw* m_debugDrawer; - - - btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer) - :m_originalManifoldResult(originalResult), - m_transformA(transformA), - m_transformB(transformB), - m_unPerturbedTransform(unPerturbedTransform), - m_perturbA(perturbA), - m_debugDrawer(debugDrawer) + btTransform m_unPerturbedTransform; + bool m_perturbA; + btIDebugDraw* m_debugDrawer; + + btPerturbedContactResult(btManifoldResult* originalResult, const btTransform& transformA, const btTransform& transformB, const btTransform& unPerturbedTransform, bool perturbA, btIDebugDraw* debugDrawer) + : m_originalManifoldResult(originalResult), + m_transformA(transformA), + m_transformB(transformB), + m_unPerturbedTransform(unPerturbedTransform), + m_perturbA(perturbA), + m_debugDrawer(debugDrawer) { } - virtual ~ btPerturbedContactResult() + virtual ~btPerturbedContactResult() { } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth) + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar orgDepth) { - btVector3 endPt,startPt; + btVector3 endPt, startPt; btScalar newDepth; btVector3 newNormal; if (m_perturbA) { - btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth; - endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg); - newDepth = (endPt - pointInWorld).dot(normalOnBInWorld); - startPt = endPt+normalOnBInWorld*newDepth; - } else + btVector3 endPtOrg = pointInWorld + normalOnBInWorld * orgDepth; + endPt = (m_unPerturbedTransform * m_transformA.inverse())(endPtOrg); + newDepth = (endPt - pointInWorld).dot(normalOnBInWorld); + startPt = endPt - normalOnBInWorld * newDepth; + } + else { - endPt = pointInWorld + normalOnBInWorld*orgDepth; - startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld); - newDepth = (endPt - startPt).dot(normalOnBInWorld); - + endPt = pointInWorld + normalOnBInWorld * orgDepth; + startPt = (m_unPerturbedTransform * m_transformB.inverse())(pointInWorld); + newDepth = (endPt - startPt).dot(normalOnBInWorld); } //#define DEBUG_CONTACTS 1 #ifdef DEBUG_CONTACTS - m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0)); - m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0)); - m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1)); -#endif //DEBUG_CONTACTS + m_debugDrawer->drawLine(startPt, endPt, btVector3(1, 0, 0)); + m_debugDrawer->drawSphere(startPt, 0.05, btVector3(0, 1, 0)); + m_debugDrawer->drawSphere(endPt, 0.05, btVector3(0, 0, 1)); +#endif //DEBUG_CONTACTS - - m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth); + m_originalManifoldResult->addContactPoint(normalOnBInWorld, startPt, newDepth); } - }; extern btScalar gContactBreakingThreshold; - // // Convex-Convex collision algorithm // -void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - if (!m_manifoldPtr) { //swapped? - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } resultOut->setPersistentManifold(m_manifoldPtr); //comment-out next line to test multi-contact generation //resultOut->getPersistentManifold()->clearManifold(); - const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape()); const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape()); - btVector3 normalOnB; - btVector3 pointOnBWorld; + btVector3 normalOnB; + btVector3 pointOnBWorld; #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) { - btCapsuleShape* capsuleA = (btCapsuleShape*) min0; - btCapsuleShape* capsuleB = (btCapsuleShape*) min1; - // btVector3 localScalingA = capsuleA->getLocalScaling(); - // btVector3 localScalingB = capsuleB->getLocalScaling(); - - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); - - btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), - capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(), - body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); - - if (dist<threshold) + //m_manifoldPtr->clearManifold(); + + btCapsuleShape* capsuleA = (btCapsuleShape*)min0; + btCapsuleShape* capsuleB = (btCapsuleShape*)min1; + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; + + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(), + capsuleB->getHalfHeight(), capsuleB->getRadius(), capsuleA->getUpAxis(), capsuleB->getUpAxis(), + body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); + + if (dist < threshold) { - btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); - resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); + btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB, pointOnBWorld, dist); } resultOut->refreshContactPoints(); return; } -#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER + if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == SPHERE_SHAPE_PROXYTYPE)) + { + //m_manifoldPtr->clearManifold(); + btCapsuleShape* capsuleA = (btCapsuleShape*)min0; + btSphereShape* capsuleB = (btSphereShape*)min1; + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; -#ifdef USE_SEPDISTANCE_UTIL2 - if (dispatchInfo.m_useConvexConservativeDistanceUtil) - { - m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); - } + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(), + 0., capsuleB->getRadius(), capsuleA->getUpAxis(), 1, + body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); - if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) -#endif //USE_SEPDISTANCE_UTIL2 + if (dist < threshold) + { + btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB, pointOnBWorld, dist); + } + resultOut->refreshContactPoints(); + return; + } + if ((min0->getShapeType() == SPHERE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) { + //m_manifoldPtr->clearManifold(); + + btSphereShape* capsuleA = (btSphereShape*)min0; + btCapsuleShape* capsuleB = (btCapsuleShape*)min1; - - btGjkPairDetector::ClosestPointInput input; + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; - btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); - //TODO: if (dispatchInfo.m_useContinuous) - gjkPairDetector.setMinkowskiA(min0); - gjkPairDetector.setMinkowskiB(min1); + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, 0., capsuleA->getRadius(), + capsuleB->getHalfHeight(), capsuleB->getRadius(), 1, capsuleB->getUpAxis(), + body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); + + if (dist < threshold) + { + btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB, pointOnBWorld, dist); + } + resultOut->refreshContactPoints(); + return; + } +#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { - input.m_maximumDistanceSquared = BT_LARGE_FLOAT; - } else -#endif //USE_SEPDISTANCE_UTIL2 - { - //if (dispatchInfo.m_convexMaxDistanceUseCPT) - //{ - // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); - //} else - //{ - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); -// } - - input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; + m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(), body1->getWorldTransform()); } - input.m_transformA = body0Wrap->getWorldTransform(); - input.m_transformB = body1Wrap->getWorldTransform(); - - + if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance() <= 0.f) +#endif //USE_SEPDISTANCE_UTIL2 - + { + btGjkPairDetector::ClosestPointInput input; + btVoronoiSimplexSolver simplexSolver; + btGjkPairDetector gjkPairDetector(min0, min1, &simplexSolver, m_pdSolver); + //TODO: if (dispatchInfo.m_useContinuous) + gjkPairDetector.setMinkowskiA(min0); + gjkPairDetector.setMinkowskiB(min1); #ifdef USE_SEPDISTANCE_UTIL2 - btScalar sepDist = 0.f; - if (dispatchInfo.m_useConvexConservativeDistanceUtil) - { - sepDist = gjkPairDetector.getCachedSeparatingDistance(); - if (sepDist>SIMD_EPSILON) + if (dispatchInfo.m_useConvexConservativeDistanceUtil) { - sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; - //now perturbe directions to get multiple contact points - + input.m_maximumDistanceSquared = BT_LARGE_FLOAT; + } + else +#endif //USE_SEPDISTANCE_UTIL2 + { + //if (dispatchInfo.m_convexMaxDistanceUseCPT) + //{ + // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); + //} else + //{ + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold() + resultOut->m_closestPointDistanceThreshold; + // } + + input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared; } - } -#endif //USE_SEPDISTANCE_UTIL2 - - if (min0->isPolyhedral() && min1->isPolyhedral()) - { + input.m_transformA = body0Wrap->getWorldTransform(); + input.m_transformB = body1Wrap->getWorldTransform(); - struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result +#ifdef USE_SEPDISTANCE_UTIL2 + btScalar sepDist = 0.f; + if (dispatchInfo.m_useConvexConservativeDistanceUtil) { - virtual void setShapeIdentifiersA(int partId0,int index0){} - virtual void setShapeIdentifiersB(int partId1,int index1){} - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + sepDist = gjkPairDetector.getCachedSeparatingDistance(); + if (sepDist > SIMD_EPSILON) { + sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; + //now perturbe directions to get multiple contact points } - }; + } +#endif //USE_SEPDISTANCE_UTIL2 - - struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result + if (min0->isPolyhedral() && min1->isPolyhedral()) { - btDiscreteCollisionDetectorInterface::Result* m_originalResult; - btVector3 m_reportedNormalOnWorld; - btScalar m_marginOnA; - btScalar m_marginOnB; - btScalar m_reportedDistance; - - bool m_foundResult; - btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB) - :m_originalResult(result), - m_marginOnA(marginOnA), - m_marginOnB(marginOnB), - m_foundResult(false) - { - } - - virtual void setShapeIdentifiersA(int partId0,int index0){} - virtual void setShapeIdentifiersB(int partId1,int index1){} - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg) + struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result { - m_reportedDistance = depthOrg; - m_reportedNormalOnWorld = normalOnBInWorld; - - btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB; - m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB); - if (m_reportedDistance<0.f) + btVector3 m_normalOnBInWorld; + btVector3 m_pointInWorld; + btScalar m_depth; + bool m_hasContact; + + btDummyResult() + : m_hasContact(false) { - m_foundResult = true; } - m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance); - } - }; - - btDummyResult dummy; - -///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it + virtual void setShapeIdentifiersA(int partId0, int index0) {} + virtual void setShapeIdentifiersB(int partId1, int index1) {} + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) + { + m_hasContact = true; + m_normalOnBInWorld = normalOnBInWorld; + m_pointInWorld = pointInWorld; + m_depth = depth; + } + }; - btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin(); - btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin(); + struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result + { + btDiscreteCollisionDetectorInterface::Result* m_originalResult; + btVector3 m_reportedNormalOnWorld; + btScalar m_marginOnA; + btScalar m_marginOnB; + btScalar m_reportedDistance; + + bool m_foundResult; + btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB) + : m_originalResult(result), + m_marginOnA(marginOnA), + m_marginOnB(marginOnB), + m_foundResult(false) + { + } - btWithoutMarginResult withoutMargin(resultOut, min0Margin,min1Margin); + virtual void setShapeIdentifiersA(int partId0, int index0) {} + virtual void setShapeIdentifiersB(int partId1, int index1) {} + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorldOrg, btScalar depthOrg) + { + m_reportedDistance = depthOrg; + m_reportedNormalOnWorld = normalOnBInWorld; - btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0; - btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1; - if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron()) - { + btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld * m_marginOnB; + m_reportedDistance = depthOrg + (m_marginOnA + m_marginOnB); + if (m_reportedDistance < 0.f) + { + m_foundResult = true; + } + m_originalResult->addContactPoint(normalOnBInWorld, adjustedPointB, m_reportedDistance); + } + }; + btDummyResult dummy; - + ///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + btScalar min0Margin = min0->getShapeType() == BOX_SHAPE_PROXYTYPE ? 0.f : min0->getMargin(); + btScalar min1Margin = min1->getShapeType() == BOX_SHAPE_PROXYTYPE ? 0.f : min1->getMargin(); - btScalar minDist = -1e30f; - btVector3 sepNormalWorldSpace; - bool foundSepAxis = true; + btWithoutMarginResult withoutMargin(resultOut, min0Margin, min1Margin); - if (dispatchInfo.m_enableSatConvex) - { - foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( - *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), - sepNormalWorldSpace,*resultOut); - } else + btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*)min0; + btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*)min1; + if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron()) { + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; + + btScalar minDist = -1e30f; + btVector3 sepNormalWorldSpace; + bool foundSepAxis = true; + + if (dispatchInfo.m_enableSatConvex) + { + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), + body1Wrap->getWorldTransform(), + sepNormalWorldSpace, *resultOut); + } + else + { #ifdef ZERO_MARGIN - gjkPairDetector.setIgnoreMargin(true); - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + gjkPairDetector.setIgnoreMargin(true); + gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); #else + gjkPairDetector.getClosestPoints(input, withoutMargin, dispatchInfo.m_debugDraw); + //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); +#endif //ZERO_MARGIN + //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + //if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld; //gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); + minDist = withoutMargin.m_reportedDistance; //gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin(); - gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw); - //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); -#endif //ZERO_MARGIN - //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - //if (l2>SIMD_EPSILON) - { - sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); - //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); - minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin(); - #ifdef ZERO_MARGIN - foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f; + foundSepAxis = true; //gjkPairDetector.getCachedSeparatingDistance()<0.f; #else - foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin()); + foundSepAxis = withoutMargin.m_foundResult && minDist < 0; //-(min0->getMargin()+min1->getMargin()); #endif + } } - } - if (foundSepAxis) - { - -// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + if (foundSepAxis) + { + // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); - btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut); - + worldVertsB1.resize(0); + btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), + body1Wrap->getWorldTransform(), minDist - threshold, threshold, worldVertsB1, worldVertsB2, + *resultOut); + } + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + return; } - if (m_ownManifold) + else { - resultOut->refreshContactPoints(); - } - return; + //we can also deal with convex versus triangle (without connectivity data) + if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE) + { + btVertexArray worldSpaceVertices; + btTriangleShape* tri = (btTriangleShape*)polyhedronB; + worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[0]); + worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[1]); + worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[2]); - } else - { - //we can also deal with convex versus triangle (without connectivity data) - if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) - { + //tri->initializePolyhedralFeatures(); - btVertexArray vertices; - btTriangleShape* tri = (btTriangleShape*)polyhedronB; - vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]); - vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]); - vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]); - - //tri->initializePolyhedralFeatures(); + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + btVector3 sepNormalWorldSpace; + btScalar minDist = -1e30f; + btScalar maxDist = threshold; - btVector3 sepNormalWorldSpace; - btScalar minDist =-1e30f; - btScalar maxDist = threshold; - - bool foundSepAxis = false; - if (0) - { - polyhedronB->initializePolyhedralFeatures(); - foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( - *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), - sepNormalWorldSpace,*resultOut); - // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); - - } else - { + bool foundSepAxis = false; + bool useSatSepNormal = true; + + if (useSatSepNormal) + { +#if 0 + if (0) + { + //initializePolyhedralFeatures performs a convex hull computation, not needed for a single triangle + polyhedronB->initializePolyhedralFeatures(); + } else +#endif + { + btVector3 uniqueEdges[3] = {tri->m_vertices1[1] - tri->m_vertices1[0], + tri->m_vertices1[2] - tri->m_vertices1[1], + tri->m_vertices1[0] - tri->m_vertices1[2]}; + + uniqueEdges[0].normalize(); + uniqueEdges[1].normalize(); + uniqueEdges[2].normalize(); + + btConvexPolyhedron polyhedron; + polyhedron.m_vertices.push_back(tri->m_vertices1[2]); + polyhedron.m_vertices.push_back(tri->m_vertices1[0]); + polyhedron.m_vertices.push_back(tri->m_vertices1[1]); + + { + btFace combinedFaceA; + combinedFaceA.m_indices.push_back(0); + combinedFaceA.m_indices.push_back(1); + combinedFaceA.m_indices.push_back(2); + btVector3 faceNormal = uniqueEdges[0].cross(uniqueEdges[1]); + faceNormal.normalize(); + btScalar planeEq = 1e30f; + for (int v = 0; v < combinedFaceA.m_indices.size(); v++) + { + btScalar eq = tri->m_vertices1[combinedFaceA.m_indices[v]].dot(faceNormal); + if (planeEq > eq) + { + planeEq = eq; + } + } + combinedFaceA.m_plane[0] = faceNormal[0]; + combinedFaceA.m_plane[1] = faceNormal[1]; + combinedFaceA.m_plane[2] = faceNormal[2]; + combinedFaceA.m_plane[3] = -planeEq; + polyhedron.m_faces.push_back(combinedFaceA); + } + { + btFace combinedFaceB; + combinedFaceB.m_indices.push_back(0); + combinedFaceB.m_indices.push_back(2); + combinedFaceB.m_indices.push_back(1); + btVector3 faceNormal = -uniqueEdges[0].cross(uniqueEdges[1]); + faceNormal.normalize(); + btScalar planeEq = 1e30f; + for (int v = 0; v < combinedFaceB.m_indices.size(); v++) + { + btScalar eq = tri->m_vertices1[combinedFaceB.m_indices[v]].dot(faceNormal); + if (planeEq > eq) + { + planeEq = eq; + } + } + + combinedFaceB.m_plane[0] = faceNormal[0]; + combinedFaceB.m_plane[1] = faceNormal[1]; + combinedFaceB.m_plane[2] = faceNormal[2]; + combinedFaceB.m_plane[3] = -planeEq; + polyhedron.m_faces.push_back(combinedFaceB); + } + + polyhedron.m_uniqueEdges.push_back(uniqueEdges[0]); + polyhedron.m_uniqueEdges.push_back(uniqueEdges[1]); + polyhedron.m_uniqueEdges.push_back(uniqueEdges[2]); + polyhedron.initialize2(); + + polyhedronB->setPolyhedralFeatures(polyhedron); + } + + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), + body1Wrap->getWorldTransform(), + sepNormalWorldSpace, *resultOut); + // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + } + else + { #ifdef ZERO_MARGIN - gjkPairDetector.setIgnoreMargin(true); - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + gjkPairDetector.setIgnoreMargin(true); + gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); #else - gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); -#endif//ZERO_MARGIN - + gjkPairDetector.getClosestPoints(input, dummy, dispatchInfo.m_debugDraw); +#endif //ZERO_MARGIN + + if (dummy.m_hasContact && dummy.m_depth < 0) + { + if (foundSepAxis) + { + if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace) < 0.99) + { + printf("?\n"); + } + } + else + { + printf("!\n"); + } + sepNormalWorldSpace.setValue(0, 0, 1); // = dummy.m_normalOnBInWorld; + //minDist = dummy.m_depth; + foundSepAxis = true; + } +#if 0 btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if (l2>SIMD_EPSILON) { @@ -563,143 +676,132 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); foundSepAxis = true; } - } +#endif + } - - if (foundSepAxis) - { - btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut); - } - - - if (m_ownManifold) - { - resultOut->refreshContactPoints(); + if (foundSepAxis) + { + worldVertsB2.resize(0); + btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2, minDist - threshold, maxDist, *resultOut); + } + + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + + return; } - - return; } - } + gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); - } - - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects - //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects - - //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points - if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) - { - - int i; - btVector3 v0,v1; - btVector3 sepNormalWorldSpace; - btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - - if (l2>SIMD_EPSILON) + //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points + if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) { - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); - - btPlaneSpace1(sepNormalWorldSpace,v0,v1); - + int i; + btVector3 v0, v1; + btVector3 sepNormalWorldSpace; + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - bool perturbeA = true; - const btScalar angleLimit = 0.125f * SIMD_PI; - btScalar perturbeAngle; - btScalar radiusA = min0->getAngularMotionDisc(); - btScalar radiusB = min1->getAngularMotionDisc(); - if (radiusA < radiusB) - { - perturbeAngle = gContactBreakingThreshold /radiusA; - perturbeA = true; - } else + if (l2 > SIMD_EPSILON) { - perturbeAngle = gContactBreakingThreshold / radiusB; - perturbeA = false; - } - if ( perturbeAngle > angleLimit ) - perturbeAngle = angleLimit; + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis() * (1.f / l2); - btTransform unPerturbedTransform; - if (perturbeA) - { - unPerturbedTransform = input.m_transformA; - } else - { - unPerturbedTransform = input.m_transformB; - } - - for ( i=0;i<m_numPerturbationIterations;i++) - { - if (v0.length2()>SIMD_EPSILON) + 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) + { + perturbeAngle = gContactBreakingThreshold / radiusA; + perturbeA = true; + } + else { - btQuaternion perturbeRot(v0,perturbeAngle); - btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); - btQuaternion rotq(sepNormalWorldSpace,iterationAngle); - - + perturbeAngle = gContactBreakingThreshold / radiusB; + perturbeA = false; + } + if (perturbeAngle > angleLimit) + perturbeAngle = angleLimit; + + btTransform unPerturbedTransform; if (perturbeA) { - input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis()); - input.m_transformB = body1Wrap->getWorldTransform(); - #ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); - #endif //DEBUG_CONTACTS - } else + unPerturbedTransform = input.m_transformA; + } + else { - input.m_transformA = body0Wrap->getWorldTransform(); - input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis()); - #ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); - #endif + 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; i < m_numPerturbationIterations; i++) + { + if (v0.length2() > SIMD_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) * body0Wrap->getWorldTransform().getBasis()); + input.m_transformB = body1Wrap->getWorldTransform(); +#ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformA, 10.0); +#endif //DEBUG_CONTACTS + } + else + { + input.m_transformA = body0Wrap->getWorldTransform(); + input.m_transformB.setBasis(btMatrix3x3(rotq.inverse() * perturbeRot * rotq) * body1Wrap->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); + } } } } - } - - #ifdef USE_SEPDISTANCE_UTIL2 - if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) - { - m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform()); - } -#endif //USE_SEPDISTANCE_UTIL2 - - + if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist > SIMD_EPSILON)) + { + m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(), sepDist, body0->getWorldTransform(), body1->getWorldTransform()); + } +#endif //USE_SEPDISTANCE_UTIL2 } if (m_ownManifold) { resultOut->refreshContactPoints(); } - } - - bool disableCcd = false; -btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold - + ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold ///col0->m_worldTransform, btScalar resultFraction = btScalar(1.); - btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); - + if (squareMot0 < col0->getCcdSquareMotionThreshold() && squareMot1 < col1->getCcdSquareMotionThreshold()) return resultFraction; @@ -707,77 +809,65 @@ btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, if (disableCcd) return btScalar(1.); - //An adhoc way of testing the Continuous Collision Detection algorithms //One object is approximated as a sphere, to simplify things //Starting in penetration should report no time of impact //For proper CCD, better accuracy and handling of 'allowed' penetration should be added //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) - /// Convex0 against sphere for Convex1 { btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape()); - btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); + btGjkConvexCast ccd1(convex0, &sphere1, &voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(), + col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result)) { - //store result.m_fraction in both bodies - - if (col0->getHitFraction()> result.m_fraction) - col0->setHitFraction( result.m_fraction ); + + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction(result.m_fraction); if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); + col1->setHitFraction(result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; - } - - - - } /// Sphere (for convex0) against Convex1 { btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape()); - btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation btConvexCast::CastResult result; btVoronoiSimplexSolver voronoiSimplex; //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); ///Simplification, one object is simplified as a sphere - btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); + btGjkConvexCast ccd1(&sphere0, convex1, &voronoiSimplex); //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); - if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), - col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(), col0->getInterpolationWorldTransform(), + col1->getWorldTransform(), col1->getInterpolationWorldTransform(), result)) { - //store result.m_fraction in both bodies - - if (col0->getHitFraction() > result.m_fraction) - col0->setHitFraction( result.m_fraction); + + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction(result.m_fraction); if (col1->getHitFraction() > result.m_fraction) - col1->setHitFraction( result.m_fraction); + col1->setHitFraction(result.m_fraction); if (resultFraction > result.m_fraction) resultFraction = result.m_fraction; - } } - - return resultFraction; + return resultFraction; } - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index 51db0c6548d..eac5b4d8248 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -23,7 +23,8 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "btCollisionCreateFunc.h" #include "btCollisionDispatcher.h" -#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil +#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil +#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h" class btConvexPenetrationDepthSolver; @@ -40,69 +41,61 @@ class btConvexPenetrationDepthSolver; class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm { #ifdef USE_SEPDISTANCE_UTIL2 - btConvexSeparatingDistanceUtil m_sepDistance; + btConvexSeparatingDistanceUtil m_sepDistance; #endif - btSimplexSolverInterface* m_simplexSolver; btConvexPenetrationDepthSolver* m_pdSolver; - - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_lowLevelOfDetail; - + btVertexArray worldVertsB1; + btVertexArray worldVertsB2; + + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_lowLevelOfDetail; + int m_numPerturbationIterations; int m_minimumPointsPerturbationThreshold; - ///cache separating vector to speedup collision detection - public: - - btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); + btConvexConvexAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); virtual ~btConvexConvexAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { ///should we use m_ownManifold to avoid adding duplicates? if (m_manifoldPtr && m_ownManifold) manifoldArray.push_back(m_manifoldPtr); } + void setLowLevelOfDetail(bool useLowLevel); - void setLowLevelOfDetail(bool useLowLevel); - - - const btPersistentManifold* getManifold() + const btPersistentManifold* getManifold() { return m_manifoldPtr; } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - - btConvexPenetrationDepthSolver* m_pdSolver; - btSimplexSolverInterface* m_simplexSolver; + btConvexPenetrationDepthSolver* m_pdSolver; int m_numPerturbationIterations; int m_minimumPointsPerturbationThreshold; - CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); - + CreateFunc(btConvexPenetrationDepthSolver* pdSolver); + virtual ~CreateFunc(); - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm)); - return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + return new (mem) btConvexConvexAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_pdSolver, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold); } }; - - }; -#endif //BT_CONVEX_CONVEX_ALGORITHM_H +#endif //BT_CONVEX_CONVEX_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp index cce2d95bcf9..ba1bc06b69a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -23,25 +23,24 @@ subject to the following restrictions: //#include <stdio.h> -btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold) -: btCollisionAlgorithm(ci), -m_ownManifold(false), -m_manifoldPtr(mf), -m_isSwapped(isSwapped), -m_numPerturbationIterations(numPerturbationIterations), -m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) +btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold) + : btCollisionAlgorithm(ci), + m_ownManifold(false), + m_manifoldPtr(mf), + m_isSwapped(isSwapped), + m_numPerturbationIterations(numPerturbationIterations), + m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) { - const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? col1Wrap : col0Wrap; - const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? col0Wrap : col1Wrap; + const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? col1Wrap : col0Wrap; + const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? col0Wrap : col1Wrap; - if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject())) + if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject())) { - m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(), planeObjWrap->getCollisionObject()); m_ownManifold = true; } } - btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() { if (m_ownManifold) @@ -51,32 +50,32 @@ btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() } } -void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexPlaneCollisionAlgorithm::collideSingleContact(const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap; + const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap; - btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape(); - btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape(); + btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape(); - bool hasCollision = false; + bool hasCollision = false; const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); - + btTransform convexWorldTransform = convexObjWrap->getWorldTransform(); btTransform convexInPlaneTrans; - convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexWorldTransform; + convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexWorldTransform; //now perturbe the convex-world transform - convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot); + convexWorldTransform.getBasis() *= btMatrix3x3(perturbeRot); btTransform planeInConvex; - planeInConvex= convexWorldTransform.inverse() * planeObjWrap->getWorldTransform(); - - btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + planeInConvex = convexWorldTransform.inverse() * planeObjWrap->getWorldTransform(); + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal); btVector3 vtxInPlane = convexInPlaneTrans(vtx); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); - btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal; btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected; hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); @@ -86,70 +85,69 @@ void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& /// report a contact. internally this will be kept persistent, and contact reduction is done btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal; btVector3 pOnB = vtxInPlaneWorld; - resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); + resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance); } } - -void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexPlaneCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)dispatchInfo; if (!m_manifoldPtr) return; - const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap; + const btCollisionObjectWrapper* convexObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* planeObjWrap = m_isSwapped ? body0Wrap : body1Wrap; - btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape(); - btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape(); + btConvexShape* convexShape = (btConvexShape*)convexObjWrap->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*)planeObjWrap->getCollisionShape(); bool hasCollision = false; const btVector3& planeNormal = planeShape->getPlaneNormal(); const btScalar& planeConstant = planeShape->getPlaneConstant(); btTransform planeInConvex; - planeInConvex= convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform(); + planeInConvex = convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform(); btTransform convexInPlaneTrans; - convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform(); + convexInPlaneTrans = planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform(); - btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis() * -planeNormal); btVector3 vtxInPlane = convexInPlaneTrans(vtx); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); - btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal; btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected; - hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); + hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; resultOut->setPersistentManifold(m_manifoldPtr); if (hasCollision) { /// report a contact. internally this will be kept persistent, and contact reduction is done btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal; btVector3 pOnB = vtxInPlaneWorld; - resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); + resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance); } //the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones: //they keep on rolling forever because of the additional off-center contact points //so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc) - if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold) + if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) { - btVector3 v0,v1; - btPlaneSpace1(planeNormal,v0,v1); + btVector3 v0, v1; + btPlaneSpace1(planeNormal, v0, v1); //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects const btScalar angleLimit = 0.125f * SIMD_PI; btScalar perturbeAngle; btScalar radius = convexShape->getAngularMotionDisc(); perturbeAngle = gContactBreakingThreshold / radius; - if ( perturbeAngle > angleLimit ) - perturbeAngle = angleLimit; + if (perturbeAngle > angleLimit) + perturbeAngle = angleLimit; - btQuaternion perturbeRot(v0,perturbeAngle); - for (int i=0;i<m_numPerturbationIterations;i++) + btQuaternion perturbeRot(v0, perturbeAngle); + for (int i = 0; i < m_numPerturbationIterations; i++) { - btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); - btQuaternion rotq(planeNormal,iterationAngle); - collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0Wrap,body1Wrap,dispatchInfo,resultOut); + btScalar iterationAngle = i * (SIMD_2_PI / btScalar(m_numPerturbationIterations)); + btQuaternion rotq(planeNormal, iterationAngle); + collideSingleContact(rotq.inverse() * perturbeRot * rotq, body0Wrap, body1Wrap, dispatchInfo, resultOut); } } @@ -162,7 +160,7 @@ void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectW } } -btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h index d28c430c4c1..b693da118fe 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -28,25 +28,24 @@ class btPersistentManifold; /// Other features are frame-coherency (persistent data) and collision response. class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_isSwapped; - int m_numPerturbationIterations; - int m_minimumPointsPerturbationThreshold; + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_isSwapped; + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; public: - - btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold); + btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped, int numPerturbationIterations, int minimumPointsPerturbationThreshold); virtual ~btConvexPlaneCollisionAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - void collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + void collideSingleContact(const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { @@ -54,31 +53,30 @@ public: } } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - int m_numPerturbationIterations; + int m_numPerturbationIterations; int m_minimumPointsPerturbationThreshold; - - CreateFunc() + + CreateFunc() : m_numPerturbationIterations(1), - m_minimumPointsPerturbationThreshold(0) + m_minimumPointsPerturbationThreshold(0) { } - - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); if (!m_swapped) { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); - } else + return new (mem) btConvexPlaneCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, false, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold); + } + else { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + return new (mem) btConvexPlaneCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, true, m_numPerturbationIterations, m_minimumPointsPerturbationThreshold); } } }; - }; -#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H - +#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index d42f00a637f..ef3ea9e394c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -26,115 +26,108 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM #include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM #include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" - - #include "LinearMath/btPoolAllocator.h" - - - - btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) //btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* stackAlloc,btPoolAllocator* persistentManifoldPool,btPoolAllocator* collisionAlgorithmPool) { - - void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16); - m_simplexSolver = new (mem)btVoronoiSimplexSolver(); - + void* mem = NULL; if (constructionInfo.m_useEpaPenetrationAlgorithm) { - mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16); - m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver; - }else + mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver), 16); + m_pdSolver = new (mem) btGjkEpaPenetrationDepthSolver; + } + else { - mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16); - m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver; + mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver), 16); + m_pdSolver = new (mem) btMinkowskiPenetrationDepthSolver; } - + //default CreationFunctions, filling the m_doubleDispatch table - mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16); - m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver); - mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); - m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); - m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; - mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16); - m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc),16); - m_compoundCompoundCreateFunc = new (mem)btCompoundCompoundCollisionAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16); - m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc; - mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16); - m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16); - m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc), 16); + m_convexConvexCreateFunc = new (mem) btConvexConvexAlgorithm::CreateFunc(m_pdSolver); + mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc), 16); + m_convexConcaveCreateFunc = new (mem) btConvexConcaveCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc), 16); + m_swappedConvexConcaveCreateFunc = new (mem) btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; + mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc), 16); + m_compoundCreateFunc = new (mem) btCompoundCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc), 16); + m_compoundCompoundCreateFunc = new (mem) btCompoundCompoundCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc), 16); + m_swappedCompoundCreateFunc = new (mem) btCompoundCollisionAlgorithm::SwappedCreateFunc; + mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc), 16); + m_emptyCreateFunc = new (mem) btEmptyAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc), 16); + m_sphereSphereCF = new (mem) btSphereSphereCollisionAlgorithm::CreateFunc; #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM - mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); - m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); - m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc), 16); + m_sphereBoxCF = new (mem) btSphereBoxCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc), 16); + m_boxSphereCF = new (mem) btSphereBoxCollisionAlgorithm::CreateFunc; m_boxSphereCF->m_swapped = true; -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM - mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); - m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); - m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc), 16); + m_sphereTriangleCF = new (mem) btSphereTriangleCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc), 16); + m_triangleSphereCF = new (mem) btSphereTriangleCollisionAlgorithm::CreateFunc; m_triangleSphereCF->m_swapped = true; - - mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc),16); - m_boxBoxCF = new(mem)btBoxBoxCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc), 16); + m_boxBoxCF = new (mem) btBoxBoxCollisionAlgorithm::CreateFunc; //convex versus plane - mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + mem = btAlignedAlloc(sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc), 16); m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; - mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + mem = btAlignedAlloc(sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc), 16); m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; m_planeConvexCF->m_swapped = true; - + ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool int maxSize = sizeof(btConvexConvexAlgorithm); int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); int maxSize3 = sizeof(btCompoundCollisionAlgorithm); int maxSize4 = sizeof(btCompoundCompoundCollisionAlgorithm); - int collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4); - + int collisionAlgorithmMaxElementSize = btMax(maxSize, constructionInfo.m_customCollisionAlgorithmMaxElementSize); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize2); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize3); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize, maxSize4); + if (constructionInfo.m_persistentManifoldPool) { m_ownsPersistentManifoldPool = false; m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool; - } else + } + else { m_ownsPersistentManifoldPool = true; - void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); - m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize); + void* mem = btAlignedAlloc(sizeof(btPoolAllocator), 16); + m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold), constructionInfo.m_defaultMaxPersistentManifoldPoolSize); } - + + collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize + 16) & 0xffffffffffff0; if (constructionInfo.m_collisionAlgorithmPool) { m_ownsCollisionAlgorithmPool = false; m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool; - } else + } + else { m_ownsCollisionAlgorithmPool = true; - void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); - m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); + void* mem = btAlignedAlloc(sizeof(btPoolAllocator), 16); + m_collisionAlgorithmPool = new (mem) btPoolAllocator(collisionAlgorithmMaxElementSize, constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); } - - } btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() @@ -151,95 +144,159 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() } m_convexConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_convexConvexCreateFunc); + btAlignedFree(m_convexConvexCreateFunc); m_convexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_convexConcaveCreateFunc); + btAlignedFree(m_convexConcaveCreateFunc); m_swappedConvexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedConvexConcaveCreateFunc); + btAlignedFree(m_swappedConvexConcaveCreateFunc); m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_compoundCreateFunc); + btAlignedFree(m_compoundCreateFunc); m_compoundCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); btAlignedFree(m_compoundCompoundCreateFunc); m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedCompoundCreateFunc); + btAlignedFree(m_swappedCompoundCreateFunc); m_emptyCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_emptyCreateFunc); + btAlignedFree(m_emptyCreateFunc); m_sphereSphereCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_sphereSphereCF); + btAlignedFree(m_sphereSphereCF); #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM m_sphereBoxCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_sphereBoxCF); + btAlignedFree(m_sphereBoxCF); m_boxSphereCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_boxSphereCF); -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + btAlignedFree(m_boxSphereCF); +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_sphereTriangleCF); + btAlignedFree(m_sphereTriangleCF); m_triangleSphereCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_triangleSphereCF); + btAlignedFree(m_triangleSphereCF); m_boxBoxCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_boxBoxCF); + btAlignedFree(m_boxBoxCF); m_convexPlaneCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_convexPlaneCF); + btAlignedFree(m_convexPlaneCF); m_planeConvexCF->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_planeConvexCF); - - m_simplexSolver->~btVoronoiSimplexSolver(); - btAlignedFree(m_simplexSolver); + btAlignedFree(m_planeConvexCF); m_pdSolver->~btConvexPenetrationDepthSolver(); - + btAlignedFree(m_pdSolver); +} +btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) +{ + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) + { + return m_sphereSphereCF; + } +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) + { + return m_sphereBoxCF; + } -} + if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) + { + return m_boxSphereCF; + } +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE)) + { + return m_sphereTriangleCF; + } -btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) -{ + if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) + { + return m_triangleSphereCF; + } + + if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE)) + { + return m_convexPlaneCF; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE)) + { + return m_planeConvexCF; + } + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) + { + return m_convexConvexCreateFunc; + } + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1)) + { + return m_convexConcaveCreateFunc; + } - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0)) { - return m_sphereSphereCF; + return m_swappedConvexConcaveCreateFunc; } -#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE)) + + if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) { - return m_sphereBoxCF; + return m_compoundCompoundCreateFunc; } - if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + if (btBroadphaseProxy::isCompound(proxyType0)) { - return m_boxSphereCF; + return m_compoundCreateFunc; + } + else + { + if (btBroadphaseProxy::isCompound(proxyType1)) + { + return m_swappedCompoundCreateFunc; + } } -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + //failed to find an algorithm + return m_emptyCreateFunc; +} - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE)) +btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) +{ + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_sphereTriangleCF; + return m_sphereSphereCF; + } +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) + { + return m_sphereBoxCF; } - if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_triangleSphereCF; - } + return m_boxSphereCF; + } +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE)) + { + return m_sphereTriangleCF; + } + + if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) + { + return m_triangleSphereCF; + } if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) { return m_boxBoxCF; } - + if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE)) { return m_convexPlaneCF; @@ -249,8 +306,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg { return m_planeConvexCF; } - - if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) { @@ -267,7 +322,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg return m_swappedConvexConcaveCreateFunc; } - if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) { return m_compoundCompoundCreateFunc; @@ -276,7 +330,8 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg if (btBroadphaseProxy::isCompound(proxyType0)) { return m_compoundCreateFunc; - } else + } + else { if (btBroadphaseProxy::isCompound(proxyType1)) { @@ -290,17 +345,17 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) { - btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc; + btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*)m_convexConvexCreateFunc; convexConvex->m_numPerturbationIterations = numPerturbationIterations; convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; } -void btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) +void btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) { btConvexPlaneCollisionAlgorithm::CreateFunc* cpCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_convexPlaneCF; cpCF->m_numPerturbationIterations = numPerturbationIterations; cpCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; - + btConvexPlaneCollisionAlgorithm::CreateFunc* pcCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_planeConvexCF; pcCF->m_numPerturbationIterations = numPerturbationIterations; pcCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 2078420e198..b39a3f41de1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -20,77 +20,68 @@ subject to the following restrictions: class btVoronoiSimplexSolver; class btConvexPenetrationDepthSolver; -struct btDefaultCollisionConstructionInfo +struct btDefaultCollisionConstructionInfo { - btPoolAllocator* m_persistentManifoldPool; - btPoolAllocator* m_collisionAlgorithmPool; - int m_defaultMaxPersistentManifoldPoolSize; - int m_defaultMaxCollisionAlgorithmPoolSize; - int m_customCollisionAlgorithmMaxElementSize; - int m_useEpaPenetrationAlgorithm; + btPoolAllocator* m_persistentManifoldPool; + btPoolAllocator* m_collisionAlgorithmPool; + int m_defaultMaxPersistentManifoldPoolSize; + int m_defaultMaxCollisionAlgorithmPoolSize; + int m_customCollisionAlgorithmMaxElementSize; + int m_useEpaPenetrationAlgorithm; btDefaultCollisionConstructionInfo() - :m_persistentManifoldPool(0), - m_collisionAlgorithmPool(0), - m_defaultMaxPersistentManifoldPoolSize(4096), - m_defaultMaxCollisionAlgorithmPoolSize(4096), - m_customCollisionAlgorithmMaxElementSize(0), - m_useEpaPenetrationAlgorithm(true) + : m_persistentManifoldPool(0), + m_collisionAlgorithmPool(0), + m_defaultMaxPersistentManifoldPoolSize(4096), + m_defaultMaxCollisionAlgorithmPoolSize(4096), + m_customCollisionAlgorithmMaxElementSize(0), + m_useEpaPenetrationAlgorithm(true) { } }; - - ///btCollisionConfiguration allows to configure Bullet collision detection ///stack allocator, pool memory allocators ///@todo: describe the meaning -class btDefaultCollisionConfiguration : public btCollisionConfiguration +class btDefaultCollisionConfiguration : public btCollisionConfiguration { - protected: + int m_persistentManifoldPoolSize; - int m_persistentManifoldPoolSize; - - - btPoolAllocator* m_persistentManifoldPool; - bool m_ownsPersistentManifoldPool; + btPoolAllocator* m_persistentManifoldPool; + bool m_ownsPersistentManifoldPool; + btPoolAllocator* m_collisionAlgorithmPool; + bool m_ownsCollisionAlgorithmPool; - btPoolAllocator* m_collisionAlgorithmPool; - bool m_ownsCollisionAlgorithmPool; + //default penetration depth solver + btConvexPenetrationDepthSolver* m_pdSolver; - //default simplex/penetration depth solvers - btVoronoiSimplexSolver* m_simplexSolver; - btConvexPenetrationDepthSolver* m_pdSolver; - //default CreationFunctions, filling the m_doubleDispatch table - btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc; - btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc; - btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; - btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; - btCollisionAlgorithmCreateFunc* m_compoundCompoundCreateFunc; - - btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc; + btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_compoundCompoundCreateFunc; + + btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; btCollisionAlgorithmCreateFunc* m_sphereSphereCF; btCollisionAlgorithmCreateFunc* m_sphereBoxCF; btCollisionAlgorithmCreateFunc* m_boxSphereCF; btCollisionAlgorithmCreateFunc* m_boxBoxCF; - btCollisionAlgorithmCreateFunc* m_sphereTriangleCF; - btCollisionAlgorithmCreateFunc* m_triangleSphereCF; - btCollisionAlgorithmCreateFunc* m_planeConvexCF; - btCollisionAlgorithmCreateFunc* m_convexPlaneCF; - -public: - + btCollisionAlgorithmCreateFunc* m_sphereTriangleCF; + btCollisionAlgorithmCreateFunc* m_triangleSphereCF; + btCollisionAlgorithmCreateFunc* m_planeConvexCF; + btCollisionAlgorithmCreateFunc* m_convexPlaneCF; +public: btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); virtual ~btDefaultCollisionConfiguration(); - ///memory pools + ///memory pools virtual btPoolAllocator* getPersistentManifoldPool() { return m_persistentManifoldPool; @@ -101,14 +92,9 @@ public: return m_collisionAlgorithmPool; } + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1); - virtual btVoronoiSimplexSolver* getSimplexSolver() - { - return m_simplexSolver; - } - - - virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); + virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1); ///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm. ///By default, this feature is disabled for best performance. @@ -117,11 +103,9 @@ public: ///3 is a good value for both params, if you want to enable the feature. This is because the default contact cache contains a maximum of 4 points, and one collision query at the unperturbed orientation is performed first. ///See Bullet/Demos/CollisionDemo for an example how this feature gathers multiple points. ///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection. - void setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); - - void setPlaneConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); + void setConvexConvexMultipointIterations(int numPerturbationIterations = 3, int minimumPointsPerturbationThreshold = 3); + void setPlaneConvexMultipointIterations(int numPerturbationIterations = 3, int minimumPointsPerturbationThreshold = 3); }; -#endif //BT_DEFAULT_COLLISION_CONFIGURATION - +#endif //BT_DEFAULT_COLLISION_CONFIGURATION diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp index 5fa1c8be5e4..7cd41bdb33e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp @@ -15,20 +15,16 @@ subject to the following restrictions: #include "btEmptyCollisionAlgorithm.h" - - btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btCollisionAlgorithm(ci) { } -void btEmptyAlgorithm::processCollision (const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* ,const btDispatcherInfo& ,btManifoldResult* ) +void btEmptyAlgorithm::processCollision(const btCollisionObjectWrapper*, const btCollisionObjectWrapper*, const btDispatcherInfo&, btManifoldResult*) { } -btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* ) +btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject*, btCollisionObject*, const btDispatcherInfo&, btManifoldResult*) { return btScalar(1.); } - - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h index cb0f152183c..65ef83e0946 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -25,30 +25,28 @@ subject to the following restrictions: ///The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame. class btEmptyAlgorithm : public btCollisionAlgorithm { - public: - btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { } - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) + { (void)body0Wrap; (void)body1Wrap; void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm)); - return new(mem) btEmptyAlgorithm(ci); + return new (mem) btEmptyAlgorithm(ci); } }; } ATTRIBUTE_ALIGNED(16); -#endif //BT_EMPTY_ALGORITH +#endif //BT_EMPTY_ALGORITH diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp index 86141fa6899..00f16fd0a8a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp @@ -29,60 +29,58 @@ btGhostObject::~btGhostObject() btAssert(!m_overlappingObjects.size()); } - -void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy) { btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btAssert(otherObject); ///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index==m_overlappingObjects.size()) + if (index == m_overlappingObjects.size()) { //not found m_overlappingObjects.push_back(otherObject); } } -void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy) +void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy) { btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btAssert(otherObject); int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index<m_overlappingObjects.size()) + if (index < m_overlappingObjects.size()) { - m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1]; + m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size() - 1]; m_overlappingObjects.pop_back(); } } - btPairCachingGhostObject::btPairCachingGhostObject() { - m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); + m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16)) btHashedOverlappingPairCache(); } btPairCachingGhostObject::~btPairCachingGhostObject() { m_hashPairCache->~btHashedOverlappingPairCache(); - btAlignedFree( m_hashPairCache ); + btAlignedFree(m_hashPairCache); } -void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy) { - btBroadphaseProxy*actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle(); + btBroadphaseProxy* actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle(); btAssert(actualThisProxy); btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btAssert(otherObject); int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index==m_overlappingObjects.size()) + if (index == m_overlappingObjects.size()) { m_overlappingObjects.push_back(otherObject); - m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy); + m_hashPairCache->addOverlappingPair(actualThisProxy, otherProxy); } } -void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1) +void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy1) { btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle(); @@ -90,82 +88,79 @@ void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy btAssert(otherObject); int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index<m_overlappingObjects.size()) + if (index < m_overlappingObjects.size()) { - m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1]; + m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size() - 1]; m_overlappingObjects.pop_back(); - m_hashPairCache->removeOverlappingPair(actualThisProxy,otherProxy,dispatcher); + m_hashPairCache->removeOverlappingPair(actualThisProxy, otherProxy, dispatcher); } } - -void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const +void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const { - btTransform convexFromTrans,convexToTrans; + btTransform convexFromTrans, convexToTrans; convexFromTrans = convexFromWorld; convexToTrans = convexToWorld; btVector3 castShapeAabbMin, castShapeAabbMax; /* Compute AABB that encompasses angular movement */ { btVector3 linVel, angVel; - btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); + btTransformUtil::calculateVelocity(convexFromTrans, convexToTrans, 1.0, linVel, angVel); btTransform R; - R.setIdentity (); - R.setRotation (convexFromTrans.getRotation()); - castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); + R.setIdentity(); + R.setRotation(convexFromTrans.getRotation()); + castShape->calculateTemporalAabb(R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); } /// go over all objects, and if the ray intersects their aabb + cast shape aabb, // do a ray-shape query using convexCaster (CCD) int i; - for (i=0;i<m_overlappingObjects.size();i++) + for (i = 0; i < m_overlappingObjects.size(); i++) { - btCollisionObject* collisionObject= m_overlappingObjects[i]; + btCollisionObject* collisionObject = m_overlappingObjects[i]; //only perform raycast if filterMask matches - if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + if (resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); - btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 collisionObjectAabbMin, collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax); + AabbExpand(collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing btVector3 hitNormal; - if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + if (btRayAabb(convexFromWorld.getOrigin(), convexToWorld.getOrigin(), collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) { - btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback, - allowedCcdPenetration); + btCollisionWorld::objectQuerySingle(castShape, convexFromTrans, convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + allowedCcdPenetration); } } } - } -void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const +void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const { btTransform rayFromTrans; rayFromTrans.setIdentity(); rayFromTrans.setOrigin(rayFromWorld); - btTransform rayToTrans; + btTransform rayToTrans; rayToTrans.setIdentity(); rayToTrans.setOrigin(rayToWorld); - int i; - for (i=0;i<m_overlappingObjects.size();i++) + for (i = 0; i < m_overlappingObjects.size(); i++) { - btCollisionObject* collisionObject= m_overlappingObjects[i]; + btCollisionObject* collisionObject = m_overlappingObjects[i]; //only perform raycast if filterMask matches - if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + if (resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { - btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback); + btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback); } } } - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h index 8ec86138575..aa7f48d5cb4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h @@ -16,7 +16,6 @@ subject to the following restrictions: #ifndef BT_GHOST_OBJECT_H #define BT_GHOST_OBJECT_H - #include "btCollisionObject.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h" #include "LinearMath/btAlignedAllocator.h" @@ -31,48 +30,47 @@ class btDispatcher; ///By default, this overlap is based on the AABB ///This is useful for creating a character controller, collision sensors/triggers, explosions etc. ///We plan on adding rayTest and other queries for the btGhostObject -ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject +ATTRIBUTE_ALIGNED16(class) +btGhostObject : public btCollisionObject { protected: - btAlignedObjectArray<btCollisionObject*> m_overlappingObjects; public: - btGhostObject(); virtual ~btGhostObject(); - void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const; + void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const; - void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; ///this method is mainly for expert/internal use only. - virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); + virtual void addOverlappingObjectInternal(btBroadphaseProxy * otherProxy, btBroadphaseProxy* thisProxy = 0); ///this method is mainly for expert/internal use only. - virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + virtual void removeOverlappingObjectInternal(btBroadphaseProxy * otherProxy, btDispatcher * dispatcher, btBroadphaseProxy* thisProxy = 0); - int getNumOverlappingObjects() const + int getNumOverlappingObjects() const { return m_overlappingObjects.size(); } - btCollisionObject* getOverlappingObject(int index) + btCollisionObject* getOverlappingObject(int index) { return m_overlappingObjects[index]; } - const btCollisionObject* getOverlappingObject(int index) const + const btCollisionObject* getOverlappingObject(int index) const { return m_overlappingObjects[index]; } - btAlignedObjectArray<btCollisionObject*>& getOverlappingPairs() + btAlignedObjectArray<btCollisionObject*>& getOverlappingPairs() { return m_overlappingObjects; } - const btAlignedObjectArray<btCollisionObject*> getOverlappingPairs() const + const btAlignedObjectArray<btCollisionObject*> getOverlappingPairs() const { return m_overlappingObjects; } @@ -81,49 +79,43 @@ public: // internal cast // - static const btGhostObject* upcast(const btCollisionObject* colObj) + static const btGhostObject* upcast(const btCollisionObject* colObj) { - if (colObj->getInternalType()==CO_GHOST_OBJECT) + if (colObj->getInternalType() == CO_GHOST_OBJECT) return (const btGhostObject*)colObj; return 0; } - static btGhostObject* upcast(btCollisionObject* colObj) + static btGhostObject* upcast(btCollisionObject * colObj) { - if (colObj->getInternalType()==CO_GHOST_OBJECT) + if (colObj->getInternalType() == CO_GHOST_OBJECT) return (btGhostObject*)colObj; return 0; } - }; -class btPairCachingGhostObject : public btGhostObject +class btPairCachingGhostObject : public btGhostObject { - btHashedOverlappingPairCache* m_hashPairCache; + btHashedOverlappingPairCache* m_hashPairCache; public: - btPairCachingGhostObject(); virtual ~btPairCachingGhostObject(); ///this method is mainly for expert/internal use only. - virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); + virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy = 0); - virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btDispatcher* dispatcher, btBroadphaseProxy* thisProxy = 0); - btHashedOverlappingPairCache* getOverlappingPairCache() + btHashedOverlappingPairCache* getOverlappingPairCache() { return m_hashPairCache; } - }; - - ///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject. class btGhostPairCallback : public btOverlappingPairCallback { - public: btGhostPairCallback() { @@ -131,15 +123,14 @@ public: virtual ~btGhostPairCallback() { - } - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { - btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; - btGhostObject* ghost0 = btGhostObject::upcast(colObj0); - btGhostObject* ghost1 = btGhostObject::upcast(colObj1); + btCollisionObject* colObj0 = (btCollisionObject*)proxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)proxy1->m_clientObject; + btGhostObject* ghost0 = btGhostObject::upcast(colObj0); + btGhostObject* ghost1 = btGhostObject::upcast(colObj1); if (ghost0) ghost0->addOverlappingObjectInternal(proxy1, proxy0); if (ghost1) @@ -147,29 +138,25 @@ public: return 0; } - virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) + virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, btDispatcher* dispatcher) { - btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; - btGhostObject* ghost0 = btGhostObject::upcast(colObj0); - btGhostObject* ghost1 = btGhostObject::upcast(colObj1); + btCollisionObject* colObj0 = (btCollisionObject*)proxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)proxy1->m_clientObject; + btGhostObject* ghost0 = btGhostObject::upcast(colObj0); + btGhostObject* ghost1 = btGhostObject::upcast(colObj1); if (ghost0) - ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0); + ghost0->removeOverlappingObjectInternal(proxy1, dispatcher, proxy0); if (ghost1) - ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1); + ghost1->removeOverlappingObjectInternal(proxy0, dispatcher, proxy1); return 0; } - virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/, btDispatcher* /*dispatcher*/) { btAssert(0); //need to keep track of all ghost objects and call them here //m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher); } - - - }; #endif - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp index 8c8a7c3c1e3..b686d98d1e7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp @@ -13,61 +13,49 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btHashedSimplePairCache.h" - #include <stdio.h> -int gOverlappingSimplePairs = 0; -int gRemoveSimplePairs =0; -int gAddedSimplePairs =0; -int gFindSimplePairs =0; - - - +#ifdef BT_DEBUG_COLLISION_PAIRS +int gOverlappingSimplePairs = 0; +int gRemoveSimplePairs = 0; +int gAddedSimplePairs = 0; +int gFindSimplePairs = 0; +#endif //BT_DEBUG_COLLISION_PAIRS -btHashedSimplePairCache::btHashedSimplePairCache() { - int initialAllocatedSize= 2; +btHashedSimplePairCache::btHashedSimplePairCache() +{ + int initialAllocatedSize = 2; m_overlappingPairArray.reserve(initialAllocatedSize); growTables(); } - - - btHashedSimplePairCache::~btHashedSimplePairCache() { } - - - - - void btHashedSimplePairCache::removeAllPairs() { m_overlappingPairArray.clear(); m_hashTable.clear(); m_next.clear(); - int initialAllocatedSize= 2; + int initialAllocatedSize = 2; m_overlappingPairArray.reserve(initialAllocatedSize); growTables(); } - - btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gFindSimplePairs++; - - +#endif + /*if (indexA > indexB) btSwap(indexA, indexB);*/ - int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); + int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); if (hash >= m_hashTable.size()) { @@ -92,9 +80,8 @@ btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) //#include <stdio.h> -void btHashedSimplePairCache::growTables() +void btHashedSimplePairCache::growTables() { - int newCapacity = m_overlappingPairArray.capacity(); if (m_hashTable.size() < newCapacity) @@ -105,10 +92,9 @@ void btHashedSimplePairCache::growTables() m_hashTable.resize(newCapacity); m_next.resize(newCapacity); - int i; - for (i= 0; i < newCapacity; ++i) + for (i = 0; i < newCapacity; ++i) { m_hashTable[i] = BT_SIMPLE_NULL_PAIR; } @@ -117,27 +103,22 @@ void btHashedSimplePairCache::growTables() m_next[i] = BT_SIMPLE_NULL_PAIR; } - for(i=0;i<curHashtableSize;i++) + for (i = 0; i < curHashtableSize; i++) { - const btSimplePair& pair = m_overlappingPairArray[i]; int indexA = pair.m_indexA; int indexB = pair.m_indexB; - - int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + + int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask m_next[i] = m_hashTable[hashValue]; m_hashTable[hashValue] = i; } - - } } btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) { - - int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask - + int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask btSimplePair* pair = internalFindPair(indexA, indexB, hash); if (pair != NULL) @@ -155,30 +136,29 @@ btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) { growTables(); //hash with new capacity - hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); + hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); } - - pair = new (mem) btSimplePair(indexA,indexB); + + pair = new (mem) btSimplePair(indexA, indexB); pair->m_userPointer = 0; - + m_next[count] = m_hashTable[hash]; m_hashTable[hash] = count; return pair; } - - void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gRemoveSimplePairs++; - +#endif /*if (indexA > indexB) btSwap(indexA, indexB);*/ - int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); + int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); btSimplePair* pair = internalFindPair(indexA, indexB, hash); if (pair == NULL) @@ -186,10 +166,8 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) return 0; } - void* userData = pair->m_userPointer; - int pairIndex = int(pair - &m_overlappingPairArray[0]); btAssert(pairIndex < m_overlappingPairArray.size()); @@ -229,8 +207,8 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) // Remove the last pair from the hash table. const btSimplePair* last = &m_overlappingPairArray[lastPairIndex]; - /* missing swap here too, Nat. */ - int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity()-1)); + /* missing swap here too, Nat. */ + int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity() - 1)); index = m_hashTable[lastHash]; btAssert(index != BT_SIMPLE_NULL_PAIR); @@ -264,13 +242,3 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) return userData; } //#include <stdio.h> - - - - - - - - - - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h index 186964d723e..fd38a4f0e19 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -16,142 +16,126 @@ subject to the following restrictions: #ifndef BT_HASHED_SIMPLE_PAIR_CACHE_H #define BT_HASHED_SIMPLE_PAIR_CACHE_H - - #include "LinearMath/btAlignedObjectArray.h" -const int BT_SIMPLE_NULL_PAIR=0xffffffff; +const int BT_SIMPLE_NULL_PAIR = 0xffffffff; struct btSimplePair { - btSimplePair(int indexA,int indexB) - :m_indexA(indexA), - m_indexB(indexB), - m_userPointer(0) + btSimplePair(int indexA, int indexB) + : m_indexA(indexA), + m_indexB(indexB), + m_userPointer(0) { } int m_indexA; int m_indexB; - union - { - void* m_userPointer; - int m_userValue; + union { + void* m_userPointer; + int m_userValue; }; }; -typedef btAlignedObjectArray<btSimplePair> btSimplePairArray; - - +typedef btAlignedObjectArray<btSimplePair> btSimplePairArray; +#ifdef BT_DEBUG_COLLISION_PAIRS extern int gOverlappingSimplePairs; extern int gRemoveSimplePairs; extern int gAddedSimplePairs; extern int gFindSimplePairs; - - - +#endif //BT_DEBUG_COLLISION_PAIRS class btHashedSimplePairCache { - btSimplePairArray m_overlappingPairArray; - + btSimplePairArray m_overlappingPairArray; protected: - - btAlignedObjectArray<int> m_hashTable; - btAlignedObjectArray<int> m_next; - + btAlignedObjectArray<int> m_hashTable; + btAlignedObjectArray<int> m_next; public: btHashedSimplePairCache(); virtual ~btHashedSimplePairCache(); - + void removeAllPairs(); - virtual void* removeOverlappingPair(int indexA,int indexB); - + virtual void* removeOverlappingPair(int indexA, int indexB); + // Add a pair and return the new pair. If the pair already exists, // no new pair is created and the old one is returned. - virtual btSimplePair* addOverlappingPair(int indexA,int indexB) + virtual btSimplePair* addOverlappingPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gAddedSimplePairs++; +#endif - return internalAddPair(indexA,indexB); + return internalAddPair(indexA, indexB); } - - virtual btSimplePair* getOverlappingPairArrayPtr() + virtual btSimplePair* getOverlappingPairArrayPtr() { return &m_overlappingPairArray[0]; } - const btSimplePair* getOverlappingPairArrayPtr() const + const btSimplePair* getOverlappingPairArrayPtr() const { return &m_overlappingPairArray[0]; } - btSimplePairArray& getOverlappingPairArray() + btSimplePairArray& getOverlappingPairArray() { return m_overlappingPairArray; } - const btSimplePairArray& getOverlappingPairArray() const + const btSimplePairArray& getOverlappingPairArray() const { return m_overlappingPairArray; } - - btSimplePair* findPair(int indexA,int indexB); + btSimplePair* findPair(int indexA, int indexB); int GetCount() const { return m_overlappingPairArray.size(); } - int getNumOverlappingPairs() const + int getNumOverlappingPairs() const { return m_overlappingPairArray.size(); } + private: - - btSimplePair* internalAddPair(int indexA, int indexB); + btSimplePair* internalAddPair(int indexA, int indexB); - void growTables(); + void growTables(); SIMD_FORCE_INLINE bool equalsPair(const btSimplePair& pair, int indexA, int indexB) - { + { return pair.m_indexA == indexA && pair.m_indexB == indexB; } - - - SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) + SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) { - int key = static_cast<int>(((unsigned int)indexA) | (((unsigned int)indexB) <<16)); + unsigned int key = indexA | (indexB << 16); // Thomas Wang's hash key += ~(key << 15); - key ^= (key >> 10); - key += (key << 3); - key ^= (key >> 6); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); key += ~(key << 11); - key ^= (key >> 16); - return static_cast<unsigned int>(key); + key ^= (key >> 16); + return key; } - - - - - SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA , int proxyIdB, int hash) + SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA, int proxyIdB, int hash) { - int index = m_hashTable[hash]; - - while( index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false) + + while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false) { index = m_next[index]; } - if ( index == BT_SIMPLE_NULL_PAIR ) + if (index == BT_SIMPLE_NULL_PAIR) { return NULL; } @@ -160,13 +144,6 @@ private: return &m_overlappingPairArray[index]; } - - }; - - - -#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H - - +#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index 6cba442ca5d..a71700f58af 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -1,6 +1,8 @@ #include "btInternalEdgeUtility.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" + #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" @@ -12,50 +14,44 @@ #ifdef DEBUG_INTERNAL_EDGE #include <stdio.h> -#endif //DEBUG_INTERNAL_EDGE - +#endif //DEBUG_INTERNAL_EDGE #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW static btIDebugDraw* gDebugDrawer = 0; -void btSetDebugDrawer(btIDebugDraw* debugDrawer) +void btSetDebugDrawer(btIDebugDraw* debugDrawer) { gDebugDrawer = debugDrawer; } -static void btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color) +static void btDebugDrawLine(const btVector3& from, const btVector3& to, const btVector3& color) { if (gDebugDrawer) - gDebugDrawer->drawLine(from,to,color); + gDebugDrawer->drawLine(from, to, color); } -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW -static int btGetHash(int partId, int triangleIndex) +static int btGetHash(int partId, int triangleIndex) { - int hash = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; + int hash = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex; return hash; } - - -static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA,const btVector3& normalB) +static btScalar btGetAngle(const btVector3& edgeA, const btVector3& normalA, const btVector3& normalB) { - const btVector3 refAxis0 = edgeA; - const btVector3 refAxis1 = normalA; + const btVector3 refAxis0 = edgeA; + const btVector3 refAxis1 = normalA; const btVector3 swingAxis = normalB; btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); - return angle; + return angle; } - struct btConnectivityProcessor : public btTriangleCallback { - int m_partIdA; - int m_triangleIndexA; - btVector3* m_triangleVerticesA; - btTriangleInfoMap* m_triangleInfoMap; - + int m_partIdA; + int m_triangleIndexA; + btVector3* m_triangleVerticesA; + btTriangleInfoMap* m_triangleInfoMap; virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { @@ -69,18 +65,17 @@ struct btConnectivityProcessor : public btTriangleCallback //search for shared vertices and edges int numshared = 0; - int sharedVertsA[3]={-1,-1,-1}; - int sharedVertsB[3]={-1,-1,-1}; + int sharedVertsA[3] = {-1, -1, -1}; + int sharedVertsB[3] = {-1, -1, -1}; ///skip degenerate triangles - btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2(); + btScalar crossBSqr = ((triangle[1] - triangle[0]).cross(triangle[2] - triangle[0])).length2(); if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold) return; - - btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2(); + btScalar crossASqr = ((m_triangleVerticesA[1] - m_triangleVerticesA[0]).cross(m_triangleVerticesA[2] - m_triangleVerticesA[0])).length2(); ///skip degenerate triangles - if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold) + if (crossASqr < m_triangleInfoMap->m_equalVertexThreshold) return; #if 0 @@ -96,36 +91,36 @@ struct btConnectivityProcessor : public btTriangleCallback triangle[2].getX(),triangle[2].getY(),triangle[2].getZ()); #endif - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - for (int j=0;j<3;j++) + for (int j = 0; j < 3; j++) { - if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold) + if ((m_triangleVerticesA[i] - triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold) { sharedVertsA[numshared] = i; sharedVertsB[numshared] = j; numshared++; ///degenerate case - if(numshared >= 3) + if (numshared >= 3) return; } } ///degenerate case - if(numshared >= 3) + if (numshared >= 3) return; } switch (numshared) { - case 0: + case 0: { break; } - case 1: + case 1: { //shared vertex break; } - case 2: + case 2: { //shared edge //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct @@ -138,26 +133,25 @@ struct btConnectivityProcessor : public btTriangleCallback sharedVertsB[0] = tmp; } - int hash = btGetHash(m_partIdA,m_triangleIndexA); + int hash = btGetHash(m_partIdA, m_triangleIndexA); btTriangleInfo* info = m_triangleInfoMap->find(hash); if (!info) { btTriangleInfo tmp; - m_triangleInfoMap->insert(hash,tmp); + m_triangleInfoMap->insert(hash, tmp); info = m_triangleInfoMap->find(hash); } - int sumvertsA = sharedVertsA[0]+sharedVertsA[1]; - int otherIndexA = 3-sumvertsA; + int sumvertsA = sharedVertsA[0] + sharedVertsA[1]; + int otherIndexA = 3 - sumvertsA; - - btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]); + btVector3 edge(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]); - btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]); - int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]); + btTriangleShape tA(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]); + int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]); - btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]); + btTriangleShape tB(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]); //btTriangleShape tB(triangle[0],triangle[1],triangle[2]); btVector3 normalA; @@ -168,26 +162,25 @@ struct btConnectivityProcessor : public btTriangleCallback btVector3 edgeCrossA = edge.cross(normalA).normalize(); { - btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]]; + btVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]]; if (edgeCrossA.dot(tmp) < 0) { - edgeCrossA*=-1; + edgeCrossA *= -1; } } btVector3 edgeCrossB = edge.cross(normalB).normalize(); { - btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]]; + btVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]]; if (edgeCrossB.dot(tmp) < 0) { - edgeCrossB*=-1; + edgeCrossB *= -1; } } - btScalar angle2 = 0; - btScalar ang4 = 0.f; - + btScalar angle2 = 0; + btScalar ang4 = 0.f; btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB); btScalar len2 = calculatedEdge.length2(); @@ -196,52 +189,47 @@ struct btConnectivityProcessor : public btTriangleCallback //btVector3 calculatedNormalB = normalA; bool isConvex = false; - if (len2<m_triangleInfoMap->m_planarEpsilon) + if (len2 < m_triangleInfoMap->m_planarEpsilon) { angle2 = 0.f; ang4 = 0.f; - } else + } + else { - calculatedEdge.normalize(); btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA); calculatedNormalA.normalize(); - angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB); - ang4 = SIMD_PI-angle2; + angle2 = btGetAngle(calculatedNormalA, edgeCrossA, edgeCrossB); + ang4 = SIMD_PI - angle2; btScalar dotA = normalA.dot(edgeCrossB); ///@todo: check if we need some epsilon, due to floating point imprecision - isConvex = (dotA<0.); + isConvex = (dotA < 0.); correctedAngle = isConvex ? ang4 : -ang4; } - - - - - //alternatively use + //alternatively use //btVector3 calculatedNormalB2 = quatRotate(orn,normalA); - switch (sumvertsA) { - case 1: + case 1: { - btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1]; - btQuaternion orn(edge,-correctedAngle); - btVector3 computedNormalB = quatRotate(orn,normalA); + btVector3 edge = m_triangleVerticesA[0] - m_triangleVerticesA[1]; + btQuaternion orn(edge, -correctedAngle); + btVector3 computedNormalB = quatRotate(orn, normalA); btScalar bla = computedNormalB.dot(normalB); - if (bla<0) + if (bla < 0) { - computedNormalB*=-1; + computedNormalB *= -1; info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB; } #ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) + if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } -#endif//DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE info->m_edgeV0V1Angle = -correctedAngle; @@ -249,44 +237,44 @@ struct btConnectivityProcessor : public btTriangleCallback info->m_flags |= TRI_INFO_V0V1_CONVEX; break; } - case 2: + case 2: { - btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0]; - btQuaternion orn(edge,-correctedAngle); - btVector3 computedNormalB = quatRotate(orn,normalA); - if (computedNormalB.dot(normalB)<0) + btVector3 edge = m_triangleVerticesA[2] - m_triangleVerticesA[0]; + btQuaternion orn(edge, -correctedAngle); + btVector3 computedNormalB = quatRotate(orn, normalA); + if (computedNormalB.dot(normalB) < 0) { - computedNormalB*=-1; + computedNormalB *= -1; info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB; } #ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) + if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } -#endif //DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE info->m_edgeV2V0Angle = -correctedAngle; if (isConvex) info->m_flags |= TRI_INFO_V2V0_CONVEX; - break; + break; } - case 3: + case 3: { - btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2]; - btQuaternion orn(edge,-correctedAngle); - btVector3 computedNormalB = quatRotate(orn,normalA); - if (computedNormalB.dot(normalB)<0) + btVector3 edge = m_triangleVerticesA[1] - m_triangleVerticesA[2]; + btQuaternion orn(edge, -correctedAngle); + btVector3 computedNormalB = quatRotate(orn, normalA); + if (computedNormalB.dot(normalB) < 0) { info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB; - computedNormalB*=-1; + computedNormalB *= -1; } #ifdef DEBUG_INTERNAL_EDGE - if ((computedNormalB-normalB).length()>0.0001) + if ((computedNormalB - normalB).length() > 0.0001) { printf("warning: normals not identical\n"); } -#endif //DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE info->m_edgeV1V2Angle = -correctedAngle; if (isConvex) @@ -297,18 +285,50 @@ struct btConnectivityProcessor : public btTriangleCallback break; } - default: + default: { // printf("warning: duplicate triangle\n"); } - } } }; + + +struct b3ProcessAllTrianglesHeightfield: public btTriangleCallback +{ + btHeightfieldTerrainShape* m_heightfieldShape; + btTriangleInfoMap* m_triangleInfoMap; + + + b3ProcessAllTrianglesHeightfield(btHeightfieldTerrainShape* heightFieldShape, btTriangleInfoMap* triangleInfoMap) + :m_heightfieldShape(heightFieldShape), + m_triangleInfoMap(triangleInfoMap) + { + } + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + btConnectivityProcessor connectivityProcessor; + connectivityProcessor.m_partIdA = partId; + connectivityProcessor.m_triangleIndexA = triangleIndex; + connectivityProcessor.m_triangleVerticesA = triangle; + connectivityProcessor.m_triangleInfoMap = m_triangleInfoMap; + btVector3 aabbMin, aabbMax; + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + aabbMin.setMin(triangle[0]); + aabbMax.setMax(triangle[0]); + aabbMin.setMin(triangle[1]); + aabbMax.setMax(triangle[1]); + aabbMin.setMin(triangle[2]); + aabbMax.setMax(triangle[2]); + + m_heightfieldShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax); + } +}; ///////////////////////////////////////////////////////// ///////////////////////////////////////////////////////// -void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap) +void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap) { //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there! if (trimeshShape->getTriangleInfoMap()) @@ -319,46 +339,51 @@ void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangle btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface(); const btVector3& meshScaling = meshInterface->getScaling(); - for (int partId = 0; partId< meshInterface->getNumSubParts();partId++) + for (int partId = 0; partId < meshInterface->getNumSubParts(); partId++) { - const unsigned char *vertexbase = 0; + const unsigned char* vertexbase = 0; int numverts = 0; PHY_ScalarType type = PHY_INTEGER; int stride = 0; - const unsigned char *indexbase = 0; + const unsigned char* indexbase = 0; int indexstride = 0; int numfaces = 0; PHY_ScalarType indicestype = PHY_INTEGER; //PHY_ScalarType indexType=0; btVector3 triangleVerts[3]; - meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId); - btVector3 aabbMin,aabbMax; + meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase, numverts, type, stride, &indexbase, indexstride, numfaces, indicestype, partId); + btVector3 aabbMin, aabbMax; - for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++) + for (int triangleIndex = 0; triangleIndex < numfaces; triangleIndex++) { - unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride); + unsigned int* gfxbase = (unsigned int*)(indexbase + triangleIndex * indexstride); - for (int j=2;j>=0;j--) + for (int j = 2; j >= 0; j--) { - - int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + int graphicsindex; + switch (indicestype) { + case PHY_INTEGER: graphicsindex = gfxbase[j]; break; + case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; + case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; + default: btAssert(0); + } if (type == PHY_FLOAT) { - float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); triangleVerts[j] = btVector3( - graphicsbase[0]*meshScaling.getX(), - graphicsbase[1]*meshScaling.getY(), - graphicsbase[2]*meshScaling.getZ()); + graphicsbase[0] * meshScaling.getX(), + graphicsbase[1] * meshScaling.getY(), + graphicsbase[2] * meshScaling.getZ()); } else { - double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); - triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ())); + double* graphicsbase = (double*)(vertexbase + graphicsindex * stride); + triangleVerts[j] = btVector3(btScalar(graphicsbase[0] * meshScaling.getX()), btScalar(graphicsbase[1] * meshScaling.getY()), btScalar(graphicsbase[2] * meshScaling.getZ())); } } - aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); aabbMin.setMin(triangleVerts[0]); aabbMax.setMax(triangleVerts[0]); aabbMin.setMin(triangleVerts[1]); @@ -370,131 +395,177 @@ void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangle connectivityProcessor.m_partIdA = partId; connectivityProcessor.m_triangleIndexA = triangleIndex; connectivityProcessor.m_triangleVerticesA = &triangleVerts[0]; - connectivityProcessor.m_triangleInfoMap = triangleInfoMap; + connectivityProcessor.m_triangleInfoMap = triangleInfoMap; - trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax); + trimeshShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax); } - } - } +void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* heightfieldShape, btTriangleInfoMap* triangleInfoMap) +{ + + //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there! + if (heightfieldShape->getTriangleInfoMap()) + return; + + heightfieldShape->setTriangleInfoMap(triangleInfoMap); + + //get all the triangles of the heightfield + btVector3 aabbMin, aabbMax; + + aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); + aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); + + b3ProcessAllTrianglesHeightfield processHeightfield(heightfieldShape, triangleInfoMap); + heightfieldShape->processAllTriangles(&processHeightfield, aabbMin, aabbMax); + +} // Given a point and a line segment (defined by two points), compute the closest point // in the line. Cap the point at the endpoints of the line segment. -void btNearestPointInLineSegment(const btVector3 &point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint) +void btNearestPointInLineSegment(const btVector3& point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint) { - btVector3 lineDelta = line1 - line0; + btVector3 lineDelta = line1 - line0; // Handle degenerate lines - if ( lineDelta.fuzzyZero()) + if (lineDelta.fuzzyZero()) { nearestPoint = line0; } else { - btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta); + btScalar delta = (point - line0).dot(lineDelta) / (lineDelta).dot(lineDelta); // Clamp the point to conform to the segment's endpoints - if ( delta < 0 ) + if (delta < 0) delta = 0; - else if ( delta > 1 ) + else if (delta > 1) delta = 1; - nearestPoint = line0 + lineDelta*delta; + nearestPoint = line0 + lineDelta * delta; } } - - - -bool btClampNormal(const btVector3& edge,const btVector3& tri_normal_org,const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3 & clampedLocalNormal) +bool btClampNormal(const btVector3& edge, const btVector3& tri_normal_org, const btVector3& localContactNormalOnB, btScalar correctedEdgeAngle, btVector3& clampedLocalNormal) { btVector3 tri_normal = tri_normal_org; //we only have a local triangle normal, not a local contact normal -> only normal in world space... //either compute the current angle all in local space, or all in world space btVector3 edgeCross = edge.cross(tri_normal).normalize(); - btScalar curAngle = btGetAngle(edgeCross,tri_normal,localContactNormalOnB); + btScalar curAngle = btGetAngle(edgeCross, tri_normal, localContactNormalOnB); - if (correctedEdgeAngle<0) + if (correctedEdgeAngle < 0) { if (curAngle < correctedEdgeAngle) { - btScalar diffAngle = correctedEdgeAngle-curAngle; - btQuaternion rotation(edge,diffAngle ); - clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB; + btScalar diffAngle = correctedEdgeAngle - curAngle; + btQuaternion rotation(edge, diffAngle); + clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB; return true; } } - if (correctedEdgeAngle>=0) + if (correctedEdgeAngle >= 0) { if (curAngle > correctedEdgeAngle) { - btScalar diffAngle = correctedEdgeAngle-curAngle; - btQuaternion rotation(edge,diffAngle ); - clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB; + btScalar diffAngle = correctedEdgeAngle - curAngle; + btQuaternion rotation(edge, diffAngle); + clampedLocalNormal = btMatrix3x3(rotation) * localContactNormalOnB; return true; } } return false; } - - /// Changes a btManifoldPoint collision normal to the normal from the mesh. -void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags) +void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags) { //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE); if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE) return; + + btTriangleInfoMap* triangleInfoMapPtr = 0; + + if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TERRAIN_SHAPE_PROXYTYPE) + { + btHeightfieldTerrainShape* heightfield = (btHeightfieldTerrainShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); + triangleInfoMapPtr = heightfield->getTriangleInfoMap(); + +//#define USE_HEIGHTFIELD_TRIANGLES +#ifdef USE_HEIGHTFIELD_TRIANGLES + btVector3 newNormal = btVector3(0, 0, 1); + + const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape()); + btVector3 tri_normal; + tri_shape->calcNormal(tri_normal); + newNormal = tri_normal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. (what about cp.m_distance1?) + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + return; +#endif + } + + btBvhTriangleMeshShape* trimesh = 0; + + if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape(); + } + else + { + if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); + } + } + if (trimesh) + { + triangleInfoMapPtr = (btTriangleInfoMap*)trimesh->getTriangleInfoMap(); + } + - if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE ) - trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape(); - else - trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); - - btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); if (!triangleInfoMapPtr) return; - int hash = btGetHash(partId0,index0); - + int hash = btGetHash(partId0, index0); btTriangleInfo* info = triangleInfoMapPtr->find(hash); if (!info) return; - btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f; - + btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE) == 0 ? 1.f : -1.f; + const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape()); - btVector3 v0,v1,v2; - tri_shape->getVertex(0,v0); - tri_shape->getVertex(1,v1); - tri_shape->getVertex(2,v2); + btVector3 v0, v1, v2; + tri_shape->getVertex(0, v0); + tri_shape->getVertex(1, v1); + tri_shape->getVertex(2, v2); //btVector3 center = (v0+v1+v2)*btScalar(1./3.); - btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0); + btVector3 red(1, 0, 0), green(0, 1, 0), blue(0, 0, 1), white(1, 1, 1), black(0, 0, 0); btVector3 tri_normal; tri_shape->calcNormal(tri_normal); //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB); btVector3 nearest; - btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest); + btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest); btVector3 contact = cp.m_localPointB; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW const btTransform& tr = colObj0->getWorldTransform(); - btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - + btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, red); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW bool isNearEdge = false; @@ -502,334 +573,325 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr int numConvexEdgeHits = 0; btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; - localContactNormalOnB.normalize();//is this necessary? - + localContactNormalOnB.normalize(); //is this necessary? + // Get closest edge - int bestedge=-1; - btScalar disttobestedge=BT_LARGE_FLOAT; + int bestedge = -1; + btScalar disttobestedge = BT_LARGE_FLOAT; // // Edge 0 -> 1 - if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) - { - btVector3 nearest; - btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest ); - btScalar len=(contact-nearest).length(); - // - if( len < disttobestedge ) - { - bestedge=0; - disttobestedge=len; - } - } + if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment(cp.m_localPointB, v0, v1, nearest); + btScalar len = (contact - nearest).length(); + // + if (len < disttobestedge) + { + bestedge = 0; + disttobestedge = len; + } + } // Edge 1 -> 2 - if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) - { - btVector3 nearest; - btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest ); - btScalar len=(contact-nearest).length(); - // - if( len < disttobestedge ) - { - bestedge=1; - disttobestedge=len; - } - } + if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment(cp.m_localPointB, v1, v2, nearest); + btScalar len = (contact - nearest).length(); + // + if (len < disttobestedge) + { + bestedge = 1; + disttobestedge = len; + } + } // Edge 2 -> 0 - if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) - { - btVector3 nearest; - btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest ); - btScalar len=(contact-nearest).length(); - // - if( len < disttobestedge ) - { - bestedge=2; - disttobestedge=len; - } - } - + if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment(cp.m_localPointB, v2, v0, nearest); + btScalar len = (contact - nearest).length(); + // + if (len < disttobestedge) + { + bestedge = 2; + disttobestedge = len; + } + } + #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f); - btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red ); -#endif - if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + btVector3 upfix = tri_normal * btVector3(0.1f, 0.1f, 0.1f); + btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red); +#endif + if (btFabs(info->m_edgeV0V1Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); #endif - btScalar len = (contact-nearest).length(); - if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) - if( bestedge==0 ) - { - btVector3 edge(v0-v1); - isNearEdge = true; - - if (info->m_edgeV0V1Angle==btScalar(0)) - { - numConcaveEdgeHits++; - } else + btScalar len = (contact - nearest).length(); + if (len < triangleInfoMapPtr->m_edgeDistanceThreshold) + if (bestedge == 0) { + btVector3 edge(v0 - v1); + isNearEdge = true; - bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX); - btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); - #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); - #endif //BT_INTERNAL_EDGE_DEBUG_DRAW + if (info->m_edgeV0V1Angle == btScalar(0)) + { + numConcaveEdgeHits++; + } + else + { + bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX); + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - btVector3 nA = swapFactor * tri_normal; + btVector3 nA = swapFactor * tri_normal; - btQuaternion orn(edge,info->m_edgeV0V1Angle); - btVector3 computedNormalB = quatRotate(orn,tri_normal); - if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB) - computedNormalB*=-1; - btVector3 nB = swapFactor*computedNormalB; + btQuaternion orn(edge, info->m_edgeV0V1Angle); + btVector3 computedNormalB = quatRotate(orn, tri_normal); + if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB) + computedNormalB *= -1; + btVector3 nB = swapFactor * computedNormalB; - btScalar NdotA = localContactNormalOnB.dot(nA); - btScalar NdotB = localContactNormalOnB.dot(nB); - bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon); #ifdef DEBUG_INTERNAL_EDGE - { - - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); - } -#endif //DEBUG_INTERNAL_EDGE - + { + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red); + } +#endif //DEBUG_INTERNAL_EDGE - if (backFacingNormal) - { - numConcaveEdgeHits++; - } - else - { - numConvexEdgeHits++; - btVector3 clampedLocalNormal; - bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal); - if (isClamped) + if (backFacingNormal) { - if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + numConcaveEdgeHits++; + } + else + { + numConvexEdgeHits++; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV0V1Angle, clampedLocalNormal); + if (isClamped) { - btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; - // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); - cp.m_normalWorldOnB = newNormal; - // Reproject collision point along normal. (what about cp.m_distance1?) - cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; - cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); - + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0)) + { + btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. (what about cp.m_distance1?) + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } } } } } - } } - btNearestPointInLineSegment(contact,v1,v2,nearest); + btNearestPointInLineSegment(contact, v1, v2, nearest); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, green); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green ); -#endif + btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix, green); +#endif - if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + if (btFabs(info->m_edgeV1V2Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - - - btScalar len = (contact-nearest).length(); - if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) - if( bestedge==1 ) - { - isNearEdge = true; -#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 edge(v1-v2); - - isNearEdge = true; + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - if (info->m_edgeV1V2Angle == btScalar(0)) + btScalar len = (contact - nearest).length(); + if (len < triangleInfoMapPtr->m_edgeDistanceThreshold) + if (bestedge == 1) { - numConcaveEdgeHits++; - } else - { - bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0; - btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); - #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); - #endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 nA = swapFactor * tri_normal; - - btQuaternion orn(edge,info->m_edgeV1V2Angle); - btVector3 computedNormalB = quatRotate(orn,tri_normal); - if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB) - computedNormalB*=-1; - btVector3 nB = swapFactor*computedNormalB; - -#ifdef DEBUG_INTERNAL_EDGE - { - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); - } -#endif //DEBUG_INTERNAL_EDGE + isNearEdge = true; +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btVector3 edge(v1 - v2); - btScalar NdotA = localContactNormalOnB.dot(nA); - btScalar NdotB = localContactNormalOnB.dot(nB); - bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); + isNearEdge = true; - if (backFacingNormal) + if (info->m_edgeV1V2Angle == btScalar(0)) { numConcaveEdgeHits++; } else { - numConvexEdgeHits++; - btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; - btVector3 clampedLocalNormal; - bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal); - if (isClamped) + bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX) != 0; + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 nA = swapFactor * tri_normal; + + btQuaternion orn(edge, info->m_edgeV1V2Angle); + btVector3 computedNormalB = quatRotate(orn, tri_normal); + if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB) + computedNormalB *= -1; + btVector3 nB = swapFactor * computedNormalB; + +#ifdef DEBUG_INTERNAL_EDGE + { + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red); + } +#endif //DEBUG_INTERNAL_EDGE + + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon); + + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else { - if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + numConvexEdgeHits++; + btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV1V2Angle, clampedLocalNormal); + if (isClamped) { - btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; - // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); - cp.m_normalWorldOnB = newNormal; - // Reproject collision point along normal. - cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; - cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0)) + { + btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } } } } } - } } - btNearestPointInLineSegment(contact,v2,v0,nearest); + btNearestPointInLineSegment(contact, v2, v0, nearest); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * cp.m_localPointB, blue); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue ); -#endif + btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix, blue); +#endif - if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + if (btFabs(info->m_edgeV2V0Angle) < triangleInfoMapPtr->m_maxEdgeAngleThreshold) { - -#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btScalar len = (contact-nearest).length(); - if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) - if( bestedge==2 ) - { - isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 edge(v2-v0); + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - if (info->m_edgeV2V0Angle==btScalar(0)) - { - numConcaveEdgeHits++; - } else + btScalar len = (contact - nearest).length(); + if (len < triangleInfoMapPtr->m_edgeDistanceThreshold) + if (bestedge == 2) { + isNearEdge = true; +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0; - btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); - #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW - btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); - #endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - btVector3 nA = swapFactor * tri_normal; - btQuaternion orn(edge,info->m_edgeV2V0Angle); - btVector3 computedNormalB = quatRotate(orn,tri_normal); - if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB) - computedNormalB*=-1; - btVector3 nB = swapFactor*computedNormalB; - -#ifdef DEBUG_INTERNAL_EDGE - { - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); - } -#endif //DEBUG_INTERNAL_EDGE - - btScalar NdotA = localContactNormalOnB.dot(nA); - btScalar NdotB = localContactNormalOnB.dot(nB); - bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); + btVector3 edge(v2 - v0); - if (backFacingNormal) + if (info->m_edgeV2V0Angle == btScalar(0)) { numConcaveEdgeHits++; } else { - numConvexEdgeHits++; - // printf("hitting convex edge\n"); + bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX) != 0; + btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * nearest, tr * (nearest + swapFactor * tri_normal * 10), white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 nA = swapFactor * tri_normal; + btQuaternion orn(edge, info->m_edgeV2V0Angle); + btVector3 computedNormalB = quatRotate(orn, tri_normal); + if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB) + computedNormalB *= -1; + btVector3 nB = swapFactor * computedNormalB; + +#ifdef DEBUG_INTERNAL_EDGE + { + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + tr.getBasis() * (nB * 20), red); + } +#endif //DEBUG_INTERNAL_EDGE + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA < triangleInfoMapPtr->m_convexEpsilon) && (NdotB < triangleInfoMapPtr->m_convexEpsilon); - btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; - btVector3 clampedLocalNormal; - bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal); - if (isClamped) + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else { - if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + numConvexEdgeHits++; + // printf("hitting convex edge\n"); + + btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge, swapFactor * tri_normal, localContactNormalOnB, info->m_edgeV2V0Angle, clampedLocalNormal); + if (isClamped) { - btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; - // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); - cp.m_normalWorldOnB = newNormal; - // Reproject collision point along normal. - cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; - cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED) != 0) || (clampedLocalNormal.dot(frontFacing * tri_normal) > 0)) + { + btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } } } - } + } } - - - } } #ifdef DEBUG_INTERNAL_EDGE { - btVector3 color(0,1,1); - btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color); + btVector3 color(0, 1, 1); + btDebugDrawLine(cp.getPositionWorldOnB(), cp.getPositionWorldOnB() + cp.m_normalWorldOnB * 10, color); } -#endif //DEBUG_INTERNAL_EDGE +#endif //DEBUG_INTERNAL_EDGE if (isNearEdge) { - - if (numConcaveEdgeHits>0) + if (numConcaveEdgeHits > 0) { - if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0) + if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED) != 0) { //fix tri_normal so it pointing the same direction as the current local contact normal if (tri_normal.dot(localContactNormalOnB) < 0) { tri_normal *= -1; } - cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis()*tri_normal; - } else + cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * tri_normal; + } + else { - btVector3 newNormal = tri_normal *frontFacing; + btVector3 newNormal = tri_normal * frontFacing; //if the tri_normal is pointing opposite direction as the current local contact normal, skip it - btScalar d = newNormal.dot(localContactNormalOnB) ; - if (d< 0) + btScalar d = newNormal.dot(localContactNormalOnB); + if (d < 0) { return; } //modify the normal to be the triangle normal (or backfacing normal) - cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() *newNormal; + cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() * newNormal; } - + // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h index 7d9aafeee69..cc6d11c2418 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h @@ -15,33 +15,30 @@ class btCollisionObject; struct btCollisionObjectWrapper; class btManifoldPoint; class btIDebugDraw; - - +class btHeightfieldTerrainShape; enum btInternalEdgeAdjustFlags { BT_TRIANGLE_CONVEX_BACKFACE_MODE = 1, - BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended + BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended BT_TRIANGLE_CONVEX_DOUBLE_SIDED = 4 }; - ///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo' -void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap); +void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap); +void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* trimeshShape, btTriangleInfoMap* triangleInfoMap); ///Call the btFixMeshNormal to adjust the collision normal, using the triangle info map (generated using btGenerateInternalEdgeInfo) ///If this info map is missing, or the triangle is not store in this map, nothing will be done -void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap,const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0); +void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap, const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0); ///Enable the BT_INTERNAL_EDGE_DEBUG_DRAW define and call btSetDebugDrawer, to get visual info to see if the internal edge utility works properly. ///If the utility doesn't work properly, you might have to adjust the threshold values in btTriangleInfoMap //#define BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW -void btSetDebugDrawer(btIDebugDraw* debugDrawer); -#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - - -#endif //BT_INTERNAL_EDGE_UTILITY_H +void btSetDebugDrawer(btIDebugDraw* debugDrawer); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW +#endif //BT_INTERNAL_EDGE_UTILITY_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 4b2986a0087..770eb243691 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -13,142 +13,191 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btManifoldResult.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" ///This is to allow MaterialCombiner/Custom Friction/Restitution values -ContactAddedCallback gContactAddedCallback=0; - +ContactAddedCallback gContactAddedCallback = 0; +CalculateCombinedCallback gCalculateCombinedRestitutionCallback = &btManifoldResult::calculateCombinedRestitution; +CalculateCombinedCallback gCalculateCombinedFrictionCallback = &btManifoldResult::calculateCombinedFriction; +CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback = &btManifoldResult::calculateCombinedRollingFriction; +CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback = &btManifoldResult::calculateCombinedSpinningFriction; +CalculateCombinedCallback gCalculateCombinedContactDampingCallback = &btManifoldResult::calculateCombinedContactDamping; +CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback = &btManifoldResult::calculateCombinedContactStiffness; -///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; -inline btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1) +btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0, const btCollisionObject* body1) { - btScalar friction = body0->getRollingFriction() * body1->getRollingFriction(); + btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction(); - const btScalar MAX_FRICTION = btScalar(10.); + const btScalar MAX_FRICTION = btScalar(10.); if (friction < -MAX_FRICTION) friction = -MAX_FRICTION; if (friction > MAX_FRICTION) friction = MAX_FRICTION; return friction; - } +btScalar btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0, const btCollisionObject* body1) +{ + btScalar friction = body0->getSpinningFriction() * body1->getFriction() + body1->getSpinningFriction() * body0->getFriction(); + + const btScalar MAX_FRICTION = btScalar(10.); + if (friction < -MAX_FRICTION) + friction = -MAX_FRICTION; + if (friction > MAX_FRICTION) + friction = MAX_FRICTION; + return friction; +} ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; -btScalar btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1) +btScalar btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0, const btCollisionObject* body1) { btScalar friction = body0->getFriction() * body1->getFriction(); - const btScalar MAX_FRICTION = btScalar(10.); + const btScalar MAX_FRICTION = btScalar(10.); if (friction < -MAX_FRICTION) friction = -MAX_FRICTION; if (friction > MAX_FRICTION) friction = MAX_FRICTION; return friction; - } -btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1) +btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0, const btCollisionObject* body1) { return body0->getRestitution() * body1->getRestitution(); } +btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0, const btCollisionObject* body1) +{ + return body0->getContactDamping() + body1->getContactDamping(); +} + +btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0, const btCollisionObject* body1) +{ + btScalar s0 = body0->getContactStiffness(); + btScalar s1 = body1->getContactStiffness(); + btScalar tmp0 = btScalar(1) / s0; + btScalar tmp1 = btScalar(1) / s1; + btScalar combinedStiffness = btScalar(1) / (tmp0 + tmp1); + return combinedStiffness; +} -btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) - :m_manifoldPtr(0), - m_body0Wrap(body0Wrap), - m_body1Wrap(body1Wrap) +btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) + : m_manifoldPtr(0), + m_body0Wrap(body0Wrap), + m_body1Wrap(body1Wrap) #ifdef DEBUG_PART_INDEX - ,m_partId0(-1), - m_partId1(-1), - m_index0(-1), - m_index1(-1) -#endif //DEBUG_PART_INDEX + , + m_partId0(-1), + m_partId1(-1), + m_index0(-1), + m_index1(-1) +#endif //DEBUG_PART_INDEX + , + m_closestPointDistanceThreshold(0) { } - -void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) +void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) { btAssert(m_manifoldPtr); //order in manifold needs to match if (depth > m_manifoldPtr->getContactBreakingThreshold()) -// if (depth > m_manifoldPtr->getContactProcessingThreshold()) + // if (depth > m_manifoldPtr->getContactProcessingThreshold()) return; bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + bool isNewCollision = m_manifoldPtr->getNumContacts() == 0; btVector3 pointA = pointInWorld + normalOnBInWorld * depth; btVector3 localA; btVector3 localB; - + if (isSwapped) { - localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA); localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); - } else + } + else { - localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA); localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); } - btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + btManifoldPoint newPt(localA, localB, normalOnBInWorld, depth); newPt.m_positionWorldOnA = pointA; newPt.m_positionWorldOnB = pointInWorld; - + int insertIndex = m_manifoldPtr->getCacheEntry(newPt); - newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); - newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); - newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); - btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2); - + newPt.m_combinedFriction = gCalculateCombinedFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); + newPt.m_combinedRestitution = gCalculateCombinedRestitutionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); + newPt.m_combinedRollingFriction = gCalculateCombinedRollingFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); + newPt.m_combinedSpinningFriction = gCalculateCombinedSpinningFrictionCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); - - //BP mod, store contact triangles. + if ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)) + { + newPt.m_combinedContactDamping1 = gCalculateCombinedContactDampingCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); + newPt.m_combinedContactStiffness1 = gCalculateCombinedContactStiffnessCallback(m_body0Wrap->getCollisionObject(), m_body1Wrap->getCollisionObject()); + newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING; + } + + if ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_HAS_FRICTION_ANCHOR)) + { + newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR; + } + + btPlaneSpace1(newPt.m_normalWorldOnB, newPt.m_lateralFrictionDir1, newPt.m_lateralFrictionDir2); + + //BP mod, store contact triangles. if (isSwapped) { newPt.m_partId0 = m_partId1; newPt.m_partId1 = m_partId0; - newPt.m_index0 = m_index1; - newPt.m_index1 = m_index0; - } else + newPt.m_index0 = m_index1; + newPt.m_index1 = m_index0; + } + else { newPt.m_partId0 = m_partId0; newPt.m_partId1 = m_partId1; - newPt.m_index0 = m_index0; - newPt.m_index1 = m_index1; + newPt.m_index0 = m_index0; + newPt.m_index1 = m_index1; } //printf("depth=%f\n",depth); ///@todo, check this for any side effects if (insertIndex >= 0) { //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); - m_manifoldPtr->replaceContactPoint(newPt,insertIndex); - } else + m_manifoldPtr->replaceContactPoint(newPt, insertIndex); + } + else { insertIndex = m_manifoldPtr->addManifoldPoint(newPt); } - + //User can override friction and/or restitution if (gContactAddedCallback && //and if either of the two bodies requires custom material - ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) || - (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK))) + ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK))) { //experimental feature info, for per-triangle material etc. - const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap; - const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap; - (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1); + const btCollisionObjectWrapper* obj0Wrap = isSwapped ? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1Wrap = isSwapped ? m_body0Wrap : m_body1Wrap; + (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex), obj0Wrap, newPt.m_partId0, newPt.m_index0, obj1Wrap, newPt.m_partId1, newPt.m_index1); } + if (gContactStartedCallback && isNewCollision) + { + gContactStartedCallback(m_manifoldPtr); + } } - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 977b9a02fc5..6c0a2d9a43f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_MANIFOLD_RESULT_H #define BT_MANIFOLD_RESULT_H @@ -29,74 +28,81 @@ class btManifoldPoint; #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" -typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1); -extern ContactAddedCallback gContactAddedCallback; +typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1); +extern ContactAddedCallback gContactAddedCallback; //#define DEBUG_PART_INDEX 1 +/// These callbacks are used to customize the algorith that combine restitution, friction, damping, Stiffness +typedef btScalar (*CalculateCombinedCallback)(const btCollisionObject* body0, const btCollisionObject* body1); + +extern CalculateCombinedCallback gCalculateCombinedRestitutionCallback; +extern CalculateCombinedCallback gCalculateCombinedFrictionCallback; +extern CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback; +extern CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback; +extern CalculateCombinedCallback gCalculateCombinedContactDampingCallback; +extern CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback; ///btManifoldResult is a helper class to manage contact results. class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result { protected: - btPersistentManifold* m_manifoldPtr; const btCollisionObjectWrapper* m_body0Wrap; const btCollisionObjectWrapper* m_body1Wrap; - int m_partId0; + int m_partId0; int m_partId1; int m_index0; int m_index1; - public: - btManifoldResult() -#ifdef DEBUG_PART_INDEX : - m_partId0(-1), - m_partId1(-1), - m_index0(-1), - m_index1(-1) -#endif //DEBUG_PART_INDEX +#ifdef DEBUG_PART_INDEX + + m_partId0(-1), + m_partId1(-1), + m_index0(-1), + m_index1(-1) +#endif //DEBUG_PART_INDEX + m_closestPointDistanceThreshold(0) { } - btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + btManifoldResult(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap); - virtual ~btManifoldResult() {}; + virtual ~btManifoldResult(){}; - void setPersistentManifold(btPersistentManifold* manifoldPtr) + void setPersistentManifold(btPersistentManifold* manifoldPtr) { m_manifoldPtr = manifoldPtr; } - const btPersistentManifold* getPersistentManifold() const + const btPersistentManifold* getPersistentManifold() const { return m_manifoldPtr; } - btPersistentManifold* getPersistentManifold() + btPersistentManifold* getPersistentManifold() { return m_manifoldPtr; } - virtual void setShapeIdentifiersA(int partId0,int index0) + virtual void setShapeIdentifiersA(int partId0, int index0) { - m_partId0=partId0; - m_index0=index0; + m_partId0 = partId0; + m_index0 = index0; } - virtual void setShapeIdentifiersB( int partId1,int index1) + virtual void setShapeIdentifiersB(int partId1, int index1) { - m_partId1=partId1; - m_index1=index1; + m_partId1 = partId1; + m_index1 = index1; } + virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth); - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth); - - SIMD_FORCE_INLINE void refreshContactPoints() + SIMD_FORCE_INLINE void refreshContactPoints() { btAssert(m_manifoldPtr); if (!m_manifoldPtr->getNumContacts()) @@ -106,10 +112,11 @@ public: if (isSwapped) { - m_manifoldPtr->refreshContactPoints(m_body1Wrap->getCollisionObject()->getWorldTransform(),m_body0Wrap->getCollisionObject()->getWorldTransform()); - } else + m_manifoldPtr->refreshContactPoints(m_body1Wrap->getCollisionObject()->getWorldTransform(), m_body0Wrap->getCollisionObject()->getWorldTransform()); + } + else { - m_manifoldPtr->refreshContactPoints(m_body0Wrap->getCollisionObject()->getWorldTransform(),m_body1Wrap->getCollisionObject()->getWorldTransform()); + m_manifoldPtr->refreshContactPoints(m_body0Wrap->getCollisionObject()->getWorldTransform(), m_body1Wrap->getCollisionObject()->getWorldTransform()); } } @@ -142,9 +149,15 @@ public: return m_body1Wrap->getCollisionObject(); } + btScalar m_closestPointDistanceThreshold; + /// in the future we can let the user override the methods to combine restitution and friction - static btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1); - static btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedRestitution(const btCollisionObject* body0, const btCollisionObject* body1); + static btScalar calculateCombinedFriction(const btCollisionObject* body0, const btCollisionObject* body1); + static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0, const btCollisionObject* body1); + static btScalar calculateCombinedSpinningFriction(const btCollisionObject* body0, const btCollisionObject* body1); + static btScalar calculateCombinedContactDamping(const btCollisionObject* body0, const btCollisionObject* body1); + static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0, const btCollisionObject* body1); }; -#endif //BT_MANIFOLD_RESULT_H +#endif //BT_MANIFOLD_RESULT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 13447822571..327b3f076a6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -14,7 +14,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "LinearMath/btScalar.h" #include "btSimulationIslandManager.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" @@ -25,8 +24,7 @@ subject to the following restrictions: //#include <stdio.h> #include "LinearMath/btQuickprof.h" -btSimulationIslandManager::btSimulationIslandManager(): -m_splitIslands(true) +btSimulationIslandManager::btSimulationIslandManager() : m_splitIslands(true) { } @@ -34,53 +32,47 @@ btSimulationIslandManager::~btSimulationIslandManager() { } - void btSimulationIslandManager::initUnionFind(int n) { - m_unionFind.reset(n); + m_unionFind.reset(n); } - -void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld) +void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */, btCollisionWorld* colWorld) { - { btOverlappingPairCache* pairCachePtr = colWorld->getPairCache(); const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs(); if (numOverlappingPairs) { - btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr(); - - for (int i=0;i<numOverlappingPairs;i++) - { - const btBroadphasePair& collisionPair = pairPtr[i]; - btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; + btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr(); - if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && - ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + for (int i = 0; i < numOverlappingPairs; i++) { + const btBroadphasePair& collisionPair = pairPtr[i]; + btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; - m_unionFind.unite((colObj0)->getIslandTag(), - (colObj1)->getIslandTag()); + if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && + ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + { + m_unionFind.unite((colObj0)->getIslandTag(), + (colObj1)->getIslandTag()); + } } } - } } } #ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION -void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) +void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher) { - - // put the index into m_controllers into m_tag + // put the index into m_controllers into m_tag int index = 0; { - int i; - for (i=0;i<colWorld->getCollisionObjectArray().size(); i++) + for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++) { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i]; //Adding filtering here if (!collisionObject->isStaticOrKinematicObject()) { @@ -92,28 +84,29 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWor } // do the union find - initUnionFind( index ); + initUnionFind(index); - findUnions(dispatcher,colWorld); + findUnions(dispatcher, colWorld); } -void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) +void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) { - // put the islandId ('find' value) into m_tag + // put the islandId ('find' value) into m_tag { int index = 0; int i; - for (i=0;i<colWorld->getCollisionObjectArray().size();i++) + for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++) { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i]; if (!collisionObject->isStaticOrKinematicObject()) { - collisionObject->setIslandTag( m_unionFind.find(index) ); + collisionObject->setIslandTag(m_unionFind.find(index)); //Set the correct object offset in Collision Object Array m_unionFind.getElement(index).m_sz = i; collisionObject->setCompanionId(-1); index++; - } else + } + else { collisionObject->setIslandTag(-1); collisionObject->setCompanionId(-2); @@ -122,49 +115,44 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* c } } - -#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION -void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) +#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION +void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher) { + initUnionFind(int(colWorld->getCollisionObjectArray().size())); - initUnionFind( int (colWorld->getCollisionObjectArray().size())); - - // put the index into m_controllers into m_tag + // put the index into m_controllers into m_tag { - int index = 0; int i; - for (i=0;i<colWorld->getCollisionObjectArray().size(); i++) + for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++) { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i]; collisionObject->setIslandTag(index); collisionObject->setCompanionId(-1); collisionObject->setHitFraction(btScalar(1.)); index++; - } } // do the union find - findUnions(dispatcher,colWorld); + findUnions(dispatcher, colWorld); } -void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) +void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) { - // put the islandId ('find' value) into m_tag + // put the islandId ('find' value) into m_tag { - - int index = 0; int i; - for (i=0;i<colWorld->getCollisionObjectArray().size();i++) + for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++) { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i]; if (!collisionObject->isStaticOrKinematicObject()) { - collisionObject->setIslandTag( m_unionFind.find(index) ); + collisionObject->setIslandTag(m_unionFind.find(index)); collisionObject->setCompanionId(-1); - } else + } + else { collisionObject->setIslandTag(-1); collisionObject->setCompanionId(-2); @@ -174,56 +162,59 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col } } -#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION +#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION -inline int getIslandId(const btPersistentManifold* lhs) +inline int getIslandId(const btPersistentManifold* lhs) { int islandId; const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0()); const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1()); - islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag(); + islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag(); return islandId; - } - - /// function object that routes calls to operator< class btPersistentManifoldSortPredicate { - public: - - SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const - { - return getIslandId(lhs) < getIslandId(rhs); - } +public: + SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const + { + return getIslandId(lhs) < getIslandId(rhs); + } }; - -void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld) +class btPersistentManifoldSortPredicateDeterministic { +public: + SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const + { + return ( + (getIslandId(lhs) < getIslandId(rhs)) || ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) || ((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) && (lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId))); + } +}; +void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld) +{ BT_PROFILE("islandUnionFindAndQuickSort"); - + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); m_islandmanifold.resize(0); //we are going to sort the unionfind array, and store the element id in the size //afterwards, we clean unionfind, to make sure no-one uses it anymore - + getUnionFind().sortIslands(); int numElem = getUnionFind().getNumElements(); - int endIslandIndex=1; + int endIslandIndex = 1; int startIslandIndex; - //update the sleeping state for bodies, if all are sleeping - for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) + for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) { int islandId = getUnionFind().getElement(startIslandIndex).m_id; - for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) + for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++) { } @@ -232,71 +223,69 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio bool allSleeping = true; int idx; - for (idx=startIslandIndex;idx<endIslandIndex;idx++) + for (idx = startIslandIndex; idx < endIslandIndex; idx++) { int i = getUnionFind().getElement(idx).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { -// printf("error in island management\n"); + // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - if (colObj0->getActivationState()== ACTIVE_TAG) - { - allSleeping = false; - } - if (colObj0->getActivationState()== DISABLE_DEACTIVATION) + if (colObj0->getActivationState() == ACTIVE_TAG || + colObj0->getActivationState() == DISABLE_DEACTIVATION) { allSleeping = false; + break; } } } - if (allSleeping) { int idx; - for (idx=startIslandIndex;idx<endIslandIndex;idx++) + for (idx = startIslandIndex; idx < endIslandIndex; idx++) { int i = getUnionFind().getElement(idx).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { -// printf("error in island management\n"); + // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - colObj0->setActivationState( ISLAND_SLEEPING ); + colObj0->setActivationState(ISLAND_SLEEPING); } } - } else + } + else { - int idx; - for (idx=startIslandIndex;idx<endIslandIndex;idx++) + for (idx = startIslandIndex; idx < endIslandIndex; idx++) { int i = getUnionFind().getElement(idx).m_sz; btCollisionObject* colObj0 = collisionObjects[i]; if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) { -// printf("error in island management\n"); + // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + if (colObj0->getIslandTag() == islandId) { - if ( colObj0->getActivationState() == ISLAND_SLEEPING) + if (colObj0->getActivationState() == ISLAND_SLEEPING) { - colObj0->setActivationState( WANTS_DEACTIVATION); + colObj0->setActivationState(WANTS_DEACTIVATION); colObj0->setDeactivationTime(0.f); } } @@ -304,29 +293,30 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio } } - int i; int maxNumManifolds = dispatcher->getNumManifolds(); -//#define SPLIT_ISLANDS 1 -//#ifdef SPLIT_ISLANDS + //#define SPLIT_ISLANDS 1 + //#ifdef SPLIT_ISLANDS - -//#endif //SPLIT_ISLANDS + //#endif //SPLIT_ISLANDS - - for (i=0;i<maxNumManifolds ;i++) + for (i = 0; i < maxNumManifolds; i++) { - btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); - - const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); - const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); - - ///@todo: check sleeping conditions! - if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || + btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); + if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) + { + if (manifold->getNumContacts() == 0) + continue; + } + + const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); + const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); + + ///@todo: check sleeping conditions! + if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) { - //kinematic objects don't merge islands, but wake up all connected objects if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) { @@ -338,10 +328,10 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio if (colObj1->hasContactResponse()) colObj0->activate(); } - if(m_splitIslands) - { + if (m_splitIslands) + { //filtering for response - if (dispatcher->needsResponse(colObj0,colObj1)) + if (dispatcher->needsResponse(colObj0, colObj1)) m_islandmanifold.push_back(manifold); } } @@ -349,25 +339,27 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio } - ///@todo: this is random access, it can be walked 'cache friendly'! -void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback) +void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback) { - btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - - buildIslands(dispatcher,collisionWorld); + buildIslands(dispatcher, collisionWorld); + processIslands(dispatcher, collisionWorld, callback); +} - int endIslandIndex=1; +void btSimulationIslandManager::processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback) +{ + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + int endIslandIndex = 1; int startIslandIndex; int numElem = getUnionFind().getNumElements(); BT_PROFILE("processIslands"); - if(!m_splitIslands) + if (!m_splitIslands) { btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); int maxNumManifolds = dispatcher->getNumManifolds(); - callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); + callback->processIsland(&collisionObjects[0], collisionObjects.size(), manifold, maxNumManifolds, -1); } else { @@ -375,11 +367,21 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, // Sort the vector using predicate and std::sort //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); - int numManifolds = int (m_islandmanifold.size()); + int numManifolds = int(m_islandmanifold.size()); //tried a radix sort, but quicksort/heapsort seems still faster //@todo rewrite island management - m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + //btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid, + //but also based on object0 unique id and object1 unique id + if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) + { + m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic()); + } + else + { + m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + } + //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate()); //now process all active islands (sets of manifolds for now) @@ -389,55 +391,49 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, //int islandId; - - - // printf("Start Islands\n"); + // printf("Start Islands\n"); //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated - for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) + for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) { int islandId = getUnionFind().getElement(startIslandIndex).m_id; + bool islandSleeping = true; - bool islandSleeping = true; - - for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) - { - int i = getUnionFind().getElement(endIslandIndex).m_sz; - btCollisionObject* colObj0 = collisionObjects[i]; - m_islandBodies.push_back(colObj0); - if (colObj0->isActive()) - islandSleeping = false; - } - + for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++) + { + int i = getUnionFind().getElement(endIslandIndex).m_sz; + btCollisionObject* colObj0 = collisionObjects[i]; + m_islandBodies.push_back(colObj0); + if (colObj0->isActive()) + islandSleeping = false; + } //find the accompanying contact manifold for this islandId int numIslandManifolds = 0; btPersistentManifold** startManifold = 0; - if (startManifoldIndex<numManifolds) + if (startManifoldIndex < numManifolds) { int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]); if (curIslandId == islandId) { startManifold = &m_islandmanifold[startManifoldIndex]; - - for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++) - { + for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex])); endManifoldIndex++) + { } /// Process the actual simulation, only if not sleeping/deactivated - numIslandManifolds = endManifoldIndex-startManifoldIndex; + numIslandManifolds = endManifoldIndex - startManifoldIndex; } - } if (!islandSleeping) { - callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); - // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); + callback->processIsland(&m_islandBodies[0], m_islandBodies.size(), startManifold, numIslandManifolds, islandId); + // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); } - + if (numIslandManifolds) { startManifoldIndex = endManifoldIndex; @@ -445,6 +441,5 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, m_islandBodies.resize(0); } - } // else if(!splitIslands) - + } // else if(!splitIslands) } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index e24c6afeca1..197bb457cf6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -26,46 +26,42 @@ class btCollisionWorld; class btDispatcher; class btPersistentManifold; - ///SimulationIslandManager creates and handles simulation islands, using btUnionFind class btSimulationIslandManager { btUnionFind m_unionFind; - btAlignedObjectArray<btPersistentManifold*> m_islandmanifold; - btAlignedObjectArray<btCollisionObject* > m_islandBodies; - + btAlignedObjectArray<btPersistentManifold*> m_islandmanifold; + btAlignedObjectArray<btCollisionObject*> m_islandBodies; + bool m_splitIslands; - + public: btSimulationIslandManager(); virtual ~btSimulationIslandManager(); + void initUnionFind(int n); - void initUnionFind(int n); - - - btUnionFind& getUnionFind() { return m_unionFind;} - - virtual void updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher); - virtual void storeIslandActivationState(btCollisionWorld* world); + btUnionFind& getUnionFind() { return m_unionFind; } + virtual void updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher); + virtual void storeIslandActivationState(btCollisionWorld* world); - void findUnions(btDispatcher* dispatcher,btCollisionWorld* colWorld); + void findUnions(btDispatcher* dispatcher, btCollisionWorld* colWorld); - - - struct IslandCallback + struct IslandCallback { - virtual ~IslandCallback() {}; + virtual ~IslandCallback(){}; - virtual void processIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; + virtual void processIsland(btCollisionObject** bodies, int numBodies, class btPersistentManifold** manifolds, int numManifolds, int islandId) = 0; }; - void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback); - - void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld); + void buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback); + + void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld); + void processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback); + bool getSplitIslands() { return m_splitIslands; @@ -74,8 +70,6 @@ public: { m_splitIslands = doSplitIslands; } - }; -#endif //BT_SIMULATION_ISLAND_MANAGER_H - +#endif //BT_SIMULATION_ISLAND_MANAGER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp index e8b567e0efc..bc68b285b8d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -21,23 +21,22 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" //#include <stdio.h> -btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped) -: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf), -m_isSwapped(isSwapped) +btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, bool isSwapped) + : btActivatingCollisionAlgorithm(ci, col0Wrap, col1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf), + m_isSwapped(isSwapped) { - const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? col1Wrap : col0Wrap; - const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? col0Wrap : col1Wrap; - - if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject())) + const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? col1Wrap : col0Wrap; + const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? col0Wrap : col1Wrap; + + if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject())) { - m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(), boxObjWrap->getCollisionObject()); m_ownManifold = true; } } - btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() { if (m_ownManifold) @@ -47,17 +46,15 @@ btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() } } - - -void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) +void btSphereBoxCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)dispatchInfo; (void)resultOut; if (!m_manifoldPtr) return; - const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? body0Wrap : body1Wrap; + const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* boxObjWrap = m_isSwapped ? body0Wrap : body1Wrap; btVector3 pOnBox; @@ -83,10 +80,9 @@ void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWra resultOut->refreshContactPoints(); } } - } -btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; @@ -97,27 +93,26 @@ btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* return btScalar(1.); } - -bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance ) +bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance) { - const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape(); - btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin(); + const btBoxShape* boxShape = (const btBoxShape*)boxObjWrap->getCollisionShape(); + btVector3 const& boxHalfExtent = boxShape->getHalfExtentsWithoutMargin(); btScalar boxMargin = boxShape->getMargin(); penetrationDepth = 1.0f; // convert the sphere position to the box's local space - btTransform const &m44T = boxObjWrap->getWorldTransform(); + btTransform const& m44T = boxObjWrap->getWorldTransform(); btVector3 sphereRelPos = m44T.invXform(sphereCenter); // Determine the closest point to the sphere center in the box btVector3 closestPoint = sphereRelPos; - closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) ); - closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) ); - closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) ); - closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) ); - closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) ); - closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) ); - + closestPoint.setX(btMin(boxHalfExtent.getX(), closestPoint.getX())); + closestPoint.setX(btMax(-boxHalfExtent.getX(), closestPoint.getX())); + closestPoint.setY(btMin(boxHalfExtent.getY(), closestPoint.getY())); + closestPoint.setY(btMax(-boxHalfExtent.getY(), closestPoint.getY())); + closestPoint.setZ(btMin(boxHalfExtent.getZ(), closestPoint.getZ())); + closestPoint.setZ(btMax(-boxHalfExtent.getZ(), closestPoint.getZ())); + btScalar intersectionDist = fRadius + boxMargin; btScalar contactDist = intersectionDist + maxContactDistance; normal = sphereRelPos - closestPoint; @@ -136,42 +131,42 @@ bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWra { distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal); } - else //compute the penetration details + else //compute the penetration details { distance = normal.length(); normal /= distance; } pointOnBox = closestPoint + normal * boxMargin; -// v3PointOnSphere = sphereRelPos - (normal * fRadius); + // v3PointOnSphere = sphereRelPos - (normal * fRadius); penetrationDepth = distance - intersectionDist; // transform back in world space btVector3 tmp = m44T(pointOnBox); pointOnBox = tmp; -// tmp = m44T(v3PointOnSphere); -// v3PointOnSphere = tmp; + // tmp = m44T(v3PointOnSphere); + // v3PointOnSphere = tmp; tmp = m44T.getBasis() * normal; normal = tmp; return true; } -btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal ) +btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration(btVector3 const& boxHalfExtent, btVector3 const& sphereRelPos, btVector3& closestPoint, btVector3& normal) { //project the center of the sphere on the closest face of the box btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX(); btScalar minDist = faceDist; - closestPoint.setX( boxHalfExtent.getX() ); - normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f)); + closestPoint.setX(boxHalfExtent.getX()); + normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f)); faceDist = boxHalfExtent.getX() + sphereRelPos.getX(); if (faceDist < minDist) { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setX( -boxHalfExtent.getX() ); - normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f)); + closestPoint.setX(-boxHalfExtent.getX()); + normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f)); } faceDist = boxHalfExtent.getY() - sphereRelPos.getY(); @@ -179,8 +174,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setY( boxHalfExtent.getY() ); - normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f)); + closestPoint.setY(boxHalfExtent.getY()); + normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f)); } faceDist = boxHalfExtent.getY() + sphereRelPos.getY(); @@ -188,8 +183,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setY( -boxHalfExtent.getY() ); - normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f)); + closestPoint.setY(-boxHalfExtent.getY()); + normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f)); } faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ(); @@ -197,8 +192,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setZ( boxHalfExtent.getZ() ); - normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f)); + closestPoint.setZ(boxHalfExtent.getZ()); + normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f)); } faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ(); @@ -206,8 +201,8 @@ btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &b { minDist = faceDist; closestPoint = sphereRelPos; - closestPoint.setZ( -boxHalfExtent.getZ() ); - normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f)); + closestPoint.setZ(-boxHalfExtent.getZ()); + normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f)); } return minDist; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index eefaedc9e7e..3348bc89af5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -28,21 +28,20 @@ class btPersistentManifold; /// Other features are frame-coherency (persistent data) and collision response. class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_isSwapped; - -public: + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_isSwapped; - btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped); +public: + btSphereBoxCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped); virtual ~btSphereBoxCollisionAlgorithm(); - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { @@ -50,26 +49,25 @@ public: } } - bool getSphereDistance( const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& v3SphereCenter, btScalar fRadius, btScalar maxContactDistance ); + bool getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& v3SphereCenter, btScalar fRadius, btScalar maxContactDistance); + + btScalar getSpherePenetration(btVector3 const& boxHalfExtent, btVector3 const& sphereRelPos, btVector3& closestPoint, btVector3& normal); - btScalar getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal ); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm)); if (!m_swapped) { - return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false); - } else + return new (mem) btSphereBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, false); + } + else { - return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true); + return new (mem) btSphereBoxCollisionAlgorithm(0, ci, body0Wrap, body1Wrap, true); } } }; - }; -#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H - +#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index 36ba21f5bb0..7fa0559f975 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -12,6 +12,7 @@ subject to the following restrictions: 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ +#define CLEAR_MANIFOLD 1 #include "btSphereSphereCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" @@ -19,14 +20,14 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap) -: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf) +btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap) + : btActivatingCollisionAlgorithm(ci, col0Wrap, col1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf) { if (!m_manifoldPtr) { - m_manifoldPtr = m_dispatcher->getNewManifold(col0Wrap->getCollisionObject(),col1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(col0Wrap->getCollisionObject(), col1Wrap->getCollisionObject()); m_ownManifold = true; } } @@ -40,7 +41,7 @@ btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm() } } -void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btSphereSphereCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)dispatchInfo; @@ -52,27 +53,27 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject btSphereShape* sphere0 = (btSphereShape*)col0Wrap->getCollisionShape(); btSphereShape* sphere1 = (btSphereShape*)col1Wrap->getCollisionShape(); - btVector3 diff = col0Wrap->getWorldTransform().getOrigin()- col1Wrap->getWorldTransform().getOrigin(); + btVector3 diff = col0Wrap->getWorldTransform().getOrigin() - col1Wrap->getWorldTransform().getOrigin(); btScalar len = diff.length(); btScalar radius0 = sphere0->getRadius(); btScalar radius1 = sphere1->getRadius(); #ifdef CLEAR_MANIFOLD - m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting + m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting #endif ///iff distance positive, don't generate a new contact - if ( len > (radius0+radius1)) + if (len > (radius0 + radius1 + resultOut->m_closestPointDistanceThreshold)) { #ifndef CLEAR_MANIFOLD resultOut->refreshContactPoints(); -#endif //CLEAR_MANIFOLD +#endif //CLEAR_MANIFOLD return; } ///distance (negative means penetration) - btScalar dist = len - (radius0+radius1); + btScalar dist = len - (radius0 + radius1); - btVector3 normalOnSurfaceB(1,0,0); + btVector3 normalOnSurfaceB(1, 0, 0); if (len > SIMD_EPSILON) { normalOnSurfaceB = diff / len; @@ -81,20 +82,18 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject ///point on A (worldspace) ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; ///point on B (worldspace) - btVector3 pos1 = col1Wrap->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB; + btVector3 pos1 = col1Wrap->getWorldTransform().getOrigin() + radius1 * normalOnSurfaceB; /// report a contact. internally this will be kept persistent, and contact reduction is done - - - resultOut->addContactPoint(normalOnSurfaceB,pos1,dist); + + resultOut->addContactPoint(normalOnSurfaceB, pos1, dist); #ifndef CLEAR_MANIFOLD resultOut->refreshContactPoints(); -#endif //CLEAR_MANIFOLD - +#endif //CLEAR_MANIFOLD } -btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)col0; (void)col1; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h index 3517a568a99..b08d0df76d8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -28,39 +28,37 @@ class btPersistentManifold; /// Also provides the most basic sample for custom/user btCollisionAlgorithm class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + public: - btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap); + btSphereSphereCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap); btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btActivatingCollisionAlgorithm(ci) {} - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { manifoldArray.push_back(m_manifoldPtr); } } - + virtual ~btSphereSphereCollisionAlgorithm(); - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm)); - return new(mem) btSphereSphereCollisionAlgorithm(0,ci,col0Wrap,col1Wrap); + return new (mem) btSphereSphereCollisionAlgorithm(0, ci, col0Wrap, col1Wrap); } }; - }; -#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H - +#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index 280a4d355fd..1bc3056c013 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btSphereTriangleCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -21,15 +20,15 @@ subject to the following restrictions: #include "SphereTriangleDetector.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped) -: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_ownManifold(false), -m_manifoldPtr(mf), -m_swapped(swapped) +btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool swapped) + : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), + m_ownManifold(false), + m_manifoldPtr(mf), + m_swapped(swapped) { if (!m_manifoldPtr) { - m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(), body1Wrap->getCollisionObject()); m_ownManifold = true; } } @@ -43,36 +42,35 @@ btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm() } } -void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btSphereTriangleCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* col0Wrap, const btCollisionObjectWrapper* col1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { if (!m_manifoldPtr) return; - const btCollisionObjectWrapper* sphereObjWrap = m_swapped? col1Wrap : col0Wrap; - const btCollisionObjectWrapper* triObjWrap = m_swapped? col0Wrap : col1Wrap; + const btCollisionObjectWrapper* sphereObjWrap = m_swapped ? col1Wrap : col0Wrap; + const btCollisionObjectWrapper* triObjWrap = m_swapped ? col0Wrap : col1Wrap; btSphereShape* sphere = (btSphereShape*)sphereObjWrap->getCollisionShape(); btTriangleShape* triangle = (btTriangleShape*)triObjWrap->getCollisionShape(); - + /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); - SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()); - + SphereTriangleDetector detector(sphere, triangle, m_manifoldPtr->getContactBreakingThreshold() + resultOut->m_closestPointDistanceThreshold); + btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds + input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT); ///@todo: tighter bounds input.m_transformA = sphereObjWrap->getWorldTransform(); input.m_transformB = triObjWrap->getWorldTransform(); bool swapResults = m_swapped; - detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults); + detector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw, swapResults); if (m_ownManifold) resultOut->refreshContactPoints(); - } -btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, btCollisionObject* col1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { (void)resultOut; (void)dispatchInfo; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h index 6b6e39a72b6..d660222f164 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -27,43 +27,39 @@ class btPersistentManifold; /// Also provides the most basic sample for custom/user btCollisionAlgorithm class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_swapped; - + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_swapped; + public: - btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped); + btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf, const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool swapped); btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btActivatingCollisionAlgorithm(ci) {} - virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual void processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut); - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_manifoldPtr && m_ownManifold) { manifoldArray.push_back(m_manifoldPtr); } } - + virtual ~btSphereTriangleCollisionAlgorithm(); - struct CreateFunc :public btCollisionAlgorithmCreateFunc + struct CreateFunc : public btCollisionAlgorithmCreateFunc { - - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap) { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm)); - return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_swapped); + return new (mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold, ci, body0Wrap, body1Wrap, m_swapped); } }; - }; -#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H - +#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp index 5222933595d..816bf1e6adc 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -15,68 +15,60 @@ subject to the following restrictions: #include "btUnionFind.h" - - btUnionFind::~btUnionFind() { Free(); - } btUnionFind::btUnionFind() -{ - +{ } -void btUnionFind::allocate(int N) +void btUnionFind::allocate(int N) { m_elements.resize(N); } -void btUnionFind::Free() +void btUnionFind::Free() { m_elements.clear(); } - -void btUnionFind::reset(int N) +void btUnionFind::reset(int N) { allocate(N); - for (int i = 0; i < N; i++) - { - m_elements[i].m_id = i; m_elements[i].m_sz = 1; - } + for (int i = 0; i < N; i++) + { + m_elements[i].m_id = i; + m_elements[i].m_sz = 1; + } } - class btUnionFindElementSortPredicate { - public: - - bool operator() ( const btElement& lhs, const btElement& rhs ) const - { - return lhs.m_id < rhs.m_id; - } +public: + bool operator()(const btElement& lhs, const btElement& rhs) const + { + return lhs.m_id < rhs.m_id; + } }; ///this is a special operation, destroying the content of btUnionFind. ///it sorts the elements, based on island id, in order to make it easy to iterate over islands -void btUnionFind::sortIslands() +void btUnionFind::sortIslands() { - //first store the original body index, and islandId int numElements = m_elements.size(); - - for (int i=0;i<numElements;i++) + + for (int i = 0; i < numElements; i++) { m_elements[i].m_id = find(i); #ifndef STATIC_SIMULATION_ISLAND_OPTIMIZATION m_elements[i].m_sz = i; -#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION +#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION } - - // Sort the vector using predicate and std::sort - //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate); - m_elements.quickSort(btUnionFindElementSortPredicate()); + // Sort the vector using predicate and std::sort + //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate); + m_elements.quickSort(btUnionFindElementSortPredicate()); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h index ef2a29202f7..d422ef55eb3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h @@ -23,107 +23,101 @@ subject to the following restrictions: ///see for discussion of static island optimizations by Vroonsh here: http://code.google.com/p/bullet/issues/detail?id=406 #define STATIC_SIMULATION_ISLAND_OPTIMIZATION 1 -struct btElement +struct btElement { - int m_id; - int m_sz; + int m_id; + int m_sz; }; ///UnionFind calculates connected subsets // Implements weighted Quick Union with path compression // optimization: could use short ints instead of ints (halving memory, would limit the number of rigid bodies to 64k, sounds reasonable) class btUnionFind - { - private: - btAlignedObjectArray<btElement> m_elements; - - public: - - btUnionFind(); - ~btUnionFind(); - - - //this is a special operation, destroying the content of btUnionFind. - //it sorts the elements, based on island id, in order to make it easy to iterate over islands - void sortIslands(); - - void reset(int N); - - SIMD_FORCE_INLINE int getNumElements() const - { - return int(m_elements.size()); - } - SIMD_FORCE_INLINE bool isRoot(int x) const - { - return (x == m_elements[x].m_id); - } - - btElement& getElement(int index) - { - return m_elements[index]; - } - const btElement& getElement(int index) const - { - return m_elements[index]; - } - - void allocate(int N); - void Free(); - - - - - int find(int p, int q) - { - return (find(p) == find(q)); - } - - void unite(int p, int q) - { - int i = find(p), j = find(q); - if (i == j) - return; +{ +private: + btAlignedObjectArray<btElement> m_elements; + +public: + btUnionFind(); + ~btUnionFind(); + + //this is a special operation, destroying the content of btUnionFind. + //it sorts the elements, based on island id, in order to make it easy to iterate over islands + void sortIslands(); + + void reset(int N); + + SIMD_FORCE_INLINE int getNumElements() const + { + return int(m_elements.size()); + } + SIMD_FORCE_INLINE bool isRoot(int x) const + { + return (x == m_elements[x].m_id); + } + + btElement& getElement(int index) + { + return m_elements[index]; + } + const btElement& getElement(int index) const + { + return m_elements[index]; + } + + void allocate(int N); + void Free(); + + int find(int p, int q) + { + return (find(p) == find(q)); + } + + void unite(int p, int q) + { + int i = find(p), j = find(q); + if (i == j) + return; #ifndef USE_PATH_COMPRESSION - //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) - if (m_elements[i].m_sz < m_elements[j].m_sz) - { - m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; - } - else - { - m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; - } -#else - m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; -#endif //USE_PATH_COMPRESSION + //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) + if (m_elements[i].m_sz < m_elements[j].m_sz) + { + m_elements[i].m_id = j; + m_elements[j].m_sz += m_elements[i].m_sz; + } + else + { + m_elements[j].m_id = i; + m_elements[i].m_sz += m_elements[j].m_sz; } +#else + m_elements[i].m_id = j; + m_elements[j].m_sz += m_elements[i].m_sz; +#endif //USE_PATH_COMPRESSION + } + + int find(int x) + { + //btAssert(x < m_N); + //btAssert(x >= 0); - int find(int x) - { + while (x != m_elements[x].m_id) + { + //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically + +#ifdef USE_PATH_COMPRESSION + const btElement* elementPtr = &m_elements[m_elements[x].m_id]; + m_elements[x].m_id = elementPtr->m_id; + x = elementPtr->m_id; +#else // + x = m_elements[x].m_id; +#endif //btAssert(x < m_N); //btAssert(x >= 0); - - while (x != m_elements[x].m_id) - { - //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically - - #ifdef USE_PATH_COMPRESSION - const btElement* elementPtr = &m_elements[m_elements[x].m_id]; - m_elements[x].m_id = elementPtr->m_id; - x = elementPtr->m_id; - #else// - x = m_elements[x].m_id; - #endif - //btAssert(x < m_N); - //btAssert(x >= 0); - - } - return x; } + return x; + } +}; - - }; - - -#endif //BT_UNION_FIND_H +#endif //BT_UNION_FIND_H |