diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision/CollisionDispatch')
57 files changed, 0 insertions, 13677 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp deleted file mode 100644 index 7647f67360..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ /dev/null @@ -1,215 +0,0 @@ -/* -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; - btScalar minDistSqr = contactCapsuleRadiusSqr; - btVector3 nearestOnEdge; - for (int i = 0; i < m_triangle->getNumEdges(); i++) - { - btVector3 pa; - btVector3 pb; - - m_triangle->getEdge(i, pa, pb); - - btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge); - if (distanceSqr < minDistSqr) - { - // Yep, we're inside a capsule, and record the capsule with smallest distance - minDistSqr = distanceSqr; - hasContact = true; - contactPoint = nearestOnEdge; - } - } - } - } - } - - if (hasContact) - { - 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 deleted file mode 100644 index d47e47530d..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ /dev/null @@ -1,43 +0,0 @@ -/* -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 deleted file mode 100644 index ac5de45d27..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp +++ /dev/null @@ -1,47 +0,0 @@ -/* -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 deleted file mode 100644 index 862060571b..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h +++ /dev/null @@ -1,35 +0,0 @@ -/* -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 deleted file mode 100644 index 6873a95d90..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp +++ /dev/null @@ -1,411 +0,0 @@ -/* -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 deleted file mode 100644 index 3b66d1fd0b..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h +++ /dev/null @@ -1,63 +0,0 @@ -/* -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 deleted file mode 100644 index 7a391e059a..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp +++ /dev/null @@ -1,80 +0,0 @@ -/* -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 deleted file mode 100644 index eb21065765..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h +++ /dev/null @@ -1,63 +0,0 @@ -/* -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 deleted file mode 100644 index 202039956e..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ /dev/null @@ -1,767 +0,0 @@ -/* - * 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 deleted file mode 100644 index 9f7d988fc1..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBoxBoxDetector.h +++ /dev/null @@ -1,40 +0,0 @@ -/* - * 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 deleted file mode 100644 index d6e15f555b..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionConfiguration.h +++ /dev/null @@ -1,43 +0,0 @@ -/* -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 deleted file mode 100644 index bd81284939..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h +++ /dev/null @@ -1,43 +0,0 @@ -/* -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 deleted file mode 100644 index 25b2b1ea46..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ /dev/null @@ -1,289 +0,0 @@ -/* -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" - -#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) -{ - //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) -{ - //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); - - { - BT_PROFILE("processAllOverlappingPairs"); - pairCache->processAllOverlappingPairs(&collisionCallback, dispatcher, dispatchInfo); - } - - //m_blockedForChanges = false; -} - -//by default, Bullet will use this near callback -void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo) -{ - btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; - - 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 deleted file mode 100644 index 04309670cf..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ /dev/null @@ -1,169 +0,0 @@ -/* -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; - - 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) - { - btAssert(index>=0); - btAssert(index<m_manifoldsPtr.size()); - return m_manifoldsPtr[index]; - } - - const btPersistentManifold* getManifoldByIndexInternal(int index) const - { - btAssert(index>=0); - btAssert(index<m_manifoldsPtr.size()); - 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 deleted file mode 100644 index 89bc8d920e..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp +++ /dev/null @@ -1,162 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btCollisionDispatcherMt.h" -#include "LinearMath/btQuickprof.h" - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" - -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" -#include "LinearMath/btPoolAllocator.h" -#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" - -btCollisionDispatcherMt::btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize) - : btCollisionDispatcher(config) -{ - m_batchManifoldsPtr.resize(btGetTaskScheduler()->getNumThreads()); - m_batchUpdating = false; - m_grainSize = grainSize; // iterations per task -} - -btPersistentManifold* btCollisionDispatcherMt::getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1) -{ - //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance) - - btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold), body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold)) - : gContactBreakingThreshold; - - btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold()); - - void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); - if (NULL == mem) - { - //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert. - if ((m_dispatcherFlags & CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION) == 0) - { - mem = btAlignedAlloc(sizeof(btPersistentManifold), 16); - } - else - { - btAssert(0); - //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration - return 0; - } - } - btPersistentManifold* manifold = new (mem) btPersistentManifold(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold); - if (!m_batchUpdating) - { - // batch updater will update manifold pointers array after finishing, so - // only need to update array when not batch-updating - //btAssert( !btThreadsAreRunning() ); - manifold->m_index1a = m_manifoldsPtr.size(); - m_manifoldsPtr.push_back(manifold); - } - else - { - m_batchManifoldsPtr[btGetCurrentThreadIndex()].push_back(manifold); - } - - return manifold; -} - -void btCollisionDispatcherMt::releaseManifold(btPersistentManifold* manifold) -{ - clearManifold(manifold); - //btAssert( !btThreadsAreRunning() ); - if (!m_batchUpdating) - { - // batch updater will update manifold pointers array after finishing, so - // only need to update array when not batch-updating - int findIndex = manifold->m_index1a; - btAssert(findIndex < m_manifoldsPtr.size()); - m_manifoldsPtr.swap(findIndex, m_manifoldsPtr.size() - 1); - m_manifoldsPtr[findIndex]->m_index1a = findIndex; - m_manifoldsPtr.pop_back(); - } - - manifold->~btPersistentManifold(); - if (m_persistentManifoldPoolAllocator->validPtr(manifold)) - { - m_persistentManifoldPoolAllocator->freeMemory(manifold); - } - else - { - btAlignedFree(manifold); - } -} - -struct CollisionDispatcherUpdater : public btIParallelForBody -{ - btBroadphasePair* mPairArray; - btNearCallback mCallback; - btCollisionDispatcher* mDispatcher; - const btDispatcherInfo* mInfo; - - CollisionDispatcherUpdater() - { - mPairArray = NULL; - mCallback = NULL; - mDispatcher = NULL; - mInfo = NULL; - } - void forLoop(int iBegin, int iEnd) const - { - for (int i = iBegin; i < iEnd; ++i) - { - btBroadphasePair* pair = &mPairArray[i]; - mCallback(*pair, *mDispatcher, *mInfo); - } - } -}; - -void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) -{ - const int pairCount = pairCache->getNumOverlappingPairs(); - if (pairCount == 0) - { - return; - } - CollisionDispatcherUpdater updater; - updater.mCallback = getNearCallback(); - updater.mPairArray = pairCache->getOverlappingPairArrayPtr(); - updater.mDispatcher = this; - updater.mInfo = &info; - - m_batchUpdating = true; - btParallelFor(0, pairCount, m_grainSize, updater); - m_batchUpdating = false; - - // merge new manifolds, if any - for (int i = 0; i < m_batchManifoldsPtr.size(); ++i) - { - btAlignedObjectArray<btPersistentManifold*>& batchManifoldsPtr = m_batchManifoldsPtr[i]; - - for (int j = 0; j < batchManifoldsPtr.size(); ++j) - { - m_manifoldsPtr.push_back(batchManifoldsPtr[j]); - } - - batchManifoldsPtr.resizeNoInitialize(0); - } - - // update the indices (used when releasing manifolds) - for (int i = 0; i < m_manifoldsPtr.size(); ++i) - { - m_manifoldsPtr[i]->m_index1a = i; - } -} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h deleted file mode 100644 index 1155de2cfe..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h +++ /dev/null @@ -1,38 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_COLLISION_DISPATCHER_MT_H -#define BT_COLLISION_DISPATCHER_MT_H - -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "LinearMath/btThreads.h" - -class btCollisionDispatcherMt : public btCollisionDispatcher -{ -public: - btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize = 40); - - virtual btPersistentManifold* getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1) BT_OVERRIDE; - virtual void releaseManifold(btPersistentManifold* manifold) BT_OVERRIDE; - - virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) BT_OVERRIDE; - -protected: - btAlignedObjectArray<btAlignedObjectArray<btPersistentManifold*> > m_batchManifoldsPtr; - bool m_batchUpdating; - int m_grainSize; -}; - -#endif //BT_COLLISION_DISPATCHER_MT_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp deleted file mode 100644 index b48d9301d7..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ /dev/null @@ -1,138 +0,0 @@ -/* -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" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" - -btCollisionObject::btCollisionObject() - : m_interpolationLinearVelocity(0.f, 0.f, 0.f), - m_interpolationAngularVelocity(0.f, 0.f, 0.f), - m_anisotropicFriction(1.f, 1.f, 1.f), - m_hasAnisotropicFriction(false), - m_contactProcessingThreshold(BT_LARGE_FLOAT), - m_broadphaseHandle(0), - m_collisionShape(0), - m_extensionPointer(0), - m_rootCollisionShape(0), - m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT), - m_islandTag1(-1), - m_companionId(-1), - m_worldArrayIndex(-1), - m_activationState1(1), - m_deactivationTime(btScalar(0.)), - m_friction(btScalar(0.5)), - m_restitution(btScalar(0.)), - m_rollingFriction(0.0f), - m_spinningFriction(0.f), - m_contactDamping(.1), - m_contactStiffness(BT_LARGE_FLOAT), - m_internalType(CO_COLLISION_OBJECT), - m_userObjectPointer(0), - m_userIndex2(-1), - m_userIndex(-1), - m_userIndex3(-1), - m_hitFraction(btScalar(1.)), - m_ccdSweptSphereRadius(btScalar(0.)), - m_ccdMotionThreshold(btScalar(0.)), - m_checkCollideWith(false), - m_updateRevision(0) -{ - m_worldTransform.setIdentity(); - m_interpolationWorldTransform.setIdentity(); -} - -btCollisionObject::~btCollisionObject() -{ -} - -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; - if (m_broadphaseHandle) - { - dataOut->m_collisionFilterGroup = m_broadphaseHandle->m_collisionFilterGroup; - dataOut->m_collisionFilterMask = m_broadphaseHandle->m_collisionFilterMask; - dataOut->m_uniqueId = m_broadphaseHandle->m_uniqueId; - } - else - { - dataOut->m_collisionFilterGroup = 0; - dataOut->m_collisionFilterMask = 0; - dataOut->m_uniqueId = -1; - } - return btCollisionObjectDataName; -} - -void btCollisionObject::serializeSingleObject(class btSerializer* serializer) const -{ - int len = calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len, 1); - 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 deleted file mode 100644 index dbe82fd61f..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h +++ /dev/null @@ -1,690 +0,0 @@ -/* -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 -#define FIXED_BASE_MULTI_BODY 6 - -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; - - int m_userIndex3; - - ///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_DYNAMIC_OBJECT = 0, - 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; - } - - int getNumObjectsWithoutCollision() const - { - return m_objectsWithoutCollisionCheck.size(); - } - - const btCollisionObject* getObjectWithoutCollision(int index) - { - return m_objectsWithoutCollisionCheck[index]; - } - - virtual bool checkCollideWithOverride(const btCollisionObject* co) const - { - int index = m_objectsWithoutCollisionCheck.findLinearSearch(co); - if (index < m_objectsWithoutCollisionCheck.size()) - { - 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() != FIXED_BASE_MULTI_BODY) && (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; - } - - int getUserIndex3() const - { - return m_userIndex3; - } - - ///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; - } - - void setUserIndex3(int index) - { - m_userIndex3 = index; - } - - int getUpdateRevisionInternal() const - { - return m_updateRevision; - } - - void setCustomDebugColor(const btVector3& colorRGB) - { - m_customDebugColorRGB = colorRGB; - m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; - } - - void removeCustomDebugColor() - { - m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; - } - - bool getCustomDebugColor(btVector3 & colorRGB) const - { - bool hasCustomColor = (0 != (m_collisionFlags & CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR)); - if (hasCustomColor) - { - colorRGB = m_customDebugColorRGB; - } - return hasCustomColor; - } - - inline bool checkCollideWith(const btCollisionObject* co) const - { - 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; -}; - -// clang-format off - -///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; - int m_collisionFilterGroup; - int m_collisionFilterMask; - int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -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; - int m_collisionFilterGroup; - int m_collisionFilterMask; - int m_uniqueId; -}; -// clang-format on - -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 deleted file mode 100644 index 56341b7d29..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h +++ /dev/null @@ -1,49 +0,0 @@ -#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; - const btTransform* m_preTransform; - int m_partId; - int m_index; - - btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, int partId, int index) - : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_preTransform(NULL), m_partId(partId), m_index(index) - { - } - - btCollisionObjectWrapper(const btCollisionObjectWrapper* parent, const btCollisionShape* shape, const btCollisionObject* collisionObject, const btTransform& worldTransform, const btTransform& preTransform, int partId, int index) - : m_parent(parent), m_shape(shape), m_collisionObject(collisionObject), m_worldTransform(worldTransform), m_preTransform(&preTransform), m_partId(partId), m_index(index) - { - } - - SIMD_FORCE_INLINE const btTransform& getWorldTransform() const { return m_worldTransform; } - SIMD_FORCE_INLINE const btCollisionObject* getCollisionObject() const { return m_collisionObject; } - SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_shape; } -}; - -#endif //BT_COLLISION_OBJECT_WRAPPER_H diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp deleted file mode 100644 index f74dcabc54..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ /dev/null @@ -1,1625 +0,0 @@ -/* -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/CollisionShapes/btHeightfieldTerrainShape.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::refreshBroadphaseProxy(btCollisionObject* collisionObject) -{ - if (collisionObject->getBroadphaseHandle()) - { - int collisionFilterGroup = collisionObject->getBroadphaseHandle()->m_collisionFilterGroup; - int collisionFilterMask = collisionObject->getBroadphaseHandle()->m_collisionFilterMask; - - getBroadphase()->destroyProxy(collisionObject->getBroadphaseHandle(), getDispatcher()); - - //calculate new AABB - btTransform trans = collisionObject->getWorldTransform(); - - btVector3 minAabb; - btVector3 maxAabb; - collisionObject->getCollisionShape()->getAabb(trans, minAabb, maxAabb); - - int type = collisionObject->getCollisionShape()->getShapeType(); - collisionObject->setBroadphaseHandle(getBroadphase()->createProxy( - minAabb, - maxAabb, - type, - collisionObject, - collisionFilterGroup, - collisionFilterMask, - m_dispatcher1)); - } -} - -void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) -{ - btAssert(collisionObject); - - //check that the object isn't already added - btAssert(m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); - btAssert(collisionObject->getWorldArrayIndex() == -1); // do not add the same object to more than one collision world - - 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 if (((resultCallback.m_flags&btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator)==0) - && collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE - ) - { - ///optimized version for btHeightfieldTerrainShape - btHeightfieldTerrainShape* heightField = (btHeightfieldTerrainShape*)collisionShape; - btTransform worldTocollisionObject = colObjWorldTransform.inverse(); - btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); - btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); - - BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), heightField, colObjWorldTransform); - rcb.m_hitFraction = resultCallback.m_closestHitFraction; - heightField->performRaycast(&rcb, rayFromLocal, rayToLocal); - } - else - { - //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.fuzzyZero() ? btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0)) : 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->drawTriangle(wv0, wv1, wv2, m_color, 1.0); - } -}; - -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->serializeSingleObject(serializer); - } - } -} - -void btCollisionWorld::serializeContactManifolds(btSerializer* serializer) -{ - if (serializer->getSerializationFlags() & BT_SERIALIZE_CONTACT_MANIFOLDS) - { - int numManifolds = getDispatcher()->getNumManifolds(); - for (int i = 0; i < numManifolds; i++) - { - const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i]; - //don't serialize empty manifolds, they just take space - //(may have to do it anyway if it destroys determinism) - if (manifold->getNumContacts() == 0) - continue; - - btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1); - const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk, structType, BT_CONTACTMANIFOLD_CODE, (void*)manifold); - } - } -} - -void btCollisionWorld::serialize(btSerializer* serializer) -{ - serializer->startSerialization(); - - serializeCollisionObjects(serializer); - - serializeContactManifolds(serializer); - - serializer->finishSerialization(); -} diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h deleted file mode 100644 index fd0e5b9bbd..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ /dev/null @@ -1,513 +0,0 @@ -/* -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); - - void serializeContactManifolds(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); - - virtual void refreshBroadphaseProxy(btCollisionObject* collisionObject); - - 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 deleted file mode 100644 index e56e73dcf5..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp +++ /dev/null @@ -1,1087 +0,0 @@ -/* -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 deleted file mode 100644 index 5e8bc95341..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h +++ /dev/null @@ -1,169 +0,0 @@ -/* -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 deleted file mode 100644 index b5f4a3c869..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ /dev/null @@ -1,390 +0,0 @@ -/* -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()); - - if (gCompoundChildShapePairCallback) - { - if (!gCompoundChildShapePairCallback(m_otherObjWrap->getCollisionShape(), childShape)) - return; - } - - //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 (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1)) - { - btTransform preTransform = childTrans; - if (this->m_compoundColObjWrap->m_preTransform) - { - preTransform = preTransform *(*(this->m_compoundColObjWrap->m_preTransform)); - } - btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, preTransform, -1, index); - - btCollisionAlgorithm* algo = 0; - bool allocatedAlgorithm = false; - - if (m_resultOut->m_closestPointDistanceThreshold > 0) - { - algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); - allocatedAlgorithm = true; - } - else - { - //the contactpoint is still projected back using the original inverted worldtrans - if (!m_childCollisionAlgorithms[index]) - { - m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); - } - algo = m_childCollisionAlgorithms[index]; - } - - const btCollisionObjectWrapper* tmpWrap = 0; - - ///detect swapping case - 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); - } - if (allocatedAlgorithm) - { - algo->~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm(algo); - } - } - } - 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 deleted file mode 100644 index 4ea5e77185..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ /dev/null @@ -1,99 +0,0 @@ -/* -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 deleted file mode 100644 index 044b60dbb1..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ /dev/null @@ -1,413 +0,0 @@ -/* -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); - bool removePair = false; - btCollisionAlgorithm* colAlgo = 0; - if (m_resultOut->m_closestPointDistanceThreshold > 0) - { - colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS); - removePair = true; - } - 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); - - if (removePair) - { - colAlgo->~btCollisionAlgorithm(); - m_dispatcher->freeCollisionAlgorithm(colAlgo); - } - } - } -}; - -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; - - { - const btCollisionShape* childShape0 = 0; - - btTransform newChildWorldTrans0; - childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA); - const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA); - newChildWorldTrans0 = col0ObjWrap->getWorldTransform() * childTrans0; - childShape0->getAabb(newChildWorldTrans0, aabbMin0, aabbMax0); - } - btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold); - aabbMin0 -= thresholdVec; - aabbMax0 += thresholdVec; - { - const btCollisionShape* childShape1 = 0; - btTransform newChildWorldTrans1; - - childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB); - const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB); - newChildWorldTrans1 = col1ObjWrap->getWorldTransform() * childTrans1; - childShape1->getAabb(newChildWorldTrans1, aabbMin1, aabbMax1); - } - - 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 deleted file mode 100644 index a940d840e0..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h +++ /dev/null @@ -1,82 +0,0 @@ -/* -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; - -extern btShapePairCallback gCompoundCompoundChildShapePairCallback; - -/// 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 deleted file mode 100644 index 9087f84398..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp +++ /dev/null @@ -1,211 +0,0 @@ -/* -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 deleted file mode 100644 index 9fca463fbe..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h +++ /dev/null @@ -1,83 +0,0 @@ -/* -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 deleted file mode 100644 index e50f85e2bb..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ /dev/null @@ -1,374 +0,0 @@ -/* -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" -#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h" - -btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) - : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), - m_btConvexTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped), - m_isSwapped(isSwapped) -{ -} - -btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm() -{ -} - -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()) - { - if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE) - { - btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape(); - if (convexBodyWrap->getCollisionShape()->isConvex()) - { - btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape(); - btAlignedObjectArray<btVector3> queryVertices; - - if (convex->isPolyhedral()) - { - btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; - for (int v = 0; v < poly->getNumVertices(); v++) - { - btVector3 vtx; - poly->getVertex(v, vtx); - queryVertices.push_back(vtx); - } - } - btScalar maxDist = SIMD_EPSILON; - - if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) - { - queryVertices.push_back(btVector3(0, 0, 0)); - btSphereShape* sphere = (btSphereShape*)convex; - maxDist = sphere->getRadius() + SIMD_EPSILON; - } - if (queryVertices.size()) - { - resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); - //m_btConvexTriangleCallback.m_manifoldPtr->clearManifold(); - - btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; - for (int v = 0; v < queryVertices.size(); v++) - { - const btVector3& vtx = queryVertices[v]; - btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform() * vtx; - btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace); - - btVector3 normalLocal; - btScalar dist; - if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal)) - { - if (dist <= maxDist) - { - normalLocal.safeNormalize(); - btVector3 normal = triBodyWrap->getWorldTransform().getBasis() * normalLocal; - - if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) - { - btSphereShape* sphere = (btSphereShape*)convex; - dist -= sphere->getRadius(); - vtxWorldSpace -= sphere->getRadius() * normal; - } - resultOut->addContactPoint(normal, vtxWorldSpace - normal * dist, dist); - } - } - } - resultOut->refreshContactPoints(); - } - } - } - else - { - const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triBodyWrap->getCollisionShape()); - - if (convexBodyWrap->getCollisionShape()->isConvex()) - { - btScalar collisionMarginTriangle = concaveShape->getMargin(); - - 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 deleted file mode 100644 index b72e402981..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ /dev/null @@ -1,117 +0,0 @@ -/* -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 deleted file mode 100644 index b48d97f2b2..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ /dev/null @@ -1,873 +0,0 @@ -/* -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/CollisionShapes/btConvexPolyhedron.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()+ resultOut->m_closestPointDistanceThreshold; - - btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(), - capsuleB->getHalfHeight(), capsuleB->getRadius(), capsuleA->getUpAxis(), capsuleB->getUpAxis(), - body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), threshold); - - if (dist < threshold) - { - btAssert(normalOnB.length2() >= (SIMD_EPSILON * SIMD_EPSILON)); - resultOut->addContactPoint(normalOnB, pointOnBWorld, dist); - } - 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()+ resultOut->m_closestPointDistanceThreshold; - - 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()+ resultOut->m_closestPointDistanceThreshold; - - 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 - { - btVector3 m_normalOnBInWorld; - btVector3 m_pointInWorld; - btScalar m_depth; - bool m_hasContact; - - btDummyResult() - : m_hasContact(false) - { - } - - virtual void setShapeIdentifiersA(int partId0, int index0) {} - virtual void setShapeIdentifiersB(int partId1, int index1) {} - virtual void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth) - { - m_hasContact = true; - m_normalOnBInWorld = normalOnBInWorld; - m_pointInWorld = pointInWorld; - m_depth = depth; - } - }; - - 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()+ resultOut->m_closestPointDistanceThreshold; - - btScalar minDist = -1e30f; - btVector3 sepNormalWorldSpace; - bool foundSepAxis = true; - - if (dispatchInfo.m_enableSatConvex) - { - foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( - *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), - sepNormalWorldSpace, *resultOut); - } - else - { -#ifdef ZERO_MARGIN - gjkPairDetector.setIgnoreMargin(true); - gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); -#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 (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE) - { - btVertexArray worldSpaceVertices; - btTriangleShape* tri = (btTriangleShape*)polyhedronB; - worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[0]); - worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[1]); - worldSpaceVertices.push_back(body1Wrap->getWorldTransform() * tri->m_vertices1[2]); - - //tri->initializePolyhedralFeatures(); - - btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold; - - btVector3 sepNormalWorldSpace; - btScalar minDist = -1e30f; - btScalar maxDist = threshold; - - bool foundSepAxis = false; - bool useSatSepNormal = true; - - if (useSatSepNormal) - { -#if 0 - if (0) - { - //initializePolyhedralFeatures performs a convex hull computation, not needed for a single triangle - polyhedronB->initializePolyhedralFeatures(); - } else -#endif - { - btVector3 uniqueEdges[3] = {tri->m_vertices1[1] - tri->m_vertices1[0], - tri->m_vertices1[2] - tri->m_vertices1[1], - tri->m_vertices1[0] - tri->m_vertices1[2]}; - - uniqueEdges[0].normalize(); - uniqueEdges[1].normalize(); - uniqueEdges[2].normalize(); - - btConvexPolyhedron polyhedron; - polyhedron.m_vertices.push_back(tri->m_vertices1[2]); - polyhedron.m_vertices.push_back(tri->m_vertices1[0]); - polyhedron.m_vertices.push_back(tri->m_vertices1[1]); - - { - btFace combinedFaceA; - combinedFaceA.m_indices.push_back(0); - combinedFaceA.m_indices.push_back(1); - combinedFaceA.m_indices.push_back(2); - btVector3 faceNormal = uniqueEdges[0].cross(uniqueEdges[1]); - faceNormal.normalize(); - btScalar planeEq = 1e30f; - for (int v = 0; v < combinedFaceA.m_indices.size(); v++) - { - btScalar eq = tri->m_vertices1[combinedFaceA.m_indices[v]].dot(faceNormal); - if (planeEq > eq) - { - planeEq = eq; - } - } - combinedFaceA.m_plane[0] = faceNormal[0]; - combinedFaceA.m_plane[1] = faceNormal[1]; - combinedFaceA.m_plane[2] = faceNormal[2]; - combinedFaceA.m_plane[3] = -planeEq; - polyhedron.m_faces.push_back(combinedFaceA); - } - { - btFace combinedFaceB; - combinedFaceB.m_indices.push_back(0); - combinedFaceB.m_indices.push_back(2); - combinedFaceB.m_indices.push_back(1); - btVector3 faceNormal = -uniqueEdges[0].cross(uniqueEdges[1]); - faceNormal.normalize(); - btScalar planeEq = 1e30f; - for (int v = 0; v < combinedFaceB.m_indices.size(); v++) - { - btScalar eq = tri->m_vertices1[combinedFaceB.m_indices[v]].dot(faceNormal); - if (planeEq > eq) - { - planeEq = eq; - } - } - - combinedFaceB.m_plane[0] = faceNormal[0]; - combinedFaceB.m_plane[1] = faceNormal[1]; - combinedFaceB.m_plane[2] = faceNormal[2]; - combinedFaceB.m_plane[3] = -planeEq; - polyhedron.m_faces.push_back(combinedFaceB); - } - - polyhedron.m_uniqueEdges.push_back(uniqueEdges[0]); - polyhedron.m_uniqueEdges.push_back(uniqueEdges[1]); - polyhedron.m_uniqueEdges.push_back(uniqueEdges[2]); - polyhedron.initialize2(); - - polyhedronB->setPolyhedralFeatures(polyhedron); - } - - foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( - *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), - sepNormalWorldSpace, *resultOut); - // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); - } - else - { -#ifdef ZERO_MARGIN - gjkPairDetector.setIgnoreMargin(true); - gjkPairDetector.getClosestPoints(input, *resultOut, dispatchInfo.m_debugDraw); -#else - gjkPairDetector.getClosestPoints(input, dummy, dispatchInfo.m_debugDraw); -#endif //ZERO_MARGIN - - if (dummy.m_hasContact && dummy.m_depth < 0) - { - if (foundSepAxis) - { - if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace) < 0.99) - { - printf("?\n"); - } - } - else - { - printf("!\n"); - } - sepNormalWorldSpace.setValue(0, 0, 1); // = dummy.m_normalOnBInWorld; - //minDist = dummy.m_depth; - foundSepAxis = true; - } -#if 0 - btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - if (l2>SIMD_EPSILON) - { - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); - //minDist = gjkPairDetector.getCachedSeparatingDistance(); - //maxDist = threshold; - minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); - foundSepAxis = true; - } -#endif - } - - if (foundSepAxis) - { - worldVertsB2.resize(0); - btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2, minDist - threshold, maxDist, *resultOut); - } - - if (m_ownManifold) - { - resultOut->refreshContactPoints(); - } - - 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 deleted file mode 100644 index eac5b4d824..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ /dev/null @@ -1,101 +0,0 @@ -/* -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 deleted file mode 100644 index ba1bc06b69..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/* -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->m_closestPointDistanceThreshold; - resultOut->setPersistentManifold(m_manifoldPtr); - if (hasCollision) - { - /// report a contact. internally this will be kept persistent, and contact reduction is done - btVector3 normalOnSurfaceB = planeObjWrap->getWorldTransform().getBasis() * planeNormal; - btVector3 pOnB = vtxInPlaneWorld; - resultOut->addContactPoint(normalOnSurfaceB, pOnB, distance); - } - - //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 deleted file mode 100644 index b693da118f..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h +++ /dev/null @@ -1,82 +0,0 @@ -/* -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 deleted file mode 100644 index ef3ea9e394..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ /dev/null @@ -1,362 +0,0 @@ -/* -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 deleted file mode 100644 index b39a3f41de..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ /dev/null @@ -1,111 +0,0 @@ -/* -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 deleted file mode 100644 index 7cd41bdb33..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp +++ /dev/null @@ -1,30 +0,0 @@ -/* -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 deleted file mode 100644 index 65ef83e094..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h +++ /dev/null @@ -1,52 +0,0 @@ -/* -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 deleted file mode 100644 index 00f16fd0a8..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp +++ /dev/null @@ -1,166 +0,0 @@ -/* -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 deleted file mode 100644 index aa7f48d5cb..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btGhostObject.h +++ /dev/null @@ -1,162 +0,0 @@ -/* -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 deleted file mode 100644 index b686d98d1e..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp +++ /dev/null @@ -1,244 +0,0 @@ -/* -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> - -#ifdef BT_DEBUG_COLLISION_PAIRS -int gOverlappingSimplePairs = 0; -int gRemoveSimplePairs = 0; -int gAddedSimplePairs = 0; -int gFindSimplePairs = 0; -#endif //BT_DEBUG_COLLISION_PAIRS - -btHashedSimplePairCache::btHashedSimplePairCache() -{ - int initialAllocatedSize = 2; - 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) -{ -#ifdef BT_DEBUG_COLLISION_PAIRS - gFindSimplePairs++; -#endif - - /*if (indexA > indexB) - btSwap(indexA, indexB);*/ - - int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); - - 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) -{ -#ifdef BT_DEBUG_COLLISION_PAIRS - gRemoveSimplePairs++; -#endif - - /*if (indexA > indexB) - btSwap(indexA, indexB);*/ - - int hash = static_cast<int>(getHash(static_cast<unsigned int>(indexA), static_cast<unsigned int>(indexB)) & (m_overlappingPairArray.capacity() - 1)); - - 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 deleted file mode 100644 index fd38a4f0e1..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h +++ /dev/null @@ -1,149 +0,0 @@ -/* -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; - -#ifdef BT_DEBUG_COLLISION_PAIRS -extern int gOverlappingSimplePairs; -extern int gRemoveSimplePairs; -extern int gAddedSimplePairs; -extern int gFindSimplePairs; -#endif //BT_DEBUG_COLLISION_PAIRS - -class btHashedSimplePairCache -{ - btSimplePairArray m_overlappingPairArray; - -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) - { -#ifdef BT_DEBUG_COLLISION_PAIRS - gAddedSimplePairs++; -#endif - - 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 deleted file mode 100644 index a71700f58a..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ /dev/null @@ -1,900 +0,0 @@ -#include "btInternalEdgeUtility.h" - -#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" - -#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#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"); - } - } - } -}; - - -struct b3ProcessAllTrianglesHeightfield: public btTriangleCallback -{ - btHeightfieldTerrainShape* m_heightfieldShape; - btTriangleInfoMap* m_triangleInfoMap; - - - b3ProcessAllTrianglesHeightfield(btHeightfieldTerrainShape* heightFieldShape, btTriangleInfoMap* triangleInfoMap) - :m_heightfieldShape(heightFieldShape), - m_triangleInfoMap(triangleInfoMap) - { - } - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) - { - btConnectivityProcessor connectivityProcessor; - connectivityProcessor.m_partIdA = partId; - connectivityProcessor.m_triangleIndexA = triangleIndex; - connectivityProcessor.m_triangleVerticesA = triangle; - connectivityProcessor.m_triangleInfoMap = m_triangleInfoMap; - btVector3 aabbMin, aabbMax; - aabbMin.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); - aabbMax.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); - aabbMin.setMin(triangle[0]); - aabbMax.setMax(triangle[0]); - aabbMin.setMin(triangle[1]); - aabbMax.setMax(triangle[1]); - aabbMin.setMin(triangle[2]); - aabbMax.setMax(triangle[2]); - - m_heightfieldShape->processAllTriangles(&connectivityProcessor, aabbMin, aabbMax); - } -}; -///////////////////////////////////////////////////////// -///////////////////////////////////////////////////////// - -void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap) -{ - //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; - switch (indicestype) { - case PHY_INTEGER: graphicsindex = gfxbase[j]; break; - case PHY_SHORT: graphicsindex = ((unsigned short*)gfxbase)[j]; break; - case PHY_UCHAR: graphicsindex = ((unsigned char*)gfxbase)[j]; break; - default: btAssert(0); - } - if (type == PHY_FLOAT) - { - float* graphicsbase = (float*)(vertexbase + graphicsindex * stride); - 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); - } - } -} - - -void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* heightfieldShape, btTriangleInfoMap* triangleInfoMap) -{ - - //the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there! - if (heightfieldShape->getTriangleInfoMap()) - return; - - heightfieldShape->setTriangleInfoMap(triangleInfoMap); - - //get all the triangles of the heightfield - - btVector3 aabbMin, aabbMax; - - aabbMax.setValue(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); - aabbMin.setValue(btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT), btScalar(-BT_LARGE_FLOAT)); - - b3ProcessAllTrianglesHeightfield processHeightfield(heightfieldShape, triangleInfoMap); - heightfieldShape->processAllTriangles(&processHeightfield, aabbMin, aabbMax); - -} - -// Given a point and a line segment (defined by two points), compute the closest point -// in the line. Cap the point at the endpoints of the line segment. -void btNearestPointInLineSegment(const btVector3& point, const btVector3& line0, const btVector3& line1, btVector3& nearestPoint) -{ - 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; - - - btTriangleInfoMap* triangleInfoMapPtr = 0; - - if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TERRAIN_SHAPE_PROXYTYPE) - { - btHeightfieldTerrainShape* heightfield = (btHeightfieldTerrainShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); - triangleInfoMapPtr = heightfield->getTriangleInfoMap(); - -//#define USE_HEIGHTFIELD_TRIANGLES -#ifdef USE_HEIGHTFIELD_TRIANGLES - btVector3 newNormal = btVector3(0, 0, 1); - - const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape()); - btVector3 tri_normal; - tri_shape->calcNormal(tri_normal); - newNormal = tri_normal; - // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); - cp.m_normalWorldOnB = newNormal; - // Reproject collision point along normal. (what about cp.m_distance1?) - cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; - cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB); - return; -#endif - } - - - btBvhTriangleMeshShape* trimesh = 0; - - if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) - { - trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape(); - } - else - { - if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) - { - trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); - } - } - if (trimesh) - { - triangleInfoMapPtr = (btTriangleInfoMap*)trimesh->getTriangleInfoMap(); - } - - - if (!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 deleted file mode 100644 index cc6d11c241..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h +++ /dev/null @@ -1,44 +0,0 @@ - -#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; -class btHeightfieldTerrainShape; - -enum btInternalEdgeAdjustFlags -{ - BT_TRIANGLE_CONVEX_BACKFACE_MODE = 1, - BT_TRIANGLE_CONCAVE_DOUBLE_SIDED = 2, //double sided options are experimental, single sided is recommended - BT_TRIANGLE_CONVEX_DOUBLE_SIDED = 4 -}; - -///Call btGenerateInternalEdgeInfo to create triangle info, store in the shape 'userInfo' -void btGenerateInternalEdgeInfo(btBvhTriangleMeshShape* trimeshShape, btTriangleInfoMap* triangleInfoMap); - -void btGenerateInternalEdgeInfo(btHeightfieldTerrainShape* trimeshShape, btTriangleInfoMap* triangleInfoMap); - -///Call the btFixMeshNormal to adjust the collision normal, using the triangle info map (generated using btGenerateInternalEdgeInfo) -///If this info map is missing, or the triangle is not store in this map, nothing will be done -void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* trimeshColObj0Wrap, const btCollisionObjectWrapper* otherColObj1Wrap, int partId0, int index0, int normalAdjustFlags = 0); - -///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 deleted file mode 100644 index 770eb24369..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ /dev/null @@ -1,203 +0,0 @@ -/* -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 deleted file mode 100644 index 6c0a2d9a43..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h +++ /dev/null @@ -1,163 +0,0 @@ -/* -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 deleted file mode 100644 index 327b3f076a..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ /dev/null @@ -1,445 +0,0 @@ - -/* -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); - } -}; - -class btPersistentManifoldSortPredicateDeterministic -{ -public: - SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const - { - return ( - (getIslandId(lhs) < getIslandId(rhs)) || ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) || ((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) && (lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId))); - } -}; - -void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld) -{ - BT_PROFILE("islandUnionFindAndQuickSort"); - - btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - - m_islandmanifold.resize(0); - - //we are going to sort the unionfind array, and store the element id in the size - //afterwards, we clean unionfind, to make sure no-one uses it anymore - - getUnionFind().sortIslands(); - int numElem = getUnionFind().getNumElements(); - - int endIslandIndex = 1; - int 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 || - colObj0->getActivationState() == DISABLE_DEACTIVATION) - { - allSleeping = false; - break; - } - } - } - - 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); - if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) - { - if (manifold->getNumContacts() == 0) - continue; - } - - const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); - const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); - - ///@todo: check sleeping conditions! - if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || - ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) - { - //kinematic objects don't merge islands, but wake up all connected objects - if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) - { - 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) -{ - buildIslands(dispatcher, collisionWorld); - processIslands(dispatcher, collisionWorld, callback); -} - -void btSimulationIslandManager::processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback) -{ - btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - int endIslandIndex = 1; - int startIslandIndex; - int numElem = getUnionFind().getNumElements(); - - BT_PROFILE("processIslands"); - - if (!m_splitIslands) - { - 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 - //btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid, - //but also based on object0 unique id and object1 unique id - if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) - { - m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic()); - } - else - { - m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); - } - - //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate()); - - //now process all active islands (sets of manifolds for now) - - 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 deleted file mode 100644 index 197bb457cf..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ /dev/null @@ -1,75 +0,0 @@ -/* -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); - - void processIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback); - - 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 deleted file mode 100644 index bc68b285b8..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ /dev/null @@ -1,209 +0,0 @@ -/* -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 deleted file mode 100644 index 3348bc89af..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ /dev/null @@ -1,73 +0,0 @@ -/* -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 deleted file mode 100644 index 7fa0559f97..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ /dev/null @@ -1,105 +0,0 @@ -/* -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 deleted file mode 100644 index b08d0df76d..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ /dev/null @@ -1,64 +0,0 @@ -/* -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 deleted file mode 100644 index 1bc3056c01..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/* -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 deleted file mode 100644 index d660222f16..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ /dev/null @@ -1,65 +0,0 @@ -/* -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 deleted file mode 100644 index 816bf1e6ad..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ /dev/null @@ -1,74 +0,0 @@ -/* -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 deleted file mode 100644 index d422ef55eb..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btUnionFind.h +++ /dev/null @@ -1,123 +0,0 @@ -/* -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 |