diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision/CollisionDispatch')
57 files changed, 4641 insertions, 5371 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index e5bac8438e..7647f67360 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -18,94 +18,95 @@ 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]); + btVector3 normal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]); btScalar l2 = normal.length2(); bool hasContact = false; btVector3 contactPoint; - if (l2 >= SIMD_EPSILON*SIMD_EPSILON) + if (l2 >= SIMD_EPSILON * SIMD_EPSILON) { normal /= btSqrt(l2); @@ -120,54 +121,59 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po } bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; - + // Check for contact / intersection - - if (isInsideContactPlane) { - if (facecontains(sphereCenter, vertices, normal)) { + + if (isInsideContactPlane) + { + if (facecontains(sphereCenter, vertices, normal)) + { // Inside the contact wedge - touches a point on the shell plane hasContact = true; - contactPoint = sphereCenter - normal*distanceFromPlane; + contactPoint = sphereCenter - normal * distanceFromPlane; } - else { + else + { // Could be inside one of the contact capsules - btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold; + btScalar contactCapsuleRadiusSqr = radiusWithThreshold * radiusWithThreshold; btScalar minDistSqr = contactCapsuleRadiusSqr; btVector3 nearestOnEdge; - for (int i = 0; i < m_triangle->getNumEdges(); i++) { - + 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) { + 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; @@ -176,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/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index 22953af43f..d47e47530d 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp index 57f1464935..ac5de45d27 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h index 0e19f1ea35..862060571b 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h @@ -21,17 +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; protected: + btActivatingCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci); - 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp index cabbb0bf6a..6873a95d90 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp +++ b/thirdparty/bullet/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(); @@ -174,9 +168,9 @@ 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); + btScalar 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h index 6ea6e89bda..3b66d1fd0b 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp index ac68968f59..7a391e059a 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h index 59808df5a9..eb21065765 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp index 7043bde34f..202039956e 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.h index 3924377705..9f7d988fc1 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionConfiguration.h index 35f77d4e65..d6e15f555b 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionConfiguration.h +++ b/thirdparty/bullet/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,13 +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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h index 62ee66c4e9..bd81284939 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index f8794dec47..25b2b1ea46 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -13,8 +13,6 @@ 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" @@ -31,40 +29,34 @@ subject to the following restrictions: #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_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(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) +void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc) { m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc; } -void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) +void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc) { m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc; } @@ -73,35 +65,33 @@ btCollisionDispatcher::~btCollisionDispatcher() { } -btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1) -{ +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 = m_persistentManifoldPoolAllocator->allocate( sizeof( btPersistentManifold ) ); - if (NULL == mem) + + 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) + //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 + 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); @@ -113,17 +103,14 @@ void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold) manifold->clearManifold(); } - void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) { - - //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(); @@ -131,19 +118,15 @@ 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, ebtDispatcherQueryType algoType) +btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType) { - btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = this; @@ -161,21 +144,18 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObje 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); @@ -192,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) { } @@ -228,87 +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); - { + { BT_PROFILE("processAllOverlappingPairs"); - pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher, dispatchInfo); + 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,0, BT_CONTACT_POINT_ALGORITHMS); - } + //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) { - void* mem = m_collisionAlgorithmPoolAllocator->allocate( size ); - if (NULL == mem) - { - //warn user for overflow? - return btAlignedAlloc(static_cast<size_t>(size), 16); - } - return mem; + void* mem = m_collisionAlgorithmPoolAllocator->allocate(size); + if (NULL == mem) + { + //warn user for overflow? + return btAlignedAlloc(static_cast<size_t>(size), 16); + } + return mem; } void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) @@ -316,7 +281,8 @@ void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) if (m_collisionAlgorithmPoolAllocator->validPtr(ptr)) { m_collisionAlgorithmPoolAllocator->freeMemory(ptr); - } else + } + else { btAlignedFree(ptr); } diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index b97ee3c1ba..6b9f5e23a5 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -37,35 +37,30 @@ 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; + btManifoldResult m_defaultManifoldResult; - btManifoldResult m_defaultManifoldResult; + btNearCallback m_nearCallback; - btNearCallback m_nearCallback; - - btPoolAllocator* m_collisionAlgorithmPoolAllocator; + btPoolAllocator* m_collisionAlgorithmPoolAllocator; - btPoolAllocator* m_persistentManifoldPoolAllocator; + btPoolAllocator* m_persistentManifoldPoolAllocator; btCollisionAlgorithmCreateFunc* m_doubleDispatchContactPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; btCollisionAlgorithmCreateFunc* m_doubleDispatchClosestPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; - btCollisionConfiguration* m_collisionConfiguration; - + btCollisionConfiguration* m_collisionConfiguration; public: - enum DispatcherFlags { CD_STATIC_STATIC_REPORTED = 1, @@ -73,103 +68,100 @@ 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); - void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc); + void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); - int getNumManifolds() const - { - return int( m_manifoldsPtr.size()); + 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) { return m_manifoldsPtr[index]; } - const btPersistentManifold* getManifoldByIndexInternal(int index) const + const btPersistentManifold* getManifoldByIndexInternal(int index) const { 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, ebtDispatcherQueryType queryType); - - 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp index 075860c503..6fe56538d2 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp @@ -13,8 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - - #include "btCollisionDispatcherMt.h" #include "LinearMath/btQuickprof.h" @@ -27,138 +25,132 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -btCollisionDispatcherMt::btCollisionDispatcherMt( btCollisionConfiguration* config, int grainSize ) - : btCollisionDispatcher( config ) +btCollisionDispatcherMt::btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize) + : btCollisionDispatcher(config) { - m_batchUpdating = false; - m_grainSize = grainSize; // iterations per task + m_batchUpdating = false; + m_grainSize = grainSize; // iterations per task } - -btPersistentManifold* btCollisionDispatcherMt::getNewManifold( const btCollisionObject* body0, const btCollisionObject* body1 ) +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 ); - } - - return manifold; + //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); + } + + return manifold; } -void btCollisionDispatcherMt::releaseManifold( btPersistentManifold* 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 ); - } + 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 ); - } - } + 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 ) +void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) { - 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; - - // reconstruct the manifolds array to ensure determinism - m_manifoldsPtr.resizeNoInitialize( 0 ); - - btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr(); - for ( int i = 0; i < pairCount; ++i ) - { - if (btCollisionAlgorithm* algo = pairs[ i ].m_algorithm) - { - algo->getAllContactManifolds( m_manifoldsPtr ); - } - } - - // update the indices (used when releasing manifolds) - for ( int i = 0; i < m_manifoldsPtr.size(); ++i ) - { - m_manifoldsPtr[ i ]->m_index1a = i; - } + 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; + + // reconstruct the manifolds array to ensure determinism + m_manifoldsPtr.resizeNoInitialize(0); + + btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr(); + for (int i = 0; i < pairCount; ++i) + { + if (btCollisionAlgorithm* algo = pairs[i].m_algorithm) + { + algo->getAllContactManifolds(m_manifoldsPtr); + } + } + + // 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h index f1d7eafdc9..28eba7f32a 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h @@ -19,21 +19,19 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "LinearMath/btThreads.h" - class btCollisionDispatcherMt : public btCollisionDispatcher { public: - btCollisionDispatcherMt( btCollisionConfiguration* config, int grainSize = 40 ); + 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 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; + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) BT_OVERRIDE; protected: - bool m_batchUpdating; - int m_grainSize; + bool m_batchUpdating; + int m_grainSize; }; -#endif //BT_COLLISION_DISPATCHER_MT_H - +#endif //BT_COLLISION_DISPATCHER_MT_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 05f96a14bc..98a02d0c45 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -13,42 +13,41 @@ 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_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_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_hitFraction(btScalar(1.)), + m_ccdSweptSphereRadius(btScalar(0.)), + m_ccdMotionThreshold(btScalar(0.)), + m_checkCollideWith(false), + m_updateRevision(0) { m_worldTransform.setIdentity(); m_interpolationWorldTransform.setIdentity(); @@ -59,8 +58,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; } @@ -71,7 +70,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.); @@ -80,7 +79,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); @@ -92,7 +90,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; @@ -104,8 +102,8 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali 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) { @@ -130,11 +128,10 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h index 135f8a033c..56b3d89e56 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/thirdparty/bullet/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,123 +42,118 @@ 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_worldArrayIndex; // index of object in world's collisionObjects array + 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;//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; - - + 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; + void* m_userObjectPointer; - int m_userIndex2; - - int m_userIndex; + int m_userIndex2; + + int m_userIndex; ///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; + 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 }; @@ -166,76 +161,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) { @@ -253,7 +249,7 @@ public: m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0; } - virtual bool checkCollideWithOverride(const btCollisionObject* co) const + virtual bool checkCollideWithOverride(const btCollisionObject* co) const { int index = m_objectsWithoutCollisionCheck.findLinearSearch(co); if (index < m_objectsWithoutCollisionCheck.size()) @@ -263,317 +259,309 @@ 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) + 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 + + 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 + + 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 int getWorldArrayIndex() const - { - return m_worldArrayIndex; - } + SIMD_FORCE_INLINE int getWorldArrayIndex() const + { + return m_worldArrayIndex; + } - // only should be called by CollisionWorld - void setWorldArrayIndex(int ix) - { - m_worldArrayIndex = ix; - } + // only should be called by CollisionWorld + void setWorldArrayIndex(int ix) + { + m_worldArrayIndex = ix; + } - SIMD_FORCE_INLINE btScalar getHitFraction() const + SIMD_FORCE_INLINE btScalar getHitFraction() const { - return m_hitFraction; + return m_hitFraction; } - void setHitFraction(btScalar 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 + + int getUserIndex2() const { return m_userIndex2; } - + ///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; } - - void setUserIndex2(int index) + + void setUserIndex2(int index) { m_userIndex2 = index; } - int getUpdateRevisionInternal() const + int getUpdateRevisionInternal() const { return m_updateRevision; } - void setCustomDebugColor(const btVector3& colorRGB) + void setCustomDebugColor(const btVector3& colorRGB) { m_customDebugColorRGB = colorRGB; m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; } - void removeCustomDebugColor() + void removeCustomDebugColor() { m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; } - bool getCustomDebugColor(btVector3& colorRGB) const + bool getCustomDebugColor(btVector3 & colorRGB) const { - bool hasCustomColor = (0!=(m_collisionFlags&CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR)); + bool hasCustomColor = (0 != (m_collisionFlags & CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR)); if (hasCustomColor) { colorRGB = m_customDebugColorRGB; @@ -589,15 +577,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 { @@ -667,14 +656,11 @@ struct btCollisionObjectFloatData 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h index 952440b7de..1cc4a5ac5f 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h +++ b/thirdparty/bullet/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,17 @@ public: const btCollisionShape* m_shape; const btCollisionObject* m_collisionObject; const btTransform& m_worldTransform; - int m_partId; - int m_index; + 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_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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 3de8d6995e..782e9efaf1 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -19,9 +19,9 @@ 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/btScaledBvhTriangleMeshShape.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/NarrowPhaseCollision/btRaycastCallback.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" @@ -38,7 +38,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 @@ -48,7 +47,6 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" - ///for debug drawing //for debug rendering @@ -65,25 +63,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) @@ -91,74 +85,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, int collisionFilterGroup, 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(collisionObject->getWorldArrayIndex() == -1); // do not add the same object to more than one collision world + 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()); + 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)) ; - - - - - + 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); @@ -168,10 +171,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) @@ -189,15 +193,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); + btAssert(colObj->getWorldArrayIndex() == i); //only update aabb of active objects if (m_forceUpdateAllAabbs || colObj->isActive()) @@ -207,14 +211,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"); @@ -228,69 +231,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); } } - - 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); + 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); @@ -304,12 +299,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; @@ -318,10 +313,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)) @@ -332,81 +327,75 @@ 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) { @@ -418,7 +407,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con 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; @@ -439,45 +428,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; @@ -485,9 +469,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()) { @@ -495,10 +481,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; } @@ -507,7 +493,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; @@ -520,7 +506,7 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con return result; } }; - + struct RayTester : btDbvt::ICollide { const btCollisionObject* m_collisionObject; @@ -529,33 +515,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); @@ -564,19 +546,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, @@ -584,39 +564,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(); @@ -626,21 +606,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)) @@ -648,25 +626,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; @@ -680,62 +657,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)) @@ -743,22 +715,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; @@ -772,44 +742,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; @@ -821,35 +786,37 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, rayAabbMaxLocal.setMax(convexToLocal); rayAabbMinLocal += boxMinLocal; rayAabbMaxLocal += boxMaxLocal; - concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); + concaveShape->processAllTriangles(&tccb, rayAabbMinLocal, rayAabbMaxLocal); } } - } else { + } + else + { if (collisionShape->isCompound()) { - struct btCompoundLeafCallback : btDbvt::ICollide + struct btCompoundLeafCallback : btDbvt::ICollide { 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* 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 btCollisionObjectWrapper* m_colObjWrap; const btConvexShape* m_castShape; const btTransform& m_convexFromTrans; const btTransform& m_convexToTrans; @@ -859,16 +826,16 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, ConvexResultCallback& m_resultCallback; public: - - void ProcessChild(int index, const btTransform& childTrans, const btCollisionShape* childCollisionShape) + void ProcessChild(int index, const btTransform& childTrans, const btCollisionShape* childCollisionShape) { btTransform childWorldTrans = m_colObjWorldTransform * childTrans; - struct LocalInfoAdder : public ConvexResultCallback { + 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; @@ -877,9 +844,9 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, { return m_userCallback->needsCollision(p); } - virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& r, bool b) + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& r, bool b) { - btCollisionWorld::LocalShapeInfo shapeInfo; + btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = -1; shapeInfo.m_triangleIndex = m_i; if (r.m_localShapeInfo == NULL) @@ -887,7 +854,6 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, const btScalar result = m_userCallback->addSingleResult(r, b); m_closestHitFraction = m_userCallback->m_closestHitFraction; return result; - } }; @@ -898,7 +864,7 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, objectQuerySingleInternal(m_castShape, m_convexFromTrans, m_convexToTrans, &tmpObj, my_cb, m_allowedPenetration); } - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { // Processing leaf node int index = leaf->dataAsInt; @@ -923,15 +889,18 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, fromLocalAabbMax.setMax(toLocalAabbMax); btCompoundLeafCallback callback(colObjWrap, castShape, convexFromTrans, convexToTrans, - allowedPenetration, compoundShape, colObjWorldTransform, resultCallback); + allowedPenetration, compoundShape, colObjWorldTransform, resultCallback); const btDbvt* tree = compoundShape->getDynamicAabbTree(); - if (tree) { - const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(fromLocalAabbMin, fromLocalAabbMax); + if (tree) + { + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(fromLocalAabbMin, fromLocalAabbMax); tree->collideTV(tree->m_root, bounds, callback); - } else { + } + else + { int i; - for (i=0;i<compoundShape->getNumChildShapes();i++) + for (i = 0; i < compoundShape->getNumChildShapes(); i++) { const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); btTransform childTrans = compoundShape->getChildTransform(i); @@ -943,33 +912,31 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, } } - 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]; @@ -978,22 +945,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; @@ -1011,57 +975,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]; @@ -1072,109 +1032,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; @@ -1182,78 +1135,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,0, BT_CLOSEST_POINT_ALGORITHMS); + 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); @@ -1263,271 +1212,247 @@ 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, 0, BT_CLOSEST_POINT_ALGORITHMS); + 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; + } + + 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(); - 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); - } - - - - } - + 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()) { @@ -1535,25 +1460,23 @@ void btCollisionWorld::debugDrawWorld() 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); } } } @@ -1563,58 +1486,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)); } }; colObj->getCustomDebugColor(color); - debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),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); } } } @@ -1622,28 +1550,27 @@ 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) @@ -1653,8 +1580,6 @@ void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) } } - - void btCollisionWorld::serializeContactManifolds(btSerializer* serializer) { if (serializer->getSerializationFlags() & BT_SERIALIZE_CONTACT_MANIFOLDS) @@ -1663,7 +1588,7 @@ void btCollisionWorld::serializeContactManifolds(btSerializer* serializer) for (int i = 0; i < numManifolds; i++) { const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i]; - //don't serialize empty manifolds, they just take space + //don't serialize empty manifolds, they just take space //(may have to do it anyway if it destroys determinism) if (manifold->getNumContacts() == 0) continue; @@ -1675,16 +1600,13 @@ void btCollisionWorld::serializeContactManifolds(btSerializer* serializer) } } - -void btCollisionWorld::serialize(btSerializer* serializer) +void btCollisionWorld::serialize(btSerializer* serializer) { - serializer->startSerialization(); - + serializeCollisionObjects(serializer); serializeContactManifolds(serializer); - + serializer->finishSerialization(); } - diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h index 886476e8ad..fd0e5b9bbd 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - /** * @mainpage Bullet Documentation * @@ -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,147 +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); 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; - int m_collisionFilterGroup; - 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) { } @@ -238,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); @@ -301,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; - int m_collisionFilterGroup; - 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; @@ -371,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; @@ -411,23 +397,23 @@ public: }; ///ContactResultCallback is used to report contact points - struct ContactResultCallback + struct ContactResultCallback { - int m_collisionFilterGroup; - int m_collisionFilterMask; - btScalar m_closestDistanceThreshold; + int m_collisionFilterGroup; + int m_collisionFilterMask; + btScalar m_closestDistanceThreshold; ContactResultCallback() - :m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter), - m_closestDistanceThreshold(0) + : 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; @@ -435,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, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); + + virtual void refreshBroadphaseProxy(btCollisionObject* collisionObject); btCollisionObjectArray& getCollisionObjectArray() { @@ -498,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() { @@ -512,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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp index f2b0837808..9f422dc157 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp +++ b/thirdparty/bullet/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,71 +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]; + { + 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++) - { - //btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i]; + btAlignedObjectArray<btCollisionShape*> childShapes; + for (int i = 0; i < compoundData->m_numChildShapes; i++) + { + //btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i]; - btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape; + 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 - { + 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; @@ -639,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; @@ -693,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; } @@ -707,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; @@ -727,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]; @@ -738,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]; @@ -759,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 @@ -776,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]; @@ -784,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; @@ -803,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) @@ -898,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; } @@ -937,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(); @@ -1027,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) { @@ -1048,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() { @@ -1066,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(); } @@ -1097,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(); @@ -1124,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]; } @@ -1143,5 +1085,3 @@ btTriangleInfoMap* btCollisionWorldImporter::getTriangleInfoMapByIndex(int index { return m_allocatedTriangleInfoMaps[index]; } - - diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h index 81c6142726..5e8bc95341 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h +++ b/thirdparty/bullet/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,14 +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 getNumBvhs() const; - btOptimizedBvh* getBvhByIndex(int index) const; + btOptimizedBvh* getBvhByIndex(int index) const; int getNumTriangleInfoMaps() const; btTriangleInfoMap* getTriangleInfoMapByIndex(int index) const; @@ -134,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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 91b7809c17..633bee4825 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -25,62 +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, BT_CONTACT_POINT_ALGORITHMS); - + 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]) { @@ -95,66 +91,56 @@ 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; - childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); + btVector3 aabbMin0, aabbMax0; + childShape->getAabb(newChildWorldTrans, aabbMin0, aabbMax0); btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold); aabbMin0 -= extendAabb; aabbMax0 += extendAabb; btVector3 aabbMin1, aabbMax1; - m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); + m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(), aabbMin1, aabbMax1); - if (gCompoundChildShapePairCallback) - { - if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape)) - return; - } - if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) { + btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, -1, index); - btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index); - btCollisionAlgorithm* algo = 0; bool allocatedAlgorithm = false; @@ -172,7 +158,7 @@ public: } algo = m_childCollisionAlgorithms[index]; } - + const btCollisionObjectWrapper* tmpWrap = 0; ///detect swapping case @@ -180,15 +166,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); } - algo->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)) @@ -202,18 +189,19 @@ public: if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject()) { m_resultOut->setBody0Wrap(tmpWrap); - } else + } + else { m_resultOut->setBody1Wrap(tmpWrap); } - if(allocatedAlgorithm) + if (allocatedAlgorithm) { algo->~btCollisionAlgorithm(); m_dispatcher->freeCollisionAlgorithm(algo); - } + } } } - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { int index = leaf->dataAsInt; @@ -230,22 +218,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: @@ -254,17 +236,17 @@ 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 @@ -272,18 +254,18 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap { int i; manifoldArray.resize(0); - 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); - 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); @@ -293,57 +275,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->collideTVNoStackAlloc(tree->m_root,bounds,stack2,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; manifoldArray.resize(0); - const btCollisionShape* childShape = 0; - btTransform orgTrans; - - btTransform newChildWorldTrans; - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; - - for (i=0;i<numChildren;i++) + 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]); @@ -354,15 +335,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 @@ -376,33 +357,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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index d2086fbc02..4ea5e77185 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -35,7 +35,7 @@ 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; @@ -44,61 +44,56 @@ 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp index 20b542f670..044b60dbb1 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -29,29 +29,25 @@ subject to the following restrictions: 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(); @@ -59,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); } @@ -92,77 +86,65 @@ 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; @@ -170,17 +152,16 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide 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); - + 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); + btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0, childIndex1); bool removePair = false; btCollisionAlgorithm* colAlgo = 0; if (m_resultOut->m_closestPointDistanceThreshold > 0) @@ -193,7 +174,6 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide if (pair) { colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; - } else { @@ -205,7 +185,7 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide } btAssert(colAlgo); - + const btCollisionObjectWrapper* tmpWrap0 = 0; const btCollisionObjectWrapper* tmpWrap1 = 0; @@ -215,105 +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, btScalar distanceThreshold) +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); + 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); + 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, btScalar distanceThreshold) +static inline void MycollideTT(const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold) { - - if(root0&&root1) - { - int depth=1; - int treshold=btDbvt::DOUBLE_STACKSIZE-4; - btAlignedObjectArray<btDbvt::sStkNN> stkStack; + 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); + ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]); + stkStack.initializeFromBuffer(&localStack, btDbvt::DOUBLE_STACKSIZE, btDbvt::DOUBLE_STACKSIZE); #else - stkStack.resize(btDbvt::DOUBLE_STACKSIZE); + stkStack.resize(btDbvt::DOUBLE_STACKSIZE); #endif - stkStack[0]=btDbvt::sStkNN(root0,root1); - do { - btDbvt::sStkNN p=stkStack[--depth]; - if(MyIntersect(p.a->volume,p.b->volume,xform, distanceThreshold)) + stkStack[0] = btDbvt::sStkNN(root0, root1); + do + { + 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.resize(stkStack.size()*2); - treshold=stkStack.size()-4; + 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]); } - if(p.a->isinternal()) + else { - 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->childs[0], p.b); + stkStack[depth++] = btDbvt::sStkNN(p.a->childs[1], p.b); + } + } + 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 { - 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()); @@ -321,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 @@ -331,28 +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 +#ifdef USE_LOCAL_STACK btPersistentManifold localManifolds[4]; - manifoldArray.initializeFromBuffer(&localManifolds,0,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()) { @@ -366,35 +339,27 @@ 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, resultOut->m_closestPointDistanceThreshold); + 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) { @@ -402,52 +367,47 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb { const btCollisionShape* childShape0 = 0; - - btTransform newChildWorldTrans0; + + btTransform newChildWorldTrans0; childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA); const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA); - newChildWorldTrans0 = col0ObjWrap->getWorldTransform()*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; { const btCollisionShape* childShape1 = 0; - btTransform newChildWorldTrans1; + btTransform newChildWorldTrans1; childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB); const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB); - newChildWorldTrans1 = col1ObjWrap->getWorldTransform()*childTrans1 ; - childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); + newChildWorldTrans1 = col1ObjWrap->getWorldTransform() * childTrans1; + childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1); } - + aabbMin1 -= thresholdVec; aabbMax1 += thresholdVec; - if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h index f29f7a709a..a940d840e0 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h @@ -34,54 +34,49 @@ class btCollisionObject; class btCollisionShape; +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/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp index 1cb3d2e7a1..9087f84398 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h index 24d1336778..9fca463fbe 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index d8cbe96142..e50f85e2bb 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -13,7 +13,6 @@ 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" @@ -29,10 +28,10 @@ subject to the following restrictions: #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_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped), -m_isSwapped(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) { } @@ -40,7 +39,7 @@ btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm() { } -void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) { if (m_btConvexTriangleCallback.m_manifoldPtr) { @@ -48,38 +47,32 @@ 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"); @@ -88,16 +81,12 @@ partId, int triangleIndex) 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 @@ -111,16 +100,15 @@ 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? + + 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); @@ -135,36 +123,32 @@ partId, int triangleIndex) { 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; @@ -185,16 +169,14 @@ void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr 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"); @@ -208,7 +190,6 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape(); if (convexBodyWrap->getCollisionShape()->isConvex()) { - btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape(); btAlignedObjectArray<btVector3> queryVertices; @@ -229,7 +210,6 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec queryVertices.push_back(btVector3(0, 0, 0)); btSphereShape* sphere = (btSphereShape*)convex; maxDist = sphere->getRadius() + SIMD_EPSILON; - } if (queryVertices.size()) { @@ -240,7 +220,7 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec for (int v = 0; v < queryVertices.size(); v++) { const btVector3& vtx = queryVertices[v]; - btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform()*vtx; + btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform() * vtx; btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace); btVector3 normalLocal; @@ -250,58 +230,52 @@ void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjec if (dist <= maxDist) { normalLocal.safeNormalize(); - btVector3 normal = triBodyWrap->getWorldTransform().getBasis()*normalLocal; + btVector3 normal = triBodyWrap->getWorldTransform().getBasis() * normalLocal; if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) { btSphereShape* sphere = (btSphereShape*)convex; dist -= sphere->getRadius(); - vtxWorldSpace -= sphere->getRadius()*normal; - + vtxWorldSpace -= sphere->getRadius() * normal; } - resultOut->addContactPoint(normal,vtxWorldSpace-normal*dist, dist); + resultOut->addContactPoint(normal, vtxWorldSpace - normal * dist, dist); } } } resultOut->refreshContactPoints(); } - } - } else + } + else { - const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape()); - + 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); + 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()); - concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax()); - 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 @@ -320,25 +294,23 @@ 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"); @@ -349,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(); @@ -379,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/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h index 93d842ef50..b72e402981 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -26,42 +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. -ATTRIBUTE_ALIGNED16(class) btConvexTriangleCallback : public btTriangleCallback +ATTRIBUTE_ALIGNED16(class) +btConvexTriangleCallback : public btTriangleCallback { - - btVector3 m_aabbMin; - btVector3 m_aabbMax ; + btVector3 m_aabbMin; + btVector3 m_aabbMax; const btCollisionObjectWrapper* m_convexBodyWrap; const btCollisionObjectWrapper* m_triBodyWrap; - - btManifoldResult* m_resultOut; - btDispatcher* m_dispatcher; + btDispatcher* m_dispatcher; const btDispatcherInfo* m_dispatchInfoPtr; btScalar m_collisionMarginTriangle; - + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - -int m_triangleCount; - - btPersistentManifold* m_manifoldPtr; - btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + int m_triangleCount; + + 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 @@ -72,56 +70,48 @@ int m_triangleCount; { return m_aabbMax; } - }; - - - /// btConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes. -ATTRIBUTE_ALIGNED16(class) btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm +ATTRIBUTE_ALIGNED16(class) +btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm { - btConvexTriangleCallback m_btConvexTriangleCallback; - bool m_isSwapped; - - + 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 3e8bc6e426..44dd3c553e 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -30,8 +30,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" - - #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" @@ -43,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" @@ -57,8 +53,6 @@ subject to the following restrictions: /////////// - - static SIMD_FORCE_INLINE void segmentsClosestPoints( btVector3& ptsVector, btVector3& offsetA, @@ -66,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; } @@ -114,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(); @@ -139,47 +138,38 @@ 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(btConvexPenetrationDepthSolver* pdSolver) { m_numPerturbationIterations = 0; @@ -187,30 +177,27 @@ btConvexConvexAlgorithm::CreateFunc::CreateFunc(btConvexPenetrationDepthSolver* m_pdSolver = pdSolver; } -btConvexConvexAlgorithm::CreateFunc::~CreateFunc() -{ +btConvexConvexAlgorithm::CreateFunc::~CreateFunc() +{ } -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), +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) @@ -220,112 +207,105 @@ 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)) { //m_manifoldPtr->clearManifold(); - btCapsuleShape* capsuleA = (btCapsuleShape*) min0; - btCapsuleShape* capsuleB = (btCapsuleShape*) min1; - - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + btCapsuleShape* capsuleA = (btCapsuleShape*)min0; + btCapsuleShape* capsuleB = (btCapsuleShape*)min1; - btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), - capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(), - body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; - if (dist<threshold) + 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; @@ -335,19 +315,19 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* { //m_manifoldPtr->clearManifold(); - btCapsuleShape* capsuleA = (btCapsuleShape*) min0; - btSphereShape* capsuleB = (btSphereShape*) min1; - - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + btCapsuleShape* capsuleA = (btCapsuleShape*)min0; + btSphereShape* capsuleB = (btSphereShape*)min1; + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; - btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), - 0.,capsuleB->getRadius(),capsuleA->getUpAxis(),1, - body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(), + 0., capsuleB->getRadius(), capsuleA->getUpAxis(), 1, + body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); - if (dist<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; @@ -357,252 +337,227 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* { //m_manifoldPtr->clearManifold(); - btSphereShape* capsuleA = (btSphereShape*) min0; - btCapsuleShape* capsuleB = (btCapsuleShape*) min1; - - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + btSphereShape* capsuleA = (btSphereShape*)min0; + btCapsuleShape* capsuleB = (btCapsuleShape*)min1; + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; - btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,0.,capsuleA->getRadius(), - capsuleB->getHalfHeight(),capsuleB->getRadius(),1,capsuleB->getUpAxis(), - body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, 0., capsuleA->getRadius(), + capsuleB->getHalfHeight(), capsuleB->getRadius(), 1, capsuleB->getUpAxis(), + body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); - if (dist<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 - - - +#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { - m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); + m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(), body1->getWorldTransform()); } - if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) -#endif //USE_SEPDISTANCE_UTIL2 + 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); + 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 - 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()+resultOut->m_closestPointDistanceThreshold; -// } - - input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; - } - - input.m_transformA = body0Wrap->getWorldTransform(); - input.m_transformB = body1Wrap->getWorldTransform(); - - - - - -#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) { - btVector3 m_normalOnBInWorld; - btVector3 m_pointInWorld; - btScalar m_depth; - bool m_hasContact; - - - btDummyResult() - : m_hasContact(false) + sepDist = gjkPairDetector.getCachedSeparatingDistance(); + if (sepDist > SIMD_EPSILON) { + sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; + //now perturbe directions to get multiple contact points } - - - 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; - } - }; + } +#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()); - - 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; - - } else - { - + if (foundSepAxis) + { + // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); - //we can also deal with convex versus triangle (without connectivity data) - if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) + 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; + } + else { + //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]); + //tri->initializePolyhedralFeatures(); - 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]); - - //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; - bool useSatSepNormal = true; + bool foundSepAxis = false; + bool useSatSepNormal = true; - if (useSatSepNormal) - { + if (useSatSepNormal) + { #if 0 if (0) { @@ -610,113 +565,107 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* 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]}; - 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(); - 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]); - 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) + 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++) { - planeEq=eq; + 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); } - 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) + 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++) { - planeEq=eq; + 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); } - 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(); - - 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); + } - 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()); } - - - - 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 - { + 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 - - if (dummy.m_hasContact && dummy.m_depth<0) - { - - if (foundSepAxis) + gjkPairDetector.getClosestPoints(input, dummy, dispatchInfo.m_debugDraw); +#endif //ZERO_MARGIN + + if (dummy.m_hasContact && dummy.m_depth < 0) { - if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace)<0.99) + if (foundSepAxis) { - printf("?\n"); + if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace) < 0.99) + { + printf("?\n"); + } } - } else - { - printf("!\n"); + else + { + printf("!\n"); + } + sepNormalWorldSpace.setValue(0, 0, 1); // = dummy.m_normalOnBInWorld; + //minDist = dummy.m_depth; + foundSepAxis = true; } - 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) @@ -728,145 +677,131 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* foundSepAxis = true; } #endif - } + } - - if (foundSepAxis) - { - worldVertsB2.resize(0); - btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2,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) { - btQuaternion perturbeRot(v0,perturbeAngle); - btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); - btQuaternion rotq(sepNormalWorldSpace,iterationAngle); - - + perturbeAngle = gContactBreakingThreshold / radiusA; + perturbeA = true; + } + else + { + 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; @@ -874,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/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index cd75ba12d7..eac5b4d824 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -23,7 +23,7 @@ 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; @@ -41,69 +41,61 @@ class btConvexPenetrationDepthSolver; class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm { #ifdef USE_SEPDISTANCE_UTIL2 - btConvexSeparatingDistanceUtil m_sepDistance; + btConvexSeparatingDistanceUtil m_sepDistance; #endif btConvexPenetrationDepthSolver* m_pdSolver; btVertexArray worldVertsB1; btVertexArray worldVertsB2; - - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_lowLevelOfDetail; - + + 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, 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; + btConvexPenetrationDepthSolver* m_pdSolver; int m_numPerturbationIterations; int m_minimumPointsPerturbationThreshold; 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_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/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp index cce2d95bcf..ba1bc06b69 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h index d28c430c4c..b693da118f 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index f6e4e57b0a..ef3ea9e394 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -26,114 +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 = NULL; + 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_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; + + 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() @@ -150,83 +144,78 @@ 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); + btAlignedFree(m_planeConvexCF); m_pdSolver->~btConvexPenetrationDepthSolver(); - - btAlignedFree(m_pdSolver); - + btAlignedFree(m_pdSolver); } btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) { - - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_sphereSphereCF; + return m_sphereSphereCF; } #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) { - return m_sphereBoxCF; + return m_sphereBoxCF; } if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_boxSphereCF; + return m_boxSphereCF; } -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM - +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE)) { - return m_sphereTriangleCF; + return m_sphereTriangleCF; } if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_triangleSphereCF; + return m_triangleSphereCF; } if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE)) @@ -239,8 +228,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPoint return m_planeConvexCF; } - - if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) { return m_convexConvexCreateFunc; @@ -256,7 +243,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPoint return m_swappedConvexConcaveCreateFunc; } - if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) { return m_compoundCompoundCreateFunc; @@ -276,46 +262,41 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPoint //failed to find an algorithm return m_emptyCreateFunc; - } -btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) +btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1) { - - - - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_sphereSphereCF; + return m_sphereSphereCF; } #ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE)) + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) { - return m_sphereBoxCF; + return m_sphereBoxCF; } - if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_boxSphereCF; + return m_boxSphereCF; } -#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM - +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM - if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE)) + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE)) { - return m_sphereTriangleCF; + return m_sphereTriangleCF; } - if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { - return m_triangleSphereCF; - } + 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; @@ -325,8 +306,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg { return m_planeConvexCF; } - - if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) { @@ -343,7 +322,6 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg return m_swappedConvexConcaveCreateFunc; } - if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) { return m_compoundCompoundCreateFunc; @@ -352,7 +330,8 @@ btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlg if (btBroadphaseProxy::isCompound(proxyType0)) { return m_compoundCreateFunc; - } else + } + else { if (btBroadphaseProxy::isCompound(proxyType1)) { @@ -366,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/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 17c7596cff..b39a3f41de 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -20,76 +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; - + 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; @@ -100,8 +92,7 @@ public: return m_collisionAlgorithmPool; } - - virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0, int proxyType1); virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1); @@ -112,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/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp index 5fa1c8be5e..7cd41bdb33 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h index cb0f152183..65ef83e094 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp index 86141fa689..00f16fd0a8 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.h index 8ec8613857..aa7f48d5cb 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp index 8271981b29..b686d98d1e 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp @@ -13,64 +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> #ifdef BT_DEBUG_COLLISION_PAIRS -int gOverlappingSimplePairs = 0; -int gRemoveSimplePairs =0; -int gAddedSimplePairs =0; -int gFindSimplePairs =0; -#endif //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()) { @@ -95,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) @@ -108,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; } @@ -120,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) @@ -158,32 +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) @@ -191,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()); @@ -234,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); @@ -269,13 +242,3 @@ void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) return userData; } //#include <stdio.h> - - - - - - - - - - diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h index 318981cda1..fd38a4f0e1 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -16,144 +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 - - +#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) { 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); + 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; } @@ -162,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/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index 898320ee1a..e74c83f9f1 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -12,50 +12,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 +63,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 +89,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 +131,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 +160,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 +187,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 +235,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 +283,17 @@ struct btConnectivityProcessor : public btTriangleCallback break; } - default: + default: { // printf("warning: duplicate triangle\n"); } - } } }; ///////////////////////////////////////////////////////// ///////////////////////////////////////////////////////// -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 +304,45 @@ 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 = indicestype == PHY_SHORT ? ((unsigned short*)gfxbase)[j] : gfxbase[j]; 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,140 +354,127 @@ 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); } - } - } - - - // 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; btBvhTriangleMeshShape* trimesh = 0; - - if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE ) + + 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) + if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); } } - if (trimesh==0) + if (trimesh == 0) return; - btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); + 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; @@ -511,334 +482,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) + { + numConcaveEdgeHits++; + } + else { - if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + 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 + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - btVector3 edge(v1-v2); - - isNearEdge = true; - - 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 + btDebugDrawLine(tr * contact, tr * (contact + cp.m_normalWorldOnB * 10), black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW - btVector3 edge(v2-v0); - - 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h index 7d9aafeee6..9d9cff040f 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h @@ -16,32 +16,26 @@ struct btCollisionObjectWrapper; class btManifoldPoint; class btIDebugDraw; - - 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); ///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/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 23c73c8825..770eb24369 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -13,106 +13,102 @@ 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; +CalculateCombinedCallback gCalculateCombinedRestitutionCallback = &btManifoldResult::calculateCombinedRestitution; +CalculateCombinedCallback gCalculateCombinedFrictionCallback = &btManifoldResult::calculateCombinedFriction; +CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback = &btManifoldResult::calculateCombinedRollingFriction; +CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback = &btManifoldResult::calculateCombinedSpinningFriction; +CalculateCombinedCallback gCalculateCombinedContactDampingCallback = &btManifoldResult::calculateCombinedContactDamping; +CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback = &btManifoldResult::calculateCombinedContactStiffness; -btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1) +btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0, const btCollisionObject* body1) { 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 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; + 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) +btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0, const btCollisionObject* body1) { - return body0->getContactDamping() + body1->getContactDamping(); + return body0->getContactDamping() + body1->getContactDamping(); } -btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1) +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; -} + 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_closestPointDistanceThreshold(0) + , + 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(); @@ -122,81 +118,82 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b 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 = 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()); - - 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. + 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()); + + 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) @@ -204,4 +201,3 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b gContactStartedCallback(m_manifoldPtr); } } - diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h index 12cdafd1b6..6c0a2d9a43 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/thirdparty/bullet/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,85 +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); +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; +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: +public: btManifoldResult() : #ifdef DEBUG_PART_INDEX - - m_partId0(-1), - m_partId1(-1), - m_index0(-1), - m_index1(-1) -#endif //DEBUG_PART_INDEX - m_closestPointDistanceThreshold(0) + + 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()) @@ -117,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()); } } @@ -153,15 +149,15 @@ public: return m_body1Wrap->getCollisionObject(); } - btScalar m_closestPointDistanceThreshold; + 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 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); + 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 91c76a8dac..e5097ccbbf 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/thirdparty/bullet/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,72 +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); + } }; class btPersistentManifoldSortPredicateDeterministic { public: - - SIMD_FORCE_INLINE bool operator() (const btPersistentManifold* lhs, const btPersistentManifold* rhs) const + 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)) - ); - + (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) +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++) { } @@ -248,69 +223,68 @@ 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)); if (colObj0->getIslandTag() == islandId) { - if (colObj0->getActivationState()== ACTIVE_TAG || - 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)); 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)); 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); } } @@ -318,34 +292,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); - if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) - { + 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) || + 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) { @@ -357,36 +327,34 @@ 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); } } } } - - ///@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); - int endIslandIndex=1; + 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 { @@ -394,7 +362,7 @@ 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 @@ -403,7 +371,8 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) { m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic()); - } else + } + else { m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); } @@ -417,55 +386,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; @@ -473,6 +436,5 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, m_islandBodies.resize(0); } - } // else if(!splitIslands) - + } // else if(!splitIslands) } diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index e24c6afeca..6c2802141c 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -26,45 +26,39 @@ 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 buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback); - void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld); + void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld); bool getSplitIslands() { @@ -74,8 +68,6 @@ public: { m_splitIslands = doSplitIslands; } - }; -#endif //BT_SIMULATION_ISLAND_MANAGER_H - +#endif //BT_SIMULATION_ISLAND_MANAGER_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp index e8b567e0ef..bc68b285b8 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index eefaedc9e7..3348bc89af 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index 27eaec3059..7fa0559f97 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -20,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; } } @@ -41,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; @@ -53,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+resultOut->m_closestPointDistanceThreshold)) + 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; @@ -82,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/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h index 3517a568a9..b08d0df76d 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index 86d4e74400..1bc3056c01 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/thirdparty/bullet/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()+ resultOut->m_closestPointDistanceThreshold); - + 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h index 6b6e39a72b..d660222f16 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp index 5222933595..816bf1e6ad 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ b/thirdparty/bullet/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/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.h index ef2a29202f..d422ef55eb 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.h +++ b/thirdparty/bullet/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 |