diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/CollisionDispatch')
31 files changed, 3190 insertions, 323 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index 9a749a03793..23a5c7526b4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -37,7 +37,7 @@ void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Res btVector3 point,normal; btScalar timeOfImpact = btScalar(1.); btScalar depth = btScalar(0.); -// output.m_distance = btScalar(1e30); +// output.m_distance = btScalar(BT_LARGE_FLOAT); //move sphere into triangle space btTransform sphereInTr = transformB.inverseTimes(transformA); @@ -57,8 +57,6 @@ void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Res } -#define MAX_OVERLAP btScalar(0.) - // See also geometrictools.com @@ -93,48 +91,39 @@ bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* ve return pointInTriangle(vertices, lnormal, &lp); } -///combined discrete/continuous sphere-triangle bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold) { const btVector3* vertices = &m_triangle->getVertexPtr(0); - const btVector3& c = sphereCenter; - btScalar r = m_sphere->getRadius(); - - btVector3 delta (0,0,0); + + btScalar radius = m_sphere->getRadius(); + btScalar radiusWithThreshold = radius + contactBreakingThreshold; btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); normal.normalize(); - btVector3 p1ToCentre = c - vertices[0]; + btVector3 p1ToCentre = sphereCenter - vertices[0]; btScalar distanceFromPlane = p1ToCentre.dot(normal); if (distanceFromPlane < btScalar(0.)) { //triangle facing the other way - distanceFromPlane *= btScalar(-1.); normal *= btScalar(-1.); } - btScalar contactMargin = contactBreakingThreshold; - bool isInsideContactPlane = distanceFromPlane < r + contactMargin; - bool isInsideShellPlane = distanceFromPlane < r; + bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; - btScalar deltaDotNormal = delta.dot(normal); - if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0)) - return false; - // Check for contact / intersection bool hasContact = false; btVector3 contactPoint; if (isInsideContactPlane) { - if (facecontains(c,vertices,normal)) { + if (facecontains(sphereCenter,vertices,normal)) { // Inside the contact wedge - touches a point on the shell plane hasContact = true; - contactPoint = c - normal*distanceFromPlane; + contactPoint = sphereCenter - normal*distanceFromPlane; } else { // Could be inside one of the contact capsules - btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); + btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold; btVector3 nearestOnEdge; for (int i = 0; i < m_triangle->getNumEdges(); i++) { @@ -143,7 +132,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po m_triangle->getEdge(i,pa,pb); - btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge); + btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge); if (distanceSqr < contactCapsuleRadiusSqr) { // Yep, we're inside a capsule hasContact = true; @@ -155,24 +144,27 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po } if (hasContact) { - btVector3 contactToCentre = c - contactPoint; + btVector3 contactToCentre = sphereCenter - contactPoint; btScalar distanceSqr = contactToCentre.length2(); - if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) { - btScalar distance = btSqrt(distanceSqr); - resultNormal = contactToCentre; - resultNormal.normalize(); - point = contactPoint; - depth = -(r-distance); + + if (distanceSqr < radiusWithThreshold*radiusWithThreshold) + { + if (distanceSqr>SIMD_EPSILON) + { + btScalar distance = btSqrt(distanceSqr); + resultNormal = contactToCentre; + resultNormal.normalize(); + point = contactPoint; + depth = -(radius-distance); + } else + { + btScalar distance = 0.f; + resultNormal = normal; + point = contactPoint; + depth = -radius; + } return true; } - - if (delta.dot(contactToCentre) >= btScalar(0.0)) - return false; - - // Moving towards the contact point -> collision - point = contactPoint; - timeOfImpact = btScalar(0.0); - return true; } return false; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index 981bd54e76c..f656e5c323a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -34,9 +34,11 @@ struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface virtual ~SphereTriangleDetector() {}; + bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold); + private: - bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold); + bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ); bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp new file mode 100644 index 00000000000..2182d0d7e49 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp @@ -0,0 +1,435 @@ +/* +Bullet Continuous Collision Detection and Physics Library +* The b2CollidePolygons routines are Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///btBox2dBox2dCollisionAlgorithm, with modified b2CollidePolygons routines from the Box2D library. +///The modifications include: switching from b2Vec to btVector3, redefinition of b2Dot, b2Cross + +#include "btBox2dBox2dCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h" +#include "BulletCollision/CollisionShapes/btBox2dShape.h" + +#define USE_PERSISTENT_CONTACTS 1 + +btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1) +: btActivatingCollisionAlgorithm(ci,obj0,obj1), +m_ownManifold(false), +m_manifoldPtr(mf) +{ + if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1)) + { + m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1); + 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); + +//#include <stdio.h> +void btBox2dBox2dCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + if (!m_manifoldPtr) + return; + + btCollisionObject* col0 = body0; + btCollisionObject* col1 = body1; + btBox2dShape* box0 = (btBox2dShape*)col0->getCollisionShape(); + btBox2dShape* box1 = (btBox2dShape*)col1->getCollisionShape(); + + resultOut->setPersistentManifold(m_manifoldPtr); + + b2CollidePolygons(resultOut,box0,col0->getWorldTransform(),box1,col1->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*/) +{ + //not yet + return 1.f; +} + + +struct ClipVertex +{ + btVector3 v; + int id; + //b2ContactID id; + //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) + +int b2_maxManifoldPoints =2; + +static int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], + const btVector3& normal, btScalar offset) +{ + // Start with no output points + int numOut = 0; + + // Calculate the distance of end points to the line + btScalar distance0 = b2Dot(normal, vIn[0].v) - offset; + btScalar distance1 = b2Dot(normal, vIn[1].v) - offset; + + // If the points are behind the plane + if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; + if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; + + // If the points are on different sides of the plane + if (distance0 * distance1 < 0.0f) + { + // Find intersection point of edge and plane + btScalar interp = distance0 / (distance0 - distance1); + vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); + if (distance0 > 0.0f) + { + vOut[numOut].id = vIn[0].id; + } + else + { + vOut[numOut].id = vIn[1].id; + } + ++numOut; + } + + return numOut; +} + +// 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 btVector3* vertices1 = poly1->getVertices(); + const btVector3* normals1 = poly1->getNormals(); + + int count2 = poly2->getVertexCount(); + const btVector3* vertices2 = poly2->getVertices(); + + btAssert(0 <= edge1 && edge1 < poly1->getVertexCount()); + + // Convert normal from poly1's frame into poly2's frame. + btVector3 normal1World = b2Mul(xf1.getBasis(), normals1[edge1]); + btVector3 normal1 = b2MulT(xf2.getBasis(), normal1World); + + // Find support vertex on poly2 for -normal. + int index = 0; + btScalar minDot = BT_LARGE_FLOAT; + + for (int i = 0; i < count2; ++i) + { + btScalar dot = b2Dot(vertices2[i], normal1); + if (dot < minDot) + { + minDot = dot; + index = i; + } + } + + btVector3 v1 = b2Mul(xf1, vertices1[edge1]); + btVector3 v2 = b2Mul(xf2, vertices2[index]); + btScalar separation = b2Dot(v2 - v1, normal1World); + return separation; +} + +// 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) +{ + int count1 = poly1->getVertexCount(); + const btVector3* normals1 = poly1->getNormals(); + + // Vector pointing from the centroid of poly1 to the centroid of poly2. + btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid()); + btVector3 dLocal1 = b2MulT(xf1.getBasis(), d); + + // Find edge normal on poly1 that has the largest projection onto d. + int edge = 0; + btScalar maxDot = -BT_LARGE_FLOAT; + for (int i = 0; i < count1; ++i) + { + btScalar dot = b2Dot(normals1[i], dLocal1); + if (dot > maxDot) + { + maxDot = dot; + edge = i; + } + } + + // Get the separation for the edge normal. + btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); + if (s > 0.0f) + { + return s; + } + + // Check the separation for the previous edge normal. + int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; + btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); + if (sPrev > 0.0f) + { + return sPrev; + } + + // Check the separation for the next edge normal. + int nextEdge = edge + 1 < count1 ? edge + 1 : 0; + btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2); + if (sNext > 0.0f) + { + return sNext; + } + + // Find the best edge and the search direction. + int bestEdge; + btScalar bestSeparation; + int increment; + if (sPrev > s && sPrev > sNext) + { + increment = -1; + bestEdge = prevEdge; + bestSeparation = sPrev; + } + else if (sNext > s) + { + increment = 1; + bestEdge = nextEdge; + bestSeparation = sNext; + } + else + { + *edgeIndex = edge; + return s; + } + + // Perform a local search for the best edge normal. + for ( ; ; ) + { + if (increment == -1) + edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; + else + edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; + + s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); + if (s > 0.0f) + { + return s; + } + + if (s > bestSeparation) + { + bestEdge = edge; + bestSeparation = s; + } + else + { + break; + } + } + + *edgeIndex = bestEdge; + return bestSeparation; +} + +static void FindIncidentEdge(ClipVertex c[2], + const btBox2dShape* poly1, const btTransform& xf1, int edge1, + const btBox2dShape* poly2, const btTransform& xf2) +{ + const btVector3* normals1 = poly1->getNormals(); + + int count2 = poly2->getVertexCount(); + const btVector3* vertices2 = poly2->getVertices(); + const btVector3* normals2 = poly2->getNormals(); + + btAssert(0 <= edge1 && edge1 < poly1->getVertexCount()); + + // Get the normal of the reference edge in poly2's frame. + btVector3 normal1 = b2MulT(xf2.getBasis(), b2Mul(xf1.getBasis(), normals1[edge1])); + + // Find the incident edge on poly2. + int index = 0; + btScalar minDot = BT_LARGE_FLOAT; + for (int i = 0; i < count2; ++i) + { + btScalar dot = b2Dot(normal1, normals2[i]); + if (dot < minDot) + { + minDot = dot; + index = i; + } + } + + // Build the clip vertices for the incident edge. + int i1 = index; + 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[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; +} + +// Find edge normal of max separation on A - return if separating axis is found +// Find edge normal of max separation on B - return if separation axis is found +// Choose reference edge as min(minA, minB) +// Find incident edge +// Clip + +// The normal points from 1 to 2 +void b2CollidePolygons(btManifoldResult* manifold, + 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) + return; + + int edgeB = 0; + btScalar separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA); + if (separationB > 0.0f) + return; + + const btBox2dShape* poly1; // reference poly + const btBox2dShape* poly2; // incident poly + btTransform xf1, xf2; + int edge1; // reference edge + unsigned char flip; + const btScalar k_relativeTol = 0.98f; + const btScalar k_absoluteTol = 0.001f; + + // TODO_ERIN use "radius" of poly for absolute tolerance. + if (separationB > k_relativeTol * separationA + k_absoluteTol) + { + poly1 = polyB; + poly2 = polyA; + xf1 = xfB; + xf2 = xfA; + edge1 = edgeB; + flip = 1; + } + else + { + poly1 = polyA; + poly2 = polyB; + xf1 = xfA; + xf2 = xfB; + edge1 = edgeA; + flip = 0; + } + + ClipVertex incidentEdge[2]; + FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); + + int count1 = poly1->getVertexCount(); + const btVector3* vertices1 = poly1->getVertices(); + + btVector3 v11 = vertices1[edge1]; + 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); + + btScalar frontOffset = b2Dot(frontNormal, v11); + btScalar sideOffset1 = -b2Dot(sideNormal, v11); + btScalar sideOffset2 = b2Dot(sideNormal, v12); + + // 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); + + ClipVertex clipPoints2[2]; + clipPoints2[0].v.setValue(0,0,0); + clipPoints2[1].v.setValue(0,0,0); + + + int np; + + // Clip to box side 1 + np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1); + + if (np < 2) + return; + + // Clip to negative box side 1 + np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2); + + if (np < 2) + { + return; + } + + // Now clipPoints2 contains the clipped points. + btVector3 manifoldNormal = flip ? -frontNormal : frontNormal; + + int pointCount = 0; + for (int i = 0; i < b2_maxManifoldPoints; ++i) + { + btScalar separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset; + + 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); + +// cp->id = clipPoints2[i].id; +// cp->id.features.flip = flip; + ++pointCount; + } + } + +// manifold->pointCount = pointCount;} +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h new file mode 100644 index 00000000000..21342175238 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BOX_2D_BOX_2D__COLLISION_ALGORITHM_H +#define BOX_2D_BOX_2D__COLLISION_ALGORITHM_H + +#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" + +class btPersistentManifold; + +///box-box collision detection +class btBox2dBox2dCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + +public: + btBox2dBox2dCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btActivatingCollisionAlgorithm(ci) {} + + virtual void processCollision (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,btCollisionObject* body0,btCollisionObject* body1); + + virtual ~btBox2dBox2dCollisionAlgorithm(); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + int bbsize = sizeof(btBox2dBox2dCollisionAlgorithm); + void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); + return new(ptr) btBox2dBox2dCollisionAlgorithm(0,ci,body0,body1); + } + }; + +}; + +#endif //BOX_2D_BOX_2D__COLLISION_ALGORITHM_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp index d3342c547b5..49628853493 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp @@ -61,7 +61,7 @@ void btBoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCo #endif //USE_PERSISTENT_CONTACTS btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = 1e30f; + input.m_maximumDistanceSquared = BT_LARGE_FLOAT; input.m_transformA = body0->getWorldTransform(); input.m_transformB = body1->getWorldTransform(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp index 31353f1b2c4..a7c8cf140ce 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -1,4 +1,3 @@ - /* * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. @@ -212,7 +211,7 @@ void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) a = 1.f/(btScalar(3.0)*(a+q)); } else { - a=1e30f; + a=BT_LARGE_FLOAT; } cx = a*(cx + q*(p[n*2-2]+p[0])); cy = a*(cy + q*(p[n*2-1]+p[1])); @@ -267,7 +266,7 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output) { const btScalar fudge_factor = btScalar(1.05); - btVector3 p,pp,normalC; + 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; @@ -333,9 +332,9 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, #undef TST #define TST(expr1,expr2,n1,n2,n3,cc) \ s2 = btFabs(expr1) - (expr2); \ - if (s2 > 0) return 0; \ + if (s2 > SIMD_EPSILON) return 0; \ l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ - if (l > 0) { \ + if (l > SIMD_EPSILON) { \ s2 /= l; \ if (s2*fudge_factor > s) { \ s = s2; \ @@ -346,6 +345,20 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, } \ } + 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); @@ -424,6 +437,7 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, output.addContactPoint(-normal,pointInWorld,-*depth); #else output.addContactPoint(-normal,pb,-*depth); + #endif // *return_code = code; } @@ -593,21 +607,30 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, if (maxc < 1) maxc = 1; if (cnum <= maxc) { - // we have less contacts than we need, so we use them all - for (j=0; j < cnum; j++) { - - //AddContactPoint... - - //dContactGeom *con = CONTACT(contact,skip*j); - //for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i]; - //con->depth = dep[j]; + 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]); + } + } } else { // we have more contacts than are wanted, some of them must be culled. @@ -632,7 +655,13 @@ int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, btVector3 posInWorld; for (i=0; i<3; i++) posInWorld[i] = point[iret[j]*3+i] + pa[i]; - output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); + if (code<4) + { + output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); + } else + { + output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]); + } } cnum = maxc; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index e6ff2130aad..9fed44a19f7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -34,9 +34,7 @@ int gNumManifold = 0; btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): - m_count(0), - m_useIslands(true), - m_staticWarningReported(false), +m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD), m_collisionConfiguration(collisionConfiguration) { int i; @@ -79,9 +77,11 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) btCollisionObject* body0 = (btCollisionObject*)b0; btCollisionObject* body1 = (btCollisionObject*)b1; - //test for Bullet 2.74: use a relative contact breaking threshold without clamping against 'gContactBreakingThreshold' - //btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold())); - btScalar contactBreakingThreshold = btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()); + //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()); @@ -169,13 +169,13 @@ bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionO bool needsCollision = true; #ifdef BT_DEBUG - if (!m_staticWarningReported) + if (!(m_dispatcherFlags & btCollisionDispatcher::CD_STATIC_STATIC_REPORTED)) { //broadphase filtering already deals with this if ((body0->isStaticObject() || body0->isKinematicObject()) && (body1->isStaticObject() || body1->isKinematicObject())) { - m_staticWarningReported = true; + m_dispatcherFlags |= btCollisionDispatcher::CD_STATIC_STATIC_REPORTED; printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index a9c9cd414c1..3c4f039504a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -42,14 +42,13 @@ typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispa ///Time of Impact, Closest Points and Penetration Depth. class btCollisionDispatcher : public btDispatcher { - int m_count; - - btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr; - bool m_useIslands; +protected: + + int m_dispatcherFlags; + + btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr; - bool m_staticWarningReported; - btManifoldResult m_defaultManifoldResult; btNearCallback m_nearCallback; @@ -59,13 +58,28 @@ class btCollisionDispatcher : public btDispatcher btPoolAllocator* m_persistentManifoldPoolAllocator; btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; - btCollisionConfiguration* m_collisionConfiguration; public: + enum DispatcherFlags + { + CD_STATIC_STATIC_REPORTED = 1, + CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2 + }; + + int getDispatcherFlags() const + { + return m_dispatcherFlags; + } + + void setDispatcherFlags(int flags) + { + m_dispatcherFlags = flags; + } + ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 285b8f174e4..580ea345860 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -15,13 +15,15 @@ subject to the following restrictions: #include "btCollisionObject.h" +#include "LinearMath/btSerializer.h" btCollisionObject::btCollisionObject() : m_anisotropicFriction(1.f,1.f,1.f), m_hasAnisotropicFriction(false), - m_contactProcessingThreshold(0.f), + 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), @@ -30,14 +32,14 @@ btCollisionObject::btCollisionObject() m_deactivationTime(btScalar(0.)), m_friction(btScalar(0.5)), m_restitution(btScalar(0.)), - m_userObjectPointer(0), m_internalType(CO_COLLISION_OBJECT), + m_userObjectPointer(0), m_hitFraction(btScalar(1.)), m_ccdSweptSphereRadius(btScalar(0.)), m_ccdMotionThreshold(btScalar(0.)), m_checkCollideWith(false) { - + m_worldTransform.setIdentity(); } btCollisionObject::~btCollisionObject() @@ -64,5 +66,51 @@ void btCollisionObject::activate(bool forceActivation) } } +const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btCollisionObjectData* dataOut = (btCollisionObjectData*)dataBuffer; + m_worldTransform.serialize(dataOut->m_worldTransform); + m_interpolationWorldTransform.serialize(dataOut->m_interpolationWorldTransform); + m_interpolationLinearVelocity.serialize(dataOut->m_interpolationLinearVelocity); + m_interpolationAngularVelocity.serialize(dataOut->m_interpolationAngularVelocity); + m_anisotropicFriction.serialize(dataOut->m_anisotropicFriction); + dataOut->m_hasAnisotropicFriction = m_hasAnisotropicFriction; + dataOut->m_contactProcessingThreshold = m_contactProcessingThreshold; + dataOut->m_broadphaseHandle = 0; + dataOut->m_collisionShape = serializer->getUniquePointer(m_collisionShape); + dataOut->m_rootCollisionShape = 0;//@todo + dataOut->m_collisionFlags = m_collisionFlags; + dataOut->m_islandTag1 = m_islandTag1; + dataOut->m_companionId = m_companionId; + dataOut->m_activationState1 = m_activationState1; + dataOut->m_activationState1 = m_activationState1; + dataOut->m_deactivationTime = m_deactivationTime; + dataOut->m_friction = m_friction; + dataOut->m_restitution = m_restitution; + dataOut->m_internalType = m_internalType; + + char* name = (char*) serializer->findNameForPointer(this); + dataOut->m_name = (char*)serializer->getUniquePointer(name); + if (dataOut->m_name) + { + serializer->serializeName(name); + } + dataOut->m_hitFraction = m_hitFraction; + dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius; + dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold; + dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold; + dataOut->m_checkCollideWith = m_checkCollideWith; + + return btCollisionObjectDataName; +} + + +void btCollisionObject::serializeSingleObject(class btSerializer* serializer) const +{ + int len = calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(len,1); + const char* structType = serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_COLLISIONOBJECT_CODE,(void*)this); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 0d5b7886443..5de829824ff 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -27,13 +27,21 @@ subject to the following restrictions: struct btBroadphaseProxy; class btCollisionShape; +struct btCollisionShapeData; #include "LinearMath/btMotionState.h" #include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btAlignedObjectArray.h" - typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray; +#ifdef BT_USE_DOUBLE_PRECISION +#define btCollisionObjectData btCollisionObjectDoubleData +#define btCollisionObjectDataName "btCollisionObjectDoubleData" +#else +#define btCollisionObjectData btCollisionObjectFloatData +#define btCollisionObjectDataName "btCollisionObjectFloatData" +#endif + /// 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. @@ -53,12 +61,14 @@ protected: btVector3 m_interpolationLinearVelocity; btVector3 m_interpolationAngularVelocity; - btVector3 m_anisotropicFriction; - bool m_hasAnisotropicFriction; - btScalar m_contactProcessingThreshold; + 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; ///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 @@ -76,13 +86,13 @@ protected: btScalar m_friction; btScalar m_restitution; - ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer - void* m_userObjectPointer; - ///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; + ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer + void* m_userObjectPointer; + ///time of impact calculation btScalar m_hitFraction; @@ -93,9 +103,7 @@ protected: btScalar m_ccdMotionThreshold; /// If some object should have elaborate collision filtering by sub-classes - bool m_checkCollideWith; - - char m_pad[7]; + int m_checkCollideWith; virtual bool checkCollideWithOverride(btCollisionObject* /* co */) { @@ -112,18 +120,21 @@ public: CF_KINEMATIC_OBJECT= 2, CF_NO_CONTACT_RESPONSE = 4, CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) - CF_CHARACTER_OBJECT = 16 + CF_CHARACTER_OBJECT = 16, + CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing + CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing }; enum CollisionObjectTypes { CO_COLLISION_OBJECT =1, - CO_RIGID_BODY, + 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, - CO_SOFT_BODY, - CO_HF_FLUID + CO_GHOST_OBJECT=4, + CO_SOFT_BODY=8, + CO_HF_FLUID=16, + CO_USER_TYPE=32 }; SIMD_FORCE_INLINE bool mergesSimulationIslands() const @@ -143,7 +154,7 @@ public: } bool hasAnisotropicFriction() const { - return m_hasAnisotropicFriction; + return m_hasAnisotropicFriction!=0; } ///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default. @@ -213,6 +224,19 @@ public: m_collisionShape = collisionShape; } + ///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 + { + 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) + { + m_extensionPointer = pointer; + } + SIMD_FORCE_INLINE int getActivationState() const { return m_activationState1;} void setActivationState(int newState); @@ -393,7 +417,7 @@ public: /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold void setCcdMotionThreshold(btScalar ccdMotionThreshold) { - m_ccdMotionThreshold = ccdMotionThreshold*ccdMotionThreshold; + m_ccdMotionThreshold = ccdMotionThreshold; } ///users can point to their objects, userPointer is not used by Bullet @@ -416,6 +440,85 @@ public: return true; } + + 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; + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btCollisionObjectDoubleData +{ + void *m_broadphaseHandle; + void *m_collisionShape; + btCollisionShapeData *m_rootCollisionShape; + char *m_name; + + btTransformDoubleData m_worldTransform; + btTransformDoubleData m_interpolationWorldTransform; + btVector3DoubleData m_interpolationLinearVelocity; + btVector3DoubleData m_interpolationAngularVelocity; + btVector3DoubleData m_anisotropicFriction; + double m_contactProcessingThreshold; + double m_deactivationTime; + double m_friction; + double m_restitution; + double m_hitFraction; + double m_ccdSweptSphereRadius; + double m_ccdMotionThreshold; + + int m_hasAnisotropicFriction; + int m_collisionFlags; + int m_islandTag1; + int m_companionId; + int m_activationState1; + int m_internalType; + int m_checkCollideWith; + + char m_padding[4]; }; +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btCollisionObjectFloatData +{ + void *m_broadphaseHandle; + void *m_collisionShape; + btCollisionShapeData *m_rootCollisionShape; + char *m_name; + + btTransformFloatData m_worldTransform; + btTransformFloatData m_interpolationWorldTransform; + btVector3FloatData m_interpolationLinearVelocity; + btVector3FloatData m_interpolationAngularVelocity; + btVector3FloatData m_anisotropicFriction; + float m_contactProcessingThreshold; + float m_deactivationTime; + float m_friction; + float m_restitution; + float m_hitFraction; + float m_ccdSweptSphereRadius; + float m_ccdMotionThreshold; + + int m_hasAnisotropicFriction; + int m_collisionFlags; + int m_islandTag1; + int m_companionId; + int m_activationState1; + int m_internalType; + int m_checkCollideWith; +}; + + + +SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const +{ + return sizeof(btCollisionObjectData); +} + + + #endif //COLLISION_OBJECT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 5c645f82a45..bfe8d4f52fb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -26,12 +26,16 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" - +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "BulletCollision/BroadphaseCollision/btDbvt.h" #include "LinearMath/btAabbUtil2.h" #include "LinearMath/btQuickprof.h" #include "LinearMath/btStackAlloc.h" -#include "BulletSoftBody/btSoftBody.h" +#include "LinearMath/btSerializer.h" + +//#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 @@ -43,10 +47,29 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" +///for debug drawing + +//for debug rendering +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/CollisionShapes/btConeShape.h" +#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btCylinderShape.h" +#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" +#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#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_debugDrawer(0), +m_forceUpdateAllAabbs(true) { m_stackAlloc = collisionConfiguration->getStackAllocator(); m_dispatchInfo.m_stackAllocator = m_stackAlloc; @@ -89,28 +112,30 @@ btCollisionWorld::~btCollisionWorld() void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) { + btAssert(collisionObject); + //check that the object isn't already added - btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); + btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); - m_collisionObjects.push_back(collisionObject); + m_collisionObjects.push_back(collisionObject); - //calculate new AABB - btTransform trans = collisionObject->getWorldTransform(); + //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( - minAabb, - maxAabb, - type, - collisionObject, - collisionFilterGroup, - collisionFilterMask, - m_dispatcher1,0 - )) ; + int type = collisionObject->getCollisionShape()->getShapeType(); + collisionObject->setBroadphaseHandle( getBroadphase()->createProxy( + minAabb, + maxAabb, + type, + collisionObject, + collisionFilterGroup, + collisionFilterMask, + m_dispatcher1,0 + )) ; @@ -129,6 +154,16 @@ void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) minAabb -= contactThreshold; maxAabb += contactThreshold; + if(getDispatchInfo().m_convexMaxDistanceUseCPT) + { + btVector3 minAabb2,maxAabb2; + colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); + minAabb2 -= contactThreshold; + maxAabb2 += contactThreshold; + minAabb.setMin(minAabb2); + maxAabb.setMax(maxAabb2); + } + btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; //moving objects should be moderately sized, probably something wrong if not @@ -163,7 +198,7 @@ void btCollisionWorld::updateAabbs() btCollisionObject* colObj = m_collisionObjects[i]; //only update aabb of active objects - if (colObj->isActive()) + if (m_forceUpdateAllAabbs || colObj->isActive()) { updateSingleAabb(colObj); } @@ -226,10 +261,10 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) { btSphereShape pointShape(btScalar(0.0)); pointShape.setMargin(0.f); @@ -237,7 +272,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra if (collisionShape->isConvex()) { -// BT_PROFILE("rayTestConvex"); + // BT_PROFILE("rayTestConvex"); btConvexCast::CastResult castResult; castResult.m_fraction = resultCallback.m_closestHitFraction; @@ -266,10 +301,10 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra castResult.m_normal.normalize(); btCollisionWorld::LocalRayResult localRayResult ( - collisionObject, - 0, - castResult.m_normal, - castResult.m_fraction + collisionObject, + 0, + castResult.m_normal, + castResult.m_fraction ); bool normalInWorldSpace = true; @@ -281,7 +316,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra } else { if (collisionShape->isConcave()) { -// BT_PROFILE("rayTestConcave"); + // BT_PROFILE("rayTestConcave"); if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) { ///optimized version for btBvhTriangleMeshShape @@ -297,15 +332,18 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra btCollisionObject* m_collisionObject; btTriangleMeshShape* m_triangleMesh; + btTransform m_colObjWorldTransform; + BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): - //@BP Mod - btTriangleRaycastCallback(from,to, resultCallback->m_flags), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) - { - } + btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* 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 ) @@ -314,19 +352,21 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; + btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; + btCollisionWorld::LocalRayResult rayResult - (m_collisionObject, + (m_collisionObject, &shapeInfo, - hitNormalLocal, + hitNormalWorld, hitFraction); - bool normalInWorldSpace = false; + bool normalInWorldSpace = true; return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); } }; - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); } else @@ -347,15 +387,18 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra btCollisionObject* m_collisionObject; btConcaveShape* m_triangleMesh; + btTransform m_colObjWorldTransform; + BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, - btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh): - //@BP Mod - btTriangleRaycastCallback(from,to, resultCallback->m_flags), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) - { - } + btCollisionWorld::RayResultCallback* resultCallback, 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 ) @@ -364,22 +407,22 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; + btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; + btCollisionWorld::LocalRayResult rayResult - (m_collisionObject, + (m_collisionObject, &shapeInfo, - hitNormalLocal, + hitNormalWorld, hitFraction); - bool normalInWorldSpace = false; + bool normalInWorldSpace = true; return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); - - } }; - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape); + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); rcb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 rayAabbMinLocal = rayFromLocal; @@ -390,27 +433,118 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); } } else { -// BT_PROFILE("rayTestCompound"); - ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt + // BT_PROFILE("rayTestCompound"); if (collisionShape->isCompound()) { + struct LocalInfoAdder2 : public RayResultCallback + { + RayResultCallback* m_userCallback; + int m_i; + + LocalInfoAdder2 (int i, RayResultCallback *user) + : m_userCallback(user), m_i(i) + { + m_closestHitFraction = m_userCallback->m_closestHitFraction; + } + virtual bool needsCollision(btBroadphaseProxy* p) const + { + return m_userCallback->needsCollision(p); + } + + virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = -1; + shapeInfo.m_triangleIndex = m_i; + if (r.m_localShapeInfo == NULL) + r.m_localShapeInfo = &shapeInfo; + + const btScalar result = m_userCallback->addSingleResult(r, b); + m_closestHitFraction = m_userCallback->m_closestHitFraction; + return result; + } + }; + + struct RayTester : btDbvt::ICollide + { + btCollisionObject* m_collisionObject; + const btCompoundShape* m_compoundShape; + const btTransform& m_colObjWorldTransform; + const btTransform& m_rayFromTrans; + const btTransform& m_rayToTrans; + RayResultCallback& m_resultCallback; + + RayTester(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) + { + + } + + void Process(int i) + { + const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i); + const btTransform& childTrans = m_compoundShape->getChildTransform(i); + btTransform childWorldTrans = m_colObjWorldTransform * childTrans; + + // replace collision shape so that callback can determine the triangle + btCollisionShape* saveCollisionShape = m_collisionObject->getCollisionShape(); + m_collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); + + LocalInfoAdder2 my_cb(i, &m_resultCallback); + + rayTestSingle( + m_rayFromTrans, + m_rayToTrans, + m_collisionObject, + childCollisionShape, + childWorldTrans, + my_cb); + + // restore + m_collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); + } + + void Process(const btDbvtNode* leaf) + { + Process(leaf->dataAsInt); + } + }; + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); - int i=0; - for (i=0;i<compoundShape->getNumChildShapes();i++) + const btDbvt* dbvt = compoundShape->getDynamicAabbTree(); + + + RayTester rayCB( + collisionObject, + compoundShape, + colObjWorldTransform, + rayFromTrans, + rayToTrans, + resultCallback); +#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION + if (dbvt) { - btTransform childTrans = compoundShape->getChildTransform(i); - const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); - btTransform childWorldTrans = colObjWorldTransform * childTrans; - // replace collision shape so that callback can determine the triangle - btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape(); - collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); - rayTestSingle(rayFromTrans,rayToTrans, - collisionObject, - childCollisionShape, - childWorldTrans, - resultCallback); - // restore - collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); + btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin(); + btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin(); + btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB); + } + else +#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION + { + for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i) + { + rayCB.Process(i); + } } } } @@ -418,10 +552,10 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra } void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, - btCollisionObject* collisionObject, - const btCollisionShape* collisionShape, - const btTransform& colObjWorldTransform, - ConvexResultCallback& resultCallback, btScalar allowedPenetration) + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback, btScalar allowedPenetration) { if (collisionShape->isConvex()) { @@ -433,15 +567,15 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt btConvexShape* convexShape = (btConvexShape*) collisionShape; btVoronoiSimplexSolver simplexSolver; btGjkEpaPenetrationDepthSolver 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)) { //add hit @@ -451,13 +585,13 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt { castResult.m_normal.normalize(); btCollisionWorld::LocalConvexResult localConvexResult - ( - collisionObject, - 0, - castResult.m_normal, - castResult.m_hitPoint, - castResult.m_fraction - ); + ( + collisionObject, + 0, + castResult.m_normal, + castResult.m_hitPoint, + castResult.m_fraction + ); bool normalInWorldSpace = true; resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); @@ -487,12 +621,12 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld): - btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) - { - } + 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 ) @@ -504,7 +638,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt { btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, + (m_collisionObject, &shapeInfo, hitNormalLocal, hitPointLocal, @@ -522,6 +656,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,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); @@ -544,12 +679,12 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): - btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) - { - } + 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 ) @@ -561,7 +696,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt { btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, + (m_collisionObject, &shapeInfo, hitNormalLocal, hitPointLocal, @@ -578,6 +713,7 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); tccb.m_hitFraction = resultCallback.m_closestHitFraction; + tccb.m_allowedPenetration = allowedPenetration; btVector3 boxMinLocal, boxMaxLocal; castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); @@ -604,11 +740,41 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt // replace collision shape so that callback can determine the triangle btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape(); collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); + struct LocalInfoAdder : public ConvexResultCallback { + ConvexResultCallback* m_userCallback; + int m_i; + + LocalInfoAdder (int i, ConvexResultCallback *user) + : m_userCallback(user), m_i(i) + { + m_closestHitFraction = m_userCallback->m_closestHitFraction; + } + virtual bool needsCollision(btBroadphaseProxy* p) const + { + return m_userCallback->needsCollision(p); + } + virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = -1; + shapeInfo.m_triangleIndex = m_i; + if (r.m_localShapeInfo == NULL) + r.m_localShapeInfo = &shapeInfo; + const btScalar result = m_userCallback->addSingleResult(r, b); + m_closestHitFraction = m_userCallback->m_closestHitFraction; + return result; + + } + }; + + LocalInfoAdder my_cb(i, &resultCallback); + + objectQuerySingle(castShape, convexFromTrans,convexToTrans, collisionObject, childCollisionShape, childWorldTrans, - resultCallback, allowedPenetration); + my_cb, allowedPenetration); // restore collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); } @@ -631,10 +797,10 @@ struct btSingleRayCallback : public btBroadphaseRayCallback 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_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_world(world), + m_resultCallback(resultCallback) { m_rayFromTrans.setIdentity(); m_rayFromTrans.setOrigin(m_rayFromWorld); @@ -644,10 +810,10 @@ struct btSingleRayCallback : public btBroadphaseRayCallback btVector3 rayDir = (rayToWorld-rayFromWorld); rayDir.normalize (); - ///what about division by zero? --> just set rayDirection[i] to INF/1e30 - m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + ///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]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; m_signs[0] = m_rayDirectionInverse[0] < 0.0; m_signs[1] = m_rayDirectionInverse[1] < 0.0; m_signs[2] = m_rayDirectionInverse[2] < 0.0; @@ -656,7 +822,7 @@ struct btSingleRayCallback : public btBroadphaseRayCallback } - + virtual bool process(const btBroadphaseProxy* proxy) { @@ -687,9 +853,9 @@ struct btSingleRayCallback : public btBroadphaseRayCallback { m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - m_resultCallback); + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback); } } return true; @@ -698,7 +864,7 @@ struct btSingleRayCallback : public btBroadphaseRayCallback void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const { - BT_PROFILE("rayTest"); + //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); @@ -737,10 +903,10 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback { btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin()); btVector3 rayDir = unnormalizedRayDir.normalized(); - ///what about division by zero? --> just set rayDirection[i] to INF/1e30 - m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + ///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]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; m_signs[0] = m_rayDirectionInverse[0] < 0.0; m_signs[1] = m_rayDirectionInverse[1] < 0.0; m_signs[2] = m_rayDirectionInverse[2] < 0.0; @@ -761,13 +927,13 @@ struct btSingleSweepCallback : public btBroadphaseRayCallback 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); + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback, + m_allowedCcdPenetration); } - + return true; } }; @@ -782,7 +948,7 @@ void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT /// 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; convexFromTrans = convexFromWorld; @@ -825,12 +991,455 @@ void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT { objectQuerySingle(castShape, convexFromTrans,convexToTrans, collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback, - allowedCcdPenetration); + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + allowedCcdPenetration); } } } #endif //USE_BRUTEFORCE_RAYBROADPHASE } + + + +struct btBridgedManifoldResult : public btManifoldResult +{ + + btCollisionWorld::ContactResultCallback& m_resultCallback; + + btBridgedManifoldResult( btCollisionObject* obj0,btCollisionObject* obj1,btCollisionWorld::ContactResultCallback& resultCallback ) + :btManifoldResult(obj0,obj1), + m_resultCallback(resultCallback) + { + } + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + { + bool isSwapped = m_manifoldPtr->getBody0() != m_body0; + btVector3 pointA = pointInWorld + normalOnBInWorld * depth; + btVector3 localA; + btVector3 localB; + if (isSwapped) + { + localA = m_rootTransB.invXform(pointA ); + localB = m_rootTransA.invXform(pointInWorld); + } else + { + localA = m_rootTransA.invXform(pointA ); + localB = m_rootTransB.invXform(pointInWorld); + } + + btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + newPt.m_positionWorldOnA = pointA; + newPt.m_positionWorldOnB = pointInWorld; + + //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_partId0 = m_partId0; + newPt.m_partId1 = m_partId1; + newPt.m_index0 = m_index0; + newPt.m_index1 = m_index1; + } + + //experimental feature info, for per-triangle material etc. + btCollisionObject* obj0 = isSwapped? m_body1 : m_body0; + btCollisionObject* obj1 = isSwapped? m_body0 : m_body1; + m_resultCallback.addSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,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) + { + } + + virtual bool process(const btBroadphaseProxy* proxy) + { + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + if (collisionObject == m_collisionObject) + return true; + + //only perform raycast if filterMask matches + if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { + btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(m_collisionObject,collisionObject); + if (algorithm) + { + btBridgedManifoldResult contactPointResult(m_collisionObject,collisionObject, m_resultCallback); + //discrete collision detection query + algorithm->processCollision(m_collisionObject,collisionObject, m_world->getDispatchInfo(),&contactPointResult); + + algorithm->~btCollisionAlgorithm(); + m_world->getDispatcher()->freeCollisionAlgorithm(algorithm); + } + } + return true; + } +}; + + +///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) +{ + 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) +{ + btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(colObjA,colObjB); + if (algorithm) + { + btBridgedManifoldResult contactPointResult(colObjA,colObjB, resultCallback); + //discrete collision detection query + algorithm->processCollision(colObjA,colObjB, getDispatchInfo(),&contactPointResult); + + algorithm->~btCollisionAlgorithm(); + getDispatcher()->freeCollisionAlgorithm(algorithm); + } + +} + + + + +class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback +{ + 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) + { + } + + 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.); + + 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 + 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--) + { + btTransform childTrans = compoundShape->getChildTransform(i); + const btCollisionShape* colShape = compoundShape->getChildShape(i); + debugDrawObject(worldTransform*childTrans,colShape,color); + } + + } 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(); + + 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: + { + + 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 polyhedral shapes + if (shape->isPolyhedral()) + { + btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; + + int i; + 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); + + } + + + } + } + } + } +} + + +void btCollisionWorld::debugDrawWorld() +{ + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) + { + int numManifolds = getDispatcher()->getNumManifolds(); + btVector3 color(0,0,0); + 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++) + { + btManifoldPoint& cp = contactManifold->getContactPoint(j); + getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); + } + } + } + + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) + { + int i; + + for ( i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0) + { + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) + { + btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.)); + switch(colObj->getActivationState()) + { + case ACTIVE_TAG: + color = btVector3(btScalar(1.),btScalar(1.),btScalar(1.)); break; + case ISLAND_SLEEPING: + color = btVector3(btScalar(0.),btScalar(1.),btScalar(0.));break; + case WANTS_DEACTIVATION: + color = btVector3(btScalar(0.),btScalar(1.),btScalar(1.));break; + case DISABLE_DEACTIVATION: + color = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));break; + case DISABLE_SIMULATION: + color = btVector3(btScalar(1.),btScalar(1.),btScalar(0.));break; + default: + { + color = btVector3(btScalar(1),btScalar(0.),btScalar(0.)); + } + }; + + debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); + } + if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + btVector3 minAabb,maxAabb; + btVector3 colorvec(1,0,0); + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + minAabb -= contactThreshold; + maxAabb += contactThreshold; + + btVector3 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); + } + } + + } + } +} + + +void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) +{ + int i; + //serialize all collision objects + for (i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) + { + colObj->serializeSingleObject(serializer); + } + } + + ///keep track of shapes already serialized + btHashMap<btHashPtr,btCollisionShape*> serializedShapes; + + 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); + shape->serializeSingleShape(serializer); + } + } + +} + + +void btCollisionWorld::serialize(btSerializer* serializer) +{ + + serializer->startSerialization(); + + serializeCollisionObjects(serializer); + + serializer->finishSerialization(); +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index 24343938e5c..b42e2c40b21 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -22,6 +22,7 @@ subject to the following restrictions: * * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ). * + * The main documentation is Bullet_User_Manual.pdf, included in the source code distribution. * There is the Physics Forum for feedback and general Collision Detection and Physics discussions. * Please visit http://www.bulletphysics.com * @@ -29,14 +30,16 @@ subject to the following restrictions: * * @subsection step1 Step 1: Download * You can download the Bullet Physics Library from the Google Code repository: http://code.google.com/p/bullet/downloads/list + * * @subsection step2 Step 2: Building - * Bullet comes with autogenerated Project Files for Microsoft Visual Studio 6, 7, 7.1 and 8. - * The main Workspace/Solution is located in Bullet/msvc/8/wksbullet.sln (replace 8 with your version). - * - * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using make, cmake, http://www.cmake.org , or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet. - * So if you are not using MSVC or cmake, you can run ./autogen.sh ./configure to create both Makefile and Jamfile and then run make or jam. - * Jam is a build system that can build the library, demos and also autogenerate the MSVC Project Files. - * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/jam + * Bullet main build system for all platforms is cmake, you can download http://www.cmake.org + * cmake can autogenerate projectfiles for Microsoft Visual Studio, Apple Xcode, KDevelop and Unix Makefiles. + * The easiest is to run the CMake cmake-gui graphical user interface and choose the options and generate projectfiles. + * You can also use cmake in the command-line. Here are some examples for various platforms: + * cmake . -G "Visual Studio 9 2008" + * cmake . -G Xcode + * cmake . -G "Unix Makefiles" + * Although cmake is recommended, you can also use autotools for UNIX: ./autogen.sh ./configure to create a Makefile and then run make. * * @subsection step3 Step 3: Testing demos * Try to run and experiment with BasicDemo executable as a starting point. @@ -53,9 +56,7 @@ subject to the following restrictions: * Bullet has been designed in a modular way keeping dependencies to a minimum. The ConvexHullDistance demo demonstrates direct use of btGjkPairDetector. * * @section copyright Copyright - * Copyright (C) 2005-2008 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon - * Special thanks to all visitors of the Bullet Physics forum, and in particular above contributors, John McCutchan, Nathanael Presson, Dave Eberle, Dirk Gregorius, Erin Catto, Dave Eberle, Adam Moravanszky, - * Pierre Terdiman, Kenny Erleben, Russell Smith, Oliver Strunk, Jan Paul van Waveren, Marten Svanfeldt. + * For up-to-data information and copyright and contributors list check out the Bullet_User_Manual.pdf * */ @@ -68,6 +69,8 @@ class btStackAlloc; class btCollisionShape; class btConvexShape; class btBroadphaseInterface; +class btSerializer; + #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" #include "btCollisionObject.h" @@ -94,7 +97,12 @@ protected: 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); + public: //this constructor doesn't own the dispatcher and paircache/broadphase @@ -147,6 +155,10 @@ public: return m_debugDrawer; } + 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 @@ -252,6 +264,45 @@ public: } }; + struct AllHitsRayResultCallback : public RayResultCallback + { + AllHitsRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) + :m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld) + { + } + + btAlignedObjectArray<btCollisionObject*> m_collisionObjects; + + btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction + btVector3 m_rayToWorld; + + btAlignedObjectArray<btVector3> m_hitNormalWorld; + btAlignedObjectArray<btVector3> m_hitPointWorld; + btAlignedObjectArray<btScalar> m_hitFractions; + + virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) + { + m_collisionObject = rayResult.m_collisionObject; + m_collisionObjects.push_back(rayResult.m_collisionObject); + btVector3 hitNormalWorld; + if (normalInWorldSpace) + { + hitNormalWorld = rayResult.m_hitNormalLocal; + } else + { + ///need to transform normal into worldspace + hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; + } + m_hitNormalWorld.push_back(hitNormalWorld); + btVector3 hitPointWorld; + 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 { @@ -347,6 +398,34 @@ public: } }; + ///ContactResultCallback is used to report contact points + struct ContactResultCallback + { + short int m_collisionFilterGroup; + short int m_collisionFilterMask; + + ContactResultCallback() + :m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter) + { + } + + virtual ~ContactResultCallback() + { + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; + collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask); + return collides; + } + + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1) = 0; + }; + + + int getNumCollisionObjects() const { return int(m_collisionObjects.size()); @@ -356,10 +435,18 @@ public: /// 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; - // 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. + /// 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; + ///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); + + ///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); + /// 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. @@ -377,7 +464,7 @@ public: const btTransform& colObjWorldTransform, ConvexResultCallback& resultCallback, btScalar allowedPenetration); - void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); + virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); btCollisionObjectArray& getCollisionObjectArray() { @@ -390,7 +477,7 @@ public: } - void removeCollisionObject(btCollisionObject* collisionObject); + virtual void removeCollisionObject(btCollisionObject* collisionObject); virtual void performDiscreteCollisionDetection(); @@ -403,6 +490,18 @@ public: { return m_dispatchInfo; } + + bool getForceUpdateAllAabbs() const + { + return m_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); }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 1dea91a0b0b..54889a6375d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -114,8 +114,9 @@ public: void ProcessChildShape(btCollisionShape* childShape,int index) { - + btAssert(index>=0); btCompoundShape* compoundShape = static_cast<btCompoundShape*>(m_compoundColObj->getCollisionShape()); + btAssert(index<compoundShape->getNumChildShapes()); //backup @@ -142,6 +143,15 @@ public: if (!m_childCollisionAlgorithms[index]) m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(m_compoundColObj,m_otherObj,m_sharedManifold); + ///detect swapping case + if (m_resultOut->getBody0Internal() == m_compoundColObj) + { + m_resultOut->setShapeIdentifiersA(-1,index); + } else + { + m_resultOut->setShapeIdentifiersB(-1,index); + } + m_childCollisionAlgorithms[index]->processCollision(m_compoundColObj,m_otherObj,m_dispatchInfo,m_resultOut); if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { @@ -224,7 +234,7 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt resultOut->setPersistentManifold(0);//??necessary? } } - manifoldArray.clear(); + manifoldArray.resize(0); } } } @@ -257,20 +267,24 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt int numChildren = m_childCollisionAlgorithms.size(); int i; btManifoldArray manifoldArray; - + btCollisionShape* childShape = 0; + btTransform orgTrans; + btTransform orgInterpolationTrans; + btTransform newChildWorldTrans; + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + for (i=0;i<numChildren;i++) { if (m_childCollisionAlgorithms[i]) { - btCollisionShape* childShape = compoundShape->getChildShape(i); + childShape = compoundShape->getChildShape(i); //if not longer overlapping, remove the algorithm - btTransform orgTrans = colObj->getWorldTransform(); - btTransform orgInterpolationTrans = colObj->getInterpolationWorldTransform(); + orgTrans = colObj->getWorldTransform(); + orgInterpolationTrans = colObj->getInterpolationWorldTransform(); const btTransform& childTrans = compoundShape->getChildTransform(i); - btTransform newChildWorldTrans = orgTrans*childTrans ; + newChildWorldTrans = orgTrans*childTrans ; //perform an AABB check first - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); otherObj->getCollisionShape()->getAabb(otherObj->getWorldTransform(),aabbMin1,aabbMax1); @@ -280,13 +294,8 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); m_childCollisionAlgorithms[i] = 0; } - } - } - - - } } @@ -311,13 +320,15 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* int numChildren = m_childCollisionAlgorithms.size(); int i; + btTransform orgTrans; + btScalar frac; for (i=0;i<numChildren;i++) { //temporarily exchange parent btCollisionShape with childShape, and recurse btCollisionShape* childShape = compoundShape->getChildShape(i); //backup - btTransform orgTrans = colObj->getWorldTransform(); + orgTrans = colObj->getWorldTransform(); const btTransform& childTrans = compoundShape->getChildTransform(i); //btTransform newChildWorldTrans = orgTrans*childTrans ; @@ -325,7 +336,7 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* btCollisionShape* tmpShape = colObj->getCollisionShape(); colObj->internalSetTemporaryCollisionShape( childShape ); - btScalar frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); + frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); if (frac<hitFraction) { hitFraction = frac; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp new file mode 100644 index 00000000000..db7f884ac82 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp @@ -0,0 +1,247 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btConvex2dConvex2dAlgorithm.h" + +//#include <stdio.h> +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#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" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" + +#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" + + + +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + +#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" + +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" + + +btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) +{ + m_numPerturbationIterations = 0; + m_minimumPointsPerturbationThreshold = 3; + m_simplexSolver = simplexSolver; + m_pdSolver = pdSolver; +} + +btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() +{ +} + +btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold) +: btActivatingCollisionAlgorithm(ci,body0,body1), +m_simplexSolver(simplexSolver), +m_pdSolver(pdSolver), +m_ownManifold (false), +m_manifoldPtr(mf), +m_lowLevelOfDetail(false), + m_numPerturbationIterations(numPerturbationIterations), +m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) +{ + (void)body0; + (void)body1; +} + + + + +btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel) +{ + m_lowLevelOfDetail = useLowLevel; +} + + + +extern btScalar gContactBreakingThreshold; + + +// +// Convex-Convex collision algorithm +// +void btConvex2dConvex2dAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + if (!m_manifoldPtr) + { + //swapped? + m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); + m_ownManifold = true; + } + resultOut->setPersistentManifold(m_manifoldPtr); + + //comment-out next line to test multi-contact generation + //resultOut->getPersistentManifold()->clearManifold(); + + + btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape()); + btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape()); + + btVector3 normalOnB; + btVector3 pointOnBWorld; + + { + + + btGjkPairDetector::ClosestPointInput input; + + 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_stackAlloc = dispatchInfo.m_stackAllocator; + input.m_transformA = body0->getWorldTransform(); + input.m_transformB = body1->getWorldTransform(); + + gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + + btVector3 v0,v1; + btVector3 sepNormalWorldSpace; + + } + + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + +} + + + + +btScalar btConvex2dConvex2dAlgorithm::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; + + + //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 + btConvexCast::CastResult result; + btVoronoiSimplexSolver voronoiSimplex; + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + 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 (col1->getHitFraction() > 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 + btConvexCast::CastResult result; + btVoronoiSimplexSolver voronoiSimplex; + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + 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 (col1->getHitFraction() > result.m_fraction) + col1->setHitFraction( result.m_fraction); + + if (resultFraction > result.m_fraction) + resultFraction = result.m_fraction; + + } + } + + return resultFraction; + +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h new file mode 100644 index 00000000000..5738401401e --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h @@ -0,0 +1,95 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef CONVEX_2D_CONVEX_2D_ALGORITHM_H +#define CONVEX_2D_CONVEX_2D_ALGORITHM_H + +#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#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; + btConvexPenetrationDepthSolver* m_pdSolver; + + + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_lowLevelOfDetail; + + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + +public: + + btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); + + + virtual ~btConvex2dConvex2dAlgorithm(); + + virtual void processCollision (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) + { + ///should we use m_ownManifold to avoid adding duplicates? + if (m_manifoldPtr && m_ownManifold) + manifoldArray.push_back(m_manifoldPtr); + } + + + void setLowLevelOfDetail(bool useLowLevel); + + + const btPersistentManifold* getManifold() + { + return m_manifoldPtr; + } + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + + btConvexPenetrationDepthSolver* m_pdSolver; + btSimplexSolverInterface* m_simplexSolver; + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + + CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); + + virtual ~CreateFunc(); + + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm)); + return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + } + }; + + +}; + +#endif //CONVEX_2D_CONVEX_2D_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index cbc5530732b..268ec4b6c7e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -95,7 +95,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i ///debug drawing of the overlapping triangles if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe )) { - btVector3 color(255,255,0); + btVector3 color(1,1,0); btTransform& tr = ob->getWorldTransform(); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); @@ -121,12 +121,16 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i ob->internalSetTemporaryCollisionShape( &tm ); btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr); - ///this should use the btDispatcher, so the actual registered algorithm is used - // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); - m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex); - // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); -// cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); + if (m_resultOut->getBody0Internal() == m_triBody) + { + m_resultOut->setShapeIdentifiersA(partId,triangleIndex); + } + else + { + m_resultOut->setShapeIdentifiersB(partId,triangleIndex); + } + colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 496fd996f8c..ede1afb2a03 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -13,6 +13,11 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance +///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums +///with reproduction case +//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1 + #include "btConvexConvexAlgorithm.h" //#include <stdio.h> @@ -20,6 +25,9 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #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" @@ -43,8 +51,127 @@ subject to the following restrictions: +/////////// + + + +static SIMD_FORCE_INLINE void segmentsClosestPoints( + btVector3& ptsVector, + btVector3& offsetA, + btVector3& offsetB, + btScalar& tA, btScalar& tB, + const btVector3& translation, + const btVector3& dirA, btScalar hlenA, + 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 denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB; + + if ( denom == 0.0f ) { + tA = 0.0f; + } else { + tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom; + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } + + tB = tA * dirA_dot_dirB - dirB_dot_trans; + + if ( tB < -hlenB ) { + tB = -hlenB; + tA = tB * dirA_dot_dirB + dirA_dot_trans; + + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } else if ( tB > hlenB ) { + tB = hlenB; + tA = tB * dirA_dot_dirB + dirA_dot_trans; + + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } + + // compute the closest points relative to segment centers. + + offsetA = dirA * tA; + offsetB = dirB * tB; + + ptsVector = translation - offsetA + offsetB; +} + + +static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance( + btVector3& normalOnB, + btVector3& pointOnB, + btScalar capsuleLengthA, + btScalar capsuleRadiusA, + btScalar capsuleLengthB, + btScalar capsuleRadiusB, + int capsuleAxisA, + int capsuleAxisB, + const btTransform& transformA, + const btTransform& transformB, + btScalar distanceThreshold ) +{ + btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA); + btVector3 translationA = transformA.getOrigin(); + btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB); + btVector3 translationB = transformB.getOrigin(); + + // translation between centers + + btVector3 translation = translationB - translationA; + + // 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 + + segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation, + directionA, capsuleLengthA, directionB, capsuleLengthB ); + + btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB; + + if ( distance > distanceThreshold ) + return distance; + + btScalar lenSqr = ptsVector.length2(); + 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 + { + // compute the contact normal + normalOnB = ptsVector*-btRecipSqrt(lenSqr); + } + pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB; + + return distance; +} + + + + + +////////// + @@ -69,7 +196,7 @@ m_ownManifold (false), m_manifoldPtr(mf), m_lowLevelOfDetail(false), #ifdef USE_SEPDISTANCE_UTIL2 -,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(), +m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(), (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()), #endif m_numPerturbationIterations(numPerturbationIterations), @@ -111,8 +238,8 @@ struct btPerturbedContactResult : public btManifoldResult :m_originalManifoldResult(originalResult), m_transformA(transformA), m_transformB(transformB), - m_perturbA(perturbA), m_unPerturbedTransform(unPerturbedTransform), + m_perturbA(perturbA), m_debugDrawer(debugDrawer) { } @@ -155,6 +282,7 @@ struct btPerturbedContactResult : public btManifoldResult extern btScalar gContactBreakingThreshold; + // // Convex-Convex collision algorithm // @@ -176,8 +304,39 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape()); btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape()); + btVector3 normalOnB; + btVector3 pointOnBWorld; +#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER + if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) + { + btCapsuleShape* capsuleA = (btCapsuleShape*) min0; + btCapsuleShape* capsuleB = (btCapsuleShape*) min1; + btVector3 localScalingA = capsuleA->getLocalScaling(); + btVector3 localScalingB = capsuleB->getLocalScaling(); + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), + capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(), + body0->getWorldTransform(),body1->getWorldTransform(),threshold); + + if (dist<threshold) + { + btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); + } + resultOut->refreshContactPoints(); + return; + } +#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER + + #ifdef USE_SEPDISTANCE_UTIL2 - m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); + if (dispatchInfo.m_useConvexConservativeDistanceUtil) + { + m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); + } + if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) #endif //USE_SEPDISTANCE_UTIL2 @@ -194,31 +353,55 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { - input.m_maximumDistanceSquared = 1e30f; + input.m_maximumDistanceSquared = BT_LARGE_FLOAT; } else #endif //USE_SEPDISTANCE_UTIL2 { - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); + if (dispatchInfo.m_convexMaxDistanceUseCPT) + { + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); + } else + { + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); + } input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; } + input.m_stackAlloc = dispatchInfo.m_stackAllocator; input.m_transformA = body0->getWorldTransform(); input.m_transformB = body1->getWorldTransform(); gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); - btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold; - //now perturbe directions to get multiple contact points - btVector3 v0,v1; - btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); - btPlaneSpace1(sepNormalWorldSpace,v0,v1); + + +#ifdef USE_SEPDISTANCE_UTIL2 + btScalar sepDist = 0.f; + if (dispatchInfo.m_useConvexConservativeDistanceUtil) + { + sepDist = gjkPairDetector.getCachedSeparatingDistance(); + if (sepDist>SIMD_EPSILON) + { + sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; + //now perturbe directions to get multiple contact points + + } + } +#endif //USE_SEPDISTANCE_UTIL2 + //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points - if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) + if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) { int i; + btVector3 v0,v1; + btVector3 sepNormalWorldSpace; + + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); + btPlaneSpace1(sepNormalWorldSpace,v0,v1); + bool perturbeA = true; const btScalar angleLimit = 0.125f * SIMD_PI; @@ -248,6 +431,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl 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); @@ -271,7 +456,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); - + } } } @@ -279,7 +464,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl #ifdef USE_SEPDISTANCE_UTIL2 - if (dispatchInfo.m_useConvexConservativeDistanceUtil) + if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) { m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform()); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index 62dd33eb98d..d38aff6862c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -31,7 +31,8 @@ class btConvexPenetrationDepthSolver; ///so the distance is not conservative. In that case, enabling this USE_SEPDISTANCE_UTIL2 would result in failing/missing collisions. ///Either improve GJK for large size ratios (testing a 100 units versus a 0.1 unit object) or only enable the util ///for certain pairs that have a small size ratio -///#define USE_SEPDISTANCE_UTIL2 1 + +//#define USE_SEPDISTANCE_UTIL2 1 ///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects. ///Multiple contact points are calculated by perturbing the orientation of the smallest object orthogonal to the separating normal. diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp index a7b3b163d66..dda85dc693f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -102,9 +102,9 @@ void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0 btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); - bool hasCollision = false; + const btVector3& planeNormal = planeShape->getPlaneNormal(); - const btScalar& planeConstant = planeShape->getPlaneConstant(); + //const btScalar& planeConstant = planeShape->getPlaneConstant(); //first perform a collision query with the non-perturbated collision objects { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h index 368ca71dda0..f49ac45e772 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -60,8 +60,8 @@ public: int m_minimumPointsPerturbationThreshold; CreateFunc() - : m_numPerturbationIterations(3), - m_minimumPointsPerturbationThreshold(3) + : m_numPerturbationIterations(1), + m_minimumPointsPerturbationThreshold(1) { } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index 3ae25f109c2..c27d8ce0752 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -45,17 +45,17 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16); m_simplexSolver = new (mem)btVoronoiSimplexSolver(); - -#define USE_EPA 1 -#ifdef USE_EPA - mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16); - m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver; -#else - mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16); - m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver; -#endif//USE_EPA - + if (constructionInfo.m_useEpaPenetrationAlgorithm) + { + mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16); + m_pdSolver = new (mem)btGjkEpaPenetrationDepthSolver; + }else + { + mem = btAlignedAlloc(sizeof(btMinkowskiPenetrationDepthSolver),16); + m_pdSolver = new (mem)btMinkowskiPenetrationDepthSolver; + } + //default CreationFunctions, filling the m_doubleDispatch table mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16); m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver); @@ -102,7 +102,8 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault int maxSize3 = sizeof(btCompoundCollisionAlgorithm); int sl = sizeof(btConvexSeparatingDistanceUtil); sl = sizeof(btGjkPairDetector); - int collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2); + int collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); if (constructionInfo.m_stackAlloc) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 6d8cab726bd..6aa0d8c270f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -27,7 +27,9 @@ struct btDefaultCollisionConstructionInfo btPoolAllocator* m_collisionAlgorithmPool; int m_defaultMaxPersistentManifoldPoolSize; int m_defaultMaxCollisionAlgorithmPoolSize; + int m_customCollisionAlgorithmMaxElementSize; int m_defaultStackAllocatorSize; + int m_useEpaPenetrationAlgorithm; btDefaultCollisionConstructionInfo() :m_stackAlloc(0), @@ -35,7 +37,9 @@ struct btDefaultCollisionConstructionInfo m_collisionAlgorithmPool(0), m_defaultMaxPersistentManifoldPoolSize(4096), m_defaultMaxCollisionAlgorithmPoolSize(4096), - m_defaultStackAllocatorSize(0) + m_customCollisionAlgorithmMaxElementSize(0), + m_defaultStackAllocatorSize(0), + m_useEpaPenetrationAlgorithm(true) { } }; @@ -108,6 +112,11 @@ public: return m_stackAlloc; } + virtual btVoronoiSimplexSolver* getSimplexSolver() + { + return m_simplexSolver; + } + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h index 95b5750240c..8ec86138575 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h @@ -160,7 +160,7 @@ public: 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 @@ -171,4 +171,5 @@ public: }; -#endif
\ No newline at end of file +#endif + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp new file mode 100644 index 00000000000..5cceb04dbb4 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -0,0 +1,772 @@ +#include "btInternalEdgeUtility.h" + +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" +#include "LinearMath/btIDebugDraw.h" + + +//#define DEBUG_INTERNAL_EDGE + + +#ifdef DEBUG_INTERNAL_EDGE +#include <stdio.h> +#endif //DEBUG_INTERNAL_EDGE + + +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW +static btIDebugDraw* gDebugDrawer = 0; + +void btSetDebugDrawer(btIDebugDraw* debugDrawer) +{ + gDebugDrawer = debugDrawer; +} + +static void btDebugDrawLine(const btVector3& from,const btVector3& to, const btVector3& color) +{ + if (gDebugDrawer) + gDebugDrawer->drawLine(from,to,color); +} +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + +static int btGetHash(int partId, int 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) +{ + const btVector3 refAxis0 = edgeA; + const btVector3 refAxis1 = normalA; + const btVector3 swingAxis = normalB; + btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); + return angle; +} + + +struct btConnectivityProcessor : public btTriangleCallback +{ + int m_partIdA; + int m_triangleIndexA; + btVector3* m_triangleVerticesA; + btTriangleInfoMap* m_triangleInfoMap; + + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + //skip self-collisions + if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex)) + return; + + //skip duplicates (disabled for now) + //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex)) + // return; + + //search for shared vertices and edges + int numshared = 0; + 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(); + if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold) + return; + + + 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) + return; + +#if 0 + printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n", + m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(), + m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(), + m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ()); + + printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex); + printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n", + triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(), + triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(), + triangle[2].getX(),triangle[2].getY(),triangle[2].getZ()); +#endif + + for (int i=0;i<3;i++) + { + for (int j=0;j<3;j++) + { + if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold) + { + sharedVertsA[numshared] = i; + sharedVertsB[numshared] = j; + numshared++; + ///degenerate case + if(numshared >= 3) + return; + } + } + ///degenerate case + if(numshared >= 3) + return; + } + switch (numshared) + { + case 0: + { + break; + } + case 1: + { + //shared vertex + break; + } + 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 + if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2) + { + sharedVertsA[0] = 2; + sharedVertsA[1] = 0; + int tmp = sharedVertsB[1]; + sharedVertsB[1] = sharedVertsB[0]; + sharedVertsB[0] = tmp; + } + + int hash = btGetHash(m_partIdA,m_triangleIndexA); + + btTriangleInfo* info = m_triangleInfoMap->find(hash); + if (!info) + { + btTriangleInfo tmp; + m_triangleInfoMap->insert(hash,tmp); + info = m_triangleInfoMap->find(hash); + } + + int sumvertsA = sharedVertsA[0]+sharedVertsA[1]; + int otherIndexA = 3-sumvertsA; + + + 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 tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]); + //btTriangleShape tB(triangle[0],triangle[1],triangle[2]); + + btVector3 normalA; + btVector3 normalB; + tA.calcNormal(normalA); + tB.calcNormal(normalB); + edge.normalize(); + btVector3 edgeCrossA = edge.cross(normalA).normalize(); + + { + btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]]; + if (edgeCrossA.dot(tmp) < 0) + { + edgeCrossA*=-1; + } + } + + btVector3 edgeCrossB = edge.cross(normalB).normalize(); + + { + btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]]; + if (edgeCrossB.dot(tmp) < 0) + { + edgeCrossB*=-1; + } + } + + btScalar angle2 = 0; + btScalar ang4 = 0.f; + + + btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB); + btScalar len2 = calculatedEdge.length2(); + + btScalar correctedAngle(0); + btVector3 calculatedNormalB = normalA; + bool isConvex = false; + + if (len2<m_triangleInfoMap->m_planarEpsilon) + { + angle2 = 0.f; + ang4 = 0.f; + } else + { + + calculatedEdge.normalize(); + btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA); + calculatedNormalA.normalize(); + 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.); + + correctedAngle = isConvex ? ang4 : -ang4; + btQuaternion orn2(calculatedEdge,-correctedAngle); + calculatedNormalB = btMatrix3x3(orn2)*normalA; + + + } + + + + + + //alternatively use + //btVector3 calculatedNormalB2 = quatRotate(orn,normalA); + + + switch (sumvertsA) + { + case 1: + { + 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) + { + computedNormalB*=-1; + info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB; + } +#ifdef DEBUG_INTERNAL_EDGE + if ((computedNormalB-normalB).length()>0.0001) + { + printf("warning: normals not identical\n"); + } +#endif//DEBUG_INTERNAL_EDGE + + info->m_edgeV0V1Angle = -correctedAngle; + + if (isConvex) + info->m_flags |= TRI_INFO_V0V1_CONVEX; + break; + } + case 2: + { + btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0]; + btQuaternion orn(edge,-correctedAngle); + btVector3 computedNormalB = quatRotate(orn,normalA); + if (computedNormalB.dot(normalB)<0) + { + computedNormalB*=-1; + info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB; + } + +#ifdef DEBUG_INTERNAL_EDGE + if ((computedNormalB-normalB).length()>0.0001) + { + printf("warning: normals not identical\n"); + } +#endif //DEBUG_INTERNAL_EDGE + info->m_edgeV2V0Angle = -correctedAngle; + if (isConvex) + info->m_flags |= TRI_INFO_V2V0_CONVEX; + break; + } + case 3: + { + 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; + } +#ifdef DEBUG_INTERNAL_EDGE + if ((computedNormalB-normalB).length()>0.0001) + { + printf("warning: normals not identical\n"); + } +#endif //DEBUG_INTERNAL_EDGE + info->m_edgeV1V2Angle = -correctedAngle; + + if (isConvex) + info->m_flags |= TRI_INFO_V1V2_CONVEX; + break; + } + } + + break; + } + default: + { + // printf("warning: duplicate triangle\n"); + } + + } + } +}; +///////////////////////////////////////////////////////// +///////////////////////////////////////////////////////// + +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()) + return; + + trimeshShape->setTriangleInfoMap(triangleInfoMap); + + btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface(); + const btVector3& meshScaling = meshInterface->getScaling(); + + for (int partId = 0; partId< meshInterface->getNumSubParts();partId++) + { + const unsigned char *vertexbase = 0; + int numverts = 0; + PHY_ScalarType type = PHY_INTEGER; + int stride = 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; + + for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++) + { + unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride); + + for (int j=2;j>=0;j--) + { + + int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j]; + if (type == PHY_FLOAT) + { + float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + triangleVerts[j] = btVector3( + 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())); + } + } + 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]); + aabbMax.setMax(triangleVerts[1]); + aabbMin.setMin(triangleVerts[2]); + aabbMax.setMax(triangleVerts[2]); + + btConnectivityProcessor connectivityProcessor; + connectivityProcessor.m_partIdA = partId; + connectivityProcessor.m_triangleIndexA = triangleIndex; + connectivityProcessor.m_triangleVerticesA = &triangleVerts[0]; + connectivityProcessor.m_triangleInfoMap = triangleInfoMap; + + 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) +{ + btVector3 lineDelta = line1 - line0; + + // Handle degenerate lines + if ( lineDelta.fuzzyZero()) + { + nearestPoint = line0; + } + else + { + btScalar delta = (point-line0).dot(lineDelta) / (lineDelta).dot(lineDelta); + + // Clamp the point to conform to the segment's endpoints + if ( delta < 0 ) + delta = 0; + else if ( delta > 1 ) + delta = 1; + + nearestPoint = line0 + lineDelta*delta; + } +} + + + + +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); + + if (correctedEdgeAngle<0) + { + if (curAngle < correctedEdgeAngle) + { + btScalar diffAngle = correctedEdgeAngle-curAngle; + btQuaternion rotation(edge,diffAngle ); + clampedLocalNormal = btMatrix3x3(rotation)*localContactNormalOnB; + return true; + } + } + + if (correctedEdgeAngle>=0) + { + if (curAngle > correctedEdgeAngle) + { + 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 btCollisionObject* colObj0,const btCollisionObject* colObj1, int partId0, int index0, int normalAdjustFlags) +{ + //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE); + if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE) + return; + + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape(); + btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); + if (!triangleInfoMapPtr) + return; + + 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; + + const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0->getCollisionShape()); + 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 tri_normal; + tri_shape->calcNormal(tri_normal); + + //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB); + btVector3 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 + + + + bool isNearEdge = false; + + int numConcaveEdgeHits = 0; + int numConvexEdgeHits = 0; + + btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + localContactNormalOnB.normalize();//is this necessary? + + if ((info->m_edgeV0V1Angle)< SIMD_2_PI) + { +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); +#endif + btScalar len = (contact-nearest).length(); + if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) + { + btVector3 edge(v0-v1); + isNearEdge = true; + + 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; + + 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); + +#ifdef 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 (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + { + btVector3 newNormal = colObj0->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 = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); + + } + } + } + } + } + } + + 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 + + if ((info->m_edgeV1V2Angle)< SIMD_2_PI) + { +#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) + { + isNearEdge = true; +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 edge(v1-v2); + + isNearEdge = true; + + if (info->m_edgeV1V2Angle == btScalar(0)) + { + 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 + + + btScalar NdotA = localContactNormalOnB.dot(nA); + btScalar NdotB = localContactNormalOnB.dot(nB); + bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); + + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else + { + numConvexEdgeHits++; + btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal); + if (isClamped) + { + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + { + btVector3 newNormal = colObj0->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 = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); + } + } + } + } + } + } + + 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 + + if ((info->m_edgeV2V0Angle)< SIMD_2_PI) + { + +#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) + { + isNearEdge = true; +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 edge(v2-v0); + + if (info->m_edgeV2V0Angle==btScalar(0)) + { + numConcaveEdgeHits++; + } else + { + + 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); + + if (backFacingNormal) + { + numConcaveEdgeHits++; + } + else + { + numConvexEdgeHits++; + // printf("hitting convex edge\n"); + + + btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + btVector3 clampedLocalNormal; + bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal); + if (isClamped) + { + if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) + { + btVector3 newNormal = colObj0->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 = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); + } + } + } + } + + + } + } + +#ifdef DEBUG_INTERNAL_EDGE + { + btVector3 color(0,1,1); + btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color); + } +#endif //DEBUG_INTERNAL_EDGE + + if (isNearEdge) + { + + if (numConcaveEdgeHits>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 = colObj0->getWorldTransform().getBasis()*tri_normal; + } else + { + //modify the normal to be the triangle normal (or backfacing normal) + cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *(tri_normal *frontFacing); + } + + + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); + } + } +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h new file mode 100644 index 00000000000..9efb0122bb8 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h @@ -0,0 +1,46 @@ + +#ifndef BT_INTERNAL_EDGE_UTILITY_H +#define BT_INTERNAL_EDGE_UTILITY_H + +#include "LinearMath/btHashMap.h" +#include "LinearMath/btVector3.h" + +#include "BulletCollision/CollisionShapes/btTriangleInfoMap.h" + +///The btInternalEdgeUtility helps to avoid or reduce artifacts due to wrong collision normals caused by internal edges. +///See also http://code.google.com/p/bullet/issues/detail?id=27 + +class btBvhTriangleMeshShape; +class btCollisionObject; +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_CONVEX_DOUBLE_SIDED = 4 +}; + + +///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo' +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 btCollisionObject* trimeshColObj0,const btCollisionObject* otherColObj1, 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 + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index f8dfa5b101f..fd684c056f7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -47,6 +47,12 @@ btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* b :m_manifoldPtr(0), m_body0(body0), m_body1(body1) +#ifdef DEBUG_PART_INDEX + ,m_partId0(-1), + m_partId1(-1), + m_index0(-1), + m_index1(-1) +#endif //DEBUG_PART_INDEX { m_rootTransA = body0->getWorldTransform(); m_rootTransB = body1->getWorldTransform(); @@ -57,8 +63,9 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b { btAssert(m_manifoldPtr); //order in manifold needs to match - - if (depth > m_manifoldPtr->getContactBreakingThreshold()) + +// if (depth > m_manifoldPtr->getContactBreakingThreshold()) + if (depth > m_manifoldPtr->getContactProcessingThreshold()) return; bool isSwapped = m_manifoldPtr->getBody0() != m_body0; @@ -88,10 +95,19 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1); //BP mod, store contact triangles. - newPt.m_partId0 = m_partId0; - newPt.m_partId1 = m_partId1; - newPt.m_index0 = m_index0; - newPt.m_index1 = m_index1; + 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_partId0 = m_partId0; + newPt.m_partId1 = m_partId1; + 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) @@ -112,7 +128,7 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b //experimental feature info, for per-triangle material etc. btCollisionObject* obj0 = isSwapped? m_body1 : m_body0; btCollisionObject* obj1 = isSwapped? m_body0 : m_body1; - (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0,m_partId0,m_index0,obj1,m_partId1,m_index1); + (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1); } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 964b6a04483..927e2bc4f76 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -28,11 +28,14 @@ class btManifoldPoint; typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1); extern ContactAddedCallback gContactAddedCallback; +//#define DEBUG_PART_INDEX 1 ///btManifoldResult is a helper class to manage contact results. class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result { +protected: + btPersistentManifold* m_manifoldPtr; //we need this for compounds @@ -50,6 +53,13 @@ class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result public: btManifoldResult() +#ifdef DEBUG_PART_INDEX + : + m_partId0(-1), + m_partId1(-1), + m_index0(-1), + m_index1(-1) +#endif //DEBUG_PART_INDEX { } @@ -71,12 +81,16 @@ public: return m_manifoldPtr; } - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + virtual void setShapeIdentifiersA(int partId0,int index0) { - m_partId0=partId0; - m_partId1=partId1; - m_index0=index0; - m_index1=index1; + m_partId0=partId0; + m_index0=index0; + } + + virtual void setShapeIdentifiersB( int partId1,int index1) + { + m_partId1=partId1; + m_index1=index1; } @@ -99,7 +113,16 @@ public: } } + const btCollisionObject* getBody0Internal() const + { + return m_body0; + } + const btCollisionObject* getBody1Internal() const + { + return m_body1; + } + }; #endif //MANIFOLD_RESULT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 0328d0f738f..bb2a7f23985 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -1,3 +1,4 @@ + /* Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ @@ -44,10 +45,12 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl { { + btOverlappingPairCache* pairCachePtr = colWorld->getPairCache(); + const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs(); + btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr(); - for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++) + for (int i=0;i<numOverlappingPairs;i++) { - btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr(); const btBroadphasePair& collisionPair = pairPtr[i]; btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; @@ -63,15 +66,69 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl } } +#ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION +void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) +{ + + // put the index into m_controllers into m_tag + int index = 0; + { + + int i; + for (i=0;i<colWorld->getCollisionObjectArray().size(); i++) + { + btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + //Adding filtering here + if (!collisionObject->isStaticOrKinematicObject()) + { + collisionObject->setIslandTag(index++); + } + collisionObject->setCompanionId(-1); + collisionObject->setHitFraction(btScalar(1.)); + } + } + // do the union find + + initUnionFind( index ); + + findUnions(dispatcher,colWorld); +} + +void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) +{ + // put the islandId ('find' value) into m_tag + { + int index = 0; + int i; + for (i=0;i<colWorld->getCollisionObjectArray().size();i++) + { + btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + if (!collisionObject->isStaticOrKinematicObject()) + { + 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 + { + collisionObject->setIslandTag(-1); + collisionObject->setCompanionId(-2); + } + } + } +} + +#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) { - + initUnionFind( int (colWorld->getCollisionObjectArray().size())); - + // put the index into m_controllers into m_tag { - + int index = 0; int i; for (i=0;i<colWorld->getCollisionObjectArray().size(); i++) @@ -81,26 +138,20 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld collisionObject->setCompanionId(-1); collisionObject->setHitFraction(btScalar(1.)); index++; - + } } // do the union find - - findUnions(dispatcher,colWorld); - - + findUnions(dispatcher,colWorld); } - - - void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) { // put the islandId ('find' value) into m_tag { - - + + int index = 0; int i; for (i=0;i<colWorld->getCollisionObjectArray().size();i++) @@ -120,6 +171,8 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col } } +#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION + inline int getIslandId(const btPersistentManifold* lhs) { int islandId; @@ -339,15 +392,15 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, int islandId = getUnionFind().getElement(startIslandIndex).m_id; - bool islandSleeping = false; + 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 = true; + if (colObj0->isActive()) + islandSleeping = false; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index 2d5efcf56ba..c327c3ff72a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -59,7 +59,7 @@ void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* co SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()); btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = btScalar(1e30);///@todo: tighter bounds + input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds input.m_transformA = sphereObj->getWorldTransform(); input.m_transformB = triObj->getWorldTransform(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp index c561df06109..4c4f58d44fa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -70,7 +70,9 @@ void btUnionFind::sortIslands() 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 } // Sort the vector using predicate and std::sort @@ -78,4 +80,3 @@ void btUnionFind::sortIslands() m_elements.quickSort(btUnionFindElementSortPredicate()); } - diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h index e105ecbff18..2cce335145b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h @@ -18,7 +18,10 @@ subject to the following restrictions: #include "LinearMath/btAlignedObjectArray.h" - #define USE_PATH_COMPRESSION 1 +#define USE_PATH_COMPRESSION 1 + +///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 { @@ -106,10 +109,12 @@ class btUnionFind //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically #ifdef USE_PATH_COMPRESSION - // - m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id; - #endif // + 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); |