diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision/CollisionDispatch')
57 files changed, 13976 insertions, 0 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp new file mode 100644 index 0000000000..c81af95672 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -0,0 +1,209 @@ +/* +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 "LinearMath/btScalar.h" +#include "SphereTriangleDetector.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + + +SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold) +:m_sphere(sphere), +m_triangle(triangle), +m_contactBreakingThreshold(contactBreakingThreshold) +{ + +} + +void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +{ + + (void)debugDraw; + const btTransform& transformA = input.m_transformA; + const btTransform& transformB = input.m_transformB; + + btVector3 point,normal; + btScalar timeOfImpact = btScalar(1.); + btScalar depth = btScalar(0.); +// output.m_distance = btScalar(BT_LARGE_FLOAT); + //move sphere into triangle space + btTransform sphereInTr = transformB.inverseTimes(transformA); + + if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold)) + { + if (swapResults) + { + btVector3 normalOnB = transformB.getBasis()*normal; + btVector3 normalOnA = -normalOnB; + btVector3 pointOnA = transformB*point+normalOnB*depth; + output.addContactPoint(normalOnA,pointOnA,depth); + } else + { + output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth); + } + } + +} + + + +// See also geometrictools.com +// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv +btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest); + +btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) { + btVector3 diff = p - from; + btVector3 v = to - from; + btScalar t = v.dot(diff); + + if (t > 0) { + btScalar dotVV = v.dot(v); + if (t < dotVV) { + t /= dotVV; + diff -= t*v; + } else { + t = 1; + diff -= v; + } + } else + t = 0; + + nearest = from + t*v; + return diff.dot(diff); +} + +bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) { + btVector3 lp(p); + btVector3 lnormal(normal); + + return pointInTriangle(vertices, lnormal, &lp); +} + +bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold) +{ + + const btVector3* vertices = &m_triangle->getVertexPtr(0); + + btScalar radius = m_sphere->getRadius(); + btScalar radiusWithThreshold = radius + contactBreakingThreshold; + + btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); + + btScalar l2 = normal.length2(); + bool hasContact = false; + btVector3 contactPoint; + + if (l2 >= SIMD_EPSILON*SIMD_EPSILON) + { + normal /= btSqrt(l2); + + btVector3 p1ToCentre = sphereCenter - vertices[0]; + btScalar distanceFromPlane = p1ToCentre.dot(normal); + + if (distanceFromPlane < btScalar(0.)) + { + //triangle facing the other way + distanceFromPlane *= btScalar(-1.); + normal *= btScalar(-1.); + } + + bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; + + // Check for contact / intersection + + if (isInsideContactPlane) { + if (facecontains(sphereCenter, vertices, normal)) { + // Inside the contact wedge - touches a point on the shell plane + hasContact = true; + contactPoint = sphereCenter - normal*distanceFromPlane; + } + else { + // Could be inside one of the contact capsules + btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold; + btVector3 nearestOnEdge; + for (int i = 0; i < m_triangle->getNumEdges(); i++) { + + btVector3 pa; + btVector3 pb; + + m_triangle->getEdge(i, pa, pb); + + btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge); + if (distanceSqr < contactCapsuleRadiusSqr) { + // Yep, we're inside a capsule + hasContact = true; + contactPoint = nearestOnEdge; + } + + } + } + } + } + + if (hasContact) { + btVector3 contactToCentre = sphereCenter - contactPoint; + btScalar distanceSqr = contactToCentre.length2(); + + if (distanceSqr < radiusWithThreshold*radiusWithThreshold) + { + if (distanceSqr>SIMD_EPSILON) + { + btScalar distance = btSqrt(distanceSqr); + resultNormal = contactToCentre; + resultNormal.normalize(); + point = contactPoint; + depth = -(radius-distance); + } else + { + resultNormal = normal; + point = contactPoint; + depth = -radius; + } + return true; + } + } + + return false; +} + + +bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ) +{ + const btVector3* p1 = &vertices[0]; + const btVector3* p2 = &vertices[1]; + const btVector3* p3 = &vertices[2]; + + btVector3 edge1( *p2 - *p1 ); + btVector3 edge2( *p3 - *p2 ); + btVector3 edge3( *p1 - *p3 ); + + btVector3 p1_to_p( *p - *p1 ); + btVector3 p2_to_p( *p - *p2 ); + btVector3 p3_to_p( *p - *p3 ); + + btVector3 edge1_normal( edge1.cross(normal)); + btVector3 edge2_normal( edge2.cross(normal)); + btVector3 edge3_normal( edge3.cross(normal)); + + btScalar r1, r2, r3; + r1 = edge1_normal.dot( p1_to_p ); + r2 = edge2_normal.dot( p2_to_p ); + r3 = edge3_normal.dot( p3_to_p ); + if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || + ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) + return true; + return false; + +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.h new file mode 100644 index 0000000000..22953af43f --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -0,0 +1,51 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SPHERE_TRIANGLE_DETECTOR_H +#define BT_SPHERE_TRIANGLE_DETECTOR_H + +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" + + + +class btSphereShape; +class btTriangleShape; + + + +/// sphere-triangle to match the btDiscreteCollisionDetectorInterface +struct SphereTriangleDetector : public btDiscreteCollisionDetectorInterface +{ + virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + + SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle, btScalar contactBreakingThreshold); + + virtual ~SphereTriangleDetector() {}; + + bool collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold); + +private: + + + bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ); + bool facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal); + + btSphereShape* m_sphere; + btTriangleShape* m_triangle; + btScalar m_contactBreakingThreshold; + +}; +#endif //BT_SPHERE_TRIANGLE_DETECTOR_H + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp new file mode 100644 index 0000000000..57f1464935 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp @@ -0,0 +1,47 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.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. +*/ + +#include "btActivatingCollisionAlgorithm.h" +#include "btCollisionDispatcher.h" +#include "btCollisionObject.h" + +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci) +:btCollisionAlgorithm(ci) +//, +//m_colObj0(0), +//m_colObj1(0) +{ +} +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* ) +:btCollisionAlgorithm(ci) +//, +//m_colObj0(0), +//m_colObj1(0) +{ +// if (ci.m_dispatcher1->needsCollision(colObj0,colObj1)) +// { +// m_colObj0 = colObj0; +// m_colObj1 = colObj1; +// +// m_colObj0->activate(); +// m_colObj1->activate(); +// } +} + +btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm() +{ +// m_colObj0->activate(); +// m_colObj1->activate(); +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h new file mode 100644 index 0000000000..0e19f1ea35 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h @@ -0,0 +1,37 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.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. +*/ + +#ifndef __BT_ACTIVATING_COLLISION_ALGORITHM_H +#define __BT_ACTIVATING_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" + +///This class is not enabled yet (work-in-progress) to more aggressively activate objects. +class btActivatingCollisionAlgorithm : public btCollisionAlgorithm +{ +// btCollisionObject* m_colObj0; +// btCollisionObject* m_colObj1; + +protected: + + btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci); + + btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + +public: + virtual ~btActivatingCollisionAlgorithm(); + +}; +#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp new file mode 100644 index 0000000000..2c36277821 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp @@ -0,0 +1,421 @@ +/* +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" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + +#define USE_PERSISTENT_CONTACTS 1 + +btBox2dBox2dCollisionAlgorithm::btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap) +: btActivatingCollisionAlgorithm(ci,obj0Wrap,obj1Wrap), +m_ownManifold(false), +m_manifoldPtr(mf) +{ + if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject())) + { + m_manifoldPtr = m_dispatcher->getNewManifold(obj0Wrap->getCollisionObject(),obj1Wrap->getCollisionObject()); + m_ownManifold = true; + } +} + +btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm() +{ + + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } + +} + + +void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB); + +//#include <stdio.h> +void btBox2dBox2dCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + if (!m_manifoldPtr) + return; + + + const btBox2dShape* box0 = (const btBox2dShape*)body0Wrap->getCollisionShape(); + const btBox2dShape* box1 = (const btBox2dShape*)body1Wrap->getCollisionShape(); + + resultOut->setPersistentManifold(m_manifoldPtr); + + b2CollidePolygons(resultOut,box0,body0Wrap->getWorldTransform(),box1,body1Wrap->getWorldTransform()); + + // 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; + + if( count2 > 0 ) + index = (int) normal1.minDot( vertices2, count2, minDot); + + 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; + if( count1 > 0 ) + edge = (int) dLocal1.maxDot( normals1, count1, maxDot); + + // Get the separation for the edge normal. + btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); + 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h new file mode 100644 index 0000000000..6ea6e89bda --- /dev/null +++ b/thirdparty/bullet/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 BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H +#define BT_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 (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btBox2dBox2dCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + + 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, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + int bbsize = sizeof(btBox2dBox2dCollisionAlgorithm); + void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); + return new(ptr) btBox2dBox2dCollisionAlgorithm(0,ci,body0Wrap,body1Wrap); + } + }; + +}; + +#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp new file mode 100644 index 0000000000..ac68968f59 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp @@ -0,0 +1,84 @@ +/* +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 "btBoxBoxCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "btBoxBoxDetector.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" +#define USE_PERSISTENT_CONTACTS 1 + +btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) +: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), +m_ownManifold(false), +m_manifoldPtr(mf) +{ + if (!m_manifoldPtr && m_dispatcher->needsCollision(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject())) + { + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_ownManifold = true; + } +} + +btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btBoxBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + if (!m_manifoldPtr) + return; + + + const btBoxShape* box0 = (btBoxShape*)body0Wrap->getCollisionShape(); + const btBoxShape* box1 = (btBoxShape*)body1Wrap->getCollisionShape(); + + + + /// report a contact. internally this will be kept persistent, and contact reduction is done + resultOut->setPersistentManifold(m_manifoldPtr); +#ifndef USE_PERSISTENT_CONTACTS + m_manifoldPtr->clearManifold(); +#endif //USE_PERSISTENT_CONTACTS + + btDiscreteCollisionDetectorInterface::ClosestPointInput input; + input.m_maximumDistanceSquared = BT_LARGE_FLOAT; + input.m_transformA = body0Wrap->getWorldTransform(); + input.m_transformB = body1Wrap->getWorldTransform(); + + btBoxBoxDetector detector(box0,box1); + detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + +#ifdef USE_PERSISTENT_CONTACTS + // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } +#endif //USE_PERSISTENT_CONTACTS + +} + +btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +{ + //not yet + return 1.f; +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h new file mode 100644 index 0000000000..59808df5a9 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.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 BT_BOX_BOX__COLLISION_ALGORITHM_H +#define BT_BOX_BOX__COLLISION_ALGORITHM_H + +#include "btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" + +class btPersistentManifold; + +///box-box collision detection +class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + +public: + btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btActivatingCollisionAlgorithm(ci) {} + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + + virtual ~btBoxBoxCollisionAlgorithm(); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + int bbsize = sizeof(btBoxBoxCollisionAlgorithm); + void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); + return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap); + } + }; + +}; + +#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp new file mode 100644 index 0000000000..7043bde34f --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -0,0 +1,718 @@ +/* + * 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. + * All rights reserved. Email: russ@q12.org Web: www.q12.org + Bullet Continuous Collision Detection and Physics Library + Bullet is 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. +*/ + +///ODE box-box collision detection is adapted to work with Bullet + +#include "btBoxBoxDetector.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" + +#include <float.h> +#include <string.h> + +btBoxBoxDetector::btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2) +: m_box1(box1), +m_box2(box2) +{ + +} + + +// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and +// generate contact points. this returns 0 if there is no contact otherwise +// it returns the number of contacts generated. +// `normal' returns the contact normal. +// `depth' returns the maximum penetration depth along that normal. +// `return_code' returns a number indicating the type of contact that was +// detected: +// 1,2,3 = box 2 intersects with a face of box 1 +// 4,5,6 = box 1 intersects with a face of box 2 +// 7..15 = edge-edge contact +// `maxc' is the maximum number of contacts allowed to be generated, i.e. +// the size of the `contact' array. +// `contact' and `skip' are the contact array information provided to the +// collision functions. this function only fills in the position and depth +// fields. +struct dContactGeom; +#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) +#define dInfinity FLT_MAX + + +/*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } +PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); } +PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); } +PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); } +*/ +static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } +static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); } +static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); } +static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); } +#define dMULTIPLYOP1_331(A,op,B,C) \ +{\ + (A)[0] op dDOT41((B),(C)); \ + (A)[1] op dDOT41((B+1),(C)); \ + (A)[2] op dDOT41((B+2),(C)); \ +} + +#define dMULTIPLYOP0_331(A,op,B,C) \ +{ \ + (A)[0] op dDOT((B),(C)); \ + (A)[1] op dDOT((B+4),(C)); \ + (A)[2] op dDOT((B+8),(C)); \ +} + +#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C) +#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) + +typedef btScalar dMatrix3[4*3]; + +void dLineClosestApproach (const btVector3& pa, const btVector3& ua, + const btVector3& pb, const btVector3& ub, + btScalar *alpha, btScalar *beta); +void dLineClosestApproach (const btVector3& pa, const btVector3& ua, + const btVector3& pb, const btVector3& ub, + btScalar *alpha, btScalar *beta) +{ + btVector3 p; + p[0] = pb[0] - pa[0]; + p[1] = pb[1] - pa[1]; + p[2] = pb[2] - pa[2]; + btScalar uaub = dDOT(ua,ub); + btScalar q1 = dDOT(ua,p); + btScalar q2 = -dDOT(ub,p); + btScalar d = 1-uaub*uaub; + if (d <= btScalar(0.0001f)) { + // @@@ this needs to be made more robust + *alpha = 0; + *beta = 0; + } + else { + d = 1.f/d; + *alpha = (q1 + uaub*q2)*d; + *beta = (uaub*q1 + q2)*d; + } +} + + + +// find all the intersection points between the 2D rectangle with vertices +// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), +// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). +// +// the intersection points are returned as x,y pairs in the 'ret' array. +// the number of intersection points is returned by the function (this will +// be in the range 0 to 8). + +static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16]) +{ + // q (and r) contain nq (and nr) coordinate points for the current (and + // chopped) polygons + int nq=4,nr=0; + btScalar buffer[16]; + btScalar *q = p; + btScalar *r = ret; + for (int dir=0; dir <= 1; dir++) { + // direction notation: xy[0] = x axis, xy[1] = y axis + for (int sign=-1; sign <= 1; sign += 2) { + // chop q along the line xy[dir] = sign*h[dir] + btScalar *pq = q; + btScalar *pr = r; + nr = 0; + for (int i=nq; i > 0; i--) { + // go through all points in q and all lines between adjacent points + if (sign*pq[dir] < h[dir]) { + // this point is inside the chopping line + pr[0] = pq[0]; + pr[1] = pq[1]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + btScalar *nextq = (i > 1) ? pq+2 : q; + if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) { + // this line crosses the chopping line + pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / + (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); + pr[dir] = sign*h[dir]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + pq += 2; + } + q = r; + r = (q==ret) ? buffer : ret; + nq = nr; + } + } + done: + if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar)); + return nr; +} + + +#define M__PI 3.14159265f + +// given n points in the plane (array p, of size 2*n), generate m points that +// best represent the whole set. the definition of 'best' here is not +// predetermined - the idea is to select points that give good box-box +// collision detection behavior. the chosen point indexes are returned in the +// array iret (of size m). 'i0' is always the first entry in the array. +// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be +// in the range [0..n-1]. + +void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]); +void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) +{ + // compute the centroid of the polygon in cx,cy + int i,j; + btScalar a,cx,cy,q; + if (n==1) { + cx = p[0]; + cy = p[1]; + } + else if (n==2) { + cx = btScalar(0.5)*(p[0] + p[2]); + cy = btScalar(0.5)*(p[1] + p[3]); + } + else { + a = 0; + cx = 0; + cy = 0; + for (i=0; i<(n-1); i++) { + q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; + a += q; + cx += q*(p[i*2]+p[i*2+2]); + cy += q*(p[i*2+1]+p[i*2+3]); + } + q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; + if (btFabs(a+q) > SIMD_EPSILON) + { + a = 1.f/(btScalar(3.0)*(a+q)); + } else + { + a=BT_LARGE_FLOAT; + } + cx = a*(cx + q*(p[n*2-2]+p[0])); + cy = a*(cy + q*(p[n*2-1]+p[1])); + } + + // compute the angle of each point w.r.t. the centroid + btScalar A[8]; + for (i=0; i<n; i++) A[i] = btAtan2(p[i*2+1]-cy,p[i*2]-cx); + + // search for points that have angles closest to A[i0] + i*(2*pi/m). + int avail[8]; + for (i=0; i<n; i++) avail[i] = 1; + avail[i0] = 0; + iret[0] = i0; + iret++; + for (j=1; j<m; j++) { + a = btScalar(j)*(2*M__PI/m) + A[i0]; + if (a > M__PI) a -= 2*M__PI; + btScalar maxdiff=1e9,diff; + + *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 + + for (i=0; i<n; i++) { + if (avail[i]) { + diff = btFabs (A[i]-a); + if (diff > M__PI) diff = 2*M__PI - diff; + if (diff < maxdiff) { + maxdiff = diff; + *iret = i; + } + } + } +#if defined(DEBUG) || defined (_DEBUG) + btAssert (*iret != i0); // ensure iret got set +#endif + avail[*iret] = 0; + iret++; + } +} + + + +int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, + const btVector3& side1, const btVector3& p2, + const dMatrix3 R2, const btVector3& side2, + btVector3& normal, btScalar *depth, int *return_code, + int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output); +int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, + const btVector3& side1, const btVector3& p2, + const dMatrix3 R2, const btVector3& side2, + btVector3& normal, btScalar *depth, int *return_code, + int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output) +{ + const btScalar fudge_factor = btScalar(1.05); + btVector3 p,pp,normalC(0.f,0.f,0.f); + const btScalar *normalR = 0; + btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, + Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l; + int i,j,invert_normal,code; + + // get vector from centers of box 1 to box 2, relative to box 1 + p = p2 - p1; + dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 + + // get side lengths / 2 + A[0] = side1[0]*btScalar(0.5); + A[1] = side1[1]*btScalar(0.5); + A[2] = side1[2]*btScalar(0.5); + B[0] = side2[0]*btScalar(0.5); + B[1] = side2[1]*btScalar(0.5); + B[2] = side2[2]*btScalar(0.5); + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 + R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); + R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); + R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); + + Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13); + Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23); + Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33); + + // for all 15 possible separating axes: + // * see if the axis separates the boxes. if so, return 0. + // * find the depth of the penetration along the separating axis (s2) + // * if this is the largest depth so far, record it. + // the normal vector will be set to the separating axis with the smallest + // depth. note: normalR is set to point to a column of R1 or R2 if that is + // the smallest depth normal so far. otherwise normalR is 0 and normalC is + // set to a vector relative to body 1. invert_normal is 1 if the sign of + // the normal should be flipped. + +#define TST(expr1,expr2,norm,cc) \ + s2 = btFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } + + s = -dInfinity; + invert_normal = 0; + code = 0; + + // separating axis = u1,u2,u3 + TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); + TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); + TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); + + // separating axis = v1,v2,v3 + TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); + TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); + TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); + + // note: cross product axes need to be scaled when s is computed. + // normal (n1,n2,n3) is relative to box 1. +#undef TST +#define TST(expr1,expr2,n1,n2,n3,cc) \ + s2 = btFabs(expr1) - (expr2); \ + if (s2 > SIMD_EPSILON) return 0; \ + l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ + if (l > SIMD_EPSILON) { \ + s2 /= l; \ + if (s2*fudge_factor > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } \ + } + + btScalar fudge2 (1.0e-5f); + + Q11 += fudge2; + Q12 += fudge2; + Q13 += fudge2; + + Q21 += fudge2; + Q22 += fudge2; + Q23 += fudge2; + + Q31 += fudge2; + Q32 += fudge2; + Q33 += fudge2; + + // separating axis = u1 x (v1,v2,v3) + TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); + TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); + TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); + + // separating axis = u2 x (v1,v2,v3) + TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); + TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); + TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); + + // separating axis = u3 x (v1,v2,v3) + TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); + TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); + TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); + +#undef TST + + if (!code) return 0; + + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + dMULTIPLY0_331 (normal,R1,normalC); + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + *depth = -s; + + // compute contact point(s) + + if (code > 6) { + // an edge from box 1 touches an edge from box 2. + // find a point pa on the intersecting edge of box 1 + btVector3 pa; + btScalar sign; + for (i=0; i<3; i++) pa[i] = p1[i]; + for (j=0; j<3; j++) { + sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0); + for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; + } + + // find a point pb on the intersecting edge of box 2 + btVector3 pb; + for (i=0; i<3; i++) pb[i] = p2[i]; + for (j=0; j<3; j++) { + sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0); + for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; + } + + btScalar alpha,beta; + btVector3 ua,ub; + for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; + for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; + + dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); + for (i=0; i<3; i++) pa[i] += ua[i]*alpha; + for (i=0; i<3; i++) pb[i] += ub[i]*beta; + + { + + //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]); + //contact[0].depth = *depth; + btVector3 pointInWorld; + +#ifdef USE_CENTER_POINT + for (i=0; i<3; i++) + pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5); + output.addContactPoint(-normal,pointInWorld,-*depth); +#else + output.addContactPoint(-normal,pb,-*depth); + +#endif // + *return_code = code; + } + return 1; + } + + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). define face 'a' to be the reference + // face (i.e. the normal vector is perpendicular to this) and face 'b' to be + // the incident face (the closest face of the other box). + + const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb; + if (code <= 3) { + Ra = R1; + Rb = R2; + pa = p1; + pb = p2; + Sa = A; + Sb = B; + } + else { + Ra = R2; + Rb = R1; + pa = p2; + pb = p1; + Sa = B; + Sb = A; + } + + // nr = normal vector of reference face dotted with axes of incident box. + // anr = absolute values of nr. + btVector3 normal2,nr,anr; + if (code <= 3) { + normal2[0] = normal[0]; + normal2[1] = normal[1]; + normal2[2] = normal[2]; + } + else { + normal2[0] = -normal[0]; + normal2[1] = -normal[1]; + normal2[2] = -normal[2]; + } + dMULTIPLY1_331 (nr,Rb,normal2); + anr[0] = btFabs (nr[0]); + anr[1] = btFabs (nr[1]); + anr[2] = btFabs (nr[2]); + + // find the largest compontent of anr: this corresponds to the normal + // for the indident face. the other axis numbers of the indicent face + // are stored in a1,a2. + int lanr,a1,a2; + if (anr[1] > anr[0]) { + if (anr[1] > anr[2]) { + a1 = 0; + lanr = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + else { + if (anr[0] > anr[2]) { + lanr = 0; + a1 = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + + // compute center point of incident face, in reference-face coordinates + btVector3 center; + if (nr[lanr] < 0) { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; + } + else { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; + } + + // find the normal and non-normal axis numbers of the reference box + int codeN,code1,code2; + if (code <= 3) codeN = code-1; else codeN = code-4; + if (codeN==0) { + code1 = 1; + code2 = 2; + } + else if (codeN==1) { + code1 = 0; + code2 = 2; + } + else { + code1 = 0; + code2 = 1; + } + + // find the four corners of the incident face, in reference-face coordinates + btScalar quad[8]; // 2D coordinate of incident face (x,y pairs) + btScalar c1,c2,m11,m12,m21,m22; + c1 = dDOT14 (center,Ra+code1); + c2 = dDOT14 (center,Ra+code2); + // optimize this? - we have already computed this data above, but it is not + // stored in an easy-to-index format. for now it's quicker just to recompute + // the four dot products. + m11 = dDOT44 (Ra+code1,Rb+a1); + m12 = dDOT44 (Ra+code1,Rb+a2); + m21 = dDOT44 (Ra+code2,Rb+a1); + m22 = dDOT44 (Ra+code2,Rb+a2); + { + btScalar k1 = m11*Sb[a1]; + btScalar k2 = m21*Sb[a1]; + btScalar k3 = m12*Sb[a2]; + btScalar k4 = m22*Sb[a2]; + quad[0] = c1 - k1 - k3; + quad[1] = c2 - k2 - k4; + quad[2] = c1 - k1 + k3; + quad[3] = c2 - k2 + k4; + quad[4] = c1 + k1 + k3; + quad[5] = c2 + k2 + k4; + quad[6] = c1 + k1 - k3; + quad[7] = c2 + k2 - k4; + } + + // find the size of the reference face + btScalar rect[2]; + rect[0] = Sa[code1]; + rect[1] = Sa[code2]; + + // intersect the incident and reference faces + btScalar ret[16]; + int n = intersectRectQuad2 (rect,quad,ret); + if (n < 1) return 0; // this should never happen + + // convert the intersection points into reference-face coordinates, + // and compute the contact position and depth for each point. only keep + // those points that have a positive (penetrating) depth. delete points in + // the 'ret' array as necessary so that 'point' and 'ret' correspond. + btScalar point[3*8]; // penetrating contact points + btScalar dep[8]; // depths for those points + btScalar det1 = 1.f/(m11*m22 - m12*m21); + m11 *= det1; + m12 *= det1; + m21 *= det1; + m22 *= det1; + int cnum = 0; // number of penetrating contact points found + for (j=0; j < n; j++) { + btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); + btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); + for (i=0; i<3; i++) point[cnum*3+i] = + center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; + dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); + if (dep[cnum] >= 0) { + ret[cnum*2] = ret[j*2]; + ret[cnum*2+1] = ret[j*2+1]; + cnum++; + } + } + if (cnum < 1) return 0; // this should never happen + + // we can't generate more contacts than we actually have + if (maxc > cnum) maxc = cnum; + if (maxc < 1) maxc = 1; + + if (cnum <= maxc) { + + if (code<4) + { + // we have less contacts than we need, so we use them all + for (j=0; j < cnum; j++) + { + 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. + // find the deepest point, it is always the first contact. + int i1 = 0; + btScalar maxdepth = dep[0]; + for (i=1; i<cnum; i++) { + if (dep[i] > maxdepth) { + maxdepth = dep[i]; + i1 = i; + } + } + + int iret[8]; + cullPoints2 (cnum,ret,maxc,i1,iret); + + for (j=0; j < maxc; j++) { +// dContactGeom *con = CONTACT(contact,skip*j); + // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; + // con->depth = dep[iret[j]]; + + btVector3 posInWorld; + for (i=0; i<3; i++) + posInWorld[i] = point[iret[j]*3+i] + pa[i]; + if (code<4) + { + output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); + } else + { + output.addContactPoint(-normal,posInWorld-normal*dep[iret[j]],-dep[iret[j]]); + } + } + cnum = maxc; + } + + *return_code = code; + return cnum; +} + +void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/) +{ + + const btTransform& transformA = input.m_transformA; + const btTransform& transformB = input.m_transformB; + + int skip = 0; + dContactGeom *contact = 0; + + dMatrix3 R1; + dMatrix3 R2; + + for (int j=0;j<3;j++) + { + R1[0+4*j] = transformA.getBasis()[j].x(); + R2[0+4*j] = transformB.getBasis()[j].x(); + + R1[1+4*j] = transformA.getBasis()[j].y(); + R2[1+4*j] = transformB.getBasis()[j].y(); + + + R1[2+4*j] = transformA.getBasis()[j].z(); + R2[2+4*j] = transformB.getBasis()[j].z(); + + } + + + + btVector3 normal; + btScalar depth; + int return_code; + int maxc = 4; + + + dBoxBox2 (transformA.getOrigin(), + R1, + 2.f*m_box1->getHalfExtentsWithMargin(), + transformB.getOrigin(), + R2, + 2.f*m_box2->getHalfExtentsWithMargin(), + normal, &depth, &return_code, + maxc, contact, skip, + output + ); + +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.h new file mode 100644 index 0000000000..3924377705 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.h @@ -0,0 +1,44 @@ +/* + * 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. + * All rights reserved. Email: russ@q12.org Web: www.q12.org + +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef BT_BOX_BOX_DETECTOR_H +#define BT_BOX_BOX_DETECTOR_H + + +class btBoxShape; +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" + + +/// btBoxBoxDetector wraps the ODE box-box collision detector +/// re-distributed under the Zlib license with permission from Russell L. Smith +struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface +{ + const btBoxShape* m_box1; + const btBoxShape* m_box2; + +public: + + btBoxBoxDetector(const btBoxShape* box1,const btBoxShape* box2); + + virtual ~btBoxBoxDetector() {}; + + virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + +}; + +#endif //BT_BOX_BOX_DETECTOR_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionConfiguration.h new file mode 100644 index 0000000000..35f77d4e65 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -0,0 +1,49 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_COLLISION_CONFIGURATION +#define BT_COLLISION_CONFIGURATION + +struct btCollisionAlgorithmCreateFunc; + +class btPoolAllocator; + +///btCollisionConfiguration allows to configure Bullet collision detection +///stack allocator size, default collision algorithms and persistent manifold pool size +///@todo: describe the meaning +class btCollisionConfiguration +{ + +public: + + virtual ~btCollisionConfiguration() + { + } + + ///memory pools + virtual btPoolAllocator* getPersistentManifoldPool() = 0; + + virtual btPoolAllocator* getCollisionAlgorithmPool() = 0; + + + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0; + + virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0; + + +}; + +#endif //BT_COLLISION_CONFIGURATION + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h new file mode 100644 index 0000000000..62ee66c4e9 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -0,0 +1,45 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_COLLISION_CREATE_FUNC +#define BT_COLLISION_CREATE_FUNC + +#include "LinearMath/btAlignedObjectArray.h" +class btCollisionAlgorithm; +class btCollisionObject; +struct btCollisionObjectWrapper; +struct btCollisionAlgorithmConstructionInfo; + +///Used by the btCollisionDispatcher to register and create instances for btCollisionAlgorithm +struct btCollisionAlgorithmCreateFunc +{ + bool m_swapped; + + btCollisionAlgorithmCreateFunc() + :m_swapped(false) + { + } + virtual ~btCollisionAlgorithmCreateFunc(){}; + + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& , const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + + (void)body0Wrap; + (void)body1Wrap; + return 0; + } +}; +#endif //BT_COLLISION_CREATE_FUNC + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp new file mode 100644 index 0000000000..5739a1ef01 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -0,0 +1,324 @@ +/* +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 "btCollisionDispatcher.h" +#include "LinearMath/btQuickprof.h" + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" + +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "LinearMath/btPoolAllocator.h" +#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + +int gNumManifold = 0; + +#ifdef BT_DEBUG +#include <stdio.h> +#endif + + +btCollisionDispatcher::btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration): +m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD), + m_collisionConfiguration(collisionConfiguration) +{ + int i; + + setNearCallback(defaultNearCallback); + + m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool(); + + m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool(); + + for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++) + { + for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++) + { + m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j); + btAssert(m_doubleDispatchContactPoints[i][j]); + m_doubleDispatchClosestPoints[i][j] = m_collisionConfiguration->getClosestPointsAlgorithmCreateFunc(i, j); + + } + } + + +} + + +void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) +{ + m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc; +} + +void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) +{ + m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc; +} + +btCollisionDispatcher::~btCollisionDispatcher() +{ +} + +btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1) +{ + gNumManifold++; + + //btAssert(gNumManifold < 65535); + + + + //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance) + + btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? + btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold) , body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold)) + : gContactBreakingThreshold ; + + btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold()); + + void* mem = m_persistentManifoldPoolAllocator->allocate( sizeof( btPersistentManifold ) ); + if (NULL == mem) + { + //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert. + if ((m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION)==0) + { + mem = btAlignedAlloc(sizeof(btPersistentManifold),16); + } else + { + btAssert(0); + //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration + return 0; + } + } + btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold); + manifold->m_index1a = m_manifoldsPtr.size(); + m_manifoldsPtr.push_back(manifold); + + return manifold; +} + +void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold) +{ + manifold->clearManifold(); +} + + +void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) +{ + + gNumManifold--; + + //printf("releaseManifold: gNumManifold %d\n",gNumManifold); + clearManifold(manifold); + + int findIndex = manifold->m_index1a; + btAssert(findIndex < m_manifoldsPtr.size()); + m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1); + m_manifoldsPtr[findIndex]->m_index1a = findIndex; + m_manifoldsPtr.pop_back(); + + manifold->~btPersistentManifold(); + if (m_persistentManifoldPoolAllocator->validPtr(manifold)) + { + m_persistentManifoldPoolAllocator->freeMemory(manifold); + } else + { + btAlignedFree(manifold); + } + +} + + + + +btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType) +{ + + btCollisionAlgorithmConstructionInfo ci; + + ci.m_dispatcher1 = this; + ci.m_manifold = sharedManifold; + btCollisionAlgorithm* algo = 0; + if (algoType == BT_CONTACT_POINT_ALGORITHMS) + { + algo = m_doubleDispatchContactPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap); + } + else + { + algo = m_doubleDispatchClosestPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap); + } + + return algo; +} + + + + +bool btCollisionDispatcher::needsResponse(const btCollisionObject* body0,const btCollisionObject* body1) +{ + //here you can do filtering + bool hasResponse = + (body0->hasContactResponse() && body1->hasContactResponse()); + //no response between two static/kinematic bodies: + hasResponse = hasResponse && + ((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject())); + return hasResponse; +} + +bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0,const btCollisionObject* body1) +{ + btAssert(body0); + btAssert(body1); + + bool needsCollision = true; + +#ifdef BT_DEBUG + if (!(m_dispatcherFlags & btCollisionDispatcher::CD_STATIC_STATIC_REPORTED)) + { + //broadphase filtering already deals with this + if (body0->isStaticOrKinematicObject() && body1->isStaticOrKinematicObject()) + { + m_dispatcherFlags |= btCollisionDispatcher::CD_STATIC_STATIC_REPORTED; + printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); + } + } +#endif //BT_DEBUG + + if ((!body0->isActive()) && (!body1->isActive())) + needsCollision = false; + else if ((!body0->checkCollideWith(body1)) || (!body1->checkCollideWith(body0))) + needsCollision = false; + + return needsCollision ; + +} + + + +///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc) +///this is useful for the collision dispatcher. +class btCollisionPairCallback : public btOverlapCallback +{ + const btDispatcherInfo& m_dispatchInfo; + btCollisionDispatcher* m_dispatcher; + +public: + + btCollisionPairCallback(const btDispatcherInfo& dispatchInfo,btCollisionDispatcher* dispatcher) + :m_dispatchInfo(dispatchInfo), + m_dispatcher(dispatcher) + { + } + + /*btCollisionPairCallback& operator=(btCollisionPairCallback& other) + { + m_dispatchInfo = other.m_dispatchInfo; + m_dispatcher = other.m_dispatcher; + return *this; + } + */ + + + virtual ~btCollisionPairCallback() {} + + + virtual bool processOverlap(btBroadphasePair& pair) + { + (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo); + return false; + } +}; + + + +void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) +{ + //m_blockedForChanges = true; + + btCollisionPairCallback collisionCallback(dispatchInfo,this); + + pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); + + //m_blockedForChanges = false; + +} + + + +//by default, Bullet will use this near callback +void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo) +{ + btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; + + if (dispatcher.needsCollision(colObj0,colObj1)) + { + btCollisionObjectWrapper obj0Wrap(0,colObj0->getCollisionShape(),colObj0,colObj0->getWorldTransform(),-1,-1); + btCollisionObjectWrapper obj1Wrap(0,colObj1->getCollisionShape(),colObj1,colObj1->getWorldTransform(),-1,-1); + + + //dispatcher will keep algorithms persistent in the collision pair + if (!collisionPair.m_algorithm) + { + collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap,0, BT_CONTACT_POINT_ALGORITHMS); + } + + if (collisionPair.m_algorithm) + { + btManifoldResult contactPointResult(&obj0Wrap,&obj1Wrap); + + if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) + { + //discrete collision detection query + + collisionPair.m_algorithm->processCollision(&obj0Wrap,&obj1Wrap,dispatchInfo,&contactPointResult); + } else + { + //continuous collision detection query, time of impact (toi) + btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0,colObj1,dispatchInfo,&contactPointResult); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; + + } + } + } + +} + + +void* btCollisionDispatcher::allocateCollisionAlgorithm(int size) +{ + void* mem = m_collisionAlgorithmPoolAllocator->allocate( size ); + if (NULL == mem) + { + //warn user for overflow? + return btAlignedAlloc(static_cast<size_t>(size), 16); + } + return mem; +} + +void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) +{ + if (m_collisionAlgorithmPoolAllocator->validPtr(ptr)) + { + m_collisionAlgorithmPoolAllocator->freeMemory(ptr); + } else + { + btAlignedFree(ptr); + } +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h new file mode 100644 index 0000000000..b97ee3c1ba --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -0,0 +1,175 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_COLLISION__DISPATCHER_H +#define BT_COLLISION__DISPATCHER_H + +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" + +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" + +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "LinearMath/btAlignedObjectArray.h" + +class btIDebugDraw; +class btOverlappingPairCache; +class btPoolAllocator; +class btCollisionConfiguration; + +#include "btCollisionCreateFunc.h" + +#define USE_DISPATCH_REGISTRY_ARRAY 1 + +class btCollisionDispatcher; +///user can override this nearcallback for collision filtering and more finegrained control over collision detection +typedef void (*btNearCallback)(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); + + +///btCollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs. +///Time of Impact, Closest Points and Penetration Depth. +class btCollisionDispatcher : public btDispatcher +{ + +protected: + + int m_dispatcherFlags; + + btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr; + + btManifoldResult m_defaultManifoldResult; + + btNearCallback m_nearCallback; + + btPoolAllocator* m_collisionAlgorithmPoolAllocator; + + btPoolAllocator* m_persistentManifoldPoolAllocator; + + btCollisionAlgorithmCreateFunc* m_doubleDispatchContactPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; + + btCollisionAlgorithmCreateFunc* m_doubleDispatchClosestPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; + + btCollisionConfiguration* m_collisionConfiguration; + + +public: + + enum DispatcherFlags + { + CD_STATIC_STATIC_REPORTED = 1, + CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2, + CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION = 4 + }; + + 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); + + void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc); + + int getNumManifolds() const + { + return int( m_manifoldsPtr.size()); + } + + btPersistentManifold** getInternalManifoldPointer() + { + return m_manifoldsPtr.size()? &m_manifoldsPtr[0] : 0; + } + + btPersistentManifold* getManifoldByIndexInternal(int index) + { + return m_manifoldsPtr[index]; + } + + const btPersistentManifold* getManifoldByIndexInternal(int index) const + { + return m_manifoldsPtr[index]; + } + + btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration); + + virtual ~btCollisionDispatcher(); + + virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1); + + virtual void releaseManifold(btPersistentManifold* manifold); + + + virtual void clearManifold(btPersistentManifold* manifold); + + btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType); + + virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1); + + virtual bool needsResponse(const btCollisionObject* body0,const btCollisionObject* body1); + + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ; + + void setNearCallback(btNearCallback nearCallback) + { + m_nearCallback = nearCallback; + } + + btNearCallback getNearCallback() const + { + return m_nearCallback; + } + + //by default, Bullet will use this near callback + static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo); + + virtual void* allocateCollisionAlgorithm(int size); + + virtual void freeCollisionAlgorithm(void* ptr); + + btCollisionConfiguration* getCollisionConfiguration() + { + return m_collisionConfiguration; + } + + const btCollisionConfiguration* getCollisionConfiguration() const + { + return m_collisionConfiguration; + } + + void setCollisionConfiguration(btCollisionConfiguration* config) + { + m_collisionConfiguration = config; + } + + virtual btPoolAllocator* getInternalManifoldPool() + { + return m_persistentManifoldPoolAllocator; + } + + virtual const btPoolAllocator* getInternalManifoldPool() const + { + return m_persistentManifoldPoolAllocator; + } + +}; + +#endif //BT_COLLISION__DISPATCHER_H + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp new file mode 100644 index 0000000000..075860c503 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp @@ -0,0 +1,164 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "btCollisionDispatcherMt.h" +#include "LinearMath/btQuickprof.h" + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" + +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "LinearMath/btPoolAllocator.h" +#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + + +btCollisionDispatcherMt::btCollisionDispatcherMt( btCollisionConfiguration* config, int grainSize ) + : btCollisionDispatcher( config ) +{ + m_batchUpdating = false; + m_grainSize = grainSize; // iterations per task +} + + +btPersistentManifold* btCollisionDispatcherMt::getNewManifold( const btCollisionObject* body0, const btCollisionObject* body1 ) +{ + //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance) + + btScalar contactBreakingThreshold = ( m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD ) ? + btMin( body0->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ), body1->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ) ) + : gContactBreakingThreshold; + + btScalar contactProcessingThreshold = btMin( body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold() ); + + void* mem = m_persistentManifoldPoolAllocator->allocate( sizeof( btPersistentManifold ) ); + if ( NULL == mem ) + { + //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert. + if ( ( m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION ) == 0 ) + { + mem = btAlignedAlloc( sizeof( btPersistentManifold ), 16 ); + } + else + { + btAssert( 0 ); + //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration + return 0; + } + } + btPersistentManifold* manifold = new( mem ) btPersistentManifold( body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold ); + if ( !m_batchUpdating ) + { + // batch updater will update manifold pointers array after finishing, so + // only need to update array when not batch-updating + //btAssert( !btThreadsAreRunning() ); + manifold->m_index1a = m_manifoldsPtr.size(); + m_manifoldsPtr.push_back( manifold ); + } + + return manifold; +} + +void btCollisionDispatcherMt::releaseManifold( btPersistentManifold* manifold ) +{ + clearManifold( manifold ); + //btAssert( !btThreadsAreRunning() ); + if ( !m_batchUpdating ) + { + // batch updater will update manifold pointers array after finishing, so + // only need to update array when not batch-updating + int findIndex = manifold->m_index1a; + btAssert( findIndex < m_manifoldsPtr.size() ); + m_manifoldsPtr.swap( findIndex, m_manifoldsPtr.size() - 1 ); + m_manifoldsPtr[ findIndex ]->m_index1a = findIndex; + m_manifoldsPtr.pop_back(); + } + + manifold->~btPersistentManifold(); + if ( m_persistentManifoldPoolAllocator->validPtr( manifold ) ) + { + m_persistentManifoldPoolAllocator->freeMemory( manifold ); + } + else + { + btAlignedFree( manifold ); + } +} + +struct CollisionDispatcherUpdater : public btIParallelForBody +{ + btBroadphasePair* mPairArray; + btNearCallback mCallback; + btCollisionDispatcher* mDispatcher; + const btDispatcherInfo* mInfo; + + CollisionDispatcherUpdater() + { + mPairArray = NULL; + mCallback = NULL; + mDispatcher = NULL; + mInfo = NULL; + } + void forLoop( int iBegin, int iEnd ) const + { + for ( int i = iBegin; i < iEnd; ++i ) + { + btBroadphasePair* pair = &mPairArray[ i ]; + mCallback( *pair, *mDispatcher, *mInfo ); + } + } +}; + + +void btCollisionDispatcherMt::dispatchAllCollisionPairs( btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher ) +{ + int pairCount = pairCache->getNumOverlappingPairs(); + if ( pairCount == 0 ) + { + return; + } + CollisionDispatcherUpdater updater; + updater.mCallback = getNearCallback(); + updater.mPairArray = pairCache->getOverlappingPairArrayPtr(); + updater.mDispatcher = this; + updater.mInfo = &info; + + m_batchUpdating = true; + btParallelFor( 0, pairCount, m_grainSize, updater ); + m_batchUpdating = false; + + // reconstruct the manifolds array to ensure determinism + m_manifoldsPtr.resizeNoInitialize( 0 ); + + btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr(); + for ( int i = 0; i < pairCount; ++i ) + { + if (btCollisionAlgorithm* algo = pairs[ i ].m_algorithm) + { + algo->getAllContactManifolds( m_manifoldsPtr ); + } + } + + // update the indices (used when releasing manifolds) + for ( int i = 0; i < m_manifoldsPtr.size(); ++i ) + { + m_manifoldsPtr[ i ]->m_index1a = i; + } +} + + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h new file mode 100644 index 0000000000..f1d7eafdc9 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h @@ -0,0 +1,39 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_COLLISION_DISPATCHER_MT_H +#define BT_COLLISION_DISPATCHER_MT_H + +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "LinearMath/btThreads.h" + + +class btCollisionDispatcherMt : public btCollisionDispatcher +{ +public: + btCollisionDispatcherMt( btCollisionConfiguration* config, int grainSize = 40 ); + + virtual btPersistentManifold* getNewManifold( const btCollisionObject* body0, const btCollisionObject* body1 ) BT_OVERRIDE; + virtual void releaseManifold( btPersistentManifold* manifold ) BT_OVERRIDE; + + virtual void dispatchAllCollisionPairs( btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher ) BT_OVERRIDE; + +protected: + bool m_batchUpdating; + int m_grainSize; +}; + +#endif //BT_COLLISION_DISPATCHER_MT_H + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp new file mode 100644 index 0000000000..b595c56bc5 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -0,0 +1,131 @@ +/* +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 "btCollisionObject.h" +#include "LinearMath/btSerializer.h" + +btCollisionObject::btCollisionObject() + : m_interpolationLinearVelocity(0.f, 0.f, 0.f), + m_interpolationAngularVelocity(0.f, 0.f, 0.f), + m_anisotropicFriction(1.f,1.f,1.f), + m_hasAnisotropicFriction(false), + m_contactProcessingThreshold(BT_LARGE_FLOAT), + m_broadphaseHandle(0), + m_collisionShape(0), + m_extensionPointer(0), + m_rootCollisionShape(0), + m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT), + m_islandTag1(-1), + m_companionId(-1), + m_worldArrayIndex(-1), + m_activationState1(1), + m_deactivationTime(btScalar(0.)), + m_friction(btScalar(0.5)), + m_restitution(btScalar(0.)), + m_rollingFriction(0.0f), + m_spinningFriction(0.f), + m_contactDamping(.1), + m_contactStiffness(1e4), + m_internalType(CO_COLLISION_OBJECT), + m_userObjectPointer(0), + m_userIndex2(-1), + m_userIndex(-1), + m_hitFraction(btScalar(1.)), + m_ccdSweptSphereRadius(btScalar(0.)), + m_ccdMotionThreshold(btScalar(0.)), + m_checkCollideWith(false), + m_updateRevision(0) +{ + m_worldTransform.setIdentity(); + m_interpolationWorldTransform.setIdentity(); +} + +btCollisionObject::~btCollisionObject() +{ +} + +void btCollisionObject::setActivationState(int newState) const +{ + if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION)) + m_activationState1 = newState; +} + +void btCollisionObject::forceActivationState(int newState) const +{ + m_activationState1 = newState; +} + +void btCollisionObject::activate(bool forceActivation) const +{ + if (forceActivation || !(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OBJECT))) + { + setActivationState(ACTIVE_TAG); + m_deactivationTime = btScalar(0.); + } +} + +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_deactivationTime = m_deactivationTime; + dataOut->m_friction = m_friction; + dataOut->m_rollingFriction = m_rollingFriction; + dataOut->m_contactDamping = m_contactDamping; + dataOut->m_contactStiffness = m_contactStiffness; + dataOut->m_restitution = m_restitution; + dataOut->m_internalType = m_internalType; + + char* name = (char*) serializer->findNameForPointer(this); + 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_checkCollideWith = m_checkCollideWith; + + // Fill padding with zeros to appease msan. + memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding)); + + 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h new file mode 100644 index 0000000000..fec831bffc --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -0,0 +1,679 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_COLLISION_OBJECT_H +#define BT_COLLISION_OBJECT_H + +#include "LinearMath/btTransform.h" + +//island management, m_activationState1 +#define ACTIVE_TAG 1 +#define ISLAND_SLEEPING 2 +#define WANTS_DEACTIVATION 3 +#define DISABLE_DEACTIVATION 4 +#define DISABLE_SIMULATION 5 + +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. +/// They can be added to the btCollisionWorld. +ATTRIBUTE_ALIGNED16(class) btCollisionObject +{ + +protected: + + btTransform m_worldTransform; + + ///m_interpolationWorldTransform is used for CCD and interpolation + ///it can be either previous or future (predicted) transform + btTransform m_interpolationWorldTransform; + //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) + //without destroying the continuous interpolated motion (which uses this interpolation velocities) + btVector3 m_interpolationLinearVelocity; + btVector3 m_interpolationAngularVelocity; + + btVector3 m_anisotropicFriction; + int m_hasAnisotropicFriction; + btScalar m_contactProcessingThreshold; + + btBroadphaseProxy* m_broadphaseHandle; + btCollisionShape* m_collisionShape; + ///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 + ///If it is NULL, the m_collisionShape is not temporarily replaced. + btCollisionShape* m_rootCollisionShape; + + int m_collisionFlags; + + int m_islandTag1; + int m_companionId; + int m_worldArrayIndex; // index of object in world's collisionObjects array + + mutable int m_activationState1; + mutable btScalar m_deactivationTime; + + btScalar m_friction; + btScalar m_restitution; + btScalar m_rollingFriction;//torsional friction orthogonal to contact normal (useful to stop spheres rolling forever) + btScalar m_spinningFriction; // torsional friction around the contact normal (useful for grasping) + btScalar m_contactDamping; + btScalar m_contactStiffness; + + + + ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc. + ///do not assign your own m_internalType unless you write a new dynamics object class. + int m_internalType; + + ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer + + void* m_userObjectPointer; + + int m_userIndex2; + + int m_userIndex; + + ///time of impact calculation + btScalar m_hitFraction; + + ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: + btScalar m_ccdSweptSphereRadius; + + /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold + btScalar m_ccdMotionThreshold; + + /// If some object should have elaborate collision filtering by sub-classes + int m_checkCollideWith; + + btAlignedObjectArray<const btCollisionObject*> m_objectsWithoutCollisionCheck; + + ///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation. + int m_updateRevision; + + btVector3 m_customDebugColorRGB; + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + enum CollisionFlags + { + CF_STATIC_OBJECT= 1, + CF_KINEMATIC_OBJECT= 2, + CF_NO_CONTACT_RESPONSE = 4, + CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) + CF_CHARACTER_OBJECT = 16, + CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing + CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing + CF_HAS_CONTACT_STIFFNESS_DAMPING = 128, + CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256, + CF_HAS_FRICTION_ANCHOR = 512, + CF_HAS_COLLISION_SOUND_TRIGGER = 1024 + }; + + enum CollisionObjectTypes + { + CO_COLLISION_OBJECT =1, + CO_RIGID_BODY=2, + ///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter + ///It is useful for collision sensors, explosion objects, character controller etc. + CO_GHOST_OBJECT=4, + CO_SOFT_BODY=8, + CO_HF_FLUID=16, + CO_USER_TYPE=32, + CO_FEATHERSTONE_LINK=64 + }; + + enum AnisotropicFrictionFlags + { + CF_ANISOTROPIC_FRICTION_DISABLED=0, + CF_ANISOTROPIC_FRICTION = 1, + CF_ANISOTROPIC_ROLLING_FRICTION = 2 + }; + + SIMD_FORCE_INLINE bool mergesSimulationIslands() const + { + ///static objects, kinematic and object without contact response don't merge islands + return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0); + } + + const btVector3& getAnisotropicFriction() const + { + return m_anisotropicFriction; + } + void setAnisotropicFriction(const btVector3& anisotropicFriction, int frictionMode = CF_ANISOTROPIC_FRICTION) + { + m_anisotropicFriction = anisotropicFriction; + bool isUnity = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f); + m_hasAnisotropicFriction = isUnity?frictionMode : 0; + } + bool hasAnisotropicFriction(int frictionMode = CF_ANISOTROPIC_FRICTION) const + { + return (m_hasAnisotropicFriction&frictionMode)!=0; + } + + ///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default. + ///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges + void setContactProcessingThreshold( btScalar contactProcessingThreshold) + { + m_contactProcessingThreshold = contactProcessingThreshold; + } + btScalar getContactProcessingThreshold() const + { + return m_contactProcessingThreshold; + } + + SIMD_FORCE_INLINE bool isStaticObject() const { + return (m_collisionFlags & CF_STATIC_OBJECT) != 0; + } + + SIMD_FORCE_INLINE bool isKinematicObject() const + { + return (m_collisionFlags & CF_KINEMATIC_OBJECT) != 0; + } + + SIMD_FORCE_INLINE bool isStaticOrKinematicObject() const + { + return (m_collisionFlags & (CF_KINEMATIC_OBJECT | CF_STATIC_OBJECT)) != 0 ; + } + + SIMD_FORCE_INLINE bool hasContactResponse() const { + return (m_collisionFlags & CF_NO_CONTACT_RESPONSE)==0; + } + + + btCollisionObject(); + + virtual ~btCollisionObject(); + + virtual void setCollisionShape(btCollisionShape* collisionShape) + { + m_updateRevision++; + m_collisionShape = collisionShape; + m_rootCollisionShape = collisionShape; + } + + SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const + { + return m_collisionShape; + } + + SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() + { + return m_collisionShape; + } + + void setIgnoreCollisionCheck(const btCollisionObject* co, bool ignoreCollisionCheck) + { + if (ignoreCollisionCheck) + { + //We don't check for duplicates. Is it ok to leave that up to the user of this API? + //int index = m_objectsWithoutCollisionCheck.findLinearSearch(co); + //if (index == m_objectsWithoutCollisionCheck.size()) + //{ + m_objectsWithoutCollisionCheck.push_back(co); + //} + } + else + { + m_objectsWithoutCollisionCheck.remove(co); + } + m_checkCollideWith = m_objectsWithoutCollisionCheck.size() > 0; + } + + virtual bool checkCollideWithOverride(const btCollisionObject* co) const + { + int index = m_objectsWithoutCollisionCheck.findLinearSearch(co); + if (index < m_objectsWithoutCollisionCheck.size()) + { + return false; + } + return true; + } + + + + + ///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) const; + + void setDeactivationTime(btScalar time) + { + m_deactivationTime = time; + } + btScalar getDeactivationTime() const + { + return m_deactivationTime; + } + + void forceActivationState(int newState) const; + + void activate(bool forceActivation = false) const; + + SIMD_FORCE_INLINE bool isActive() const + { + return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); + } + + void setRestitution(btScalar rest) + { + m_updateRevision++; + m_restitution = rest; + } + btScalar getRestitution() const + { + return m_restitution; + } + void setFriction(btScalar frict) + { + m_updateRevision++; + m_friction = frict; + } + btScalar getFriction() const + { + return m_friction; + } + + void setRollingFriction(btScalar frict) + { + m_updateRevision++; + m_rollingFriction = frict; + } + btScalar getRollingFriction() const + { + return m_rollingFriction; + } + void setSpinningFriction(btScalar frict) + { + m_updateRevision++; + m_spinningFriction = frict; + } + btScalar getSpinningFriction() const + { + return m_spinningFriction; + } + void setContactStiffnessAndDamping(btScalar stiffness, btScalar damping) + { + m_updateRevision++; + m_contactStiffness = stiffness; + m_contactDamping = damping; + + m_collisionFlags |=CF_HAS_CONTACT_STIFFNESS_DAMPING; + + //avoid divisions by zero... + if (m_contactStiffness< SIMD_EPSILON) + { + m_contactStiffness = SIMD_EPSILON; + } + } + + btScalar getContactStiffness() const + { + return m_contactStiffness; + } + + btScalar getContactDamping() const + { + return m_contactDamping; + } + + ///reserved for Bullet internal usage + int getInternalType() const + { + return m_internalType; + } + + btTransform& getWorldTransform() + { + return m_worldTransform; + } + + const btTransform& getWorldTransform() const + { + return m_worldTransform; + } + + void setWorldTransform(const btTransform& worldTrans) + { + m_updateRevision++; + m_worldTransform = worldTrans; + } + + + SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle() + { + return m_broadphaseHandle; + } + + SIMD_FORCE_INLINE const btBroadphaseProxy* getBroadphaseHandle() const + { + return m_broadphaseHandle; + } + + void setBroadphaseHandle(btBroadphaseProxy* handle) + { + m_broadphaseHandle = handle; + } + + + const btTransform& getInterpolationWorldTransform() const + { + return m_interpolationWorldTransform; + } + + btTransform& getInterpolationWorldTransform() + { + return m_interpolationWorldTransform; + } + + void setInterpolationWorldTransform(const btTransform& trans) + { + m_updateRevision++; + m_interpolationWorldTransform = trans; + } + + void setInterpolationLinearVelocity(const btVector3& linvel) + { + m_updateRevision++; + m_interpolationLinearVelocity = linvel; + } + + void setInterpolationAngularVelocity(const btVector3& angvel) + { + m_updateRevision++; + m_interpolationAngularVelocity = angvel; + } + + const btVector3& getInterpolationLinearVelocity() const + { + return m_interpolationLinearVelocity; + } + + const btVector3& getInterpolationAngularVelocity() const + { + return m_interpolationAngularVelocity; + } + + SIMD_FORCE_INLINE int getIslandTag() const + { + return m_islandTag1; + } + + void setIslandTag(int tag) + { + m_islandTag1 = tag; + } + + SIMD_FORCE_INLINE int getCompanionId() const + { + return m_companionId; + } + + void setCompanionId(int id) + { + m_companionId = id; + } + + SIMD_FORCE_INLINE int getWorldArrayIndex() const + { + return m_worldArrayIndex; + } + + // only should be called by CollisionWorld + void setWorldArrayIndex(int ix) + { + m_worldArrayIndex = ix; + } + + SIMD_FORCE_INLINE btScalar getHitFraction() const + { + return m_hitFraction; + } + + void setHitFraction(btScalar hitFraction) + { + m_hitFraction = hitFraction; + } + + + SIMD_FORCE_INLINE int getCollisionFlags() const + { + return m_collisionFlags; + } + + void setCollisionFlags(int flags) + { + m_collisionFlags = flags; + } + + ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: + btScalar getCcdSweptSphereRadius() const + { + return m_ccdSweptSphereRadius; + } + + ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: + void setCcdSweptSphereRadius(btScalar radius) + { + m_ccdSweptSphereRadius = radius; + } + + btScalar getCcdMotionThreshold() const + { + return m_ccdMotionThreshold; + } + + btScalar getCcdSquareMotionThreshold() const + { + return m_ccdMotionThreshold*m_ccdMotionThreshold; + } + + + + /// Don't do continuous collision detection if the motion (in one step) is less then m_ccdMotionThreshold + void setCcdMotionThreshold(btScalar ccdMotionThreshold) + { + m_ccdMotionThreshold = ccdMotionThreshold; + } + + ///users can point to their objects, userPointer is not used by Bullet + void* getUserPointer() const + { + return m_userObjectPointer; + } + + int getUserIndex() const + { + return m_userIndex; + } + + int getUserIndex2() const + { + return m_userIndex2; + } + + ///users can point to their objects, userPointer is not used by Bullet + void setUserPointer(void* userPointer) + { + m_userObjectPointer = userPointer; + } + + ///users can point to their objects, userPointer is not used by Bullet + void setUserIndex(int index) + { + m_userIndex = index; + } + + void setUserIndex2(int index) + { + m_userIndex2 = index; + } + + int getUpdateRevisionInternal() const + { + return m_updateRevision; + } + + void setCustomDebugColor(const btVector3& colorRGB) + { + m_customDebugColorRGB = colorRGB; + m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; + } + + void removeCustomDebugColor() + { + m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; + } + + bool getCustomDebugColor(btVector3& colorRGB) const + { + bool hasCustomColor = (0!=(m_collisionFlags&CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR)); + if (hasCustomColor) + { + colorRGB = m_customDebugColorRGB; + } + return hasCustomColor; + } + + inline bool checkCollideWith(const btCollisionObject* co) const + { + if (m_checkCollideWith) + return checkCollideWithOverride(co); + + 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_rollingFriction; + double m_contactDamping; + double m_contactStiffness; + double m_restitution; + double m_hitFraction; + double m_ccdSweptSphereRadius; + double m_ccdMotionThreshold; + + int m_hasAnisotropicFriction; + int m_collisionFlags; + int m_islandTag1; + 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_rollingFriction; + float m_contactDamping; + float m_contactStiffness; + float m_restitution; + float m_hitFraction; + float m_ccdSweptSphereRadius; + float m_ccdMotionThreshold; + + int m_hasAnisotropicFriction; + int m_collisionFlags; + int m_islandTag1; + int m_companionId; + int m_activationState1; + int m_internalType; + int m_checkCollideWith; + char m_padding[4]; +}; + + + +SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const +{ + return sizeof(btCollisionObjectData); +} + + + +#endif //BT_COLLISION_OBJECT_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h new file mode 100644 index 0000000000..952440b7de --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h @@ -0,0 +1,43 @@ +#ifndef BT_COLLISION_OBJECT_WRAPPER_H +#define BT_COLLISION_OBJECT_WRAPPER_H + +///btCollisionObjectWrapperis an internal data structure. +///Most users can ignore this and use btCollisionObject and btCollisionShape instead +class btCollisionShape; +class btCollisionObject; +class btTransform; +#include "LinearMath/btScalar.h" // for SIMD_FORCE_INLINE definition + +#define BT_DECLARE_STACK_ONLY_OBJECT \ + private: \ + void* operator new(size_t size); \ + void operator delete(void*); + +struct btCollisionObjectWrapper; +struct btCollisionObjectWrapper +{ +BT_DECLARE_STACK_ONLY_OBJECT + +private: + btCollisionObjectWrapper(const btCollisionObjectWrapper&); // not implemented. Not allowed. + btCollisionObjectWrapper* operator=(const btCollisionObjectWrapper&); + +public: + const btCollisionObjectWrapper* m_parent; + const btCollisionShape* m_shape; + const btCollisionObject* m_collisionObject; + const btTransform& m_worldTransform; + int m_partId; + int m_index; + + btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, int partId, int index) + : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), + m_partId(partId), m_index(index) + {} + + SIMD_FORCE_INLINE const btTransform& getWorldTransform() const { return m_worldTransform; } + SIMD_FORCE_INLINE const btCollisionObject* getCollisionObject() const { return m_collisionObject; } + SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_shape; } +}; + +#endif //BT_COLLISION_OBJECT_WRAPPER_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp new file mode 100644 index 0000000000..c3e912fdca --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -0,0 +1,1666 @@ +/* +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 "btCollisionWorld.h" +#include "btCollisionDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#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/btSerializer.h" +#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.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 +//#define RECALCULATE_AABB_RAYCAST 1 + +//When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor) +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#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_forceUpdateAllAabbs(true) +{ +} + + +btCollisionWorld::~btCollisionWorld() +{ + + //clean up remaining objects + int i; + for (i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* collisionObject= m_collisionObjects[i]; + + btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); + if (bp) + { + // + // only clear the cached algorithms + // + getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); + getBroadphase()->destroyProxy(bp,m_dispatcher1); + collisionObject->setBroadphaseHandle(0); + } + } + + +} + + + + + + + + + + +void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) +{ + + btAssert(collisionObject); + + //check that the object isn't already added + btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); + btAssert(collisionObject->getWorldArrayIndex() == -1); // do not add the same object to more than one collision world + + collisionObject->setWorldArrayIndex(m_collisionObjects.size()); + m_collisionObjects.push_back(collisionObject); + + //calculate new AABB + btTransform trans = collisionObject->getWorldTransform(); + + btVector3 minAabb; + btVector3 maxAabb; + collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb); + + int type = collisionObject->getCollisionShape()->getShapeType(); + collisionObject->setBroadphaseHandle( getBroadphase()->createProxy( + minAabb, + maxAabb, + type, + collisionObject, + collisionFilterGroup, + collisionFilterMask, + m_dispatcher1)) ; + + + + + +} + + + +void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) +{ + btVector3 minAabb,maxAabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + //need to increase the aabb for contact thresholds + btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + minAabb -= contactThreshold; + maxAabb += contactThreshold; + + if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) + { + 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 + if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) + { + bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); + } else + { + //something went wrong, investigate + //this assert is unwanted in 3D modelers (danger of loosing work) + colObj->setActivationState(DISABLE_SIMULATION); + + static bool reportMe = true; + if (reportMe && m_debugDrawer) + { + reportMe = false; + m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); + m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); + m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); + m_debugDrawer->reportErrorWarning("Thanks.\n"); + } + } +} + +void btCollisionWorld::updateAabbs() +{ + BT_PROFILE("updateAabbs"); + + btTransform predictedTrans; + for ( int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + btAssert(colObj->getWorldArrayIndex() == i); + + //only update aabb of active objects + if (m_forceUpdateAllAabbs || colObj->isActive()) + { + updateSingleAabb(colObj); + } + } +} + + +void btCollisionWorld::computeOverlappingPairs() +{ + BT_PROFILE("calculateOverlappingPairs"); + m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); +} + +void btCollisionWorld::performDiscreteCollisionDetection() +{ + BT_PROFILE("performDiscreteCollisionDetection"); + + btDispatcherInfo& dispatchInfo = getDispatchInfo(); + + updateAabbs(); + + computeOverlappingPairs(); + + btDispatcher* dispatcher = getDispatcher(); + { + BT_PROFILE("dispatchAllCollisionPairs"); + if (dispatcher) + dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); + } + +} + + + +void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) +{ + + + //bool removeFromBroadphase = false; + + { + + btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); + if (bp) + { + // + // only clear the cached algorithms + // + getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); + getBroadphase()->destroyProxy(bp,m_dispatcher1); + collisionObject->setBroadphaseHandle(0); + } + } + + + int iObj = collisionObject->getWorldArrayIndex(); +// btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously? + if (iObj >= 0 && iObj < m_collisionObjects.size()) + { + btAssert(collisionObject == m_collisionObjects[iObj]); + m_collisionObjects.swap(iObj, m_collisionObjects.size()-1); + m_collisionObjects.pop_back(); + if (iObj < m_collisionObjects.size()) + { + m_collisionObjects[iObj]->setWorldArrayIndex(iObj); + } + } + else + { + // slow linear search + //swapremove + m_collisionObjects.remove(collisionObject); + } + collisionObject->setWorldArrayIndex(-1); +} + + +void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) +{ + btCollisionObjectWrapper colObWrap(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1); + btCollisionWorld::rayTestSingleInternal(rayFromTrans,rayToTrans,&colObWrap,resultCallback); +} + +void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans, + const btCollisionObjectWrapper* collisionObjectWrap, + RayResultCallback& resultCallback) +{ + btSphereShape pointShape(btScalar(0.0)); + pointShape.setMargin(0.f); + const btConvexShape* castShape = &pointShape; + const btCollisionShape* collisionShape = collisionObjectWrap->getCollisionShape(); + const btTransform& colObjWorldTransform = collisionObjectWrap->getWorldTransform(); + + if (collisionShape->isConvex()) + { + // BT_PROFILE("rayTestConvex"); + btConvexCast::CastResult castResult; + castResult.m_fraction = resultCallback.m_closestHitFraction; + + btConvexShape* convexShape = (btConvexShape*) collisionShape; + btVoronoiSimplexSolver simplexSolver; + btSubsimplexConvexCast subSimplexConvexCaster(castShape,convexShape,&simplexSolver); + + btGjkConvexCast gjkConvexCaster(castShape,convexShape,&simplexSolver); + + //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); + + btConvexCast* convexCasterPtr = 0; + //use kF_UseSubSimplexConvexCastRaytest by default + if (resultCallback.m_flags & btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest) + convexCasterPtr = &gjkConvexCaster; + else + convexCasterPtr = &subSimplexConvexCaster; + + btConvexCast& convexCaster = *convexCasterPtr; + + if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + { + //add hit + if (castResult.m_normal.length2() > btScalar(0.0001)) + { + if (castResult.m_fraction < resultCallback.m_closestHitFraction) + { + //todo: figure out what this is about. When is rayFromTest.getBasis() not identity? +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + //rotate normal into worldspace + castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal; +#endif //USE_SUBSIMPLEX_CONVEX_CAST + + castResult.m_normal.normalize(); + btCollisionWorld::LocalRayResult localRayResult + ( + collisionObjectWrap->getCollisionObject(), + 0, + castResult.m_normal, + castResult.m_fraction + ); + + bool normalInWorldSpace = true; + resultCallback.addSingleResult(localRayResult, normalInWorldSpace); + + } + } + } + } else { + if (collisionShape->isConcave()) + { + + //ConvexCast::CastResult + struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + { + btCollisionWorld::RayResultCallback* m_resultCallback; + const btCollisionObject* m_collisionObject; + const btConcaveShape* m_triangleMesh; + + btTransform m_colObjWorldTransform; + + BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,const btConcaveShape* triangleMesh,const btTransform& colObjWorldTransform): + //@BP Mod + btTriangleRaycastCallback(from,to, resultCallback->m_flags), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh), + m_colObjWorldTransform(colObjWorldTransform) + { + } + + + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + + btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; + + btCollisionWorld::LocalRayResult rayResult + (m_collisionObject, + &shapeInfo, + hitNormalWorld, + hitFraction); + + bool normalInWorldSpace = true; + return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); + } + + }; + + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + // BT_PROFILE("rayTestConcave"); + if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + ///optimized version for btBvhTriangleMeshShape + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),triangleMesh,colObjWorldTransform); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); + } + else if (collisionShape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + ///optimized version for btScaledBvhTriangleMeshShape + btScaledBvhTriangleMeshShape* scaledTriangleMesh = (btScaledBvhTriangleMeshShape*)collisionShape; + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)scaledTriangleMesh->getChildShape(); + + //scale the ray positions + btVector3 scale = scaledTriangleMesh->getLocalScaling(); + btVector3 rayFromLocalScaled = rayFromLocal / scale; + btVector3 rayToLocalScaled = rayToLocal / scale; + + //perform raycast in the underlying btBvhTriangleMeshShape + BridgeTriangleRaycastCallback rcb(rayFromLocalScaled, rayToLocalScaled, &resultCallback, collisionObjectWrap->getCollisionObject(), triangleMesh, colObjWorldTransform); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + triangleMesh->performRaycast(&rcb, rayFromLocalScaled, rayToLocalScaled); + } + else + { + //generic (slower) case + btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; + + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + //ConvexCast::CastResult + + struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + { + btCollisionWorld::RayResultCallback* m_resultCallback; + const btCollisionObject* m_collisionObject; + btConcaveShape* m_triangleMesh; + + btTransform m_colObjWorldTransform; + + BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform): + //@BP Mod + btTriangleRaycastCallback(from,to, resultCallback->m_flags), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh), + m_colObjWorldTransform(colObjWorldTransform) + { + } + + + virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + + btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; + + btCollisionWorld::LocalRayResult rayResult + (m_collisionObject, + &shapeInfo, + hitNormalWorld, + hitFraction); + + bool normalInWorldSpace = true; + return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); + } + + }; + + + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObjectWrap->getCollisionObject(),concaveShape, colObjWorldTransform); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + + btVector3 rayAabbMinLocal = rayFromLocal; + rayAabbMinLocal.setMin(rayToLocal); + btVector3 rayAabbMaxLocal = rayFromLocal; + rayAabbMaxLocal.setMax(rayToLocal); + + concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); + } + } else { + // 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; + m_flags = m_userCallback->m_flags; + } + 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 + { + const btCollisionObject* m_collisionObject; + const btCompoundShape* m_compoundShape; + const btTransform& m_colObjWorldTransform; + const btTransform& m_rayFromTrans; + const btTransform& m_rayToTrans; + RayResultCallback& m_resultCallback; + + RayTester(const btCollisionObject* collisionObject, + const btCompoundShape* compoundShape, + const btTransform& colObjWorldTransform, + const btTransform& rayFromTrans, + const btTransform& rayToTrans, + RayResultCallback& resultCallback): + m_collisionObject(collisionObject), + m_compoundShape(compoundShape), + m_colObjWorldTransform(colObjWorldTransform), + m_rayFromTrans(rayFromTrans), + m_rayToTrans(rayToTrans), + m_resultCallback(resultCallback) + { + + } + + void ProcessLeaf(int i) + { + const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i); + const btTransform& childTrans = m_compoundShape->getChildTransform(i); + btTransform childWorldTrans = m_colObjWorldTransform * childTrans; + + btCollisionObjectWrapper tmpOb(0,childCollisionShape,m_collisionObject,childWorldTrans,-1,i); + // replace collision shape so that callback can determine the triangle + + + + LocalInfoAdder2 my_cb(i, &m_resultCallback); + + rayTestSingleInternal( + m_rayFromTrans, + m_rayToTrans, + &tmpOb, + my_cb); + + } + + void Process(const btDbvtNode* leaf) + { + ProcessLeaf(leaf->dataAsInt); + } + }; + + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); + const btDbvt* dbvt = compoundShape->getDynamicAabbTree(); + + + RayTester rayCB( + collisionObjectWrap->getCollisionObject(), + compoundShape, + colObjWorldTransform, + rayFromTrans, + rayToTrans, + resultCallback); +#ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION + if (dbvt) + { + btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin(); + btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin(); + btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB); + } + else +#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION + { + for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i) + { + rayCB.ProcessLeaf(i); + } + } + } + } + } +} + +void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback, btScalar allowedPenetration) +{ + btCollisionObjectWrapper tmpOb(0,collisionShape,collisionObject,colObjWorldTransform,-1,-1); + btCollisionWorld::objectQuerySingleInternal(castShape,convexFromTrans,convexToTrans,&tmpOb,resultCallback,allowedPenetration); +} + +void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, + const btCollisionObjectWrapper* colObjWrap, + ConvexResultCallback& resultCallback, btScalar allowedPenetration) +{ + const btCollisionShape* collisionShape = colObjWrap->getCollisionShape(); + const btTransform& colObjWorldTransform = colObjWrap->getWorldTransform(); + + if (collisionShape->isConvex()) + { + //BT_PROFILE("convexSweepConvex"); + btConvexCast::CastResult castResult; + castResult.m_allowedPenetration = allowedPenetration; + castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//?? + + 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 + if (castResult.m_normal.length2() > btScalar(0.0001)) + { + if (castResult.m_fraction < resultCallback.m_closestHitFraction) + { + castResult.m_normal.normalize(); + btCollisionWorld::LocalConvexResult localConvexResult + ( + colObjWrap->getCollisionObject(), + 0, + castResult.m_normal, + castResult.m_hitPoint, + castResult.m_fraction + ); + + bool normalInWorldSpace = true; + resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); + + } + } + } + } else { + if (collisionShape->isConcave()) + { + if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + //BT_PROFILE("convexSweepbtBvhTriangleMesh"); + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); + btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); + // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation + btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); + + //ConvexCast::CastResult + struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback + { + btCollisionWorld::ConvexResultCallback* m_resultCallback; + const btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; + + BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, + btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld): + btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } + + + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + if (hitFraction <= m_resultCallback->m_closestHitFraction) + { + + btCollisionWorld::LocalConvexResult convexResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); + + bool normalInWorldSpace = true; + + + return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); + } + return hitFraction; + } + + }; + + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),triangleMesh, colObjWorldTransform); + tccb.m_hitFraction = resultCallback.m_closestHitFraction; + tccb.m_allowedPenetration = allowedPenetration; + btVector3 boxMinLocal, boxMaxLocal; + castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); + triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal); + } else + { + if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE) + { + btConvexCast::CastResult castResult; + castResult.m_allowedPenetration = allowedPenetration; + castResult.m_fraction = resultCallback.m_closestHitFraction; + btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape; + btContinuousConvexCollision convexCaster1(castShape,planeShape); + btConvexCast* castPtr = &convexCaster1; + + if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + { + //add hit + if (castResult.m_normal.length2() > btScalar(0.0001)) + { + if (castResult.m_fraction < resultCallback.m_closestHitFraction) + { + castResult.m_normal.normalize(); + btCollisionWorld::LocalConvexResult localConvexResult + ( + colObjWrap->getCollisionObject(), + 0, + castResult.m_normal, + castResult.m_hitPoint, + castResult.m_fraction + ); + + bool normalInWorldSpace = true; + resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); + } + } + } + + } else + { + //BT_PROFILE("convexSweepConcave"); + btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); + btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); + // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation + btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); + + //ConvexCast::CastResult + struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback + { + btCollisionWorld::ConvexResultCallback* m_resultCallback; + const btCollisionObject* m_collisionObject; + btConcaveShape* m_triangleMesh; + + BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, + btCollisionWorld::ConvexResultCallback* resultCallback, const btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): + btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } + + + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + if (hitFraction <= m_resultCallback->m_closestHitFraction) + { + + btCollisionWorld::LocalConvexResult convexResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); + + bool normalInWorldSpace = true; + + return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); + } + return hitFraction; + } + + }; + + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,colObjWrap->getCollisionObject(),concaveShape, colObjWorldTransform); + tccb.m_hitFraction = resultCallback.m_closestHitFraction; + tccb.m_allowedPenetration = allowedPenetration; + btVector3 boxMinLocal, boxMaxLocal; + castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); + + btVector3 rayAabbMinLocal = convexFromLocal; + rayAabbMinLocal.setMin(convexToLocal); + btVector3 rayAabbMaxLocal = convexFromLocal; + rayAabbMaxLocal.setMax(convexToLocal); + rayAabbMinLocal += boxMinLocal; + rayAabbMaxLocal += boxMaxLocal; + concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); + } + } + } else { + if (collisionShape->isCompound()) + { + struct btCompoundLeafCallback : btDbvt::ICollide + { + btCompoundLeafCallback( + const btCollisionObjectWrapper* colObjWrap, + const btConvexShape* castShape, + const btTransform& convexFromTrans, + const btTransform& convexToTrans, + btScalar allowedPenetration, + const btCompoundShape* compoundShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback) + : + m_colObjWrap(colObjWrap), + m_castShape(castShape), + m_convexFromTrans(convexFromTrans), + m_convexToTrans(convexToTrans), + m_allowedPenetration(allowedPenetration), + m_compoundShape(compoundShape), + m_colObjWorldTransform(colObjWorldTransform), + m_resultCallback(resultCallback) { + } + + const btCollisionObjectWrapper* m_colObjWrap; + const btConvexShape* m_castShape; + const btTransform& m_convexFromTrans; + const btTransform& m_convexToTrans; + btScalar m_allowedPenetration; + const btCompoundShape* m_compoundShape; + const btTransform& m_colObjWorldTransform; + ConvexResultCallback& m_resultCallback; + + public: + + void ProcessChild(int index, const btTransform& childTrans, const btCollisionShape* childCollisionShape) + { + btTransform childWorldTrans = m_colObjWorldTransform * childTrans; + + struct LocalInfoAdder : public ConvexResultCallback { + ConvexResultCallback* m_userCallback; + int m_i; + + LocalInfoAdder(int i, ConvexResultCallback *user) + : 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(index, &m_resultCallback); + + btCollisionObjectWrapper tmpObj(m_colObjWrap, childCollisionShape, m_colObjWrap->getCollisionObject(), childWorldTrans, -1, index); + + objectQuerySingleInternal(m_castShape, m_convexFromTrans, m_convexToTrans, &tmpObj, my_cb, m_allowedPenetration); + } + + void Process(const btDbvtNode* leaf) + { + // Processing leaf node + int index = leaf->dataAsInt; + + btTransform childTrans = m_compoundShape->getChildTransform(index); + const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(index); + + ProcessChild(index, childTrans, childCollisionShape); + } + }; + + BT_PROFILE("convexSweepCompound"); + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); + + btVector3 fromLocalAabbMin, fromLocalAabbMax; + btVector3 toLocalAabbMin, toLocalAabbMax; + + castShape->getAabb(colObjWorldTransform.inverse() * convexFromTrans, fromLocalAabbMin, fromLocalAabbMax); + castShape->getAabb(colObjWorldTransform.inverse() * convexToTrans, toLocalAabbMin, toLocalAabbMax); + + fromLocalAabbMin.setMin(toLocalAabbMin); + fromLocalAabbMax.setMax(toLocalAabbMax); + + btCompoundLeafCallback callback(colObjWrap, castShape, convexFromTrans, convexToTrans, + allowedPenetration, compoundShape, colObjWorldTransform, resultCallback); + + const btDbvt* tree = compoundShape->getDynamicAabbTree(); + if (tree) { + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(fromLocalAabbMin, fromLocalAabbMax); + tree->collideTV(tree->m_root, bounds, callback); + } else { + int i; + for (i=0;i<compoundShape->getNumChildShapes();i++) + { + const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); + btTransform childTrans = compoundShape->getChildTransform(i); + callback.ProcessChild(i, childTrans, childCollisionShape); + } + } + } + } + } +} + + +struct btSingleRayCallback : public btBroadphaseRayCallback +{ + + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; + + const btCollisionWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; + + btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback) + :m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_world(world), + m_resultCallback(resultCallback) + { + m_rayFromTrans.setIdentity(); + m_rayFromTrans.setOrigin(m_rayFromWorld); + m_rayToTrans.setIdentity(); + m_rayToTrans.setOrigin(m_rayToWorld); + + btVector3 rayDir = (rayToWorld-rayFromWorld); + + rayDir.normalize (); + ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; + 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; + + m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld); + + } + + + + virtual bool process(const btBroadphaseProxy* proxy) + { + ///terminate further ray tests, once the closestHitFraction reached zero + if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) + return false; + + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + + //only perform raycast if filterMask matches + if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; +#if 0 +#ifdef RECALCULATE_AABB + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); +#else + //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); + const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; + const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; +#endif +#endif + //btScalar hitLambda = m_resultCallback.m_closestHitFraction; + //culling already done by broadphase + //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) + { + m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback); + } + } + return true; + } +}; + +void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const +{ + //BT_PROFILE("rayTest"); + /// use the broadphase to accelerate the search for objects, based on their aabb + /// and for each object with ray-aabb overlap, perform an exact ray test + btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); + +#ifndef USE_BRUTEFORCE_RAYBROADPHASE + m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); +#else + for (int i=0;i<this->getNumCollisionObjects();i++) + { + rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE + +} + + +struct btSingleSweepCallback : public btBroadphaseRayCallback +{ + + btTransform m_convexFromTrans; + btTransform m_convexToTrans; + btVector3 m_hitNormal; + const btCollisionWorld* m_world; + btCollisionWorld::ConvexResultCallback& m_resultCallback; + btScalar m_allowedCcdPenetration; + const btConvexShape* m_castShape; + + + btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration) + :m_convexFromTrans(convexFromTrans), + m_convexToTrans(convexToTrans), + m_world(world), + m_resultCallback(resultCallback), + m_allowedCcdPenetration(allowedPenetration), + m_castShape(castShape) + { + btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin()); + btVector3 rayDir = unnormalizedRayDir.normalized(); + ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; + 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; + + m_lambda_max = rayDir.dot(unnormalizedRayDir); + + } + + virtual bool process(const btBroadphaseProxy* proxy) + { + ///terminate further convex sweep tests, once the closestHitFraction reached zero + if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) + return false; + + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + + //only perform raycast if filterMask matches + 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); + } + + return true; + } +}; + + + +void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const +{ + + BT_PROFILE("convexSweepTest"); + /// use the broadphase to accelerate the search for objects, based on their aabb + /// and for each object with ray-aabb overlap, perform an exact ray test + /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical + + + + btTransform convexFromTrans,convexToTrans; + convexFromTrans = convexFromWorld; + convexToTrans = convexToWorld; + btVector3 castShapeAabbMin, castShapeAabbMax; + /* Compute AABB that encompasses angular movement */ + { + btVector3 linVel, angVel; + btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0f, linVel, angVel); + btVector3 zeroLinVel; + zeroLinVel.setValue(0,0,0); + btTransform R; + R.setIdentity (); + R.setRotation (convexFromTrans.getRotation()); + castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0f, castShapeAabbMin, castShapeAabbMax); + } + +#ifndef USE_BRUTEFORCE_RAYBROADPHASE + + btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration); + + m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax); + +#else + /// go over all objects, and if the ray intersects their aabb + cast shape aabb, + // do a ray-shape query using convexCaster (CCD) + int i; + for (i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* collisionObject= m_collisionObjects[i]; + //only perform raycast if filterMask matches + if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); + AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 hitNormal; + if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + { + objectQuerySingle(castShape, convexFromTrans,convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + allowedCcdPenetration); + } + } + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE +} + + + +struct btBridgedManifoldResult : public btManifoldResult +{ + + btCollisionWorld::ContactResultCallback& m_resultCallback; + + btBridgedManifoldResult( const btCollisionObjectWrapper* obj0Wrap,const btCollisionObjectWrapper* obj1Wrap,btCollisionWorld::ContactResultCallback& resultCallback ) + :btManifoldResult(obj0Wrap,obj1Wrap), + m_resultCallback(resultCallback) + { + } + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + { + bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + btVector3 pointA = pointInWorld + normalOnBInWorld * depth; + btVector3 localA; + btVector3 localB; + if (isSwapped) + { + localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } else + { + localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localB = m_body1Wrap->getCollisionObject()->getWorldTransform().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. + const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap; + m_resultCallback.addSingleResult(newPt,obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1); + + } + +}; + + + +struct btSingleContactCallback : public btBroadphaseAabbCallback +{ + + btCollisionObject* m_collisionObject; + btCollisionWorld* m_world; + btCollisionWorld::ContactResultCallback& m_resultCallback; + + + btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback) + :m_collisionObject(collisionObject), + m_world(world), + m_resultCallback(resultCallback) + { + } + + 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())) + { + btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1); + btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1); + + btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1,0, BT_CLOSEST_POINT_ALGORITHMS); + if (algorithm) + { + btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback); + //discrete collision detection query + + algorithm->processCollision(&ob0,&ob1, 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) +{ + btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1); + btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1); + + btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB, 0, BT_CLOSEST_POINT_ALGORITHMS); + if (algorithm) + { + btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback); + contactPointResult.m_closestPointDistanceThreshold = resultCallback.m_closestDistanceThreshold; + //discrete collision detection query + algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult); + + algorithm->~btCollisionAlgorithm(); + getDispatcher()->freeCollisionAlgorithm(algorithm); + } + +} + + + + +class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback +{ + btIDebugDraw* m_debugDrawer; + btVector3 m_color; + btTransform m_worldTrans; + +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.); + + if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals ) + { + btVector3 normal = (wv1-wv0).cross(wv2-wv0); + normal.normalize(); + btVector3 normalColor(1,1,0); + m_debugDrawer->drawLine(center,center+normal,normalColor); + } + m_debugDrawer->drawLine(wv0,wv1,m_color); + m_debugDrawer->drawLine(wv1,wv2,m_color); + m_debugDrawer->drawLine(wv2,wv0,m_color); + } +}; + + +void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) +{ + // Draw a small simplex at the center of the object + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames) + { + getDebugDrawer()->drawTransform(worldTransform,.1); + } + + 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: + { + + /// for polyhedral shapes + if (shape->isPolyhedral()) + { + btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; + + int i; + if (polyshape->getConvexPolyhedron()) + { + const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron(); + for (i=0;i<poly->m_faces.size();i++) + { + btVector3 centroid(0,0,0); + int numVerts = poly->m_faces[i].m_indices.size(); + if (numVerts) + { + int lastV = poly->m_faces[i].m_indices[numVerts-1]; + for (int v=0;v<poly->m_faces[i].m_indices.size();v++) + { + int curVert = poly->m_faces[i].m_indices[v]; + centroid+=poly->m_vertices[curVert]; + getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color); + lastV = curVert; + } + } + centroid*= btScalar(1.f)/btScalar(numVerts); + if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals) + { + btVector3 normalColor(1,1,0); + btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]); + getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor); + } + + } + + + } else + { + for (i=0;i<polyshape->getNumEdges();i++) + { + btVector3 a,b; + polyshape->getEdge(i,a,b); + btVector3 wa = worldTransform * a; + btVector3 wb = worldTransform * b; + getDebugDrawer()->drawLine(wa,wb,color); + } + } + + + } + + if (shape->isConcave()) + { + btConcaveShape* concaveMesh = (btConcaveShape*) shape; + + ///@todo pass camera, for some culling? no -> we are not a graphics lib + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); + + } + + if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + { + btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; + //todo: pass camera for some culling + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + //DebugDrawcallback drawCallback; + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + } + + + + } + + } + } +} + + +void btCollisionWorld::debugDrawWorld() +{ + if (getDebugDrawer()) + { + getDebugDrawer()->clearLines(); + + btIDebugDraw::DefaultColors defaultColors = getDebugDrawer()->getDefaultColors(); + + if ( getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) + { + + + if (getDispatcher()) + { + int numManifolds = getDispatcher()->getNumManifolds(); + + 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(),defaultColors.m_contactPoint); + } + } + } + } + + if ((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(0.4),btScalar(0.4),btScalar(0.4)); + + switch(colObj->getActivationState()) + { + case ACTIVE_TAG: + color = defaultColors.m_activeObject; break; + case ISLAND_SLEEPING: + color = defaultColors.m_deactivatedObject;break; + case WANTS_DEACTIVATION: + color = defaultColors.m_wantsDeactivationObject;break; + case DISABLE_DEACTIVATION: + color = defaultColors.m_disabledDeactivationObject;break; + case DISABLE_SIMULATION: + color = defaultColors.m_disabledSimulationObject;break; + default: + { + color = btVector3(btScalar(.3),btScalar(0.3),btScalar(0.3)); + } + }; + + colObj->getCustomDebugColor(color); + + debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); + } + if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + btVector3 minAabb,maxAabb; + btVector3 colorvec = defaultColors.m_aabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); + minAabb -= contactThreshold; + maxAabb += contactThreshold; + + btVector3 minAabb2,maxAabb2; + + if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) + { + 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; + + ///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); + } + } + + //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->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)) + { + colObj->serializeSingleObject(serializer); + } + } +} + + +void btCollisionWorld::serialize(btSerializer* serializer) +{ + + serializer->startSerialization(); + + serializeCollisionObjects(serializer); + + serializer->finishSerialization(); +} + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h new file mode 100644 index 0000000000..eede2b28ca --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -0,0 +1,528 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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. +*/ + + +/** + * @mainpage Bullet Documentation + * + * @section intro_sec Introduction + * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ). + * + * 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.org + * + * @section install_sec Installation + * + * @subsection step1 Step 1: Download + * You can download the Bullet Physics Library from the github repository: https://github.com/bulletphysics/bullet3/releases + * + * @subsection step2 Step 2: Building + * Bullet has multiple build systems, including premake, cmake and autotools. Premake and cmake support all platforms. + * Premake is included in the Bullet/build folder for Windows, Mac OSX and Linux. + * Under Windows you can click on Bullet/build/vs2010.bat to create Microsoft Visual Studio projects. + * On Mac OSX and Linux you can open a terminal and generate Makefile, codeblocks or Xcode4 projects: + * cd Bullet/build + * ./premake4_osx gmake or ./premake4_linux gmake or ./premake4_linux64 gmake or (for Mac) ./premake4_osx xcode4 + * cd Bullet/build/gmake + * make + * + * An alternative to premake is cmake. You can download cmake from 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. + * Bullet can be used in several ways, as Full Rigid Body simulation, as Collision Detector Library or Low Level / Snippets like the GJK Closest Point calculation. + * The Dependencies can be seen in this documentation under Directories + * + * @subsection step4 Step 4: Integrating in your application, full Rigid Body and Soft Body simulation + * Check out BasicDemo how to create a btDynamicsWorld, btRigidBody and btCollisionShape, Stepping the simulation and synchronizing your graphics object transform. + * Check out SoftDemo how to use soft body dynamics, using btSoftRigidDynamicsWorld. + * @subsection step5 Step 5 : Integrate the Collision Detection Library (without Dynamics and other Extras) + * Bullet Collision Detection can also be used without the Dynamics/Extras. + * Check out btCollisionWorld and btCollisionObject, and the CollisionInterfaceDemo. + * @subsection step6 Step 6 : Use Snippets like the GJK Closest Point calculation. + * Bullet has been designed in a modular way keeping dependencies to a minimum. The ConvexHullDistance demo demonstrates direct use of btGjkPairDetector. + * + * @section copyright Copyright + * For up-to-data information and copyright and contributors list check out the Bullet_User_Manual.pdf + * + */ + + + +#ifndef BT_COLLISION_WORLD_H +#define BT_COLLISION_WORLD_H + +class btCollisionShape; +class btConvexShape; +class btBroadphaseInterface; +class btSerializer; + +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" +#include "btCollisionObject.h" +#include "btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "LinearMath/btAlignedObjectArray.h" + +///CollisionWorld is interface and container for the collision detection +class btCollisionWorld +{ + + +protected: + + btAlignedObjectArray<btCollisionObject*> m_collisionObjects; + + btDispatcher* m_dispatcher1; + + btDispatcherInfo m_dispatchInfo; + + btBroadphaseInterface* m_broadphasePairCache; + + 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 + btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, btCollisionConfiguration* collisionConfiguration); + + virtual ~btCollisionWorld(); + + void setBroadphase(btBroadphaseInterface* pairCache) + { + m_broadphasePairCache = pairCache; + } + + const btBroadphaseInterface* getBroadphase() const + { + return m_broadphasePairCache; + } + + btBroadphaseInterface* getBroadphase() + { + return m_broadphasePairCache; + } + + btOverlappingPairCache* getPairCache() + { + return m_broadphasePairCache->getOverlappingPairCache(); + } + + + btDispatcher* getDispatcher() + { + return m_dispatcher1; + } + + const btDispatcher* getDispatcher() const + { + return m_dispatcher1; + } + + void updateSingleAabb(btCollisionObject* colObj); + + virtual void updateAabbs(); + + ///the computeOverlappingPairs is usually already called by performDiscreteCollisionDetection (or stepSimulation) + ///it can be useful to use if you perform ray tests without collision detection/simulation + virtual void computeOverlappingPairs(); + + + virtual void setDebugDrawer(btIDebugDraw* debugDrawer) + { + m_debugDrawer = debugDrawer; + } + + virtual btIDebugDraw* getDebugDrawer() + { + 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 + struct LocalShapeInfo + { + int m_shapePart; + int m_triangleIndex; + + //const btCollisionShape* m_shapeTemp; + //const btTransform* m_shapeLocalTransform; + }; + + struct LocalRayResult + { + LocalRayResult(const btCollisionObject* collisionObject, + LocalShapeInfo* localShapeInfo, + const btVector3& hitNormalLocal, + btScalar hitFraction) + :m_collisionObject(collisionObject), + m_localShapeInfo(localShapeInfo), + m_hitNormalLocal(hitNormalLocal), + m_hitFraction(hitFraction) + { + } + + const btCollisionObject* m_collisionObject; + LocalShapeInfo* m_localShapeInfo; + btVector3 m_hitNormalLocal; + btScalar m_hitFraction; + + }; + + ///RayResultCallback is used to report new raycast results + struct RayResultCallback + { + btScalar m_closestHitFraction; + const btCollisionObject* m_collisionObject; + int m_collisionFilterGroup; + int m_collisionFilterMask; + //@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback.h. Apply any of the EFlags defined there on m_flags here to invoke. + unsigned int m_flags; + + virtual ~RayResultCallback() + { + } + bool hasHit() const + { + return (m_collisionObject != 0); + } + + RayResultCallback() + :m_closestHitFraction(btScalar(1.)), + m_collisionObject(0), + m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter), + //@BP Mod + m_flags(0) + { + } + + 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(LocalRayResult& rayResult,bool normalInWorldSpace) = 0; + }; + + struct ClosestRayResultCallback : public RayResultCallback + { + ClosestRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) + :m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld) + { + } + + btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction + btVector3 m_rayToWorld; + + btVector3 m_hitNormalWorld; + btVector3 m_hitPointWorld; + + virtual btScalar addSingleResult(LocalRayResult& rayResult,bool normalInWorldSpace) + { + //caller already does the filter on the m_closestHitFraction + btAssert(rayResult.m_hitFraction <= m_closestHitFraction); + + m_closestHitFraction = rayResult.m_hitFraction; + m_collisionObject = rayResult.m_collisionObject; + if (normalInWorldSpace) + { + m_hitNormalWorld = rayResult.m_hitNormalLocal; + } else + { + ///need to transform normal into worldspace + m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal; + } + m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction); + return rayResult.m_hitFraction; + } + }; + + struct AllHitsRayResultCallback : public RayResultCallback + { + AllHitsRayResultCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld) + :m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld) + { + } + + btAlignedObjectArray<const 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 + { + LocalConvexResult(const btCollisionObject* hitCollisionObject, + LocalShapeInfo* localShapeInfo, + const btVector3& hitNormalLocal, + const btVector3& hitPointLocal, + btScalar hitFraction + ) + :m_hitCollisionObject(hitCollisionObject), + m_localShapeInfo(localShapeInfo), + m_hitNormalLocal(hitNormalLocal), + m_hitPointLocal(hitPointLocal), + m_hitFraction(hitFraction) + { + } + + const btCollisionObject* m_hitCollisionObject; + LocalShapeInfo* m_localShapeInfo; + btVector3 m_hitNormalLocal; + btVector3 m_hitPointLocal; + btScalar m_hitFraction; + }; + + ///RayResultCallback is used to report new raycast results + struct ConvexResultCallback + { + btScalar m_closestHitFraction; + int m_collisionFilterGroup; + int m_collisionFilterMask; + + ConvexResultCallback() + :m_closestHitFraction(btScalar(1.)), + m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter) + { + } + + virtual ~ConvexResultCallback() + { + } + + bool hasHit() const + { + return (m_closestHitFraction < btScalar(1.)); + } + + + + 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(LocalConvexResult& convexResult,bool normalInWorldSpace) = 0; + }; + + struct ClosestConvexResultCallback : public ConvexResultCallback + { + ClosestConvexResultCallback(const btVector3& convexFromWorld,const btVector3& convexToWorld) + :m_convexFromWorld(convexFromWorld), + m_convexToWorld(convexToWorld), + m_hitCollisionObject(0) + { + } + + btVector3 m_convexFromWorld;//used to calculate hitPointWorld from hitFraction + btVector3 m_convexToWorld; + + btVector3 m_hitNormalWorld; + btVector3 m_hitPointWorld; + const btCollisionObject* m_hitCollisionObject; + + virtual btScalar addSingleResult(LocalConvexResult& convexResult,bool normalInWorldSpace) + { +//caller already does the filter on the m_closestHitFraction + btAssert(convexResult.m_hitFraction <= m_closestHitFraction); + + m_closestHitFraction = convexResult.m_hitFraction; + m_hitCollisionObject = convexResult.m_hitCollisionObject; + if (normalInWorldSpace) + { + m_hitNormalWorld = convexResult.m_hitNormalLocal; + } else + { + ///need to transform normal into worldspace + m_hitNormalWorld = m_hitCollisionObject->getWorldTransform().getBasis()*convexResult.m_hitNormalLocal; + } + m_hitPointWorld = convexResult.m_hitPointLocal; + return convexResult.m_hitFraction; + } + }; + + ///ContactResultCallback is used to report contact points + struct ContactResultCallback + { + int m_collisionFilterGroup; + int m_collisionFilterMask; + btScalar m_closestDistanceThreshold; + + ContactResultCallback() + :m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter), + m_closestDistanceThreshold(0) + { + } + + virtual ~ContactResultCallback() + { + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; + collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask); + return collides; + } + + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) = 0; + }; + + + + int getNumCollisionObjects() const + { + return int(m_collisionObjects.size()); + } + + /// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback + /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. + virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; + + /// 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. + /// This allows more customization. + static void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback); + + static void rayTestSingleInternal(const btTransform& rayFromTrans,const btTransform& rayToTrans, + const btCollisionObjectWrapper* collisionObjectWrap, + RayResultCallback& resultCallback); + + /// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest. + static void objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans,const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback, btScalar allowedPenetration); + + static void objectQuerySingleInternal(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, + const btCollisionObjectWrapper* colObjWrap, + ConvexResultCallback& resultCallback, btScalar allowedPenetration); + + virtual void addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter); + + btCollisionObjectArray& getCollisionObjectArray() + { + return m_collisionObjects; + } + + const btCollisionObjectArray& getCollisionObjectArray() const + { + return m_collisionObjects; + } + + + virtual void removeCollisionObject(btCollisionObject* collisionObject); + + virtual void performDiscreteCollisionDetection(); + + btDispatcherInfo& getDispatchInfo() + { + return m_dispatchInfo; + } + + const btDispatcherInfo& getDispatchInfo() const + { + 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); + +}; + + +#endif //BT_COLLISION_WORLD_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp new file mode 100644 index 0000000000..f2b0837808 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp @@ -0,0 +1,1147 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org + +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 "btCollisionWorldImporter.h" +#include "btBulletCollisionCommon.h" +#include "LinearMath/btSerializer.h" //for btBulletSerializedArrays definition + +#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT +#include "BulletCollision/Gimpact/btGImpactShape.h" +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT + +btCollisionWorldImporter::btCollisionWorldImporter(btCollisionWorld* world) +:m_collisionWorld(world), +m_verboseMode(0) +{ + +} + +btCollisionWorldImporter::~btCollisionWorldImporter() +{ +} + + + + + +bool btCollisionWorldImporter::convertAllObjects( btBulletSerializedArrays* arrays) +{ + + m_shapeMap.clear(); + m_bodyMap.clear(); + + int i; + + for (i=0;i<arrays->m_bvhsDouble.size();i++) + { + btOptimizedBvh* bvh = createOptimizedBvh(); + btQuantizedBvhDoubleData* bvhData = arrays->m_bvhsDouble[i]; + bvh->deSerializeDouble(*bvhData); + m_bvhMap.insert(arrays->m_bvhsDouble[i],bvh); + } + for (i=0;i<arrays->m_bvhsFloat.size();i++) + { + btOptimizedBvh* bvh = createOptimizedBvh(); + btQuantizedBvhFloatData* bvhData = arrays->m_bvhsFloat[i]; + bvh->deSerializeFloat(*bvhData); + m_bvhMap.insert(arrays->m_bvhsFloat[i],bvh); + } + + + + + + for (i=0;i<arrays->m_colShapeData.size();i++) + { + btCollisionShapeData* shapeData = arrays->m_colShapeData[i]; + btCollisionShape* shape = convertCollisionShape(shapeData); + if (shape) + { + // printf("shapeMap.insert(%x,%x)\n",shapeData,shape); + m_shapeMap.insert(shapeData,shape); + } + + if (shape&& shapeData->m_name) + { + char* newname = duplicateName(shapeData->m_name); + m_objectNameMap.insert(shape,newname); + m_nameShapeMap.insert(newname,shape); + } + } + + + for (i=0;i<arrays->m_collisionObjectDataDouble.size();i++) + { + btCollisionObjectDoubleData* colObjData = arrays->m_collisionObjectDataDouble[i]; + btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); + if (shapePtr && *shapePtr) + { + btTransform startTransform; + colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; + startTransform.deSerializeDouble(colObjData->m_worldTransform); + + btCollisionShape* shape = (btCollisionShape*)*shapePtr; + btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); + body->setFriction(btScalar(colObjData->m_friction)); + body->setRestitution(btScalar(colObjData->m_restitution)); + +#ifdef USE_INTERNAL_EDGE_UTILITY + if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; + if (trimesh->getTriangleInfoMap()) + { + body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + } + } +#endif //USE_INTERNAL_EDGE_UTILITY + m_bodyMap.insert(colObjData,body); + } else + { + printf("error: no shape found\n"); + } + } + for (i=0;i<arrays->m_collisionObjectDataFloat.size();i++) + { + btCollisionObjectFloatData* colObjData = arrays->m_collisionObjectDataFloat[i]; + btCollisionShape** shapePtr = m_shapeMap.find(colObjData->m_collisionShape); + if (shapePtr && *shapePtr) + { + btTransform startTransform; + colObjData->m_worldTransform.m_origin.m_floats[3] = 0.f; + startTransform.deSerializeFloat(colObjData->m_worldTransform); + + btCollisionShape* shape = (btCollisionShape*)*shapePtr; + btCollisionObject* body = createCollisionObject(startTransform,shape,colObjData->m_name); + +#ifdef USE_INTERNAL_EDGE_UTILITY + if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)shape; + if (trimesh->getTriangleInfoMap()) + { + body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + } + } +#endif //USE_INTERNAL_EDGE_UTILITY + m_bodyMap.insert(colObjData,body); + } else + { + printf("error: no shape found\n"); + } + } + + return true; +} + + + +void btCollisionWorldImporter::deleteAllData() +{ + int i; + + for (i=0;i<m_allocatedCollisionObjects.size();i++) + { + if(m_collisionWorld) + m_collisionWorld->removeCollisionObject(m_allocatedCollisionObjects[i]); + delete m_allocatedCollisionObjects[i]; + } + + m_allocatedCollisionObjects.clear(); + + + for (i=0;i<m_allocatedCollisionShapes.size();i++) + { + delete m_allocatedCollisionShapes[i]; + } + m_allocatedCollisionShapes.clear(); + + + for (i=0;i<m_allocatedBvhs.size();i++) + { + delete m_allocatedBvhs[i]; + } + m_allocatedBvhs.clear(); + + for (i=0;i<m_allocatedTriangleInfoMaps.size();i++) + { + delete m_allocatedTriangleInfoMaps[i]; + } + m_allocatedTriangleInfoMaps.clear(); + for (i=0;i<m_allocatedTriangleIndexArrays.size();i++) + { + delete m_allocatedTriangleIndexArrays[i]; + } + m_allocatedTriangleIndexArrays.clear(); + for (i=0;i<m_allocatedNames.size();i++) + { + delete[] m_allocatedNames[i]; + } + m_allocatedNames.clear(); + + for (i=0;i<m_allocatedbtStridingMeshInterfaceDatas.size();i++) + { + btStridingMeshInterfaceData* curData = m_allocatedbtStridingMeshInterfaceDatas[i]; + + for(int a = 0;a < curData->m_numMeshParts;a++) + { + btMeshPartData* curPart = &curData->m_meshPartsPtr[a]; + if(curPart->m_vertices3f) + delete [] curPart->m_vertices3f; + + if(curPart->m_vertices3d) + delete [] curPart->m_vertices3d; + + if(curPart->m_indices32) + delete [] curPart->m_indices32; + + if(curPart->m_3indices16) + delete [] curPart->m_3indices16; + + if(curPart->m_indices16) + delete [] curPart->m_indices16; + + if (curPart->m_3indices8) + delete [] curPart->m_3indices8; + + } + delete [] curData->m_meshPartsPtr; + delete curData; + } + m_allocatedbtStridingMeshInterfaceDatas.clear(); + + for (i=0;i<m_indexArrays.size();i++) + { + btAlignedFree(m_indexArrays[i]); + } + m_indexArrays.clear(); + + for (i=0;i<m_shortIndexArrays.size();i++) + { + btAlignedFree(m_shortIndexArrays[i]); + } + m_shortIndexArrays.clear(); + + for (i=0;i<m_charIndexArrays.size();i++) + { + btAlignedFree(m_charIndexArrays[i]); + } + m_charIndexArrays.clear(); + + for (i=0;i<m_floatVertexArrays.size();i++) + { + btAlignedFree(m_floatVertexArrays[i]); + } + m_floatVertexArrays.clear(); + + for (i=0;i<m_doubleVertexArrays.size();i++) + { + btAlignedFree(m_doubleVertexArrays[i]); + } + m_doubleVertexArrays.clear(); + + +} + + + +btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionShapeData* shapeData ) +{ + btCollisionShape* shape = 0; + + switch (shapeData->m_shapeType) + { + case STATIC_PLANE_PROXYTYPE: + { + btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)shapeData; + btVector3 planeNormal,localScaling; + planeNormal.deSerializeFloat(planeData->m_planeNormal); + localScaling.deSerializeFloat(planeData->m_localScaling); + shape = createPlaneShape(planeNormal,planeData->m_planeConstant); + shape->setLocalScaling(localScaling); + + break; + } + case SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE: + { + btScaledTriangleMeshShapeData* scaledMesh = (btScaledTriangleMeshShapeData*) shapeData; + btCollisionShapeData* colShapeData = (btCollisionShapeData*) &scaledMesh->m_trimeshShapeData; + colShapeData->m_shapeType = TRIANGLE_MESH_SHAPE_PROXYTYPE; + btCollisionShape* childShape = convertCollisionShape(colShapeData); + btBvhTriangleMeshShape* meshShape = (btBvhTriangleMeshShape*)childShape; + btVector3 localScaling; + localScaling.deSerializeFloat(scaledMesh->m_localScaling); + + shape = createScaledTrangleMeshShape(meshShape, localScaling); + break; + } +#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT + case GIMPACT_SHAPE_PROXYTYPE: + { + btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*) shapeData; + if (gimpactData->m_gimpactSubType == CONST_GIMPACT_TRIMESH_SHAPE) + { + btStridingMeshInterfaceData* interfaceData = createStridingMeshInterfaceData(&gimpactData->m_meshInterface); + btTriangleIndexVertexArray* meshInterface = createMeshInterface(*interfaceData); + + + btGImpactMeshShape* gimpactShape = createGimpactShape(meshInterface); + btVector3 localScaling; + localScaling.deSerializeFloat(gimpactData->m_localScaling); + gimpactShape->setLocalScaling(localScaling); + gimpactShape->setMargin(btScalar(gimpactData->m_collisionMargin)); + gimpactShape->updateBound(); + shape = gimpactShape; + } else + { + printf("unsupported gimpact sub type\n"); + } + break; + } +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT + //The btCapsuleShape* API has issue passing the margin/scaling/halfextents unmodified through the API + //so deal with this + case CAPSULE_SHAPE_PROXYTYPE: + { + btCapsuleShapeData* capData = (btCapsuleShapeData*)shapeData; + + + switch (capData->m_upAxis) + { + case 0: + { + shape = createCapsuleShapeX(1,1); + break; + } + case 1: + { + shape = createCapsuleShapeY(1,1); + break; + } + case 2: + { + shape = createCapsuleShapeZ(1,1); + break; + } + default: + { + printf("error: wrong up axis for btCapsuleShape\n"); + } + + + }; + if (shape) + { + btCapsuleShape* cap = (btCapsuleShape*) shape; + cap->deSerializeFloat(capData); + } + break; + } + case CYLINDER_SHAPE_PROXYTYPE: + case CONE_SHAPE_PROXYTYPE: + case BOX_SHAPE_PROXYTYPE: + case SPHERE_SHAPE_PROXYTYPE: + case MULTI_SPHERE_SHAPE_PROXYTYPE: + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData; + btVector3 implicitShapeDimensions; + implicitShapeDimensions.deSerializeFloat(bsd->m_implicitShapeDimensions); + btVector3 localScaling; + localScaling.deSerializeFloat(bsd->m_localScaling); + btVector3 margin(bsd->m_collisionMargin,bsd->m_collisionMargin,bsd->m_collisionMargin); + switch (shapeData->m_shapeType) + { + case BOX_SHAPE_PROXYTYPE: + { + btBoxShape* box= (btBoxShape*)createBoxShape(implicitShapeDimensions/localScaling+margin); + //box->initializePolyhedralFeatures(); + shape = box; + + break; + } + case SPHERE_SHAPE_PROXYTYPE: + { + shape = createSphereShape(implicitShapeDimensions.getX()); + break; + } + + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShapeData* cylData = (btCylinderShapeData*) shapeData; + btVector3 halfExtents = implicitShapeDimensions+margin; + switch (cylData->m_upAxis) + { + case 0: + { + shape = createCylinderShapeX(halfExtents.getY(),halfExtents.getX()); + break; + } + case 1: + { + shape = createCylinderShapeY(halfExtents.getX(),halfExtents.getY()); + break; + } + case 2: + { + shape = createCylinderShapeZ(halfExtents.getX(),halfExtents.getZ()); + break; + } + default: + { + printf("unknown Cylinder up axis\n"); + } + + }; + + + + break; + } + case CONE_SHAPE_PROXYTYPE: + { + btConeShapeData* conData = (btConeShapeData*) shapeData; + btVector3 halfExtents = implicitShapeDimensions;//+margin; + switch (conData->m_upIndex) + { + case 0: + { + shape = createConeShapeX(halfExtents.getY(),halfExtents.getX()); + break; + } + case 1: + { + shape = createConeShapeY(halfExtents.getX(),halfExtents.getY()); + break; + } + case 2: + { + shape = createConeShapeZ(halfExtents.getX(),halfExtents.getZ()); + break; + } + default: + { + printf("unknown Cone up axis\n"); + } + + }; + + + + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + btMultiSphereShapeData* mss = (btMultiSphereShapeData*)bsd; + int numSpheres = mss->m_localPositionArraySize; + + btAlignedObjectArray<btVector3> tmpPos; + btAlignedObjectArray<btScalar> radii; + radii.resize(numSpheres); + tmpPos.resize(numSpheres); + int i; + for ( i=0;i<numSpheres;i++) + { + tmpPos[i].deSerializeFloat(mss->m_localPositionArrayPtr[i].m_pos); + radii[i] = mss->m_localPositionArrayPtr[i].m_radius; + } + shape = createMultiSphereShape(&tmpPos[0],&radii[0],numSpheres); + break; + } + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + // int sz = sizeof(btConvexHullShapeData); + // int sz2 = sizeof(btConvexInternalShapeData); + // int sz3 = sizeof(btCollisionShapeData); + btConvexHullShapeData* convexData = (btConvexHullShapeData*)bsd; + int numPoints = convexData->m_numUnscaledPoints; + + btAlignedObjectArray<btVector3> tmpPoints; + tmpPoints.resize(numPoints); + int i; + for ( i=0;i<numPoints;i++) + { +#ifdef BT_USE_DOUBLE_PRECISION + if (convexData->m_unscaledPointsDoublePtr) + tmpPoints[i].deSerialize(convexData->m_unscaledPointsDoublePtr[i]); + if (convexData->m_unscaledPointsFloatPtr) + tmpPoints[i].deSerializeFloat(convexData->m_unscaledPointsFloatPtr[i]); +#else + if (convexData->m_unscaledPointsFloatPtr) + tmpPoints[i].deSerialize(convexData->m_unscaledPointsFloatPtr[i]); + if (convexData->m_unscaledPointsDoublePtr) + tmpPoints[i].deSerializeDouble(convexData->m_unscaledPointsDoublePtr[i]); +#endif //BT_USE_DOUBLE_PRECISION + } + btConvexHullShape* hullShape = createConvexHullShape(); + for (i=0;i<numPoints;i++) + { + hullShape->addPoint(tmpPoints[i]); + } + hullShape->setMargin(bsd->m_collisionMargin); + //hullShape->initializePolyhedralFeatures(); + shape = hullShape; + break; + } + default: + { + printf("error: cannot create shape type (%d)\n",shapeData->m_shapeType); + } + } + + if (shape) + { + shape->setMargin(bsd->m_collisionMargin); + + btVector3 localScaling; + localScaling.deSerializeFloat(bsd->m_localScaling); + shape->setLocalScaling(localScaling); + + } + break; + } + case TRIANGLE_MESH_SHAPE_PROXYTYPE: + { + btTriangleMeshShapeData* trimesh = (btTriangleMeshShapeData*)shapeData; + btStridingMeshInterfaceData* interfaceData = createStridingMeshInterfaceData(&trimesh->m_meshInterface); + btTriangleIndexVertexArray* meshInterface = createMeshInterface(*interfaceData); + if (!meshInterface->getNumSubParts()) + { + return 0; + } + + btVector3 scaling; scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling); + meshInterface->setScaling(scaling); + + + btOptimizedBvh* bvh = 0; +#if 1 + if (trimesh->m_quantizedFloatBvh) + { + btOptimizedBvh** bvhPtr = m_bvhMap.find(trimesh->m_quantizedFloatBvh); + if (bvhPtr && *bvhPtr) + { + bvh = *bvhPtr; + } else + { + bvh = createOptimizedBvh(); + bvh->deSerializeFloat(*trimesh->m_quantizedFloatBvh); + } + } + if (trimesh->m_quantizedDoubleBvh) + { + btOptimizedBvh** bvhPtr = m_bvhMap.find(trimesh->m_quantizedDoubleBvh); + if (bvhPtr && *bvhPtr) + { + bvh = *bvhPtr; + } else + { + bvh = createOptimizedBvh(); + bvh->deSerializeDouble(*trimesh->m_quantizedDoubleBvh); + } + } +#endif + + + btBvhTriangleMeshShape* trimeshShape = createBvhTriangleMeshShape(meshInterface,bvh); + trimeshShape->setMargin(trimesh->m_collisionMargin); + shape = trimeshShape; + + if (trimesh->m_triangleInfoMap) + { + btTriangleInfoMap* map = createTriangleInfoMap(); + map->deSerialize(*trimesh->m_triangleInfoMap); + trimeshShape->setTriangleInfoMap(map); + +#ifdef USE_INTERNAL_EDGE_UTILITY + gContactAddedCallback = btAdjustInternalEdgeContactsCallback; +#endif //USE_INTERNAL_EDGE_UTILITY + + } + + //printf("trimesh->m_collisionMargin=%f\n",trimesh->m_collisionMargin); + break; + } + case COMPOUND_SHAPE_PROXYTYPE: + { + btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData; + btCompoundShape* compoundShape = createCompoundShape(); + + //btCompoundShapeChildData* childShapeDataArray = &compoundData->m_childShapePtr[0]; + + + btAlignedObjectArray<btCollisionShape*> childShapes; + for (int i=0;i<compoundData->m_numChildShapes;i++) + { + //btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i]; + + btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape; + + btCollisionShape* childShape = convertCollisionShape(cd); + if (childShape) + { + btTransform localTransform; + localTransform.deSerializeFloat(compoundData->m_childShapePtr[i].m_transform); + compoundShape->addChildShape(localTransform,childShape); + } else + { +#ifdef _DEBUG + printf("error: couldn't create childShape for compoundShape\n"); +#endif + } + + } + shape = compoundShape; + + break; + } + case SOFTBODY_SHAPE_PROXYTYPE: + { + return 0; + } + default: + { +#ifdef _DEBUG + printf("unsupported shape type (%d)\n",shapeData->m_shapeType); +#endif + } + } + + return shape; + +} + + + +char* btCollisionWorldImporter::duplicateName(const char* name) +{ + if (name) + { + int l = (int)strlen(name); + char* newName = new char[l+1]; + memcpy(newName,name,l); + newName[l] = 0; + m_allocatedNames.push_back(newName); + return newName; + } + return 0; +} + + + + + + + + + + + +btTriangleIndexVertexArray* btCollisionWorldImporter::createMeshInterface(btStridingMeshInterfaceData& meshData) +{ + btTriangleIndexVertexArray* meshInterface = createTriangleMeshContainer(); + + for (int i=0;i<meshData.m_numMeshParts;i++) + { + btIndexedMesh meshPart; + meshPart.m_numTriangles = meshData.m_meshPartsPtr[i].m_numTriangles; + meshPart.m_numVertices = meshData.m_meshPartsPtr[i].m_numVertices; + + + if (meshData.m_meshPartsPtr[i].m_indices32) + { + meshPart.m_indexType = PHY_INTEGER; + meshPart.m_triangleIndexStride = 3*sizeof(int); + int* indexArray = (int*)btAlignedAlloc(sizeof(int)*3*meshPart.m_numTriangles,16); + m_indexArrays.push_back(indexArray); + for (int j=0;j<3*meshPart.m_numTriangles;j++) + { + indexArray[j] = meshData.m_meshPartsPtr[i].m_indices32[j].m_value; + } + meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; + } else + { + if (meshData.m_meshPartsPtr[i].m_3indices16) + { + meshPart.m_indexType = PHY_SHORT; + meshPart.m_triangleIndexStride = sizeof(short int)*3;//sizeof(btShortIntIndexTripletData); + + short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int)*3*meshPart.m_numTriangles,16); + m_shortIndexArrays.push_back(indexArray); + + for (int j=0;j<meshPart.m_numTriangles;j++) + { + indexArray[3*j] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[0]; + indexArray[3*j+1] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[1]; + indexArray[3*j+2] = meshData.m_meshPartsPtr[i].m_3indices16[j].m_values[2]; + } + + meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; + } + if (meshData.m_meshPartsPtr[i].m_indices16) + { + meshPart.m_indexType = PHY_SHORT; + meshPart.m_triangleIndexStride = 3*sizeof(short int); + short int* indexArray = (short int*)btAlignedAlloc(sizeof(short int)*3*meshPart.m_numTriangles,16); + m_shortIndexArrays.push_back(indexArray); + for (int j=0;j<3*meshPart.m_numTriangles;j++) + { + indexArray[j] = meshData.m_meshPartsPtr[i].m_indices16[j].m_value; + } + + meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; + } + + if (meshData.m_meshPartsPtr[i].m_3indices8) + { + meshPart.m_indexType = PHY_UCHAR; + meshPart.m_triangleIndexStride = sizeof(unsigned char)*3; + + unsigned char* indexArray = (unsigned char*)btAlignedAlloc(sizeof(unsigned char)*3*meshPart.m_numTriangles,16); + m_charIndexArrays.push_back(indexArray); + + for (int j=0;j<meshPart.m_numTriangles;j++) + { + indexArray[3*j] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[0]; + indexArray[3*j+1] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[1]; + indexArray[3*j+2] = meshData.m_meshPartsPtr[i].m_3indices8[j].m_values[2]; + } + + meshPart.m_triangleIndexBase = (const unsigned char*)indexArray; + } + } + + if (meshData.m_meshPartsPtr[i].m_vertices3f) + { + meshPart.m_vertexType = PHY_FLOAT; + meshPart.m_vertexStride = sizeof(btVector3FloatData); + btVector3FloatData* vertices = (btVector3FloatData*) btAlignedAlloc(sizeof(btVector3FloatData)*meshPart.m_numVertices,16); + m_floatVertexArrays.push_back(vertices); + + for (int j=0;j<meshPart.m_numVertices;j++) + { + vertices[j].m_floats[0] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[0]; + vertices[j].m_floats[1] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[1]; + vertices[j].m_floats[2] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[2]; + vertices[j].m_floats[3] = meshData.m_meshPartsPtr[i].m_vertices3f[j].m_floats[3]; + } + meshPart.m_vertexBase = (const unsigned char*)vertices; + } else + { + meshPart.m_vertexType = PHY_DOUBLE; + meshPart.m_vertexStride = sizeof(btVector3DoubleData); + + + btVector3DoubleData* vertices = (btVector3DoubleData*) btAlignedAlloc(sizeof(btVector3DoubleData)*meshPart.m_numVertices,16); + m_doubleVertexArrays.push_back(vertices); + + for (int j=0;j<meshPart.m_numVertices;j++) + { + vertices[j].m_floats[0] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[0]; + vertices[j].m_floats[1] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[1]; + vertices[j].m_floats[2] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[2]; + vertices[j].m_floats[3] = meshData.m_meshPartsPtr[i].m_vertices3d[j].m_floats[3]; + } + meshPart.m_vertexBase = (const unsigned char*)vertices; + } + + if (meshPart.m_triangleIndexBase && meshPart.m_vertexBase) + { + meshInterface->addIndexedMesh(meshPart,meshPart.m_indexType); + } + } + + return meshInterface; +} + + +btStridingMeshInterfaceData* btCollisionWorldImporter::createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData) +{ + //create a new btStridingMeshInterfaceData that is an exact copy of shapedata and store it in the WorldImporter + btStridingMeshInterfaceData* newData = new btStridingMeshInterfaceData; + + newData->m_scaling = interfaceData->m_scaling; + newData->m_numMeshParts = interfaceData->m_numMeshParts; + newData->m_meshPartsPtr = new btMeshPartData[newData->m_numMeshParts]; + + for(int i = 0;i < newData->m_numMeshParts;i++) + { + btMeshPartData* curPart = &interfaceData->m_meshPartsPtr[i]; + btMeshPartData* curNewPart = &newData->m_meshPartsPtr[i]; + + curNewPart->m_numTriangles = curPart->m_numTriangles; + curNewPart->m_numVertices = curPart->m_numVertices; + + if(curPart->m_vertices3f) + { + curNewPart->m_vertices3f = new btVector3FloatData[curNewPart->m_numVertices]; + memcpy(curNewPart->m_vertices3f,curPart->m_vertices3f,sizeof(btVector3FloatData) * curNewPart->m_numVertices); + } + else + curNewPart->m_vertices3f = NULL; + + if(curPart->m_vertices3d) + { + curNewPart->m_vertices3d = new btVector3DoubleData[curNewPart->m_numVertices]; + memcpy(curNewPart->m_vertices3d,curPart->m_vertices3d,sizeof(btVector3DoubleData) * curNewPart->m_numVertices); + } + else + curNewPart->m_vertices3d = NULL; + + int numIndices = curNewPart->m_numTriangles * 3; + ///the m_3indices8 was not initialized in some Bullet versions, this can cause crashes at loading time + ///we catch it by only dealing with m_3indices8 if none of the other indices are initialized + bool uninitialized3indices8Workaround =false; + + if(curPart->m_indices32) + { + uninitialized3indices8Workaround=true; + curNewPart->m_indices32 = new btIntIndexData[numIndices]; + memcpy(curNewPart->m_indices32,curPart->m_indices32,sizeof(btIntIndexData) * numIndices); + } + else + curNewPart->m_indices32 = NULL; + + if(curPart->m_3indices16) + { + uninitialized3indices8Workaround=true; + curNewPart->m_3indices16 = new btShortIntIndexTripletData[curNewPart->m_numTriangles]; + memcpy(curNewPart->m_3indices16,curPart->m_3indices16,sizeof(btShortIntIndexTripletData) * curNewPart->m_numTriangles); + } + else + curNewPart->m_3indices16 = NULL; + + if(curPart->m_indices16) + { + uninitialized3indices8Workaround=true; + curNewPart->m_indices16 = new btShortIntIndexData[numIndices]; + memcpy(curNewPart->m_indices16,curPart->m_indices16,sizeof(btShortIntIndexData) * numIndices); + } + else + curNewPart->m_indices16 = NULL; + + if(!uninitialized3indices8Workaround && curPart->m_3indices8) + { + curNewPart->m_3indices8 = new btCharIndexTripletData[curNewPart->m_numTriangles]; + memcpy(curNewPart->m_3indices8,curPart->m_3indices8,sizeof(btCharIndexTripletData) * curNewPart->m_numTriangles); + } + else + curNewPart->m_3indices8 = NULL; + + } + + m_allocatedbtStridingMeshInterfaceDatas.push_back(newData); + + return(newData); +} + +#ifdef USE_INTERNAL_EDGE_UTILITY +extern ContactAddedCallback gContactAddedCallback; + +static bool btAdjustInternalEdgeContactsCallback(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1) +{ + + btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1); + //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_BACKFACE_MODE); + //btAdjustInternalEdgeContacts(cp,colObj1,colObj0, partId1,index1, BT_TRIANGLE_CONVEX_DOUBLE_SIDED+BT_TRIANGLE_CONCAVE_DOUBLE_SIDED); + return true; +} +#endif //USE_INTERNAL_EDGE_UTILITY + + +/* +btRigidBody* btWorldImporter::createRigidBody(bool isDynamic, btScalar mass, const btTransform& startTransform,btCollisionShape* shape,const char* bodyName) +{ + btVector3 localInertia; + localInertia.setZero(); + + if (mass) + shape->calculateLocalInertia(mass,localInertia); + + btRigidBody* body = new btRigidBody(mass,0,shape,localInertia); + body->setWorldTransform(startTransform); + + if (m_dynamicsWorld) + m_dynamicsWorld->addRigidBody(body); + + if (bodyName) + { + char* newname = duplicateName(bodyName); + m_objectNameMap.insert(body,newname); + m_nameBodyMap.insert(newname,body); + } + m_allocatedRigidBodies.push_back(body); + return body; + +} +*/ + +btCollisionObject* btCollisionWorldImporter::getCollisionObjectByName(const char* name) +{ + btCollisionObject** bodyPtr = m_nameColObjMap.find(name); + if (bodyPtr && *bodyPtr) + { + return *bodyPtr; + } + return 0; +} + +btCollisionObject* btCollisionWorldImporter::createCollisionObject(const btTransform& startTransform,btCollisionShape* shape, const char* bodyName) +{ + btCollisionObject* colObj = new btCollisionObject(); + colObj->setWorldTransform(startTransform); + colObj->setCollisionShape(shape); + m_collisionWorld->addCollisionObject(colObj);//todo: flags etc + + if (bodyName) + { + char* newname = duplicateName(bodyName); + m_objectNameMap.insert(colObj,newname); + m_nameColObjMap.insert(newname,colObj); + } + m_allocatedCollisionObjects.push_back(colObj); + + return colObj; +} + + + +btCollisionShape* btCollisionWorldImporter::createPlaneShape(const btVector3& planeNormal,btScalar planeConstant) +{ + btStaticPlaneShape* shape = new btStaticPlaneShape(planeNormal,planeConstant); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} +btCollisionShape* btCollisionWorldImporter::createBoxShape(const btVector3& halfExtents) +{ + btBoxShape* shape = new btBoxShape(halfExtents); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} +btCollisionShape* btCollisionWorldImporter::createSphereShape(btScalar radius) +{ + btSphereShape* shape = new btSphereShape(radius); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + + +btCollisionShape* btCollisionWorldImporter::createCapsuleShapeX(btScalar radius, btScalar height) +{ + btCapsuleShapeX* shape = new btCapsuleShapeX(radius,height); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createCapsuleShapeY(btScalar radius, btScalar height) +{ + btCapsuleShape* shape = new btCapsuleShape(radius,height); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createCapsuleShapeZ(btScalar radius, btScalar height) +{ + btCapsuleShapeZ* shape = new btCapsuleShapeZ(radius,height); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createCylinderShapeX(btScalar radius,btScalar height) +{ + btCylinderShapeX* shape = new btCylinderShapeX(btVector3(height,radius,radius)); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createCylinderShapeY(btScalar radius,btScalar height) +{ + btCylinderShape* shape = new btCylinderShape(btVector3(radius,height,radius)); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createCylinderShapeZ(btScalar radius,btScalar height) +{ + btCylinderShapeZ* shape = new btCylinderShapeZ(btVector3(radius,radius,height)); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createConeShapeX(btScalar radius,btScalar height) +{ + btConeShapeX* shape = new btConeShapeX(radius,height); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createConeShapeY(btScalar radius,btScalar height) +{ + btConeShape* shape = new btConeShape(radius,height); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCollisionShape* btCollisionWorldImporter::createConeShapeZ(btScalar radius,btScalar height) +{ + btConeShapeZ* shape = new btConeShapeZ(radius,height); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btTriangleIndexVertexArray* btCollisionWorldImporter::createTriangleMeshContainer() +{ + btTriangleIndexVertexArray* in = new btTriangleIndexVertexArray(); + m_allocatedTriangleIndexArrays.push_back(in); + return in; +} + +btOptimizedBvh* btCollisionWorldImporter::createOptimizedBvh() +{ + btOptimizedBvh* bvh = new btOptimizedBvh(); + m_allocatedBvhs.push_back(bvh); + return bvh; +} + + +btTriangleInfoMap* btCollisionWorldImporter::createTriangleInfoMap() +{ + btTriangleInfoMap* tim = new btTriangleInfoMap(); + m_allocatedTriangleInfoMaps.push_back(tim); + return tim; +} + +btBvhTriangleMeshShape* btCollisionWorldImporter::createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh) +{ + if (bvh) + { + btBvhTriangleMeshShape* bvhTriMesh = new btBvhTriangleMeshShape(trimesh,bvh->isQuantized(), false); + bvhTriMesh->setOptimizedBvh(bvh); + m_allocatedCollisionShapes.push_back(bvhTriMesh); + return bvhTriMesh; + } + + btBvhTriangleMeshShape* ts = new btBvhTriangleMeshShape(trimesh,true); + m_allocatedCollisionShapes.push_back(ts); + return ts; + +} +btCollisionShape* btCollisionWorldImporter::createConvexTriangleMeshShape(btStridingMeshInterface* trimesh) +{ + return 0; +} +#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT +btGImpactMeshShape* btCollisionWorldImporter::createGimpactShape(btStridingMeshInterface* trimesh) +{ + btGImpactMeshShape* shape = new btGImpactMeshShape(trimesh); + m_allocatedCollisionShapes.push_back(shape); + return shape; + +} +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT + +btConvexHullShape* btCollisionWorldImporter::createConvexHullShape() +{ + btConvexHullShape* shape = new btConvexHullShape(); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btCompoundShape* btCollisionWorldImporter::createCompoundShape() +{ + btCompoundShape* shape = new btCompoundShape(); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + + +btScaledBvhTriangleMeshShape* btCollisionWorldImporter::createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScaling) +{ + btScaledBvhTriangleMeshShape* shape = new btScaledBvhTriangleMeshShape(meshShape,localScaling); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + +btMultiSphereShape* btCollisionWorldImporter::createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres) +{ + btMultiSphereShape* shape = new btMultiSphereShape(positions, radi, numSpheres); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + + + + // query for data +int btCollisionWorldImporter::getNumCollisionShapes() const +{ + return m_allocatedCollisionShapes.size(); +} + +btCollisionShape* btCollisionWorldImporter::getCollisionShapeByIndex(int index) +{ + return m_allocatedCollisionShapes[index]; +} + +btCollisionShape* btCollisionWorldImporter::getCollisionShapeByName(const char* name) +{ + btCollisionShape** shapePtr = m_nameShapeMap.find(name); + if (shapePtr&& *shapePtr) + { + return *shapePtr; + } + return 0; +} + + +const char* btCollisionWorldImporter::getNameForPointer(const void* ptr) const +{ + const char*const * namePtr = m_objectNameMap.find(ptr); + if (namePtr && *namePtr) + return *namePtr; + return 0; +} + + +int btCollisionWorldImporter::getNumRigidBodies() const +{ + return m_allocatedRigidBodies.size(); +} + +btCollisionObject* btCollisionWorldImporter::getRigidBodyByIndex(int index) const +{ + return m_allocatedRigidBodies[index]; +} + + +int btCollisionWorldImporter::getNumBvhs() const +{ + return m_allocatedBvhs.size(); +} + btOptimizedBvh* btCollisionWorldImporter::getBvhByIndex(int index) const +{ + return m_allocatedBvhs[index]; +} + +int btCollisionWorldImporter::getNumTriangleInfoMaps() const +{ + return m_allocatedTriangleInfoMaps.size(); +} + +btTriangleInfoMap* btCollisionWorldImporter::getTriangleInfoMapByIndex(int index) const +{ + return m_allocatedTriangleInfoMaps[index]; +} + + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h new file mode 100644 index 0000000000..81c6142726 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h @@ -0,0 +1,189 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2014 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef BT_COLLISION_WORLD_IMPORTER_H +#define BT_COLLISION_WORLD_IMPORTER_H + +#include "LinearMath/btTransform.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btHashMap.h" + +class btCollisionShape; +class btCollisionObject; +struct btBulletSerializedArrays; + + +struct ConstraintInput; +class btCollisionWorld; +struct btCollisionShapeData; +class btTriangleIndexVertexArray; +class btStridingMeshInterface; +struct btStridingMeshInterfaceData; +class btGImpactMeshShape; +class btOptimizedBvh; +struct btTriangleInfoMap; +class btBvhTriangleMeshShape; +class btPoint2PointConstraint; +class btHingeConstraint; +class btConeTwistConstraint; +class btGeneric6DofConstraint; +class btGeneric6DofSpringConstraint; +class btSliderConstraint; +class btGearConstraint; +struct btContactSolverInfo; + + + + +class btCollisionWorldImporter +{ +protected: + btCollisionWorld* m_collisionWorld; + + int m_verboseMode; + + btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes; + btAlignedObjectArray<btCollisionObject*> m_allocatedRigidBodies; + + btAlignedObjectArray<btOptimizedBvh*> m_allocatedBvhs; + btAlignedObjectArray<btTriangleInfoMap*> m_allocatedTriangleInfoMaps; + btAlignedObjectArray<btTriangleIndexVertexArray*> m_allocatedTriangleIndexArrays; + btAlignedObjectArray<btStridingMeshInterfaceData*> m_allocatedbtStridingMeshInterfaceDatas; + btAlignedObjectArray<btCollisionObject*> m_allocatedCollisionObjects; + + + btAlignedObjectArray<char*> m_allocatedNames; + + btAlignedObjectArray<int*> m_indexArrays; + btAlignedObjectArray<short int*> m_shortIndexArrays; + btAlignedObjectArray<unsigned char*> m_charIndexArrays; + + btAlignedObjectArray<btVector3FloatData*> m_floatVertexArrays; + btAlignedObjectArray<btVector3DoubleData*> m_doubleVertexArrays; + + + btHashMap<btHashPtr,btOptimizedBvh*> m_bvhMap; + btHashMap<btHashPtr,btTriangleInfoMap*> m_timMap; + + btHashMap<btHashString,btCollisionShape*> m_nameShapeMap; + btHashMap<btHashString,btCollisionObject*> m_nameColObjMap; + + btHashMap<btHashPtr,const char*> m_objectNameMap; + + btHashMap<btHashPtr,btCollisionShape*> m_shapeMap; + btHashMap<btHashPtr,btCollisionObject*> m_bodyMap; + + + //methods + + + + char* duplicateName(const char* name); + + btCollisionShape* convertCollisionShape( btCollisionShapeData* shapeData ); + + +public: + + btCollisionWorldImporter(btCollisionWorld* world); + + virtual ~btCollisionWorldImporter(); + + bool convertAllObjects( btBulletSerializedArrays* arrays); + + ///delete all memory collision shapes, rigid bodies, constraints etc. allocated during the load. + ///make sure you don't use the dynamics world containing objects after you call this method + virtual void deleteAllData(); + + void setVerboseMode(int verboseMode) + { + m_verboseMode = verboseMode; + } + + int getVerboseMode() const + { + return m_verboseMode; + } + + // query for data + int getNumCollisionShapes() const; + btCollisionShape* getCollisionShapeByIndex(int index); + int getNumRigidBodies() const; + btCollisionObject* getRigidBodyByIndex(int index) const; + + int getNumBvhs() const; + btOptimizedBvh* getBvhByIndex(int index) const; + int getNumTriangleInfoMaps() const; + btTriangleInfoMap* getTriangleInfoMapByIndex(int index) const; + + // queris involving named objects + btCollisionShape* getCollisionShapeByName(const char* name); + btCollisionObject* getCollisionObjectByName(const char* name); + + + const char* getNameForPointer(const void* ptr) const; + + ///those virtuals are called by load and can be overridden by the user + + + + //bodies + + virtual btCollisionObject* createCollisionObject( const btTransform& startTransform, btCollisionShape* shape,const char* bodyName); + + ///shapes + + virtual btCollisionShape* createPlaneShape(const btVector3& planeNormal,btScalar planeConstant); + virtual btCollisionShape* createBoxShape(const btVector3& halfExtents); + virtual btCollisionShape* createSphereShape(btScalar radius); + virtual btCollisionShape* createCapsuleShapeX(btScalar radius, btScalar height); + virtual btCollisionShape* createCapsuleShapeY(btScalar radius, btScalar height); + virtual btCollisionShape* createCapsuleShapeZ(btScalar radius, btScalar height); + + virtual btCollisionShape* createCylinderShapeX(btScalar radius,btScalar height); + virtual btCollisionShape* createCylinderShapeY(btScalar radius,btScalar height); + virtual btCollisionShape* createCylinderShapeZ(btScalar radius,btScalar height); + virtual btCollisionShape* createConeShapeX(btScalar radius,btScalar height); + virtual btCollisionShape* createConeShapeY(btScalar radius,btScalar height); + virtual btCollisionShape* createConeShapeZ(btScalar radius,btScalar height); + virtual class btTriangleIndexVertexArray* createTriangleMeshContainer(); + virtual btBvhTriangleMeshShape* createBvhTriangleMeshShape(btStridingMeshInterface* trimesh, btOptimizedBvh* bvh); + virtual btCollisionShape* createConvexTriangleMeshShape(btStridingMeshInterface* trimesh); +#ifdef SUPPORT_GIMPACT_SHAPE_IMPORT + virtual btGImpactMeshShape* createGimpactShape(btStridingMeshInterface* trimesh); +#endif //SUPPORT_GIMPACT_SHAPE_IMPORT + virtual btStridingMeshInterfaceData* createStridingMeshInterfaceData(btStridingMeshInterfaceData* interfaceData); + + virtual class btConvexHullShape* createConvexHullShape(); + virtual class btCompoundShape* createCompoundShape(); + virtual class btScaledBvhTriangleMeshShape* createScaledTrangleMeshShape(btBvhTriangleMeshShape* meshShape,const btVector3& localScalingbtBvhTriangleMeshShape); + + virtual class btMultiSphereShape* createMultiSphereShape(const btVector3* positions,const btScalar* radi,int numSpheres); + + virtual btTriangleIndexVertexArray* createMeshInterface(btStridingMeshInterfaceData& meshData); + + ///acceleration and connectivity structures + virtual btOptimizedBvh* createOptimizedBvh(); + virtual btTriangleInfoMap* createTriangleInfoMap(); + + + + +}; + + +#endif //BT_WORLD_IMPORTER_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp new file mode 100644 index 0000000000..7f4dea1c6d --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -0,0 +1,402 @@ +/* +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 "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/BroadphaseCollision/btDbvt.h" +#include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btAabbUtil2.h" +#include "btManifoldResult.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + +btShapePairCallback gCompoundChildShapePairCallback = 0; + +btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) +:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), +m_isSwapped(isSwapped), +m_sharedManifold(ci.m_manifold) +{ + m_ownsManifold = false; + + const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; + btAssert (colObjWrap->getCollisionShape()->isCompound()); + + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape()); + m_compoundShapeRevision = compoundShape->getUpdateRevision(); + + + preallocateChildAlgorithms(body0Wrap,body1Wrap); +} + +void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) +{ + const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap; + btAssert (colObjWrap->getCollisionShape()->isCompound()); + + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape()); + + int numChildren = compoundShape->getNumChildShapes(); + int i; + + m_childCollisionAlgorithms.resize(numChildren); + for (i=0;i<numChildren;i++) + { + if (compoundShape->getDynamicAabbTree()) + { + m_childCollisionAlgorithms[i] = 0; + } else + { + + const btCollisionShape* childShape = compoundShape->getChildShape(i); + + btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully) + m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + + + btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsContact; + btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithmsClosestPoints; + + + } + } +} + +void btCompoundCollisionAlgorithm::removeChildAlgorithms() +{ + int numChildren = m_childCollisionAlgorithms.size(); + int i; + for (i=0;i<numChildren;i++) + { + if (m_childCollisionAlgorithms[i]) + { + m_childCollisionAlgorithms[i]->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); + } + } +} + +btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() +{ + removeChildAlgorithms(); +} + + + + +struct btCompoundLeafCallback : btDbvt::ICollide +{ + +public: + + const btCollisionObjectWrapper* m_compoundColObjWrap; + const btCollisionObjectWrapper* m_otherObjWrap; + btDispatcher* m_dispatcher; + const btDispatcherInfo& m_dispatchInfo; + btManifoldResult* m_resultOut; + btCollisionAlgorithm** m_childCollisionAlgorithms; + btPersistentManifold* m_sharedManifold; + + btCompoundLeafCallback (const btCollisionObjectWrapper* compoundObjWrap,const btCollisionObjectWrapper* otherObjWrap,btDispatcher* dispatcher,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut,btCollisionAlgorithm** childCollisionAlgorithms,btPersistentManifold* sharedManifold) + :m_compoundColObjWrap(compoundObjWrap),m_otherObjWrap(otherObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), + m_childCollisionAlgorithms(childCollisionAlgorithms), + m_sharedManifold(sharedManifold) + { + + } + + + void ProcessChildShape(const btCollisionShape* childShape,int index) + { + btAssert(index>=0); + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape()); + btAssert(index<compoundShape->getNumChildShapes()); + + + //backup + btTransform orgTrans = m_compoundColObjWrap->getWorldTransform(); + + const btTransform& childTrans = compoundShape->getChildTransform(index); + btTransform newChildWorldTrans = orgTrans*childTrans ; + + //perform an AABB check first + btVector3 aabbMin0,aabbMax0; + childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); + + btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold); + aabbMin0 -= extendAabb; + aabbMax0 += extendAabb; + + btVector3 aabbMin1, aabbMax1; + m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); + + if (gCompoundChildShapePairCallback) + { + if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape)) + return; + } + + if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + + btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index); + + btCollisionAlgorithm* algo = 0; + + if (m_resultOut->m_closestPointDistanceThreshold > 0) + { + algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); + } + else + { + //the contactpoint is still projected back using the original inverted worldtrans + if (!m_childCollisionAlgorithms[index]) + { + m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + } + algo = m_childCollisionAlgorithms[index]; + } + + const btCollisionObjectWrapper* tmpWrap = 0; + + ///detect swapping case + if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject()) + { + tmpWrap = m_resultOut->getBody0Wrap(); + m_resultOut->setBody0Wrap(&compoundWrap); + m_resultOut->setShapeIdentifiersA(-1,index); + } else + { + tmpWrap = m_resultOut->getBody1Wrap(); + m_resultOut->setBody1Wrap(&compoundWrap); + m_resultOut->setShapeIdentifiersB(-1,index); + } + + algo->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut); + +#if 0 + if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + btVector3 worldAabbMin,worldAabbMax; + m_dispatchInfo.m_debugDraw->drawAabb(aabbMin0,aabbMax0,btVector3(1,1,1)); + m_dispatchInfo.m_debugDraw->drawAabb(aabbMin1,aabbMax1,btVector3(1,1,1)); + } +#endif + + if (m_resultOut->getBody0Internal() == m_compoundColObjWrap->getCollisionObject()) + { + m_resultOut->setBody0Wrap(tmpWrap); + } else + { + m_resultOut->setBody1Wrap(tmpWrap); + } + + } + } + void Process(const btDbvtNode* leaf) + { + int index = leaf->dataAsInt; + + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(m_compoundColObjWrap->getCollisionShape()); + const btCollisionShape* childShape = compoundShape->getChildShape(index); + +#if 0 + if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + btVector3 worldAabbMin,worldAabbMax; + btTransform orgTrans = m_compoundColObjWrap->getWorldTransform(); + btTransformAabb(leaf->volume.Mins(),leaf->volume.Maxs(),0.,orgTrans,worldAabbMin,worldAabbMax); + m_dispatchInfo.m_debugDraw->drawAabb(worldAabbMin,worldAabbMax,btVector3(1,0,0)); + } +#endif + + ProcessChildShape(childShape,index); + + } +}; + + + + + + +void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + const btCollisionObjectWrapper* colObjWrap = m_isSwapped? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* otherObjWrap = m_isSwapped? body0Wrap : body1Wrap; + + btAssert (colObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(colObjWrap->getCollisionShape()); + + ///btCompoundShape might have changed: + ////make sure the internal child collision algorithm caches are still valid + if (compoundShape->getUpdateRevision() != m_compoundShapeRevision) + { + ///clear and update all + removeChildAlgorithms(); + + preallocateChildAlgorithms(body0Wrap,body1Wrap); + m_compoundShapeRevision = compoundShape->getUpdateRevision(); + } + + if (m_childCollisionAlgorithms.size()==0) + return; + + const btDbvt* tree = compoundShape->getDynamicAabbTree(); + //use a dynamic aabb tree to cull potential child-overlaps + btCompoundLeafCallback callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold); + + ///we need to refresh all contact manifolds + ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep + ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm + { + int i; + manifoldArray.resize(0); + for (i=0;i<m_childCollisionAlgorithms.size();i++) + { + if (m_childCollisionAlgorithms[i]) + { + m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray); + for (int m=0;m<manifoldArray.size();m++) + { + if (manifoldArray[m]->getNumContacts()) + { + resultOut->setPersistentManifold(manifoldArray[m]); + resultOut->refreshContactPoints(); + resultOut->setPersistentManifold(0);//??necessary? + } + } + manifoldArray.resize(0); + } + } + } + + if (tree) + { + + btVector3 localAabbMin,localAabbMax; + btTransform otherInCompoundSpace; + otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform(); + otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax); + btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold); + localAabbMin -= extraExtends; + localAabbMax += extraExtends; + + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); + //process all children, that overlap with the given AABB bounds + tree->collideTVNoStackAlloc(tree->m_root,bounds,stack2,callback); + + } else + { + //iterate over all children, perform an AABB check inside ProcessChildShape + int numChildren = m_childCollisionAlgorithms.size(); + int i; + for (i=0;i<numChildren;i++) + { + callback.ProcessChildShape(compoundShape->getChildShape(i),i); + } + } + + { + //iterate over all children, perform an AABB check inside ProcessChildShape + int numChildren = m_childCollisionAlgorithms.size(); + int i; + manifoldArray.resize(0); + const btCollisionShape* childShape = 0; + btTransform orgTrans; + + btTransform newChildWorldTrans; + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + + for (i=0;i<numChildren;i++) + { + if (m_childCollisionAlgorithms[i]) + { + childShape = compoundShape->getChildShape(i); + //if not longer overlapping, remove the algorithm + orgTrans = colObjWrap->getWorldTransform(); + + const btTransform& childTrans = compoundShape->getChildTransform(i); + newChildWorldTrans = orgTrans*childTrans ; + + //perform an AABB check first + childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); + otherObjWrap->getCollisionShape()->getAabb(otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); + + if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + m_childCollisionAlgorithms[i]->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); + m_childCollisionAlgorithms[i] = 0; + } + } + } + } +} + +btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + btAssert(0); + //needs to be fixed, using btCollisionObjectWrapper and NOT modifying internal data structures + btCollisionObject* colObj = m_isSwapped? body1 : body0; + btCollisionObject* otherObj = m_isSwapped? body0 : body1; + + btAssert (colObj->getCollisionShape()->isCompound()); + + btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape()); + + //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps + //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals + //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: + //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 + //then use each overlapping node AABB against Tree0 + //and vise versa. + + btScalar hitFraction = btScalar(1.); + + int numChildren = m_childCollisionAlgorithms.size(); + int i; + btTransform orgTrans; + btScalar frac; + for (i=0;i<numChildren;i++) + { + //btCollisionShape* childShape = compoundShape->getChildShape(i); + + //backup + orgTrans = colObj->getWorldTransform(); + + const btTransform& childTrans = compoundShape->getChildTransform(i); + //btTransform newChildWorldTrans = orgTrans*childTrans ; + colObj->setWorldTransform( orgTrans*childTrans ); + + //btCollisionShape* tmpShape = colObj->getCollisionShape(); + //colObj->internalSetTemporaryCollisionShape( childShape ); + frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); + if (frac<hitFraction) + { + hitFraction = frac; + } + //revert back + //colObj->internalSetTemporaryCollisionShape( tmpShape); + colObj->setWorldTransform( orgTrans); + } + return hitFraction; + +} + + + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h new file mode 100644 index 0000000000..d2086fbc02 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -0,0 +1,104 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +*/ + +#ifndef BT_COMPOUND_COLLISION_ALGORITHM_H +#define BT_COMPOUND_COLLISION_ALGORITHM_H + +#include "btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" + +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +class btDispatcher; +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "btCollisionCreateFunc.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/BroadphaseCollision/btDbvt.h" +class btDispatcher; +class btCollisionObject; + +class btCollisionShape; +typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1); +extern btShapePairCallback gCompoundChildShapePairCallback; + +/// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes +class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + btNodeStack stack2; + btManifoldArray manifoldArray; + +protected: + btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms; + bool m_isSwapped; + + class btPersistentManifold* m_sharedManifold; + bool m_ownsManifold; + + + int m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated + + void removeChildAlgorithms(); + + void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + +public: + + btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + + virtual ~btCompoundCollisionAlgorithm(); + + btCollisionAlgorithm* getChildAlgorithm (int n) const + { + return m_childCollisionAlgorithms[n]; + } + + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + int i; + for (i=0;i<m_childCollisionAlgorithms.size();i++) + { + if (m_childCollisionAlgorithms[i]) + m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray); + } + } + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm)); + return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + } + }; + + struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm)); + return new(mem) btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + } + }; + +}; + +#endif //BT_COMPOUND_COLLISION_ALGORITHM_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp new file mode 100644 index 0000000000..d4a1aa78e4 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -0,0 +1,456 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +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 "btCompoundCompoundCollisionAlgorithm.h" +#include "LinearMath/btQuickprof.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/BroadphaseCollision/btDbvt.h" +#include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btAabbUtil2.h" +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + +//USE_LOCAL_STACK will avoid most (often all) dynamic memory allocations due to resizing in processCollision and MycollideTT +#define USE_LOCAL_STACK 1 + +btShapePairCallback gCompoundCompoundChildShapePairCallback = 0; + +btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) +:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped) +{ + + void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16); + m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache(); + + const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; + btAssert (col0ObjWrap->getCollisionShape()->isCompound()); + + const btCollisionObjectWrapper* col1ObjWrap = body1Wrap; + btAssert (col1ObjWrap->getCollisionShape()->isCompound()); + + const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape()); + m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); + + const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape()); + m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); + + +} + + +btCompoundCompoundCollisionAlgorithm::~btCompoundCompoundCollisionAlgorithm() +{ + removeChildAlgorithms(); + m_childCollisionAlgorithmCache->~btHashedSimplePairCache(); + btAlignedFree(m_childCollisionAlgorithmCache); +} + +void btCompoundCompoundCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +{ + int i; + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + for (i=0;i<pairs.size();i++) + { + if (pairs[i].m_userPointer) + { + + ((btCollisionAlgorithm*)pairs[i].m_userPointer)->getAllContactManifolds(manifoldArray); + } + } +} + + +void btCompoundCompoundCollisionAlgorithm::removeChildAlgorithms() +{ + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + + int numChildren = pairs.size(); + int i; + for (i=0;i<numChildren;i++) + { + if (pairs[i].m_userPointer) + { + btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer; + algo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algo); + } + } + m_childCollisionAlgorithmCache->removeAllPairs(); +} + +struct btCompoundCompoundLeafCallback : btDbvt::ICollide +{ + int m_numOverlapPairs; + + + const btCollisionObjectWrapper* m_compound0ColObjWrap; + const btCollisionObjectWrapper* m_compound1ColObjWrap; + btDispatcher* m_dispatcher; + const btDispatcherInfo& m_dispatchInfo; + btManifoldResult* m_resultOut; + + + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; + + btPersistentManifold* m_sharedManifold; + + btCompoundCompoundLeafCallback (const btCollisionObjectWrapper* compound1ObjWrap, + const btCollisionObjectWrapper* compound0ObjWrap, + btDispatcher* dispatcher, + const btDispatcherInfo& dispatchInfo, + btManifoldResult* resultOut, + btHashedSimplePairCache* childAlgorithmsCache, + btPersistentManifold* sharedManifold) + :m_numOverlapPairs(0),m_compound0ColObjWrap(compound1ObjWrap),m_compound1ColObjWrap(compound0ObjWrap),m_dispatcher(dispatcher),m_dispatchInfo(dispatchInfo),m_resultOut(resultOut), + m_childCollisionAlgorithmCache(childAlgorithmsCache), + m_sharedManifold(sharedManifold) + { + + } + + + + + void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1) + { + BT_PROFILE("btCompoundCompoundLeafCallback::Process"); + m_numOverlapPairs++; + + + int childIndex0 = leaf0->dataAsInt; + int childIndex1 = leaf1->dataAsInt; + + + btAssert(childIndex0>=0); + btAssert(childIndex1>=0); + + + const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(m_compound0ColObjWrap->getCollisionShape()); + btAssert(childIndex0<compoundShape0->getNumChildShapes()); + + const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(m_compound1ColObjWrap->getCollisionShape()); + btAssert(childIndex1<compoundShape1->getNumChildShapes()); + + const btCollisionShape* childShape0 = compoundShape0->getChildShape(childIndex0); + const btCollisionShape* childShape1 = compoundShape1->getChildShape(childIndex1); + + //backup + btTransform orgTrans0 = m_compound0ColObjWrap->getWorldTransform(); + const btTransform& childTrans0 = compoundShape0->getChildTransform(childIndex0); + btTransform newChildWorldTrans0 = orgTrans0*childTrans0 ; + + btTransform orgTrans1 = m_compound1ColObjWrap->getWorldTransform(); + const btTransform& childTrans1 = compoundShape1->getChildTransform(childIndex1); + btTransform newChildWorldTrans1 = orgTrans1*childTrans1 ; + + + //perform an AABB check first + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); + childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); + + btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold); + + aabbMin0 -= thresholdVec; + aabbMax0 += thresholdVec; + + if (gCompoundCompoundChildShapePairCallback) + { + if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1)) + return; + } + + if (TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + btCollisionObjectWrapper compoundWrap0(this->m_compound0ColObjWrap,childShape0, m_compound0ColObjWrap->getCollisionObject(),newChildWorldTrans0,-1,childIndex0); + btCollisionObjectWrapper compoundWrap1(this->m_compound1ColObjWrap,childShape1,m_compound1ColObjWrap->getCollisionObject(),newChildWorldTrans1,-1,childIndex1); + + + btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1); + + btCollisionAlgorithm* colAlgo = 0; + if (m_resultOut->m_closestPointDistanceThreshold > 0) + { + colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS); + } + else + { + if (pair) + { + colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; + + } + else + { + colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1); + btAssert(pair); + pair->m_userPointer = colAlgo; + } + } + + btAssert(colAlgo); + + const btCollisionObjectWrapper* tmpWrap0 = 0; + const btCollisionObjectWrapper* tmpWrap1 = 0; + + tmpWrap0 = m_resultOut->getBody0Wrap(); + tmpWrap1 = m_resultOut->getBody1Wrap(); + + m_resultOut->setBody0Wrap(&compoundWrap0); + m_resultOut->setBody1Wrap(&compoundWrap1); + + m_resultOut->setShapeIdentifiersA(-1,childIndex0); + m_resultOut->setShapeIdentifiersB(-1,childIndex1); + + + colAlgo->processCollision(&compoundWrap0,&compoundWrap1,m_dispatchInfo,m_resultOut); + + m_resultOut->setBody0Wrap(tmpWrap0); + m_resultOut->setBody1Wrap(tmpWrap1); + + + + } + } +}; + + +static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold) +{ + btVector3 newmin,newmax; + btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax); + newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold); + newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold); + btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax); + return Intersect(a,newb); +} + + +static inline void MycollideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold) +{ + + if(root0&&root1) + { + int depth=1; + int treshold=btDbvt::DOUBLE_STACKSIZE-4; + btAlignedObjectArray<btDbvt::sStkNN> stkStack; +#ifdef USE_LOCAL_STACK + ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]); + stkStack.initializeFromBuffer(&localStack,btDbvt::DOUBLE_STACKSIZE,btDbvt::DOUBLE_STACKSIZE); +#else + stkStack.resize(btDbvt::DOUBLE_STACKSIZE); +#endif + stkStack[0]=btDbvt::sStkNN(root0,root1); + do { + btDbvt::sStkNN p=stkStack[--depth]; + if(MyIntersect(p.a->volume,p.b->volume,xform, distanceThreshold)) + { + if(depth>treshold) + { + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=btDbvt::sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=btDbvt::sStkNN(p.a,p.b->childs[1]); + } + else + { + callback->Process(p.a,p.b); + } + } + } + } while(depth); + } +} + +void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + const btCollisionObjectWrapper* col0ObjWrap = body0Wrap; + const btCollisionObjectWrapper* col1ObjWrap= body1Wrap; + + btAssert (col0ObjWrap->getCollisionShape()->isCompound()); + btAssert (col1ObjWrap->getCollisionShape()->isCompound()); + const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape()); + const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape()); + + const btDbvt* tree0 = compoundShape0->getDynamicAabbTree(); + const btDbvt* tree1 = compoundShape1->getDynamicAabbTree(); + if (!tree0 || !tree1) + { + return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut); + } + ///btCompoundShape might have changed: + ////make sure the internal child collision algorithm caches are still valid + if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1)) + { + ///clear all + removeChildAlgorithms(); + m_compoundShapeRevision0 = compoundShape0->getUpdateRevision(); + m_compoundShapeRevision1 = compoundShape1->getUpdateRevision(); + + } + + + ///we need to refresh all contact manifolds + ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep + ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm + { + int i; + btManifoldArray manifoldArray; +#ifdef USE_LOCAL_STACK + btPersistentManifold localManifolds[4]; + manifoldArray.initializeFromBuffer(&localManifolds,0,4); +#endif + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + for (i=0;i<pairs.size();i++) + { + if (pairs[i].m_userPointer) + { + btCollisionAlgorithm* algo = (btCollisionAlgorithm*) pairs[i].m_userPointer; + algo->getAllContactManifolds(manifoldArray); + for (int m=0;m<manifoldArray.size();m++) + { + if (manifoldArray[m]->getNumContacts()) + { + resultOut->setPersistentManifold(manifoldArray[m]); + resultOut->refreshContactPoints(); + resultOut->setPersistentManifold(0); + } + } + manifoldArray.resize(0); + } + } + } + + + + + btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold); + + + const btTransform xform=col0ObjWrap->getWorldTransform().inverse()*col1ObjWrap->getWorldTransform(); + MycollideTT(tree0->m_root,tree1->m_root,xform,&callback, resultOut->m_closestPointDistanceThreshold); + + //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs); + + //remove non-overlapping child pairs + + { + btAssert(m_removePairs.size()==0); + + //iterate over all children, perform an AABB check inside ProcessChildShape + btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); + + int i; + btManifoldArray manifoldArray; + + + + + + btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + + for (i=0;i<pairs.size();i++) + { + if (pairs[i].m_userPointer) + { + btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer; + + { + btTransform orgTrans0; + const btCollisionShape* childShape0 = 0; + + btTransform newChildWorldTrans0; + btTransform orgInterpolationTrans0; + childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA); + orgTrans0 = col0ObjWrap->getWorldTransform(); + orgInterpolationTrans0 = col0ObjWrap->getWorldTransform(); + const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA); + newChildWorldTrans0 = orgTrans0*childTrans0 ; + childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); + } + btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold); + aabbMin0 -= thresholdVec; + aabbMax0 += thresholdVec; + { + btTransform orgInterpolationTrans1; + const btCollisionShape* childShape1 = 0; + btTransform orgTrans1; + btTransform newChildWorldTrans1; + + childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB); + orgTrans1 = col1ObjWrap->getWorldTransform(); + orgInterpolationTrans1 = col1ObjWrap->getWorldTransform(); + const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB); + newChildWorldTrans1 = orgTrans1*childTrans1 ; + childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); + } + + aabbMin1 -= thresholdVec; + aabbMax1 += thresholdVec; + + if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) + { + algo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algo); + m_removePairs.push_back(btSimplePair(pairs[i].m_indexA,pairs[i].m_indexB)); + } + } + } + for (int i=0;i<m_removePairs.size();i++) + { + m_childCollisionAlgorithmCache->removeOverlappingPair(m_removePairs[i].m_indexA,m_removePairs[i].m_indexB); + } + m_removePairs.clear(); + } + +} + +btScalar btCompoundCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + btAssert(0); + return 0.f; + +} + + + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h new file mode 100644 index 0000000000..f29f7a709a --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h @@ -0,0 +1,87 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +*/ + +#ifndef BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H +#define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H + +#include "btCompoundCollisionAlgorithm.h" + +#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" + +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +class btDispatcher; +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/CollisionDispatch/btHashedSimplePairCache.h" +class btDispatcher; +class btCollisionObject; + +class btCollisionShape; + +/// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes +class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm +{ + + class btHashedSimplePairCache* m_childCollisionAlgorithmCache; + btSimplePairArray m_removePairs; + + + int m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated + int m_compoundShapeRevision1; + + void removeChildAlgorithms(); + +// void preallocateChildAlgorithms(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + +public: + + btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + + virtual ~btCompoundCompoundCollisionAlgorithm(); + + + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray); + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); + return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + } + }; + + struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCompoundCollisionAlgorithm)); + return new(mem) btCompoundCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + } + }; + +}; + +#endif //BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp new file mode 100644 index 0000000000..1cb3d2e7a1 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp @@ -0,0 +1,242 @@ +/* +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" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + +btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) +{ + m_simplexSolver = simplexSolver; + m_pdSolver = pdSolver; +} + +btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() +{ +} + +btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int /* numPerturbationIterations */, int /* minimumPointsPerturbationThreshold */) +: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), +m_simplexSolver(simplexSolver), +m_pdSolver(pdSolver), +m_ownManifold (false), +m_manifoldPtr(mf), +m_lowLevelOfDetail(false) +{ + (void)body0Wrap; + (void)body1Wrap; +} + + + + +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 (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + if (!m_manifoldPtr) + { + //swapped? + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_ownManifold = true; + } + resultOut->setPersistentManifold(m_manifoldPtr); + + //comment-out next line to test multi-contact generation + //resultOut->getPersistentManifold()->clearManifold(); + + + const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape()); + const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape()); + + btVector3 normalOnB; + btVector3 pointOnBWorld; + + { + + + 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_transformA = body0Wrap->getWorldTransform(); + input.m_transformB = body1Wrap->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/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h new file mode 100644 index 0000000000..24d1336778 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h @@ -0,0 +1,92 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H +#define BT_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; + +public: + + btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); + + + virtual ~btConvex2dConvex2dAlgorithm(); + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual 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, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm)); + return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + } + }; + + +}; + +#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp new file mode 100644 index 0000000000..39ff7934d9 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -0,0 +1,346 @@ +/* +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 "btConvexConcaveCollisionAlgorithm.h" +#include "LinearMath/btQuickprof.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionShapes/btConcaveShape.h" +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" +#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "LinearMath/btIDebugDraw.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + +btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) +: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), +m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped), +m_isSwapped(isSwapped) +{ +} + +btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm() +{ +} + +void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) +{ + if (m_btConvexTriangleCallback.m_manifoldPtr) + { + manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr); + } +} + + +btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped): + m_dispatcher(dispatcher), + m_dispatchInfoPtr(0) +{ + m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap; + m_triBodyWrap = isSwapped? body0Wrap:body1Wrap; + + // + // create the manifold from the dispatcher 'manifold pool' + // + m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject()); + + clearCache(); +} + +btConvexTriangleCallback::~btConvexTriangleCallback() +{ + clearCache(); + m_dispatcher->releaseManifold( m_manifoldPtr ); + +} + + +void btConvexTriangleCallback::clearCache() +{ + m_dispatcher->clearManifold(m_manifoldPtr); +} + + +void btConvexTriangleCallback::processTriangle(btVector3* triangle,int +partId, int triangleIndex) +{ + BT_PROFILE("btConvexTriangleCallback::processTriangle"); + + if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax)) + { + return; + } + + //just for debugging purposes + //printf("triangle %d",m_triangleCount++); + + + + btCollisionAlgorithmConstructionInfo ci; + ci.m_dispatcher1 = m_dispatcher; + + + +#if 0 + + ///debug drawing of the overlapping triangles + if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe )) + { + const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject()); + 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); + m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); + } +#endif + + if (m_convexBodyWrap->getCollisionShape()->isConvex()) + { + btTriangleShape tm(triangle[0],triangle[1],triangle[2]); + tm.setMargin(m_collisionMarginTriangle); + + + btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform? + btCollisionAlgorithm* colAlgo = 0; + + if (m_resultOut->m_closestPointDistanceThreshold > 0) + { + colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); + } + else + { + colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS); + } + const btCollisionObjectWrapper* tmpWrap = 0; + + if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) + { + tmpWrap = m_resultOut->getBody0Wrap(); + m_resultOut->setBody0Wrap(&triObWrap); + m_resultOut->setShapeIdentifiersA(partId,triangleIndex); + } + else + { + tmpWrap = m_resultOut->getBody1Wrap(); + m_resultOut->setBody1Wrap(&triObWrap); + m_resultOut->setShapeIdentifiersB(partId,triangleIndex); + } + + colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut); + + if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) + { + m_resultOut->setBody0Wrap(tmpWrap); + } else + { + m_resultOut->setBody1Wrap(tmpWrap); + } + + + + colAlgo->~btCollisionAlgorithm(); + ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); + } + +} + + + +void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut) +{ + m_convexBodyWrap = convexBodyWrap; + m_triBodyWrap = triBodyWrap; + + m_dispatchInfoPtr = &dispatchInfo; + m_collisionMarginTriangle = collisionMarginTriangle; + m_resultOut = resultOut; + + //recalc aabbs + btTransform convexInTriangleSpace; + convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform(); + const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape()); + //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); + convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); + btScalar extraMargin = collisionMarginTriangle+ resultOut->m_closestPointDistanceThreshold; + + btVector3 extra(extraMargin,extraMargin,extraMargin); + + m_aabbMax += extra; + m_aabbMin -= extra; + +} + +void btConvexConcaveCollisionAlgorithm::clearCache() +{ + m_btConvexTriangleCallback.clearCache(); + +} + +void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision"); + + const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap; + + if (triBodyWrap->getCollisionShape()->isConcave()) + { + + + + const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape()); + + if (convexBodyWrap->getCollisionShape()->isConvex()) + { + btScalar collisionMarginTriangle = concaveShape->getMargin(); + + resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); + m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut); + + m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject()); + + concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax()); + + resultOut->refreshContactPoints(); + + m_btConvexTriangleCallback.clearWrapperData(); + + } + + } + +} + + +btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + btCollisionObject* convexbody = m_isSwapped ? body1 : body0; + btCollisionObject* triBody = m_isSwapped ? body0 : body1; + + + //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) + + //only perform CCD above a certain threshold, this prevents blocking on the long run + //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... + btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2(); + if (squareMot0 < convexbody->getCcdSquareMotionThreshold()) + { + return btScalar(1.); + } + + //const btVector3& from = convexbody->m_worldTransform.getOrigin(); + //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); + //todo: only do if the motion exceeds the 'radius' + + btTransform triInv = triBody->getWorldTransform().inverse(); + btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); + btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); + + struct LocalTriangleSphereCastCallback : public btTriangleCallback + { + btTransform m_ccdSphereFromTrans; + btTransform m_ccdSphereToTrans; + btTransform m_meshTransform; + + btScalar m_ccdSphereRadius; + btScalar m_hitFraction; + + + LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) + :m_ccdSphereFromTrans(from), + m_ccdSphereToTrans(to), + m_ccdSphereRadius(ccdSphereRadius), + m_hitFraction(hitFraction) + { + } + + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + BT_PROFILE("processTriangle"); + (void)partId; + (void)triangleIndex; + //do a swept sphere for now + btTransform ident; + ident.setIdentity(); + btConvexCast::CastResult castResult; + castResult.m_fraction = m_hitFraction; + btSphereShape pointShape(m_ccdSphereRadius); + btTriangleShape triShape(triangle[0],triangle[1],triangle[2]); + btVoronoiSimplexSolver simplexSolver; + btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); + //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); + //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); + //local space? + + if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, + ident,ident,castResult)) + { + if (m_hitFraction > castResult.m_fraction) + m_hitFraction = castResult.m_fraction; + } + + } + + }; + + + + + + if (triBody->getCollisionShape()->isConcave()) + { + btVector3 rayAabbMin = convexFromLocal.getOrigin(); + rayAabbMin.setMin(convexToLocal.getOrigin()); + btVector3 rayAabbMax = convexFromLocal.getOrigin(); + rayAabbMax.setMax(convexToLocal.getOrigin()); + btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); + rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); + rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); + + btScalar curHitFraction = btScalar(1.); //is this available? + LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, + convexbody->getCcdSweptSphereRadius(),curHitFraction); + + raycastCallback.m_hitFraction = convexbody->getHitFraction(); + + btCollisionObject* concavebody = triBody; + + btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); + + if (triangleMesh) + { + triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); + } + + + + if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) + { + convexbody->setHitFraction( raycastCallback.m_hitFraction); + return raycastCallback.m_hitFraction; + } + } + + return btScalar(1.); + +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h new file mode 100644 index 0000000000..93d842ef50 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -0,0 +1,127 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H +#define BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H + +#include "btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +class btDispatcher; +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "btCollisionCreateFunc.h" + +///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called. +ATTRIBUTE_ALIGNED16(class) btConvexTriangleCallback : public btTriangleCallback +{ + + btVector3 m_aabbMin; + btVector3 m_aabbMax ; + + const btCollisionObjectWrapper* m_convexBodyWrap; + const btCollisionObjectWrapper* m_triBodyWrap; + + + + btManifoldResult* m_resultOut; + btDispatcher* m_dispatcher; + const btDispatcherInfo* m_dispatchInfoPtr; + btScalar m_collisionMarginTriangle; + +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + +int m_triangleCount; + + btPersistentManifold* m_manifoldPtr; + + btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + + void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut); + + void clearWrapperData() + { + m_convexBodyWrap = 0; + m_triBodyWrap = 0; + } + virtual ~btConvexTriangleCallback(); + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); + + void clearCache(); + + SIMD_FORCE_INLINE const btVector3& getAabbMin() const + { + return m_aabbMin; + } + SIMD_FORCE_INLINE const btVector3& getAabbMax() const + { + return m_aabbMax; + } + +}; + + + + +/// btConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes. +ATTRIBUTE_ALIGNED16(class) btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + + btConvexTriangleCallback m_btConvexTriangleCallback; + + bool m_isSwapped; + + + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); + + virtual ~btConvexConcaveCollisionAlgorithm(); + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray); + + void clearCache(); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm)); + return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,false); + } + }; + + struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm)); + return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0Wrap,body1Wrap,true); + } + }; + +}; + +#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp new file mode 100644 index 0000000000..b54bd48932 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -0,0 +1,828 @@ +/* +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. +*/ + +///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance +///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums +///with reproduction case +//#define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1 +//#define ZERO_MARGIN + +#include "btConvexConvexAlgorithm.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/CollisionShapes/btTriangleShape.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" +#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + +/////////// + + + +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; +} + + + + + + + +////////// + + + + + +btConvexConvexAlgorithm::CreateFunc::CreateFunc(btConvexPenetrationDepthSolver* pdSolver) +{ + m_numPerturbationIterations = 0; + m_minimumPointsPerturbationThreshold = 3; + m_pdSolver = pdSolver; +} + +btConvexConvexAlgorithm::CreateFunc::~CreateFunc() +{ +} + +btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold) +: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), +m_pdSolver(pdSolver), +m_ownManifold (false), +m_manifoldPtr(mf), +m_lowLevelOfDetail(false), +#ifdef USE_SEPDISTANCE_UTIL2 +m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(), + (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()), +#endif +m_numPerturbationIterations(numPerturbationIterations), +m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) +{ + (void)body0Wrap; + (void)body1Wrap; +} + + + + +btConvexConvexAlgorithm::~btConvexConvexAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel) +{ + m_lowLevelOfDetail = useLowLevel; +} + + +struct btPerturbedContactResult : public btManifoldResult +{ + btManifoldResult* m_originalManifoldResult; + btTransform m_transformA; + btTransform m_transformB; + btTransform m_unPerturbedTransform; + bool m_perturbA; + btIDebugDraw* m_debugDrawer; + + + btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer) + :m_originalManifoldResult(originalResult), + m_transformA(transformA), + m_transformB(transformB), + m_unPerturbedTransform(unPerturbedTransform), + m_perturbA(perturbA), + m_debugDrawer(debugDrawer) + { + } + virtual ~ btPerturbedContactResult() + { + } + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth) + { + btVector3 endPt,startPt; + btScalar newDepth; + btVector3 newNormal; + + if (m_perturbA) + { + btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth; + endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg); + newDepth = (endPt - pointInWorld).dot(normalOnBInWorld); + startPt = endPt+normalOnBInWorld*newDepth; + } else + { + endPt = pointInWorld + normalOnBInWorld*orgDepth; + startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld); + newDepth = (endPt - startPt).dot(normalOnBInWorld); + + } + +//#define DEBUG_CONTACTS 1 +#ifdef DEBUG_CONTACTS + m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0)); + m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0)); + m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1)); +#endif //DEBUG_CONTACTS + + + m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth); + } + +}; + +extern btScalar gContactBreakingThreshold; + + +// +// Convex-Convex collision algorithm +// +void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + if (!m_manifoldPtr) + { + //swapped? + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_ownManifold = true; + } + resultOut->setPersistentManifold(m_manifoldPtr); + + //comment-out next line to test multi-contact generation + //resultOut->getPersistentManifold()->clearManifold(); + + + const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape()); + const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape()); + + btVector3 normalOnB; + btVector3 pointOnBWorld; +#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER + if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) + { + //m_manifoldPtr->clearManifold(); + + btCapsuleShape* capsuleA = (btCapsuleShape*) min0; + btCapsuleShape* capsuleB = (btCapsuleShape*) min1; + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), + capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(), + body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); + + if (dist<threshold) + { + btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); + } + resultOut->refreshContactPoints(); + return; + } + + if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == SPHERE_SHAPE_PROXYTYPE)) + { + //m_manifoldPtr->clearManifold(); + + btCapsuleShape* capsuleA = (btCapsuleShape*) min0; + btSphereShape* capsuleB = (btSphereShape*) min1; + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), + 0.,capsuleB->getRadius(),capsuleA->getUpAxis(),1, + body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); + + if (dist<threshold) + { + btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); + } + resultOut->refreshContactPoints(); + return; + } + + if ((min0->getShapeType() == SPHERE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) + { + //m_manifoldPtr->clearManifold(); + + btSphereShape* capsuleA = (btSphereShape*) min0; + btCapsuleShape* capsuleB = (btCapsuleShape*) min1; + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,0.,capsuleA->getRadius(), + capsuleB->getHalfHeight(),capsuleB->getRadius(),1,capsuleB->getUpAxis(), + body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); + + if (dist<threshold) + { + btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); + } + resultOut->refreshContactPoints(); + return; + } +#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER + + + + +#ifdef USE_SEPDISTANCE_UTIL2 + if (dispatchInfo.m_useConvexConservativeDistanceUtil) + { + m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform()); + } + + if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f) +#endif //USE_SEPDISTANCE_UTIL2 + + { + + + btGjkPairDetector::ClosestPointInput input; + btVoronoiSimplexSolver simplexSolver; + btGjkPairDetector gjkPairDetector( min0, min1, &simplexSolver, m_pdSolver ); + //TODO: if (dispatchInfo.m_useContinuous) + gjkPairDetector.setMinkowskiA(min0); + gjkPairDetector.setMinkowskiB(min1); + +#ifdef USE_SEPDISTANCE_UTIL2 + if (dispatchInfo.m_useConvexConservativeDistanceUtil) + { + input.m_maximumDistanceSquared = BT_LARGE_FLOAT; + } else +#endif //USE_SEPDISTANCE_UTIL2 + { + //if (dispatchInfo.m_convexMaxDistanceUseCPT) + //{ + // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); + //} else + //{ + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold()+resultOut->m_closestPointDistanceThreshold; +// } + + input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; + } + + input.m_transformA = body0Wrap->getWorldTransform(); + input.m_transformB = body1Wrap->getWorldTransform(); + + + + + +#ifdef USE_SEPDISTANCE_UTIL2 + btScalar sepDist = 0.f; + if (dispatchInfo.m_useConvexConservativeDistanceUtil) + { + sepDist = gjkPairDetector.getCachedSeparatingDistance(); + if (sepDist>SIMD_EPSILON) + { + sepDist += dispatchInfo.m_convexConservativeDistanceThreshold; + //now perturbe directions to get multiple contact points + + } + } +#endif //USE_SEPDISTANCE_UTIL2 + + if (min0->isPolyhedral() && min1->isPolyhedral()) + { + + + struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result + { + virtual void setShapeIdentifiersA(int partId0,int index0){} + virtual void setShapeIdentifiersB(int partId1,int index1){} + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + { + } + }; + + + struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result + { + btDiscreteCollisionDetectorInterface::Result* m_originalResult; + btVector3 m_reportedNormalOnWorld; + btScalar m_marginOnA; + btScalar m_marginOnB; + btScalar m_reportedDistance; + + bool m_foundResult; + btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB) + :m_originalResult(result), + m_marginOnA(marginOnA), + m_marginOnB(marginOnB), + m_foundResult(false) + { + } + + virtual void setShapeIdentifiersA(int partId0,int index0){} + virtual void setShapeIdentifiersB(int partId1,int index1){} + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg) + { + m_reportedDistance = depthOrg; + m_reportedNormalOnWorld = normalOnBInWorld; + + btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB; + m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB); + if (m_reportedDistance<0.f) + { + m_foundResult = true; + } + m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance); + } + }; + + + btDummyResult dummy; + +///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it + + btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin(); + btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin(); + + btWithoutMarginResult withoutMargin(resultOut, min0Margin,min1Margin); + + btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0; + btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1; + if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron()) + { + + + + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + + btScalar minDist = -1e30f; + btVector3 sepNormalWorldSpace; + bool foundSepAxis = true; + + if (dispatchInfo.m_enableSatConvex) + { + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), + body1Wrap->getWorldTransform(), + sepNormalWorldSpace,*resultOut); + } else + { +#ifdef ZERO_MARGIN + gjkPairDetector.setIgnoreMargin(true); + gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); +#else + + + gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw); + //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); +#endif //ZERO_MARGIN + //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + //if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); + minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin(); + +#ifdef ZERO_MARGIN + foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f; +#else + foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin()); +#endif + } + } + if (foundSepAxis) + { + +// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + + worldVertsB1.resize(0); + btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), + body1Wrap->getWorldTransform(), minDist-threshold, threshold, worldVertsB1,worldVertsB2, + *resultOut); + + } + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + return; + + } else + { + //we can also deal with convex versus triangle (without connectivity data) + if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) + { + + btVertexArray vertices; + btTriangleShape* tri = (btTriangleShape*)polyhedronB; + vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]); + vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]); + vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]); + + //tri->initializePolyhedralFeatures(); + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + + btVector3 sepNormalWorldSpace; + btScalar minDist =-1e30f; + btScalar maxDist = threshold; + + bool foundSepAxis = false; + if (0) + { + polyhedronB->initializePolyhedralFeatures(); + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), + body1Wrap->getWorldTransform(), + sepNormalWorldSpace,*resultOut); + // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + + } else + { +#ifdef ZERO_MARGIN + gjkPairDetector.setIgnoreMargin(true); + gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); +#else + gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); +#endif//ZERO_MARGIN + + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + //minDist = gjkPairDetector.getCachedSeparatingDistance(); + //maxDist = threshold; + minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); + foundSepAxis = true; + } + } + + + if (foundSepAxis) + { + worldVertsB2.resize(0); + btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut); + } + + + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + + return; + } + + } + + + } + + gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + + //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects + + //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points + if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold) + { + + int i; + btVector3 v0,v1; + btVector3 sepNormalWorldSpace; + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + + if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + + btPlaneSpace1(sepNormalWorldSpace,v0,v1); + + + bool perturbeA = true; + const btScalar angleLimit = 0.125f * SIMD_PI; + btScalar perturbeAngle; + btScalar radiusA = min0->getAngularMotionDisc(); + btScalar radiusB = min1->getAngularMotionDisc(); + if (radiusA < radiusB) + { + perturbeAngle = gContactBreakingThreshold /radiusA; + perturbeA = true; + } else + { + perturbeAngle = gContactBreakingThreshold / radiusB; + perturbeA = false; + } + if ( perturbeAngle > angleLimit ) + perturbeAngle = angleLimit; + + btTransform unPerturbedTransform; + if (perturbeA) + { + unPerturbedTransform = input.m_transformA; + } else + { + unPerturbedTransform = input.m_transformB; + } + + for ( i=0;i<m_numPerturbationIterations;i++) + { + if (v0.length2()>SIMD_EPSILON) + { + btQuaternion perturbeRot(v0,perturbeAngle); + btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); + btQuaternion rotq(sepNormalWorldSpace,iterationAngle); + + + if (perturbeA) + { + input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis()); + input.m_transformB = body1Wrap->getWorldTransform(); + #ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); + #endif //DEBUG_CONTACTS + } else + { + input.m_transformA = body0Wrap->getWorldTransform(); + input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis()); + #ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); + #endif + } + + btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); + gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); + } + } + } + } + + + +#ifdef USE_SEPDISTANCE_UTIL2 + if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON)) + { + m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform()); + } +#endif //USE_SEPDISTANCE_UTIL2 + + + } + + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + +} + + + +bool disableCcd = false; +btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold + + ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold + ///col0->m_worldTransform, + btScalar resultFraction = btScalar(1.); + + + btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); + btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); + + if (squareMot0 < col0->getCcdSquareMotionThreshold() && + squareMot1 < col1->getCcdSquareMotionThreshold()) + return resultFraction; + + if (disableCcd) + return btScalar(1.); + + + //An adhoc way of testing the Continuous Collision Detection algorithms + //One object is approximated as a sphere, to simplify things + //Starting in penetration should report no time of impact + //For proper CCD, better accuracy and handling of 'allowed' penetration should be added + //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) + + + /// Convex0 against sphere for Convex1 + { + btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape()); + + btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + 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/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h new file mode 100644 index 0000000000..cd75ba12d7 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -0,0 +1,109 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_CONVEX_CONVEX_ALGORITHM_H +#define BT_CONVEX_CONVEX_ALGORITHM_H + +#include "btActivatingCollisionAlgorithm.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "btCollisionCreateFunc.h" +#include "btCollisionDispatcher.h" +#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil +#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h" + +class btConvexPenetrationDepthSolver; + +///Enabling USE_SEPDISTANCE_UTIL2 requires 100% reliable distance computation. However, when using large size ratios GJK can be imprecise +///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 + +///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. +///This idea was described by Gino van den Bergen in this forum topic http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888 +class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm +{ +#ifdef USE_SEPDISTANCE_UTIL2 + btConvexSeparatingDistanceUtil m_sepDistance; +#endif + btConvexPenetrationDepthSolver* m_pdSolver; + + btVertexArray worldVertsB1; + btVertexArray worldVertsB2; + + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_lowLevelOfDetail; + + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + + + ///cache separating vector to speedup collision detection + + +public: + + btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); + + virtual ~btConvexConvexAlgorithm(); + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual 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; + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + + CreateFunc(btConvexPenetrationDepthSolver* pdSolver); + + virtual ~CreateFunc(); + + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm)); + return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + } + }; + + +}; + +#endif //BT_CONVEX_CONVEX_ALGORITHM_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp new file mode 100644 index 0000000000..cce2d95bcf --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -0,0 +1,174 @@ +/* +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 "btConvexPlaneCollisionAlgorithm.h" + +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + +//#include <stdio.h> + +btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold) +: btCollisionAlgorithm(ci), +m_ownManifold(false), +m_manifoldPtr(mf), +m_isSwapped(isSwapped), +m_numPerturbationIterations(numPerturbationIterations), +m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) +{ + const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? col1Wrap : col0Wrap; + const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? col0Wrap : col1Wrap; + + if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject())) + { + m_manifoldPtr = m_dispatcher->getNewManifold(convexObjWrap->getCollisionObject(),planeObjWrap->getCollisionObject()); + m_ownManifold = true; + } +} + + +btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap; + + btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape(); + + bool hasCollision = false; + const btVector3& planeNormal = planeShape->getPlaneNormal(); + const btScalar& planeConstant = planeShape->getPlaneConstant(); + + btTransform convexWorldTransform = convexObjWrap->getWorldTransform(); + btTransform convexInPlaneTrans; + convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexWorldTransform; + //now perturbe the convex-world transform + convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot); + btTransform planeInConvex; + planeInConvex= convexWorldTransform.inverse() * planeObjWrap->getWorldTransform(); + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + + btVector3 vtxInPlane = convexInPlaneTrans(vtx); + btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); + + btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected; + + hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); + resultOut->setPersistentManifold(m_manifoldPtr); + if (hasCollision) + { + /// report a contact. internally this will be kept persistent, and contact reduction is done + btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal; + btVector3 pOnB = vtxInPlaneWorld; + resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); + } +} + + +void btConvexPlaneCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)dispatchInfo; + if (!m_manifoldPtr) + return; + + const btCollisionObjectWrapper* convexObjWrap = m_isSwapped? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* planeObjWrap = m_isSwapped? body0Wrap: body1Wrap; + + btConvexShape* convexShape = (btConvexShape*) convexObjWrap->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObjWrap->getCollisionShape(); + + bool hasCollision = false; + const btVector3& planeNormal = planeShape->getPlaneNormal(); + const btScalar& planeConstant = planeShape->getPlaneConstant(); + btTransform planeInConvex; + planeInConvex= convexObjWrap->getWorldTransform().inverse() * planeObjWrap->getWorldTransform(); + btTransform convexInPlaneTrans; + convexInPlaneTrans= planeObjWrap->getWorldTransform().inverse() * convexObjWrap->getWorldTransform(); + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + btVector3 vtxInPlane = convexInPlaneTrans(vtx); + btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); + + btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected; + + hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); + resultOut->setPersistentManifold(m_manifoldPtr); + if (hasCollision) + { + /// report a contact. internally this will be kept persistent, and contact reduction is done + btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal; + btVector3 pOnB = vtxInPlaneWorld; + resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); + } + + //the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones: + //they keep on rolling forever because of the additional off-center contact points + //so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc) + if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold) + { + btVector3 v0,v1; + btPlaneSpace1(planeNormal,v0,v1); + //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects + + const btScalar angleLimit = 0.125f * SIMD_PI; + btScalar perturbeAngle; + btScalar radius = convexShape->getAngularMotionDisc(); + perturbeAngle = gContactBreakingThreshold / radius; + if ( perturbeAngle > angleLimit ) + perturbeAngle = angleLimit; + + btQuaternion perturbeRot(v0,perturbeAngle); + for (int i=0;i<m_numPerturbationIterations;i++) + { + btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); + btQuaternion rotq(planeNormal,iterationAngle); + collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0Wrap,body1Wrap,dispatchInfo,resultOut); + } + } + + if (m_ownManifold) + { + if (m_manifoldPtr->getNumContacts()) + { + resultOut->refreshContactPoints(); + } + } +} + +btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + + //not yet + return btScalar(1.); +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h new file mode 100644 index 0000000000..d28c430c4c --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -0,0 +1,84 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_CONVEX_PLANE_COLLISION_ALGORITHM_H +#define BT_CONVEX_PLANE_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; +#include "btCollisionDispatcher.h" + +#include "LinearMath/btVector3.h" + +/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_isSwapped; + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + +public: + + btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold); + + virtual ~btConvexPlaneCollisionAlgorithm(); + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + void collideSingleContact (const btQuaternion& perturbeRot, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + + CreateFunc() + : m_numPerturbationIterations(1), + m_minimumPointsPerturbationThreshold(0) + { + } + + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); + if (!m_swapped) + { + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + } else + { + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + } + } + }; + +}; + +#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp new file mode 100644 index 0000000000..f6e4e57b0a --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -0,0 +1,383 @@ +/* +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 "btDefaultCollisionConfiguration.h" + +#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h" + +#include "BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM +#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM +#include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" + + + +#include "LinearMath/btPoolAllocator.h" + + + + + +btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) +//btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* stackAlloc,btPoolAllocator* persistentManifoldPool,btPoolAllocator* collisionAlgorithmPool) +{ + + void* mem = NULL; + 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_pdSolver); + mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); + m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); + m_swappedConvexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; + mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::CreateFunc),16); + m_compoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btCompoundCompoundCollisionAlgorithm::CreateFunc),16); + m_compoundCompoundCreateFunc = new (mem)btCompoundCompoundCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btCompoundCollisionAlgorithm::SwappedCreateFunc),16); + m_swappedCompoundCreateFunc = new (mem)btCompoundCollisionAlgorithm::SwappedCreateFunc; + mem = btAlignedAlloc(sizeof(btEmptyAlgorithm::CreateFunc),16); + m_emptyCreateFunc = new(mem) btEmptyAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btSphereSphereCollisionAlgorithm::CreateFunc),16); + m_sphereSphereCF = new(mem) btSphereSphereCollisionAlgorithm::CreateFunc; +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); + m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereBoxCollisionAlgorithm::CreateFunc),16); + m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc; + m_boxSphereCF->m_swapped = true; +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); + m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc(sizeof(btSphereTriangleCollisionAlgorithm::CreateFunc),16); + m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; + m_triangleSphereCF->m_swapped = true; + + mem = btAlignedAlloc(sizeof(btBoxBoxCollisionAlgorithm::CreateFunc),16); + m_boxBoxCF = new(mem)btBoxBoxCollisionAlgorithm::CreateFunc; + + //convex versus plane + mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + m_convexPlaneCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; + mem = btAlignedAlloc (sizeof(btConvexPlaneCollisionAlgorithm::CreateFunc),16); + m_planeConvexCF = new (mem) btConvexPlaneCollisionAlgorithm::CreateFunc; + m_planeConvexCF->m_swapped = true; + + ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool + int maxSize = sizeof(btConvexConvexAlgorithm); + int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); + int maxSize3 = sizeof(btCompoundCollisionAlgorithm); + int maxSize4 = sizeof(btCompoundCompoundCollisionAlgorithm); + + int collisionAlgorithmMaxElementSize = btMax(maxSize,constructionInfo.m_customCollisionAlgorithmMaxElementSize); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4); + + if (constructionInfo.m_persistentManifoldPool) + { + m_ownsPersistentManifoldPool = false; + m_persistentManifoldPool = constructionInfo.m_persistentManifoldPool; + } else + { + m_ownsPersistentManifoldPool = true; + void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); + m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize); + } + + collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize+16)&0xffffffffffff0; + if (constructionInfo.m_collisionAlgorithmPool) + { + m_ownsCollisionAlgorithmPool = false; + m_collisionAlgorithmPool = constructionInfo.m_collisionAlgorithmPool; + } else + { + m_ownsCollisionAlgorithmPool = true; + void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); + m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); + } + + +} + +btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() +{ + if (m_ownsCollisionAlgorithmPool) + { + m_collisionAlgorithmPool->~btPoolAllocator(); + btAlignedFree(m_collisionAlgorithmPool); + } + if (m_ownsPersistentManifoldPool) + { + m_persistentManifoldPool->~btPoolAllocator(); + btAlignedFree(m_persistentManifoldPool); + } + + m_convexConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_convexConvexCreateFunc); + + m_convexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_convexConcaveCreateFunc); + m_swappedConvexConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_swappedConvexConcaveCreateFunc); + + m_compoundCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_compoundCreateFunc); + + m_compoundCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree(m_compoundCompoundCreateFunc); + + m_swappedCompoundCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_swappedCompoundCreateFunc); + + m_emptyCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_emptyCreateFunc); + + m_sphereSphereCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_sphereSphereCF); + +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + m_sphereBoxCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_sphereBoxCF); + m_boxSphereCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_boxSphereCF); +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + m_sphereTriangleCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_sphereTriangleCF); + m_triangleSphereCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_triangleSphereCF); + m_boxBoxCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_boxBoxCF); + + m_convexPlaneCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_convexPlaneCF); + m_planeConvexCF->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_planeConvexCF); + + m_pdSolver->~btConvexPenetrationDepthSolver(); + + btAlignedFree(m_pdSolver); + + +} + +btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) +{ + + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) + { + return m_sphereSphereCF; + } +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) + { + return m_sphereBoxCF; + } + + if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) + { + return m_boxSphereCF; + } +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE)) + { + return m_sphereTriangleCF; + } + + if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) + { + return m_triangleSphereCF; + } + + if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE)) + { + return m_convexPlaneCF; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE)) + { + return m_planeConvexCF; + } + + + + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) + { + return m_convexConvexCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1)) + { + return m_convexConcaveCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0)) + { + return m_swappedConvexConcaveCreateFunc; + } + + + if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) + { + return m_compoundCompoundCreateFunc; + } + + if (btBroadphaseProxy::isCompound(proxyType0)) + { + return m_compoundCreateFunc; + } + else + { + if (btBroadphaseProxy::isCompound(proxyType1)) + { + return m_swappedCompoundCreateFunc; + } + } + + //failed to find an algorithm + return m_emptyCreateFunc; + +} + +btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) +{ + + + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + { + return m_sphereSphereCF; + } +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE)) + { + return m_sphereBoxCF; + } + + if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + { + return m_boxSphereCF; + } +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE)) + { + return m_sphereTriangleCF; + } + + if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + { + return m_triangleSphereCF; + } + + if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) + { + return m_boxBoxCF; + } + + if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE)) + { + return m_convexPlaneCF; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE)) + { + return m_planeConvexCF; + } + + + + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) + { + return m_convexConvexCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1)) + { + return m_convexConcaveCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0)) + { + return m_swappedConvexConcaveCreateFunc; + } + + + if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) + { + return m_compoundCompoundCreateFunc; + } + + if (btBroadphaseProxy::isCompound(proxyType0)) + { + return m_compoundCreateFunc; + } else + { + if (btBroadphaseProxy::isCompound(proxyType1)) + { + return m_swappedCompoundCreateFunc; + } + } + + //failed to find an algorithm + return m_emptyCreateFunc; +} + +void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) +{ + btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc; + convexConvex->m_numPerturbationIterations = numPerturbationIterations; + convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; +} + +void btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) +{ + btConvexPlaneCollisionAlgorithm::CreateFunc* cpCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_convexPlaneCF; + cpCF->m_numPerturbationIterations = numPerturbationIterations; + cpCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; + + btConvexPlaneCollisionAlgorithm::CreateFunc* pcCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_planeConvexCF; + pcCF->m_numPerturbationIterations = numPerturbationIterations; + pcCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h new file mode 100644 index 0000000000..17c7596cff --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -0,0 +1,122 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_DEFAULT_COLLISION_CONFIGURATION +#define BT_DEFAULT_COLLISION_CONFIGURATION + +#include "btCollisionConfiguration.h" +class btVoronoiSimplexSolver; +class btConvexPenetrationDepthSolver; + +struct btDefaultCollisionConstructionInfo +{ + btPoolAllocator* m_persistentManifoldPool; + btPoolAllocator* m_collisionAlgorithmPool; + int m_defaultMaxPersistentManifoldPoolSize; + int m_defaultMaxCollisionAlgorithmPoolSize; + int m_customCollisionAlgorithmMaxElementSize; + int m_useEpaPenetrationAlgorithm; + + btDefaultCollisionConstructionInfo() + :m_persistentManifoldPool(0), + m_collisionAlgorithmPool(0), + m_defaultMaxPersistentManifoldPoolSize(4096), + m_defaultMaxCollisionAlgorithmPoolSize(4096), + m_customCollisionAlgorithmMaxElementSize(0), + m_useEpaPenetrationAlgorithm(true) + { + } +}; + + + +///btCollisionConfiguration allows to configure Bullet collision detection +///stack allocator, pool memory allocators +///@todo: describe the meaning +class btDefaultCollisionConfiguration : public btCollisionConfiguration +{ + +protected: + + int m_persistentManifoldPoolSize; + + + btPoolAllocator* m_persistentManifoldPool; + bool m_ownsPersistentManifoldPool; + + + btPoolAllocator* m_collisionAlgorithmPool; + bool m_ownsCollisionAlgorithmPool; + + //default penetration depth solver + btConvexPenetrationDepthSolver* m_pdSolver; + + //default CreationFunctions, filling the m_doubleDispatch table + btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc; + btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_compoundCompoundCreateFunc; + + btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; + btCollisionAlgorithmCreateFunc* m_sphereSphereCF; + btCollisionAlgorithmCreateFunc* m_sphereBoxCF; + btCollisionAlgorithmCreateFunc* m_boxSphereCF; + + btCollisionAlgorithmCreateFunc* m_boxBoxCF; + btCollisionAlgorithmCreateFunc* m_sphereTriangleCF; + btCollisionAlgorithmCreateFunc* m_triangleSphereCF; + btCollisionAlgorithmCreateFunc* m_planeConvexCF; + btCollisionAlgorithmCreateFunc* m_convexPlaneCF; + +public: + + + btDefaultCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); + + virtual ~btDefaultCollisionConfiguration(); + + ///memory pools + virtual btPoolAllocator* getPersistentManifoldPool() + { + return m_persistentManifoldPool; + } + + virtual btPoolAllocator* getCollisionAlgorithmPool() + { + return m_collisionAlgorithmPool; + } + + + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); + + virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1); + + ///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm. + ///By default, this feature is disabled for best performance. + ///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature. + ///@param minimumPointsPerturbationThreshold is the minimum number of points in the contact cache, above which the feature is disabled + ///3 is a good value for both params, if you want to enable the feature. This is because the default contact cache contains a maximum of 4 points, and one collision query at the unperturbed orientation is performed first. + ///See Bullet/Demos/CollisionDemo for an example how this feature gathers multiple points. + ///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection. + void setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); + + void setPlaneConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); + +}; + +#endif //BT_DEFAULT_COLLISION_CONFIGURATION + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp new file mode 100644 index 0000000000..5fa1c8be5e --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp @@ -0,0 +1,34 @@ +/* +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 "btEmptyCollisionAlgorithm.h" + + + +btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btCollisionAlgorithm(ci) +{ +} + +void btEmptyAlgorithm::processCollision (const btCollisionObjectWrapper* ,const btCollisionObjectWrapper* ,const btDispatcherInfo& ,btManifoldResult* ) +{ +} + +btScalar btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* ,btCollisionObject* ,const btDispatcherInfo& ,btManifoldResult* ) +{ + return btScalar(1.); +} + + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h new file mode 100644 index 0000000000..cb0f152183 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -0,0 +1,54 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_EMPTY_ALGORITH +#define BT_EMPTY_ALGORITH +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btCollisionCreateFunc.h" +#include "btCollisionDispatcher.h" + +#define ATTRIBUTE_ALIGNED(a) + +///EmptyAlgorithm is a stub for unsupported collision pairs. +///The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame. +class btEmptyAlgorithm : public btCollisionAlgorithm +{ + +public: + + btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci); + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + } + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + (void)body0Wrap; + (void)body1Wrap; + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm)); + return new(mem) btEmptyAlgorithm(ci); + } + }; + +} ATTRIBUTE_ALIGNED(16); + +#endif //BT_EMPTY_ALGORITH diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp new file mode 100644 index 0000000000..86141fa689 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp @@ -0,0 +1,171 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.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. +*/ + +#include "btGhostObject.h" +#include "btCollisionWorld.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "LinearMath/btAabbUtil2.h" + +btGhostObject::btGhostObject() +{ + m_internalType = CO_GHOST_OBJECT; +} + +btGhostObject::~btGhostObject() +{ + ///btGhostObject should have been removed from the world, so no overlapping objects + btAssert(!m_overlappingObjects.size()); +} + + +void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +{ + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btAssert(otherObject); + ///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (index==m_overlappingObjects.size()) + { + //not found + m_overlappingObjects.push_back(otherObject); + } +} + +void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy) +{ + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btAssert(otherObject); + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (index<m_overlappingObjects.size()) + { + m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1]; + m_overlappingObjects.pop_back(); + } +} + + +btPairCachingGhostObject::btPairCachingGhostObject() +{ + m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); +} + +btPairCachingGhostObject::~btPairCachingGhostObject() +{ + m_hashPairCache->~btHashedOverlappingPairCache(); + btAlignedFree( m_hashPairCache ); +} + +void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +{ + btBroadphaseProxy*actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle(); + btAssert(actualThisProxy); + + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btAssert(otherObject); + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (index==m_overlappingObjects.size()) + { + m_overlappingObjects.push_back(otherObject); + m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy); + } +} + +void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1) +{ + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle(); + btAssert(actualThisProxy); + + btAssert(otherObject); + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (index<m_overlappingObjects.size()) + { + m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1]; + m_overlappingObjects.pop_back(); + m_hashPairCache->removeOverlappingPair(actualThisProxy,otherProxy,dispatcher); + } +} + + +void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const +{ + btTransform convexFromTrans,convexToTrans; + convexFromTrans = convexFromWorld; + convexToTrans = convexToWorld; + btVector3 castShapeAabbMin, castShapeAabbMax; + /* Compute AABB that encompasses angular movement */ + { + btVector3 linVel, angVel; + btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); + btTransform R; + R.setIdentity (); + R.setRotation (convexFromTrans.getRotation()); + castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); + } + + /// go over all objects, and if the ray intersects their aabb + cast shape aabb, + // do a ray-shape query using convexCaster (CCD) + int i; + for (i=0;i<m_overlappingObjects.size();i++) + { + btCollisionObject* collisionObject= m_overlappingObjects[i]; + //only perform raycast if filterMask matches + if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); + AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 hitNormal; + if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + { + btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + allowedCcdPenetration); + } + } + } + +} + +void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const +{ + btTransform rayFromTrans; + rayFromTrans.setIdentity(); + rayFromTrans.setOrigin(rayFromWorld); + btTransform rayToTrans; + rayToTrans.setIdentity(); + rayToTrans.setOrigin(rayToWorld); + + + int i; + for (i=0;i<m_overlappingObjects.size();i++) + { + btCollisionObject* collisionObject= m_overlappingObjects[i]; + //only perform raycast if filterMask matches + if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { + btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback); + } + } +} + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.h new file mode 100644 index 0000000000..8ec8613857 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.h @@ -0,0 +1,175 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.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. +*/ + +#ifndef BT_GHOST_OBJECT_H +#define BT_GHOST_OBJECT_H + + +#include "btCollisionObject.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h" +#include "LinearMath/btAlignedAllocator.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "btCollisionWorld.h" + +class btConvexShape; + +class btDispatcher; + +///The btGhostObject can keep track of all objects that are overlapping +///By default, this overlap is based on the AABB +///This is useful for creating a character controller, collision sensors/triggers, explosions etc. +///We plan on adding rayTest and other queries for the btGhostObject +ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject +{ +protected: + + btAlignedObjectArray<btCollisionObject*> m_overlappingObjects; + +public: + + btGhostObject(); + + virtual ~btGhostObject(); + + void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const; + + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; + + ///this method is mainly for expert/internal use only. + virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); + ///this method is mainly for expert/internal use only. + virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + + int getNumOverlappingObjects() const + { + return m_overlappingObjects.size(); + } + + btCollisionObject* getOverlappingObject(int index) + { + return m_overlappingObjects[index]; + } + + const btCollisionObject* getOverlappingObject(int index) const + { + return m_overlappingObjects[index]; + } + + btAlignedObjectArray<btCollisionObject*>& getOverlappingPairs() + { + return m_overlappingObjects; + } + + const btAlignedObjectArray<btCollisionObject*> getOverlappingPairs() const + { + return m_overlappingObjects; + } + + // + // internal cast + // + + static const btGhostObject* upcast(const btCollisionObject* colObj) + { + if (colObj->getInternalType()==CO_GHOST_OBJECT) + return (const btGhostObject*)colObj; + return 0; + } + static btGhostObject* upcast(btCollisionObject* colObj) + { + if (colObj->getInternalType()==CO_GHOST_OBJECT) + return (btGhostObject*)colObj; + return 0; + } + +}; + +class btPairCachingGhostObject : public btGhostObject +{ + btHashedOverlappingPairCache* m_hashPairCache; + +public: + + btPairCachingGhostObject(); + + virtual ~btPairCachingGhostObject(); + + ///this method is mainly for expert/internal use only. + virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); + + virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + + btHashedOverlappingPairCache* getOverlappingPairCache() + { + return m_hashPairCache; + } + +}; + + + +///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject. +class btGhostPairCallback : public btOverlappingPairCallback +{ + +public: + btGhostPairCallback() + { + } + + virtual ~btGhostPairCallback() + { + + } + + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + { + btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; + btGhostObject* ghost0 = btGhostObject::upcast(colObj0); + btGhostObject* ghost1 = btGhostObject::upcast(colObj1); + if (ghost0) + ghost0->addOverlappingObjectInternal(proxy1, proxy0); + if (ghost1) + ghost1->addOverlappingObjectInternal(proxy0, proxy1); + return 0; + } + + virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) + { + btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; + btGhostObject* ghost0 = btGhostObject::upcast(colObj0); + btGhostObject* ghost1 = btGhostObject::upcast(colObj1); + if (ghost0) + ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0); + if (ghost1) + ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1); + return 0; + } + + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) + { + btAssert(0); + //need to keep track of all ghost objects and call them here + //m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher); + } + + + +}; + +#endif + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp new file mode 100644 index 0000000000..8c8a7c3c1e --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp @@ -0,0 +1,276 @@ +/* +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 "btHashedSimplePairCache.h" + + +#include <stdio.h> + +int gOverlappingSimplePairs = 0; +int gRemoveSimplePairs =0; +int gAddedSimplePairs =0; +int gFindSimplePairs =0; + + + + +btHashedSimplePairCache::btHashedSimplePairCache() { + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); + growTables(); +} + + + + +btHashedSimplePairCache::~btHashedSimplePairCache() +{ +} + + + + + + +void btHashedSimplePairCache::removeAllPairs() +{ + m_overlappingPairArray.clear(); + m_hashTable.clear(); + m_next.clear(); + + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); + growTables(); +} + + + +btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) +{ + gFindSimplePairs++; + + + /*if (indexA > indexB) + btSwap(indexA, indexB);*/ + + int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); + + if (hash >= m_hashTable.size()) + { + return NULL; + } + + int index = m_hashTable[hash]; + while (index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], indexA, indexB) == false) + { + index = m_next[index]; + } + + if (index == BT_SIMPLE_NULL_PAIR) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; +} + +//#include <stdio.h> + +void btHashedSimplePairCache::growTables() +{ + + int newCapacity = m_overlappingPairArray.capacity(); + + if (m_hashTable.size() < newCapacity) + { + //grow hashtable and next table + int curHashtableSize = m_hashTable.size(); + + m_hashTable.resize(newCapacity); + m_next.resize(newCapacity); + + + int i; + + for (i= 0; i < newCapacity; ++i) + { + m_hashTable[i] = BT_SIMPLE_NULL_PAIR; + } + for (i = 0; i < newCapacity; ++i) + { + m_next[i] = BT_SIMPLE_NULL_PAIR; + } + + for(i=0;i<curHashtableSize;i++) + { + + const btSimplePair& pair = m_overlappingPairArray[i]; + int indexA = pair.m_indexA; + int indexB = pair.m_indexB; + + int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + m_next[i] = m_hashTable[hashValue]; + m_hashTable[hashValue] = i; + } + + + } +} + +btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) +{ + + int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + + + btSimplePair* pair = internalFindPair(indexA, indexB, hash); + if (pair != NULL) + { + return pair; + } + + int count = m_overlappingPairArray.size(); + int oldCapacity = m_overlappingPairArray.capacity(); + void* mem = &m_overlappingPairArray.expandNonInitializing(); + + int newCapacity = m_overlappingPairArray.capacity(); + + if (oldCapacity < newCapacity) + { + growTables(); + //hash with new capacity + hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); + } + + pair = new (mem) btSimplePair(indexA,indexB); + + pair->m_userPointer = 0; + + m_next[count] = m_hashTable[hash]; + m_hashTable[hash] = count; + + return pair; +} + + + +void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) +{ + gRemoveSimplePairs++; + + + /*if (indexA > indexB) + btSwap(indexA, indexB);*/ + + int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA),static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity()-1)); + + btSimplePair* pair = internalFindPair(indexA, indexB, hash); + if (pair == NULL) + { + return 0; + } + + + void* userData = pair->m_userPointer; + + + int pairIndex = int(pair - &m_overlappingPairArray[0]); + btAssert(pairIndex < m_overlappingPairArray.size()); + + // Remove the pair from the hash table. + int index = m_hashTable[hash]; + btAssert(index != BT_SIMPLE_NULL_PAIR); + + int previous = BT_SIMPLE_NULL_PAIR; + while (index != pairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_SIMPLE_NULL_PAIR) + { + btAssert(m_next[previous] == pairIndex); + m_next[previous] = m_next[pairIndex]; + } + else + { + m_hashTable[hash] = m_next[pairIndex]; + } + + // We now move the last pair into spot of the + // pair being removed. We need to fix the hash + // table indices to support the move. + + int lastPairIndex = m_overlappingPairArray.size() - 1; + + // If the removed pair is the last pair, we are done. + if (lastPairIndex == pairIndex) + { + m_overlappingPairArray.pop_back(); + return userData; + } + + // Remove the last pair from the hash table. + const btSimplePair* last = &m_overlappingPairArray[lastPairIndex]; + /* missing swap here too, Nat. */ + int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_indexA), static_cast<unsigned int>(last->m_indexB)) & (m_overlappingPairArray.capacity()-1)); + + index = m_hashTable[lastHash]; + btAssert(index != BT_SIMPLE_NULL_PAIR); + + previous = BT_SIMPLE_NULL_PAIR; + while (index != lastPairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_SIMPLE_NULL_PAIR) + { + btAssert(m_next[previous] == lastPairIndex); + m_next[previous] = m_next[lastPairIndex]; + } + else + { + m_hashTable[lastHash] = m_next[lastPairIndex]; + } + + // Copy the last pair into the remove pair's spot. + m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex]; + + // Insert the last pair into the hash table + m_next[pairIndex] = m_hashTable[lastHash]; + m_hashTable[lastHash] = pairIndex; + + m_overlappingPairArray.pop_back(); + + return userData; +} +//#include <stdio.h> + + + + + + + + + + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h new file mode 100644 index 0000000000..2aaf6201f3 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -0,0 +1,172 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_HASHED_SIMPLE_PAIR_CACHE_H +#define BT_HASHED_SIMPLE_PAIR_CACHE_H + + + +#include "LinearMath/btAlignedObjectArray.h" + +const int BT_SIMPLE_NULL_PAIR=0xffffffff; + +struct btSimplePair +{ + btSimplePair(int indexA,int indexB) + :m_indexA(indexA), + m_indexB(indexB), + m_userPointer(0) + { + } + + int m_indexA; + int m_indexB; + union + { + void* m_userPointer; + int m_userValue; + }; +}; + +typedef btAlignedObjectArray<btSimplePair> btSimplePairArray; + + + +extern int gOverlappingSimplePairs; +extern int gRemoveSimplePairs; +extern int gAddedSimplePairs; +extern int gFindSimplePairs; + + + + +class btHashedSimplePairCache +{ + btSimplePairArray m_overlappingPairArray; + + +protected: + + btAlignedObjectArray<int> m_hashTable; + btAlignedObjectArray<int> m_next; + + +public: + btHashedSimplePairCache(); + virtual ~btHashedSimplePairCache(); + + void removeAllPairs(); + + virtual void* removeOverlappingPair(int indexA,int indexB); + + // Add a pair and return the new pair. If the pair already exists, + // no new pair is created and the old one is returned. + virtual btSimplePair* addOverlappingPair(int indexA,int indexB) + { + gAddedSimplePairs++; + + return internalAddPair(indexA,indexB); + } + + + virtual btSimplePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + + const btSimplePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + + btSimplePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const btSimplePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + + + btSimplePair* findPair(int indexA,int indexB); + + int GetCount() const { return m_overlappingPairArray.size(); } + + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } +private: + + btSimplePair* internalAddPair(int indexA, int indexB); + + void growTables(); + + SIMD_FORCE_INLINE bool equalsPair(const btSimplePair& pair, int indexA, int indexB) + { + return pair.m_indexA == indexA && pair.m_indexB == indexB; + } + + + + SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) + { + unsigned int key = indexA | (indexB << 16); + // Thomas Wang's hash + + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return key; + } + + + + + + SIMD_FORCE_INLINE btSimplePair* internalFindPair(int proxyIdA , int proxyIdB, int hash) + { + + int index = m_hashTable[hash]; + + while( index != BT_SIMPLE_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyIdA, proxyIdB) == false) + { + index = m_next[index]; + } + + if ( index == BT_SIMPLE_NULL_PAIR ) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; + } + + +}; + + + + +#endif //BT_HASHED_SIMPLE_PAIR_CACHE_H + + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp new file mode 100644 index 0000000000..6cba442ca5 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -0,0 +1,838 @@ +#include "btInternalEdgeUtility.h" + +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" +#include "LinearMath/btIDebugDraw.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.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; + } + + + + + + //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 btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags) +{ + //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE); + if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE) + return; + + btBvhTriangleMeshShape* trimesh = 0; + + if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE ) + trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape(); + else + trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); + + btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); + if (!triangleInfoMapPtr) + return; + + int hash = btGetHash(partId0,index0); + + + 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*>(colObj0Wrap->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 = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; + localContactNormalOnB.normalize();//is this necessary? + + // Get closest edge + int bestedge=-1; + btScalar disttobestedge=BT_LARGE_FLOAT; + // + // Edge 0 -> 1 + if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest ); + btScalar len=(contact-nearest).length(); + // + if( len < disttobestedge ) + { + bestedge=0; + disttobestedge=len; + } + } + // Edge 1 -> 2 + if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest ); + btScalar len=(contact-nearest).length(); + // + if( len < disttobestedge ) + { + bestedge=1; + disttobestedge=len; + } + } + // Edge 2 -> 0 + if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest ); + btScalar len=(contact-nearest).length(); + // + if( len < disttobestedge ) + { + bestedge=2; + disttobestedge=len; + } + } + +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f); + btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red ); +#endif + if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { +#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) + if( bestedge==0 ) + { + 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 = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. (what about cp.m_distance1?) + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + + } + } + } + } + } + } + + btNearestPointInLineSegment(contact,v1,v2,nearest); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green ); +#endif + + if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + + + btScalar len = (contact-nearest).length(); + if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) + if( bestedge==1 ) + { + isNearEdge = true; +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 edge(v1-v2); + + isNearEdge = true; + + 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 = colObj0Wrap->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 = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } + } + } + } + } + } + + btNearestPointInLineSegment(contact,v2,v0,nearest); +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue ); +#endif + + if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btScalar len = (contact-nearest).length(); + if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) + if( bestedge==2 ) + { + isNearEdge = true; +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + btVector3 edge(v2-v0); + + 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 = colObj0Wrap->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 = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal; + // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); + cp.m_normalWorldOnB = newNormal; + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } + } + } + } + + + } + } + +#ifdef DEBUG_INTERNAL_EDGE + { + btVector3 color(0,1,1); + btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color); + } +#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 = colObj0Wrap->getWorldTransform().getBasis()*tri_normal; + } else + { + btVector3 newNormal = tri_normal *frontFacing; + //if the tri_normal is pointing opposite direction as the current local contact normal, skip it + btScalar d = newNormal.dot(localContactNormalOnB) ; + if (d< 0) + { + return; + } + //modify the normal to be the triangle normal (or backfacing normal) + cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() *newNormal; + } + + // Reproject collision point along normal. + cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; + cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); + } + } +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h new file mode 100644 index 0000000000..7d9aafeee6 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h @@ -0,0 +1,47 @@ + +#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; +struct btCollisionObjectWrapper; +class btManifoldPoint; +class btIDebugDraw; + + + +enum btInternalEdgeAdjustFlags +{ + BT_TRIANGLE_CONVEX_BACKFACE_MODE = 1, + BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended + BT_TRIANGLE_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 btCollisionObjectWrapper* trimeshColObj0Wrap,const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0); + +///Enable the BT_INTERNAL_EDGE_DEBUG_DRAW define and call btSetDebugDrawer, to get visual info to see if the internal edge utility works properly. +///If the utility doesn't work properly, you might have to adjust the threshold values in btTriangleInfoMap +//#define BT_INTERNAL_EDGE_DEBUG_DRAW + +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW +void btSetDebugDrawer(btIDebugDraw* debugDrawer); +#endif //BT_INTERNAL_EDGE_DEBUG_DRAW + + +#endif //BT_INTERNAL_EDGE_UTILITY_H + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp new file mode 100644 index 0000000000..23c73c8825 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -0,0 +1,207 @@ +/* +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 "btManifoldResult.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + +///This is to allow MaterialCombiner/Custom Friction/Restitution values +ContactAddedCallback gContactAddedCallback=0; + +CalculateCombinedCallback gCalculateCombinedRestitutionCallback = &btManifoldResult::calculateCombinedRestitution; +CalculateCombinedCallback gCalculateCombinedFrictionCallback = &btManifoldResult::calculateCombinedFriction; +CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback = &btManifoldResult::calculateCombinedRollingFriction; +CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback = &btManifoldResult::calculateCombinedSpinningFriction; +CalculateCombinedCallback gCalculateCombinedContactDampingCallback = &btManifoldResult::calculateCombinedContactDamping; +CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback = &btManifoldResult::calculateCombinedContactStiffness; + +btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1) +{ + btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction(); + + const btScalar MAX_FRICTION = btScalar(10.); + if (friction < -MAX_FRICTION) + friction = -MAX_FRICTION; + if (friction > MAX_FRICTION) + friction = MAX_FRICTION; + return friction; + +} + +btScalar btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1) +{ + btScalar friction = body0->getSpinningFriction() * body1->getFriction() + body1->getSpinningFriction() * body0->getFriction(); + + const btScalar MAX_FRICTION = btScalar(10.); + if (friction < -MAX_FRICTION) + friction = -MAX_FRICTION; + if (friction > MAX_FRICTION) + friction = MAX_FRICTION; + return friction; +} + +///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; +btScalar btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1) +{ + btScalar friction = body0->getFriction() * body1->getFriction(); + + const btScalar MAX_FRICTION = btScalar(10.); + if (friction < -MAX_FRICTION) + friction = -MAX_FRICTION; + if (friction > MAX_FRICTION) + friction = MAX_FRICTION; + return friction; + +} + +btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1) +{ + return body0->getRestitution() * body1->getRestitution(); +} + +btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1) +{ + return body0->getContactDamping() + body1->getContactDamping(); +} + +btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1) +{ + + btScalar s0 = body0->getContactStiffness(); + btScalar s1 = body1->getContactStiffness(); + + btScalar tmp0 = btScalar(1)/s0; + btScalar tmp1 = btScalar(1)/s1; + btScalar combinedStiffness = btScalar(1) / (tmp0+tmp1); + return combinedStiffness; +} + + +btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + :m_manifoldPtr(0), + m_body0Wrap(body0Wrap), + m_body1Wrap(body1Wrap) +#ifdef DEBUG_PART_INDEX + ,m_partId0(-1), + m_partId1(-1), + m_index0(-1), + m_index1(-1) +#endif //DEBUG_PART_INDEX + , m_closestPointDistanceThreshold(0) +{ +} + + +void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) +{ + btAssert(m_manifoldPtr); + //order in manifold needs to match + + if (depth > m_manifoldPtr->getContactBreakingThreshold()) +// if (depth > m_manifoldPtr->getContactProcessingThreshold()) + return; + + bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + bool isNewCollision = m_manifoldPtr->getNumContacts() == 0; + + btVector3 pointA = pointInWorld + normalOnBInWorld * depth; + + btVector3 localA; + btVector3 localB; + + if (isSwapped) + { + localA = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localB = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } else + { + localA = m_body0Wrap->getCollisionObject()->getWorldTransform().invXform(pointA ); + localB = m_body1Wrap->getCollisionObject()->getWorldTransform().invXform(pointInWorld); + } + + btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + newPt.m_positionWorldOnA = pointA; + newPt.m_positionWorldOnB = pointInWorld; + + int insertIndex = m_manifoldPtr->getCacheEntry(newPt); + + newPt.m_combinedFriction = gCalculateCombinedFrictionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_combinedRestitution = gCalculateCombinedRestitutionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_combinedRollingFriction = gCalculateCombinedRollingFrictionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_combinedSpinningFriction = gCalculateCombinedSpinningFrictionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + + if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)) + { + newPt.m_combinedContactDamping1 = gCalculateCombinedContactDampingCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_combinedContactStiffness1 = gCalculateCombinedContactStiffnessCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING; + } + + if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR)) + { + newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR; + } + + btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2); + + + + //BP mod, store contact triangles. + 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) + { + //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); + m_manifoldPtr->replaceContactPoint(newPt,insertIndex); + } else + { + insertIndex = m_manifoldPtr->addManifoldPoint(newPt); + } + + //User can override friction and/or restitution + if (gContactAddedCallback && + //and if either of the two bodies requires custom material + ((m_body0Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags() & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK))) + { + //experimental feature info, for per-triangle material etc. + const btCollisionObjectWrapper* obj0Wrap = isSwapped? m_body1Wrap : m_body0Wrap; + const btCollisionObjectWrapper* obj1Wrap = isSwapped? m_body0Wrap : m_body1Wrap; + (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1); + } + + if (gContactStartedCallback && isNewCollision) + { + gContactStartedCallback(m_manifoldPtr); + } +} + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h new file mode 100644 index 0000000000..12cdafd1b6 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -0,0 +1,167 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef BT_MANIFOLD_RESULT_H +#define BT_MANIFOLD_RESULT_H + +class btCollisionObject; +struct btCollisionObjectWrapper; + +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +class btManifoldPoint; + +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" + +#include "LinearMath/btTransform.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1); +extern ContactAddedCallback gContactAddedCallback; + +//#define DEBUG_PART_INDEX 1 + +/// These callbacks are used to customize the algorith that combine restitution, friction, damping, Stiffness +typedef btScalar (*CalculateCombinedCallback)(const btCollisionObject* body0,const btCollisionObject* body1); + +extern CalculateCombinedCallback gCalculateCombinedRestitutionCallback; +extern CalculateCombinedCallback gCalculateCombinedFrictionCallback; +extern CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback; +extern CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback; +extern CalculateCombinedCallback gCalculateCombinedContactDampingCallback; +extern CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback; + +///btManifoldResult is a helper class to manage contact results. +class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result +{ +protected: + + btPersistentManifold* m_manifoldPtr; + + const btCollisionObjectWrapper* m_body0Wrap; + const btCollisionObjectWrapper* m_body1Wrap; + int m_partId0; + int m_partId1; + int m_index0; + int m_index1; + + +public: + + btManifoldResult() + : +#ifdef DEBUG_PART_INDEX + + m_partId0(-1), + m_partId1(-1), + m_index0(-1), + m_index1(-1) +#endif //DEBUG_PART_INDEX + m_closestPointDistanceThreshold(0) + { + } + + btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); + + virtual ~btManifoldResult() {}; + + void setPersistentManifold(btPersistentManifold* manifoldPtr) + { + m_manifoldPtr = manifoldPtr; + } + + const btPersistentManifold* getPersistentManifold() const + { + return m_manifoldPtr; + } + btPersistentManifold* getPersistentManifold() + { + return m_manifoldPtr; + } + + virtual void setShapeIdentifiersA(int partId0,int index0) + { + m_partId0=partId0; + m_index0=index0; + } + + virtual void setShapeIdentifiersB( int partId1,int index1) + { + m_partId1=partId1; + m_index1=index1; + } + + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth); + + SIMD_FORCE_INLINE void refreshContactPoints() + { + btAssert(m_manifoldPtr); + if (!m_manifoldPtr->getNumContacts()) + return; + + bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + + if (isSwapped) + { + m_manifoldPtr->refreshContactPoints(m_body1Wrap->getCollisionObject()->getWorldTransform(),m_body0Wrap->getCollisionObject()->getWorldTransform()); + } else + { + m_manifoldPtr->refreshContactPoints(m_body0Wrap->getCollisionObject()->getWorldTransform(),m_body1Wrap->getCollisionObject()->getWorldTransform()); + } + } + + const btCollisionObjectWrapper* getBody0Wrap() const + { + return m_body0Wrap; + } + const btCollisionObjectWrapper* getBody1Wrap() const + { + return m_body1Wrap; + } + + void setBody0Wrap(const btCollisionObjectWrapper* obj0Wrap) + { + m_body0Wrap = obj0Wrap; + } + + void setBody1Wrap(const btCollisionObjectWrapper* obj1Wrap) + { + m_body1Wrap = obj1Wrap; + } + + const btCollisionObject* getBody0Internal() const + { + return m_body0Wrap->getCollisionObject(); + } + + const btCollisionObject* getBody1Internal() const + { + return m_body1Wrap->getCollisionObject(); + } + + btScalar m_closestPointDistanceThreshold; + + /// in the future we can let the user override the methods to combine restitution and friction + static btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1); +}; + +#endif //BT_MANIFOLD_RESULT_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp new file mode 100644 index 0000000000..1344782257 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -0,0 +1,450 @@ + +/* +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 "LinearMath/btScalar.h" +#include "btSimulationIslandManager.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" + +//#include <stdio.h> +#include "LinearMath/btQuickprof.h" + +btSimulationIslandManager::btSimulationIslandManager(): +m_splitIslands(true) +{ +} + +btSimulationIslandManager::~btSimulationIslandManager() +{ +} + + +void btSimulationIslandManager::initUnionFind(int n) +{ + m_unionFind.reset(n); +} + + +void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld) +{ + + { + btOverlappingPairCache* pairCachePtr = colWorld->getPairCache(); + const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs(); + if (numOverlappingPairs) + { + btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr(); + + for (int i=0;i<numOverlappingPairs;i++) + { + const btBroadphasePair& collisionPair = pairPtr[i]; + btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; + + if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && + ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + { + + m_unionFind.unite((colObj0)->getIslandTag(), + (colObj1)->getIslandTag()); + } + } + } + } +} + +#ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION +void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) +{ + + // 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++) + { + btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + collisionObject->setIslandTag(index); + collisionObject->setCompanionId(-1); + collisionObject->setHitFraction(btScalar(1.)); + index++; + + } + } + // do the union find + + 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) ); + collisionObject->setCompanionId(-1); + } else + { + collisionObject->setIslandTag(-1); + collisionObject->setCompanionId(-2); + } + index++; + } + } +} + +#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION + +inline int getIslandId(const btPersistentManifold* lhs) +{ + int islandId; + const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0()); + const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1()); + islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag(); + return islandId; + +} + + + +/// function object that routes calls to operator< +class btPersistentManifoldSortPredicate +{ + public: + + SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const + { + return getIslandId(lhs) < getIslandId(rhs); + } +}; + + +void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld) +{ + + BT_PROFILE("islandUnionFindAndQuickSort"); + + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + + m_islandmanifold.resize(0); + + //we are going to sort the unionfind array, and store the element id in the size + //afterwards, we clean unionfind, to make sure no-one uses it anymore + + getUnionFind().sortIslands(); + int numElem = getUnionFind().getNumElements(); + + int endIslandIndex=1; + int startIslandIndex; + + + //update the sleeping state for bodies, if all are sleeping + for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) + { + int islandId = getUnionFind().getElement(startIslandIndex).m_id; + for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) + { + } + + //int numSleeping = 0; + + bool allSleeping = true; + + int idx; + for (idx=startIslandIndex;idx<endIslandIndex;idx++) + { + int i = getUnionFind().getElement(idx).m_sz; + + btCollisionObject* colObj0 = collisionObjects[i]; + if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) + { +// printf("error in island management\n"); + } + + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + if (colObj0->getIslandTag() == islandId) + { + if (colObj0->getActivationState()== ACTIVE_TAG) + { + allSleeping = false; + } + if (colObj0->getActivationState()== DISABLE_DEACTIVATION) + { + allSleeping = false; + } + } + } + + + if (allSleeping) + { + int idx; + for (idx=startIslandIndex;idx<endIslandIndex;idx++) + { + int i = getUnionFind().getElement(idx).m_sz; + btCollisionObject* colObj0 = collisionObjects[i]; + if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) + { +// printf("error in island management\n"); + } + + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + + if (colObj0->getIslandTag() == islandId) + { + colObj0->setActivationState( ISLAND_SLEEPING ); + } + } + } else + { + + int idx; + for (idx=startIslandIndex;idx<endIslandIndex;idx++) + { + int i = getUnionFind().getElement(idx).m_sz; + + btCollisionObject* colObj0 = collisionObjects[i]; + if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) + { +// printf("error in island management\n"); + } + + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + + if (colObj0->getIslandTag() == islandId) + { + if ( colObj0->getActivationState() == ISLAND_SLEEPING) + { + colObj0->setActivationState( WANTS_DEACTIVATION); + colObj0->setDeactivationTime(0.f); + } + } + } + } + } + + + int i; + int maxNumManifolds = dispatcher->getNumManifolds(); + +//#define SPLIT_ISLANDS 1 +//#ifdef SPLIT_ISLANDS + + +//#endif //SPLIT_ISLANDS + + + for (i=0;i<maxNumManifolds ;i++) + { + btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); + + const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); + const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); + + ///@todo: check sleeping conditions! + if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || + ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) + { + + //kinematic objects don't merge islands, but wake up all connected objects + if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) + { + if (colObj0->hasContactResponse()) + colObj1->activate(); + } + if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) + { + if (colObj1->hasContactResponse()) + colObj0->activate(); + } + if(m_splitIslands) + { + //filtering for response + if (dispatcher->needsResponse(colObj0,colObj1)) + m_islandmanifold.push_back(manifold); + } + } + } +} + + + +///@todo: this is random access, it can be walked 'cache friendly'! +void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback) +{ + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + + buildIslands(dispatcher,collisionWorld); + + int endIslandIndex=1; + int startIslandIndex; + int numElem = getUnionFind().getNumElements(); + + BT_PROFILE("processIslands"); + + if(!m_splitIslands) + { + btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); + int maxNumManifolds = dispatcher->getNumManifolds(); + callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); + } + else + { + // Sort manifolds, based on islands + // Sort the vector using predicate and std::sort + //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); + + int numManifolds = int (m_islandmanifold.size()); + + //tried a radix sort, but quicksort/heapsort seems still faster + //@todo rewrite island management + m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate()); + + //now process all active islands (sets of manifolds for now) + + int startManifoldIndex = 0; + int endManifoldIndex = 1; + + //int islandId; + + + + // printf("Start Islands\n"); + + //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated + for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) + { + int islandId = getUnionFind().getElement(startIslandIndex).m_id; + + + bool islandSleeping = true; + + for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) + { + int i = getUnionFind().getElement(endIslandIndex).m_sz; + btCollisionObject* colObj0 = collisionObjects[i]; + m_islandBodies.push_back(colObj0); + if (colObj0->isActive()) + islandSleeping = false; + } + + + //find the accompanying contact manifold for this islandId + int numIslandManifolds = 0; + btPersistentManifold** startManifold = 0; + + if (startManifoldIndex<numManifolds) + { + int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]); + if (curIslandId == islandId) + { + startManifold = &m_islandmanifold[startManifoldIndex]; + + for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++) + { + + } + /// Process the actual simulation, only if not sleeping/deactivated + numIslandManifolds = endManifoldIndex-startManifoldIndex; + } + + } + + if (!islandSleeping) + { + callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); + // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); + } + + if (numIslandManifolds) + { + startManifoldIndex = endManifoldIndex; + } + + m_islandBodies.resize(0); + } + } // else if(!splitIslands) + +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h new file mode 100644 index 0000000000..e24c6afeca --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -0,0 +1,81 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SIMULATION_ISLAND_MANAGER_H +#define BT_SIMULATION_ISLAND_MANAGER_H + +#include "BulletCollision/CollisionDispatch/btUnionFind.h" +#include "btCollisionCreateFunc.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "btCollisionObject.h" + +class btCollisionObject; +class btCollisionWorld; +class btDispatcher; +class btPersistentManifold; + + +///SimulationIslandManager creates and handles simulation islands, using btUnionFind +class btSimulationIslandManager +{ + btUnionFind m_unionFind; + + btAlignedObjectArray<btPersistentManifold*> m_islandmanifold; + btAlignedObjectArray<btCollisionObject* > m_islandBodies; + + bool m_splitIslands; + +public: + btSimulationIslandManager(); + virtual ~btSimulationIslandManager(); + + + void initUnionFind(int n); + + + btUnionFind& getUnionFind() { return m_unionFind;} + + virtual void updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher); + virtual void storeIslandActivationState(btCollisionWorld* world); + + + void findUnions(btDispatcher* dispatcher,btCollisionWorld* colWorld); + + + + struct IslandCallback + { + virtual ~IslandCallback() {}; + + virtual void processIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; + }; + + void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback); + + void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld); + + bool getSplitIslands() + { + return m_splitIslands; + } + void setSplitIslands(bool doSplitIslands) + { + m_splitIslands = doSplitIslands; + } + +}; + +#endif //BT_SIMULATION_ISLAND_MANAGER_H + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp new file mode 100644 index 0000000000..e8b567e0ef --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -0,0 +1,214 @@ +/* +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 "btSphereBoxCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" +//#include <stdio.h> + +btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap, bool isSwapped) +: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap), +m_ownManifold(false), +m_manifoldPtr(mf), +m_isSwapped(isSwapped) +{ + const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? col1Wrap : col0Wrap; + const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? col0Wrap : col1Wrap; + + if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject())) + { + m_manifoldPtr = m_dispatcher->getNewManifold(sphereObjWrap->getCollisionObject(),boxObjWrap->getCollisionObject()); + m_ownManifold = true; + } +} + + +btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + + + +void btSphereBoxCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) +{ + (void)dispatchInfo; + (void)resultOut; + if (!m_manifoldPtr) + return; + + const btCollisionObjectWrapper* sphereObjWrap = m_isSwapped? body1Wrap : body0Wrap; + const btCollisionObjectWrapper* boxObjWrap = m_isSwapped? body0Wrap : body1Wrap; + + btVector3 pOnBox; + + btVector3 normalOnSurfaceB; + btScalar penetrationDepth; + btVector3 sphereCenter = sphereObjWrap->getWorldTransform().getOrigin(); + const btSphereShape* sphere0 = (const btSphereShape*)sphereObjWrap->getCollisionShape(); + btScalar radius = sphere0->getRadius(); + btScalar maxContactDistance = m_manifoldPtr->getContactBreakingThreshold(); + + resultOut->setPersistentManifold(m_manifoldPtr); + + if (getSphereDistance(boxObjWrap, pOnBox, normalOnSurfaceB, penetrationDepth, sphereCenter, radius, maxContactDistance)) + { + /// report a contact. internally this will be kept persistent, and contact reduction is done + resultOut->addContactPoint(normalOnSurfaceB, pOnBox, penetrationDepth); + } + + if (m_ownManifold) + { + if (m_manifoldPtr->getNumContacts()) + { + resultOut->refreshContactPoints(); + } + } + +} + +btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + + //not yet + return btScalar(1.); +} + + +bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance ) +{ + const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape(); + btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin(); + btScalar boxMargin = boxShape->getMargin(); + penetrationDepth = 1.0f; + + // convert the sphere position to the box's local space + btTransform const &m44T = boxObjWrap->getWorldTransform(); + btVector3 sphereRelPos = m44T.invXform(sphereCenter); + + // Determine the closest point to the sphere center in the box + btVector3 closestPoint = sphereRelPos; + closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) ); + closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) ); + closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) ); + closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) ); + closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) ); + closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) ); + + btScalar intersectionDist = fRadius + boxMargin; + btScalar contactDist = intersectionDist + maxContactDistance; + normal = sphereRelPos - closestPoint; + + //if there is no penetration, we are done + btScalar dist2 = normal.length2(); + if (dist2 > contactDist * contactDist) + { + return false; + } + + btScalar distance; + + //special case if the sphere center is inside the box + if (dist2 <= SIMD_EPSILON) + { + distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal); + } + else //compute the penetration details + { + distance = normal.length(); + normal /= distance; + } + + pointOnBox = closestPoint + normal * boxMargin; +// v3PointOnSphere = sphereRelPos - (normal * fRadius); + penetrationDepth = distance - intersectionDist; + + // transform back in world space + btVector3 tmp = m44T(pointOnBox); + pointOnBox = tmp; +// tmp = m44T(v3PointOnSphere); +// v3PointOnSphere = tmp; + tmp = m44T.getBasis() * normal; + normal = tmp; + + return true; +} + +btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal ) +{ + //project the center of the sphere on the closest face of the box + btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX(); + btScalar minDist = faceDist; + closestPoint.setX( boxHalfExtent.getX() ); + normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f)); + + faceDist = boxHalfExtent.getX() + sphereRelPos.getX(); + if (faceDist < minDist) + { + minDist = faceDist; + closestPoint = sphereRelPos; + closestPoint.setX( -boxHalfExtent.getX() ); + normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f)); + } + + faceDist = boxHalfExtent.getY() - sphereRelPos.getY(); + if (faceDist < minDist) + { + minDist = faceDist; + closestPoint = sphereRelPos; + closestPoint.setY( boxHalfExtent.getY() ); + normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f)); + } + + faceDist = boxHalfExtent.getY() + sphereRelPos.getY(); + if (faceDist < minDist) + { + minDist = faceDist; + closestPoint = sphereRelPos; + closestPoint.setY( -boxHalfExtent.getY() ); + normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f)); + } + + faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ(); + if (faceDist < minDist) + { + minDist = faceDist; + closestPoint = sphereRelPos; + closestPoint.setZ( boxHalfExtent.getZ() ); + normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f)); + } + + faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ(); + if (faceDist < minDist) + { + minDist = faceDist; + closestPoint = sphereRelPos; + closestPoint.setZ( -boxHalfExtent.getZ() ); + normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f)); + } + + return minDist; +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h new file mode 100644 index 0000000000..eefaedc9e7 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -0,0 +1,75 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SPHERE_BOX_COLLISION_ALGORITHM_H +#define BT_SPHERE_BOX_COLLISION_ALGORITHM_H + +#include "btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; +#include "btCollisionDispatcher.h" + +#include "LinearMath/btVector3.h" + +/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_isSwapped; + +public: + + btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, bool isSwapped); + + virtual ~btSphereBoxCollisionAlgorithm(); + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + bool getSphereDistance( const btCollisionObjectWrapper* boxObjWrap, btVector3& v3PointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& v3SphereCenter, btScalar fRadius, btScalar maxContactDistance ); + + btScalar getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal ); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm)); + if (!m_swapped) + { + return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,false); + } else + { + return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0Wrap,body1Wrap,true); + } + } + }; + +}; + +#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp new file mode 100644 index 0000000000..27eaec3059 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -0,0 +1,107 @@ +/* +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. +*/ +#define CLEAR_MANIFOLD 1 + +#include "btSphereSphereCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + +btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap) +: btActivatingCollisionAlgorithm(ci,col0Wrap,col1Wrap), +m_ownManifold(false), +m_manifoldPtr(mf) +{ + if (!m_manifoldPtr) + { + m_manifoldPtr = m_dispatcher->getNewManifold(col0Wrap->getCollisionObject(),col1Wrap->getCollisionObject()); + m_ownManifold = true; + } +} + +btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)dispatchInfo; + + if (!m_manifoldPtr) + return; + + resultOut->setPersistentManifold(m_manifoldPtr); + + btSphereShape* sphere0 = (btSphereShape*)col0Wrap->getCollisionShape(); + btSphereShape* sphere1 = (btSphereShape*)col1Wrap->getCollisionShape(); + + btVector3 diff = col0Wrap->getWorldTransform().getOrigin()- col1Wrap->getWorldTransform().getOrigin(); + btScalar len = diff.length(); + btScalar radius0 = sphere0->getRadius(); + btScalar radius1 = sphere1->getRadius(); + +#ifdef CLEAR_MANIFOLD + m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting +#endif + + ///iff distance positive, don't generate a new contact + if ( len > (radius0+radius1+resultOut->m_closestPointDistanceThreshold)) + { +#ifndef CLEAR_MANIFOLD + resultOut->refreshContactPoints(); +#endif //CLEAR_MANIFOLD + return; + } + ///distance (negative means penetration) + btScalar dist = len - (radius0+radius1); + + btVector3 normalOnSurfaceB(1,0,0); + if (len > SIMD_EPSILON) + { + normalOnSurfaceB = diff / len; + } + + ///point on A (worldspace) + ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; + ///point on B (worldspace) + btVector3 pos1 = col1Wrap->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB; + + /// report a contact. internally this will be kept persistent, and contact reduction is done + + + resultOut->addContactPoint(normalOnSurfaceB,pos1,dist); + +#ifndef CLEAR_MANIFOLD + resultOut->refreshContactPoints(); +#endif //CLEAR_MANIFOLD + +} + +btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)col0; + (void)col1; + (void)dispatchInfo; + (void)resultOut; + + //not yet + return btScalar(1.); +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h new file mode 100644 index 0000000000..3517a568a9 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.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 BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H +#define BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H + +#include "btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "btCollisionDispatcher.h" + +class btPersistentManifold; + +/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +/// Also provides the most basic sample for custom/user btCollisionAlgorithm +class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + +public: + btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap); + + btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btActivatingCollisionAlgorithm(ci) {} + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + virtual ~btSphereSphereCollisionAlgorithm(); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm)); + return new(mem) btSphereSphereCollisionAlgorithm(0,ci,col0Wrap,col1Wrap); + } + }; + +}; + +#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp new file mode 100644 index 0000000000..86d4e74400 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -0,0 +1,84 @@ +/* +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 "btSphereTriangleCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "SphereTriangleDetector.h" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + +btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped) +: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), +m_ownManifold(false), +m_manifoldPtr(mf), +m_swapped(swapped) +{ + if (!m_manifoldPtr) + { + m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject()); + m_ownManifold = true; + } +} + +btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* col0Wrap,const btCollisionObjectWrapper* col1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + if (!m_manifoldPtr) + return; + + const btCollisionObjectWrapper* sphereObjWrap = m_swapped? col1Wrap : col0Wrap; + const btCollisionObjectWrapper* triObjWrap = m_swapped? col0Wrap : col1Wrap; + + btSphereShape* sphere = (btSphereShape*)sphereObjWrap->getCollisionShape(); + btTriangleShape* triangle = (btTriangleShape*)triObjWrap->getCollisionShape(); + + /// report a contact. internally this will be kept persistent, and contact reduction is done + resultOut->setPersistentManifold(m_manifoldPtr); + SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold); + + btDiscreteCollisionDetectorInterface::ClosestPointInput input; + input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds + input.m_transformA = sphereObjWrap->getWorldTransform(); + input.m_transformB = triObjWrap->getWorldTransform(); + + bool swapResults = m_swapped; + + detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults); + + if (m_ownManifold) + resultOut->refreshContactPoints(); + +} + +btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + + //not yet + return btScalar(1.); +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h new file mode 100644 index 0000000000..6b6e39a72b --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -0,0 +1,69 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H +#define BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H + +#include "btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; +#include "btCollisionDispatcher.h" + +/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +/// Also provides the most basic sample for custom/user btCollisionAlgorithm +class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_swapped; + +public: + btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool swapped); + + btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btActivatingCollisionAlgorithm(ci) {} + + virtual void processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + virtual ~btSphereTriangleCollisionAlgorithm(); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) + { + + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm)); + + return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_swapped); + } + }; + +}; + +#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H + diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp new file mode 100644 index 0000000000..5222933595 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -0,0 +1,82 @@ +/* +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 "btUnionFind.h" + + + +btUnionFind::~btUnionFind() +{ + Free(); + +} + +btUnionFind::btUnionFind() +{ + +} + +void btUnionFind::allocate(int N) +{ + m_elements.resize(N); +} +void btUnionFind::Free() +{ + m_elements.clear(); +} + + +void btUnionFind::reset(int N) +{ + allocate(N); + + for (int i = 0; i < N; i++) + { + m_elements[i].m_id = i; m_elements[i].m_sz = 1; + } +} + + +class btUnionFindElementSortPredicate +{ + public: + + bool operator() ( const btElement& lhs, const btElement& rhs ) const + { + return lhs.m_id < rhs.m_id; + } +}; + +///this is a special operation, destroying the content of btUnionFind. +///it sorts the elements, based on island id, in order to make it easy to iterate over islands +void btUnionFind::sortIslands() +{ + + //first store the original body index, and islandId + int numElements = m_elements.size(); + + 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 + //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate); + m_elements.quickSort(btUnionFindElementSortPredicate()); + +} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.h new file mode 100644 index 0000000000..ef2a29202f --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.h @@ -0,0 +1,129 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_UNION_FIND_H +#define BT_UNION_FIND_H + +#include "LinearMath/btAlignedObjectArray.h" + +#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 +{ + int m_id; + int m_sz; +}; + +///UnionFind calculates connected subsets +// Implements weighted Quick Union with path compression +// optimization: could use short ints instead of ints (halving memory, would limit the number of rigid bodies to 64k, sounds reasonable) +class btUnionFind + { + private: + btAlignedObjectArray<btElement> m_elements; + + public: + + btUnionFind(); + ~btUnionFind(); + + + //this is a special operation, destroying the content of btUnionFind. + //it sorts the elements, based on island id, in order to make it easy to iterate over islands + void sortIslands(); + + void reset(int N); + + SIMD_FORCE_INLINE int getNumElements() const + { + return int(m_elements.size()); + } + SIMD_FORCE_INLINE bool isRoot(int x) const + { + return (x == m_elements[x].m_id); + } + + btElement& getElement(int index) + { + return m_elements[index]; + } + const btElement& getElement(int index) const + { + return m_elements[index]; + } + + void allocate(int N); + void Free(); + + + + + int find(int p, int q) + { + return (find(p) == find(q)); + } + + void unite(int p, int q) + { + int i = find(p), j = find(q); + if (i == j) + return; + +#ifndef USE_PATH_COMPRESSION + //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) + if (m_elements[i].m_sz < m_elements[j].m_sz) + { + m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; + } + else + { + m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; + } +#else + m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; +#endif //USE_PATH_COMPRESSION + } + + int find(int x) + { + //btAssert(x < m_N); + //btAssert(x >= 0); + + while (x != m_elements[x].m_id) + { + //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically + + #ifdef USE_PATH_COMPRESSION + const btElement* elementPtr = &m_elements[m_elements[x].m_id]; + m_elements[x].m_id = elementPtr->m_id; + x = elementPtr->m_id; + #else// + x = m_elements[x].m_id; + #endif + //btAssert(x < m_N); + //btAssert(x >= 0); + + } + return x; + } + + + }; + + +#endif //BT_UNION_FIND_H |