Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorErwin Coumans <blender@erwincoumans.com>2006-10-23 06:54:30 +0400
committerErwin Coumans <blender@erwincoumans.com>2006-10-23 06:54:30 +0400
commit44d16f056215e6068f0b186a0ab766165cf3966e (patch)
treef0ad85e29c32563d1d4c1c46db4e2cd22f7f78dc /extern/bullet2/src/BulletCollision/NarrowPhaseCollision
parente459764b4b056959e354edca3868a91ff9bc272f (diff)
Added refactored Bullet 2.x library. Important: these files are not part of the Blender build yet. First, the integration will be updated to make use of the new Bullet version. Then all build systems needs to be updated.
The refactoring didn't leave a single file the same, all filenames and classes have bt prefix, methodnames start with lowercase, a single headerfile can be included, and also a single include path. Plan is to make use of this Bullet 2.x version in extern/bullet2 within the coming weeks, then extern/bullet can be discarded/ignored/content removed.
Diffstat (limited to 'extern/bullet2/src/BulletCollision/NarrowPhaseCollision')
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp200
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h52
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp20
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h71
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h42
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h87
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp174
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h50
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp218
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h84
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h98
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp246
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h37
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp246
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h140
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h57
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp101
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h42
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h64
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp133
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h50
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp598
-rw-r--r--extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h157
23 files changed, 2967 insertions, 0 deletions
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
new file mode 100644
index 00000000000..ae3ce42e77f
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
@@ -0,0 +1,200 @@
+/*
+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 "btContinuousConvexCollision.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+#include "LinearMath/btTransformUtil.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+
+#include "btGjkPairDetector.h"
+#include "btPointCollector.h"
+
+
+
+btContinuousConvexCollision::btContinuousConvexCollision ( btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver)
+:m_simplexSolver(simplexSolver),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_convexA(convexA),m_convexB(convexB)
+{
+}
+
+/// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
+/// You don't want your game ever to lock-up.
+#define MAX_ITERATIONS 1000
+
+bool btContinuousConvexCollision::calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result)
+{
+
+ m_simplexSolver->reset();
+
+ /// compute linear and angular velocity for this interval, to interpolate
+ btVector3 linVelA,angVelA,linVelB,angVelB;
+ btTransformUtil::calculateVelocity(fromA,toA,1.f,linVelA,angVelA);
+ btTransformUtil::calculateVelocity(fromB,toB,1.f,linVelB,angVelB);
+
+ btScalar boundingRadiusA = m_convexA->getAngularMotionDisc();
+ btScalar boundingRadiusB = m_convexB->getAngularMotionDisc();
+
+ btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
+
+ float radius = 0.001f;
+
+ btScalar lambda = 0.f;
+ btVector3 v(1,0,0);
+
+ int maxIter = MAX_ITERATIONS;
+
+ btVector3 n;
+ n.setValue(0.f,0.f,0.f);
+ bool hasResult = false;
+ btVector3 c;
+
+ float lastLambda = lambda;
+ //float epsilon = 0.001f;
+
+ int numIter = 0;
+ //first solution, using GJK
+
+
+ btTransform identityTrans;
+ identityTrans.setIdentity();
+
+ btSphereShape raySphere(0.0f);
+ raySphere.setMargin(0.f);
+
+
+// result.drawCoordSystem(sphereTr);
+
+ btPointCollector pointCollector1;
+
+ {
+
+ btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
+ btGjkPairDetector::ClosestPointInput input;
+
+ //we don't use margins during CCD
+ gjk.setIgnoreMargin(true);
+
+ input.m_transformA = fromA;
+ input.m_transformB = fromB;
+ gjk.getClosestPoints(input,pointCollector1,0);
+
+ hasResult = pointCollector1.m_hasResult;
+ c = pointCollector1.m_pointInWorld;
+ }
+
+ if (hasResult)
+ {
+ btScalar dist;
+ dist = pointCollector1.m_distance;
+ n = pointCollector1.m_normalOnBInWorld;
+
+ //not close enough
+ while (dist > radius)
+ {
+ numIter++;
+ if (numIter > maxIter)
+ return false; //todo: report a failure
+
+ float dLambda = 0.f;
+
+ //calculate safe moving fraction from distance / (linear+rotational velocity)
+
+ //float clippedDist = GEN_min(angularConservativeRadius,dist);
+ //float clippedDist = dist;
+
+ float projectedLinearVelocity = (linVelB-linVelA).dot(n);
+
+ dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
+
+ lambda = lambda + dLambda;
+
+ if (lambda > 1.f)
+ return false;
+
+ if (lambda < 0.f)
+ return false;
+
+ //todo: next check with relative epsilon
+ if (lambda <= lastLambda)
+ break;
+ lastLambda = lambda;
+
+
+
+ //interpolate to next lambda
+ btTransform interpolatedTransA,interpolatedTransB,relativeTrans;
+
+ btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA);
+ btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB);
+ relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
+
+ result.DebugDraw( lambda );
+
+ btPointCollector pointCollector;
+ btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
+ btGjkPairDetector::ClosestPointInput input;
+ input.m_transformA = interpolatedTransA;
+ input.m_transformB = interpolatedTransB;
+ gjk.getClosestPoints(input,pointCollector,0);
+ if (pointCollector.m_hasResult)
+ {
+ if (pointCollector.m_distance < 0.f)
+ {
+ //degenerate ?!
+ result.m_fraction = lastLambda;
+ result.m_normal = n;
+ return true;
+ }
+ c = pointCollector.m_pointInWorld;
+
+ dist = pointCollector.m_distance;
+ } else
+ {
+ //??
+ return false;
+ }
+
+ }
+
+ result.m_fraction = lambda;
+ result.m_normal = n;
+ return true;
+ }
+
+ return false;
+
+/*
+//todo:
+ //if movement away from normal, discard result
+ btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
+ if (result.m_fraction < 1.f)
+ {
+ if (move.dot(result.m_normal) <= 0.f)
+ {
+ }
+ }
+*/
+
+}
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
new file mode 100644
index 00000000000..9901bab4b45
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h
@@ -0,0 +1,52 @@
+/*
+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 CONTINUOUS_COLLISION_CONVEX_CAST_H
+#define CONTINUOUS_COLLISION_CONVEX_CAST_H
+
+#include "btConvexCast.h"
+#include "btSimplexSolverInterface.h"
+class btConvexPenetrationDepthSolver;
+class btConvexShape;
+
+/// btContinuousConvexCollision implements angular and linear time of impact for convex objects.
+/// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
+/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
+/// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
+class btContinuousConvexCollision : public btConvexCast
+{
+ btSimplexSolverInterface* m_simplexSolver;
+ btConvexPenetrationDepthSolver* m_penetrationDepthSolver;
+ btConvexShape* m_convexA;
+ btConvexShape* m_convexB;
+
+
+public:
+
+ btContinuousConvexCollision (btConvexShape* shapeA,btConvexShape* shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
+
+ virtual bool calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result);
+
+
+};
+
+#endif //CONTINUOUS_COLLISION_CONVEX_CAST_H
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp
new file mode 100644
index 00000000000..d2a1310b232
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp
@@ -0,0 +1,20 @@
+/*
+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 "btConvexCast.h"
+
+btConvexCast::~btConvexCast()
+{
+}
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
new file mode 100644
index 00000000000..4258d829cca
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
@@ -0,0 +1,71 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef CONVEX_CAST_H
+#define CONVEX_CAST_H
+
+#include <LinearMath/btTransform.h>
+#include <LinearMath/btVector3.h>
+#include <LinearMath/btScalar.h>
+class btMinkowskiSumShape;
+#include "LinearMath/btIDebugDraw.h"
+
+/// btConvexCast is an interface for Casting
+class btConvexCast
+{
+public:
+
+
+ virtual ~btConvexCast();
+
+ ///RayResult stores the closest result
+ /// alternatively, add a callback method to decide about closest/all results
+ struct CastResult
+ {
+ //virtual bool addRayResult(const btVector3& normal,btScalar fraction) = 0;
+
+ virtual void DebugDraw(btScalar fraction) {}
+ virtual void drawCoordSystem(const btTransform& trans) {}
+
+ CastResult()
+ :m_fraction(1e30f),
+ m_debugDrawer(0)
+ {
+ }
+
+
+ virtual ~CastResult() {};
+
+ btVector3 m_normal;
+ btScalar m_fraction;
+ btTransform m_hitTransformA;
+ btTransform m_hitTransformB;
+
+ btIDebugDraw* m_debugDrawer;
+
+ };
+
+
+ /// cast a convex against another convex object
+ virtual bool calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result) = 0;
+};
+
+#endif //CONVEX_CAST_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
new file mode 100644
index 00000000000..ba02ea56e83
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h
@@ -0,0 +1,42 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef CONVEX_PENETRATION_DEPTH_H
+#define CONVEX_PENETRATION_DEPTH_H
+
+class btVector3;
+#include "btSimplexSolverInterface.h"
+class btConvexShape;
+#include "LinearMath/btPoint3.h"
+class btTransform;
+
+///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
+class btConvexPenetrationDepthSolver
+{
+public:
+
+ virtual ~btConvexPenetrationDepthSolver() {};
+ virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
+ btConvexShape* convexA,btConvexShape* convexB,
+ const btTransform& transA,const btTransform& transB,
+ btVector3& v, btPoint3& pa, btPoint3& pb,
+ class btIDebugDraw* debugDraw
+ ) = 0;
+
+
+};
+#endif //CONVEX_PENETRATION_DEPTH_H
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
new file mode 100644
index 00000000000..8889699b395
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h
@@ -0,0 +1,87 @@
+/*
+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 DISCRETE_COLLISION_DETECTOR_INTERFACE_H
+#define DISCRETE_COLLISION_DETECTOR_INTERFACE_H
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+
+
+/// This interface is made to be used by an iterative approach to do TimeOfImpact calculations
+/// This interface allows to query for closest points and penetration depth between two (convex) objects
+/// the closest point is on the second object (B), and the normal points from the surface on B towards A.
+/// distance is between closest points on B and closest point on A. So you can calculate closest point on A
+/// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB
+struct btDiscreteCollisionDetectorInterface
+{
+
+ struct Result
+ {
+ void operator delete(void* ptr) {};
+
+ virtual ~Result(){}
+
+ ///setShapeIdentifiers provides experimental support for per-triangle material / custom material combiner
+ virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)=0;
+ virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)=0;
+ };
+
+ struct ClosestPointInput
+ {
+ ClosestPointInput()
+ :m_maximumDistanceSquared(1e30f)
+ {
+ }
+
+ btTransform m_transformA;
+ btTransform m_transformB;
+ btScalar m_maximumDistanceSquared;
+ };
+
+ virtual ~btDiscreteCollisionDetectorInterface() {};
+
+ //
+ // give either closest points (distance > 0) or penetration (distance)
+ // the normal always points from B towards A
+ //
+ virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) = 0;
+
+};
+
+struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result
+{
+ btVector3 m_normalOnSurfaceB;
+ btVector3 m_closestPointInB;
+ btScalar m_distance; //negative means penetration !
+
+ btStorageResult() : m_distance(1e30f)
+ {
+
+ }
+ virtual ~btStorageResult() {};
+
+ virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
+ {
+ if (depth < m_distance)
+ {
+ m_normalOnSurfaceB = normalOnBInWorld;
+ m_closestPointInB = pointInWorld;
+ m_distance = depth;
+ }
+ }
+};
+
+#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
new file mode 100644
index 00000000000..bf465b61857
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp
@@ -0,0 +1,174 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "btGjkConvexCast.h"
+#include "BulletCollision/CollisionShapes/btSphereShape.h"
+#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+#include "btGjkPairDetector.h"
+#include "btPointCollector.h"
+
+
+btGjkConvexCast::btGjkConvexCast(btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
+:m_simplexSolver(simplexSolver),
+m_convexA(convexA),
+m_convexB(convexB)
+{
+}
+
+bool btGjkConvexCast::calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result)
+{
+
+
+ btMinkowskiSumShape combi(m_convexA,m_convexB);
+ btMinkowskiSumShape* convex = &combi;
+
+ btTransform rayFromLocalA;
+ btTransform rayToLocalA;
+
+ rayFromLocalA = fromA.inverse()* fromB;
+ rayToLocalA = toA.inverse()* toB;
+
+
+ btTransform trA,trB;
+ trA = btTransform(fromA);
+ trB = btTransform(fromB);
+ trA.setOrigin(btPoint3(0,0,0));
+ trB.setOrigin(btPoint3(0,0,0));
+
+ convex->setTransformA(trA);
+ convex->setTransformB(trB);
+
+
+
+
+ float radius = 0.01f;
+
+ btScalar lambda = 0.f;
+ btVector3 s = rayFromLocalA.getOrigin();
+ btVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin();
+ btVector3 x = s;
+ btVector3 n;
+ n.setValue(0,0,0);
+ bool hasResult = false;
+ btVector3 c;
+
+ float lastLambda = lambda;
+
+ //first solution, using GJK
+
+ //no penetration support for now, perhaps pass a pointer when we really want it
+ btConvexPenetrationDepthSolver* penSolverPtr = 0;
+
+ btTransform identityTrans;
+ identityTrans.setIdentity();
+
+ btSphereShape raySphere(0.0f);
+ raySphere.setMargin(0.f);
+
+ btTransform sphereTr;
+ sphereTr.setIdentity();
+ sphereTr.setOrigin( rayFromLocalA.getOrigin());
+
+ result.drawCoordSystem(sphereTr);
+ {
+ btPointCollector pointCollector1;
+ btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);
+
+ btGjkPairDetector::ClosestPointInput input;
+ input.m_transformA = sphereTr;
+ input.m_transformB = identityTrans;
+ gjk.getClosestPoints(input,pointCollector1,0);
+
+ hasResult = pointCollector1.m_hasResult;
+ c = pointCollector1.m_pointInWorld;
+ n = pointCollector1.m_normalOnBInWorld;
+ }
+
+
+
+ if (hasResult)
+ {
+ btScalar dist;
+ dist = (c-x).length();
+ if (dist < radius)
+ {
+ //penetration
+ lastLambda = 1.f;
+ }
+
+ //not close enough
+ while (dist > radius)
+ {
+
+ n = x - c;
+ btScalar nDotr = n.dot(r);
+
+ if (nDotr >= -(SIMD_EPSILON*SIMD_EPSILON))
+ return false;
+
+ lambda = lambda - n.dot(n) / nDotr;
+ if (lambda <= lastLambda)
+ break;
+
+ lastLambda = lambda;
+
+ x = s + lambda * r;
+
+ sphereTr.setOrigin( x );
+ result.drawCoordSystem(sphereTr);
+ btPointCollector pointCollector;
+ btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr);
+ btGjkPairDetector::ClosestPointInput input;
+ input.m_transformA = sphereTr;
+ input.m_transformB = identityTrans;
+ gjk.getClosestPoints(input,pointCollector,0);
+ if (pointCollector.m_hasResult)
+ {
+ if (pointCollector.m_distance < 0.f)
+ {
+ //degeneracy, report a hit
+ result.m_fraction = lastLambda;
+ result.m_normal = n;
+ return true;
+ }
+ c = pointCollector.m_pointInWorld;
+ dist = (c-x).length();
+ } else
+ {
+ //??
+ return false;
+ }
+
+ }
+
+ if (lastLambda < 1.f)
+ {
+
+ result.m_fraction = lastLambda;
+ result.m_normal = n;
+ return true;
+ }
+ }
+
+ return false;
+}
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
new file mode 100644
index 00000000000..66b34b88363
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h
@@ -0,0 +1,50 @@
+/*
+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 GJK_CONVEX_CAST_H
+#define GJK_CONVEX_CAST_H
+
+#include <BulletCollision/CollisionShapes/btCollisionMargin.h>
+
+#include "LinearMath/btVector3.h"
+#include "btConvexCast.h"
+class btConvexShape;
+class btMinkowskiSumShape;
+#include "btSimplexSolverInterface.h"
+
+///GjkConvexCast performs a raycast on a convex object using support mapping.
+class btGjkConvexCast : public btConvexCast
+{
+ btSimplexSolverInterface* m_simplexSolver;
+ btConvexShape* m_convexA;
+ btConvexShape* m_convexB;
+
+public:
+
+ btGjkConvexCast(btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver);
+
+ /// cast a convex against another convex object
+ virtual bool calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result);
+
+};
+
+#endif //GJK_CONVEX_CAST_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
new file mode 100644
index 00000000000..f6fdd6435cf
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
@@ -0,0 +1,218 @@
+/*
+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 "btGjkPairDetector.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
+
+#if defined(DEBUG) || defined (_DEBUG)
+#include <stdio.h> //for debug printf
+#endif
+
+static const btScalar rel_error = btScalar(1.0e-5);
+btScalar rel_error2 = rel_error * rel_error;
+float maxdist2 = 1.e30f;
+
+#ifdef __SPU__
+#include <spu_printf.h>
+#endif //__SPU__
+
+btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
+:m_cachedSeparatingAxis(0.f,0.f,1.f),
+m_penetrationDepthSolver(penetrationDepthSolver),
+m_simplexSolver(simplexSolver),
+m_minkowskiA(objectA),
+m_minkowskiB(objectB),
+m_ignoreMargin(false),
+m_partId0(-1),
+m_index0(-1),
+m_partId1(-1),
+m_index1(-1)
+{
+}
+
+void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
+{
+ btScalar distance=0.f;
+ btVector3 normalInB(0.f,0.f,0.f);
+ btVector3 pointOnA,pointOnB;
+
+ float marginA = m_minkowskiA->getMargin();
+ float marginB = m_minkowskiB->getMargin();
+
+ //for CCD we don't use margins
+ if (m_ignoreMargin)
+ {
+ marginA = 0.f;
+ marginB = 0.f;
+ }
+
+int curIter = 0;
+
+ bool isValid = false;
+ bool checkSimplex = false;
+ bool checkPenetration = true;
+
+ {
+ btScalar squaredDistance = SIMD_INFINITY;
+ btScalar delta = 0.f;
+
+ btScalar margin = marginA + marginB;
+
+
+
+ m_simplexSolver->reset();
+
+ while (true)
+ {
+
+ btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
+ btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
+
+ btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
+ btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
+ btPoint3 pWorld = input.m_transformA(pInA);
+ btPoint3 qWorld = input.m_transformB(qInB);
+
+ btVector3 w = pWorld - qWorld;
+ delta = m_cachedSeparatingAxis.dot(w);
+
+ // potential exit, they don't overlap
+ if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
+ {
+ checkPenetration = false;
+ break;
+ }
+
+ //exit 0: the new point is already in the simplex, or we didn't come any closer
+ if (m_simplexSolver->inSimplex(w))
+ {
+ checkSimplex = true;
+ break;
+ }
+ // are we getting any closer ?
+ if (squaredDistance - delta <= squaredDistance * rel_error2)
+ {
+ checkSimplex = true;
+ break;
+ }
+ //add current vertex to simplex
+ m_simplexSolver->addVertex(w, pWorld, qWorld);
+
+ //calculate the closest point to the origin (update vector v)
+ if (!m_simplexSolver->closest(m_cachedSeparatingAxis))
+ {
+ checkSimplex = true;
+ break;
+ }
+
+ btScalar previousSquaredDistance = squaredDistance;
+ squaredDistance = m_cachedSeparatingAxis.length2();
+
+ //redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
+
+ //are we getting any closer ?
+ if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
+ {
+ m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+ checkSimplex = true;
+ break;
+ }
+ bool check = (!m_simplexSolver->fullSimplex());
+ //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
+
+ if (!check)
+ {
+ //do we need this backup_closest here ?
+ m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
+ break;
+ }
+ }
+
+ if (checkSimplex)
+ {
+ m_simplexSolver->compute_points(pointOnA, pointOnB);
+ normalInB = pointOnA-pointOnB;
+ float lenSqr = m_cachedSeparatingAxis.length2();
+ //valid normal
+ if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+ {
+ float rlen = 1.f / btSqrt(lenSqr );
+ normalInB *= rlen; //normalize
+ btScalar s = btSqrt(squaredDistance);
+ ASSERT(s > btScalar(0.0));
+ pointOnA -= m_cachedSeparatingAxis * (marginA / s);
+ pointOnB += m_cachedSeparatingAxis * (marginB / s);
+ distance = ((1.f/rlen) - margin);
+ isValid = true;
+ }
+ }
+
+ if (checkPenetration && !isValid)
+ {
+ //penetration case
+
+ //if there is no way to handle penetrations, bail out
+ if (m_penetrationDepthSolver)
+ {
+ // Penetration depth case.
+ isValid = m_penetrationDepthSolver->calcPenDepth(
+ *m_simplexSolver,
+ m_minkowskiA,m_minkowskiB,
+ input.m_transformA,input.m_transformB,
+ m_cachedSeparatingAxis, pointOnA, pointOnB,
+ debugDraw
+ );
+
+ if (isValid)
+ {
+ normalInB = pointOnB-pointOnA;
+ float lenSqr = normalInB.length2();
+ if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
+ {
+ normalInB /= btSqrt(lenSqr);
+ distance = -(pointOnA-pointOnB).length();
+ } else
+ {
+ isValid = false;
+ }
+ }
+ }
+ }
+ }
+
+ if (isValid)
+ {
+#ifdef __SPU__
+ //spu_printf("distance\n");
+#endif //__CELLOS_LV2__
+
+ output.setShapeIdentifiers(m_partId0,m_index0,m_partId1,m_index1);
+
+ output.addContactPoint(
+ normalInB,
+ pointOnB,
+ distance);
+ //printf("gjk add:%f",distance);
+ }
+
+
+}
+
+
+
+
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
new file mode 100644
index 00000000000..bccb0542370
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h
@@ -0,0 +1,84 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+
+#ifndef GJK_PAIR_DETECTOR_H
+#define GJK_PAIR_DETECTOR_H
+
+#include "btDiscreteCollisionDetectorInterface.h"
+#include "LinearMath/btPoint3.h"
+
+#include <BulletCollision/CollisionShapes/btCollisionMargin.h>
+
+class btConvexShape;
+#include "btSimplexSolverInterface.h"
+class btConvexPenetrationDepthSolver;
+
+/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
+class btGjkPairDetector : public btDiscreteCollisionDetectorInterface
+{
+
+
+ btVector3 m_cachedSeparatingAxis;
+ btConvexPenetrationDepthSolver* m_penetrationDepthSolver;
+ btSimplexSolverInterface* m_simplexSolver;
+ btConvexShape* m_minkowskiA;
+ btConvexShape* m_minkowskiB;
+ bool m_ignoreMargin;
+
+
+public:
+
+ //experimental feature information, per triangle, per convex etc.
+ //'material combiner' / contact added callback
+ int m_partId0;
+ int m_index0;
+ int m_partId1;
+ int m_index1;
+
+ btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver);
+ virtual ~btGjkPairDetector() {};
+
+ virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw);
+
+ void setMinkowskiA(btConvexShape* minkA)
+ {
+ m_minkowskiA = minkA;
+ }
+
+ void setMinkowskiB(btConvexShape* minkB)
+ {
+ m_minkowskiB = minkB;
+ }
+ void setCachedSeperatingAxis(const btVector3& seperatingAxis)
+ {
+ m_cachedSeparatingAxis = seperatingAxis;
+ }
+
+ void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver)
+ {
+ m_penetrationDepthSolver = penetrationDepthSolver;
+ }
+
+ void setIgnoreMargin(bool ignoreMargin)
+ {
+ m_ignoreMargin = ignoreMargin;
+ }
+
+};
+
+#endif //GJK_PAIR_DETECTOR_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
new file mode 100644
index 00000000000..00a9206fef0
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
@@ -0,0 +1,98 @@
+/*
+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 MANIFOLD_CONTACT_POINT_H
+#define MANIFOLD_CONTACT_POINT_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransformUtil.h"
+
+
+
+
+
+/// ManifoldContactPoint collects and maintains persistent contactpoints.
+/// used to improve stability and performance of rigidbody dynamics response.
+class btManifoldPoint
+ {
+ public:
+ btManifoldPoint()
+ :m_userPersistentData(0)
+ {
+ }
+
+ btManifoldPoint( const btVector3 &pointA, const btVector3 &pointB,
+ const btVector3 &normal,
+ btScalar distance ) :
+ m_localPointA( pointA ),
+ m_localPointB( pointB ),
+ m_normalWorldOnB( normal ),
+ m_distance1( distance ),
+ m_combinedFriction(0.f),
+ m_combinedRestitution(0.f),
+ m_userPersistentData(0),
+ m_lifeTime(0)
+ {
+
+
+ }
+
+
+
+ btVector3 m_localPointA;
+ btVector3 m_localPointB;
+ btVector3 m_positionWorldOnB;
+ ///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity
+ btVector3 m_positionWorldOnA;
+ btVector3 m_normalWorldOnB;
+
+ float m_distance1;
+ float m_combinedFriction;
+ float m_combinedRestitution;
+
+
+ void* m_userPersistentData;
+
+ int m_lifeTime;//lifetime of the contactpoint in frames
+
+ float getDistance() const
+ {
+ return m_distance1;
+ }
+ int getLifeTime() const
+ {
+ return m_lifeTime;
+ }
+
+ btVector3 getPositionWorldOnA() {
+ return m_positionWorldOnA;
+// return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
+ }
+
+ const btVector3& getPositionWorldOnB()
+ {
+ return m_positionWorldOnB;
+ }
+
+ void setDistance(float dist)
+ {
+ m_distance1 = dist;
+ }
+
+
+
+ };
+
+#endif //MANIFOLD_CONTACT_POINT_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
new file mode 100644
index 00000000000..34daacf26ac
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp
@@ -0,0 +1,246 @@
+/*
+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 "btMinkowskiPenetrationDepthSolver.h"
+#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
+
+
+struct MyResult : public btDiscreteCollisionDetectorInterface::Result
+{
+
+ MyResult():m_hasResult(false)
+ {
+ }
+
+ btVector3 m_normalOnBInWorld;
+ btVector3 m_pointInWorld;
+ float m_depth;
+ bool m_hasResult;
+
+ virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)
+ {
+ }
+ void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
+ {
+ m_normalOnBInWorld = normalOnBInWorld;
+ m_pointInWorld = pointInWorld;
+ m_depth = depth;
+ m_hasResult = true;
+ }
+};
+
+#define NUM_UNITSPHERE_POINTS 42
+static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS] =
+{
+btVector3(0.000000f , -0.000000f,-1.000000f),
+btVector3(0.723608f , -0.525725f,-0.447219f),
+btVector3(-0.276388f , -0.850649f,-0.447219f),
+btVector3(-0.894426f , -0.000000f,-0.447216f),
+btVector3(-0.276388f , 0.850649f,-0.447220f),
+btVector3(0.723608f , 0.525725f,-0.447219f),
+btVector3(0.276388f , -0.850649f,0.447220f),
+btVector3(-0.723608f , -0.525725f,0.447219f),
+btVector3(-0.723608f , 0.525725f,0.447219f),
+btVector3(0.276388f , 0.850649f,0.447219f),
+btVector3(0.894426f , 0.000000f,0.447216f),
+btVector3(-0.000000f , 0.000000f,1.000000f),
+btVector3(0.425323f , -0.309011f,-0.850654f),
+btVector3(-0.162456f , -0.499995f,-0.850654f),
+btVector3(0.262869f , -0.809012f,-0.525738f),
+btVector3(0.425323f , 0.309011f,-0.850654f),
+btVector3(0.850648f , -0.000000f,-0.525736f),
+btVector3(-0.525730f , -0.000000f,-0.850652f),
+btVector3(-0.688190f , -0.499997f,-0.525736f),
+btVector3(-0.162456f , 0.499995f,-0.850654f),
+btVector3(-0.688190f , 0.499997f,-0.525736f),
+btVector3(0.262869f , 0.809012f,-0.525738f),
+btVector3(0.951058f , 0.309013f,0.000000f),
+btVector3(0.951058f , -0.309013f,0.000000f),
+btVector3(0.587786f , -0.809017f,0.000000f),
+btVector3(0.000000f , -1.000000f,0.000000f),
+btVector3(-0.587786f , -0.809017f,0.000000f),
+btVector3(-0.951058f , -0.309013f,-0.000000f),
+btVector3(-0.951058f , 0.309013f,-0.000000f),
+btVector3(-0.587786f , 0.809017f,-0.000000f),
+btVector3(-0.000000f , 1.000000f,-0.000000f),
+btVector3(0.587786f , 0.809017f,-0.000000f),
+btVector3(0.688190f , -0.499997f,0.525736f),
+btVector3(-0.262869f , -0.809012f,0.525738f),
+btVector3(-0.850648f , 0.000000f,0.525736f),
+btVector3(-0.262869f , 0.809012f,0.525738f),
+btVector3(0.688190f , 0.499997f,0.525736f),
+btVector3(0.525730f , 0.000000f,0.850652f),
+btVector3(0.162456f , -0.499995f,0.850654f),
+btVector3(-0.425323f , -0.309011f,0.850654f),
+btVector3(-0.425323f , 0.309011f,0.850654f),
+btVector3(0.162456f , 0.499995f,0.850654f)
+};
+
+
+bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
+ btConvexShape* convexA,btConvexShape* convexB,
+ const btTransform& transA,const btTransform& transB,
+ btVector3& v, btPoint3& pa, btPoint3& pb,
+ class btIDebugDraw* debugDraw
+ )
+{
+
+ //just take fixed number of orientation, and sample the penetration depth in that direction
+ float minProj = 1e30f;
+ btVector3 minNorm;
+ btVector3 minVertex;
+ btVector3 minA,minB;
+ btVector3 seperatingAxisInA,seperatingAxisInB;
+ btVector3 pInA,qInB,pWorld,qWorld,w;
+
+#define USE_BATCHED_SUPPORT 1
+#ifdef USE_BATCHED_SUPPORT
+ btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS];
+ btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS];
+ btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS];
+ btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS];
+ int i;
+
+ for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
+ {
+ const btVector3& norm = sPenetrationDirections[i];
+ seperatingAxisInABatch[i] = (-norm)* transA.getBasis();
+ seperatingAxisInBBatch[i] = norm * transB.getBasis();
+ }
+
+ convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,NUM_UNITSPHERE_POINTS);
+ convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,NUM_UNITSPHERE_POINTS);
+ for (i=0;i<NUM_UNITSPHERE_POINTS;i++)
+ {
+ const btVector3& norm = sPenetrationDirections[i];
+ seperatingAxisInA = seperatingAxisInABatch[i];
+ seperatingAxisInB = seperatingAxisInBBatch[i];
+
+ pInA = supportVerticesABatch[i];
+ qInB = supportVerticesBBatch[i];
+
+ pWorld = transA(pInA);
+ qWorld = transB(qInB);
+ w = qWorld - pWorld;
+ float delta = norm.dot(w);
+ //find smallest delta
+ if (delta < minProj)
+ {
+ minProj = delta;
+ minNorm = norm;
+ minA = pWorld;
+ minB = qWorld;
+ }
+ }
+#else
+ for (int i=0;i<NUM_UNITSPHERE_POINTS;i++)
+ {
+ const btVector3& norm = sPenetrationDirections[i];
+ seperatingAxisInA = (-norm)* transA.getBasis();
+ seperatingAxisInB = norm* transB.getBasis();
+ pInA = convexA->localGetSupportingVertexWithoutMargin(seperatingAxisInA);
+ qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB);
+ pWorld = transA(pInA);
+ qWorld = transB(qInB);
+ w = qWorld - pWorld;
+ float delta = norm.dot(w);
+ //find smallest delta
+ if (delta < minProj)
+ {
+ minProj = delta;
+ minNorm = norm;
+ minA = pWorld;
+ minB = qWorld;
+ }
+ }
+#endif //USE_BATCHED_SUPPORT
+
+ //add the margins
+
+ minA += minNorm*convexA->getMargin();
+ minB -= minNorm*convexB->getMargin();
+ minProj += (convexA->getMargin() + convexB->getMargin());
+
+
+
+
+//#define DEBUG_DRAW 1
+#ifdef DEBUG_DRAW
+ if (debugDraw)
+ {
+ btVector3 color(0,1,0);
+ debugDraw->drawLine(minA,minB,color);
+ color = btVector3 (1,1,1);
+ btVector3 vec = minB-minA;
+ float prj2 = minNorm.dot(vec);
+ debugDraw->drawLine(minA,minA+(minNorm*minProj),color);
+
+ }
+#endif //DEBUG_DRAW
+
+
+
+ btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
+
+ btScalar offsetDist = minProj;
+ btVector3 offset = minNorm * offsetDist;
+
+
+
+ btGjkPairDetector::ClosestPointInput input;
+
+ btVector3 newOrg = transA.getOrigin() + offset;
+
+ btTransform displacedTrans = transA;
+ displacedTrans.setOrigin(newOrg);
+
+ input.m_transformA = displacedTrans;
+ input.m_transformB = transB;
+ input.m_maximumDistanceSquared = 1e30f;//minProj;
+
+ MyResult res;
+ gjkdet.getClosestPoints(input,res,debugDraw);
+
+ float correctedMinNorm = minProj - res.m_depth;
+
+
+ //the penetration depth is over-estimated, relax it
+ float penetration_relaxation= 1.f;
+ minNorm*=penetration_relaxation;
+
+ if (res.m_hasResult)
+ {
+
+ pa = res.m_pointInWorld - minNorm * correctedMinNorm;
+ pb = res.m_pointInWorld;
+
+#ifdef DEBUG_DRAW
+ if (debugDraw)
+ {
+ btVector3 color(1,0,0);
+ debugDraw->drawLine(pa,pb,color);
+ }
+#endif//DEBUG_DRAW
+
+
+ }
+ return res.m_hasResult;
+}
+
+
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
new file mode 100644
index 00000000000..c287195317e
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h
@@ -0,0 +1,37 @@
+/*
+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 MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+
+#include "btConvexPenetrationDepthSolver.h"
+
+///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
+///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
+class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver
+{
+public:
+
+ virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver,
+ btConvexShape* convexA,btConvexShape* convexB,
+ const btTransform& transA,const btTransform& transB,
+ btVector3& v, btPoint3& pa, btPoint3& pb,
+ class btIDebugDraw* debugDraw
+ );
+
+};
+
+#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
new file mode 100644
index 00000000000..fafceafa5ed
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
@@ -0,0 +1,246 @@
+/*
+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 "btPersistentManifold.h"
+#include "LinearMath/btTransform.h"
+#include <assert.h>
+
+float gContactBreakingTreshold = 0.02f;
+ContactDestroyedCallback gContactDestroyedCallback = 0;
+
+
+
+btPersistentManifold::btPersistentManifold()
+:m_body0(0),
+m_body1(0),
+m_cachedPoints (0),
+m_index1(0)
+{
+}
+
+
+void btPersistentManifold::clearManifold()
+{
+ int i;
+ for (i=0;i<m_cachedPoints;i++)
+ {
+ clearUserCache(m_pointCache[i]);
+ }
+ m_cachedPoints = 0;
+}
+
+#ifdef DEBUG_PERSISTENCY
+#include <stdio.h>
+void btPersistentManifold::DebugPersistency()
+{
+ int i;
+ printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
+ for (i=0;i<m_cachedPoints;i++)
+ {
+ printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
+ }
+}
+#endif //DEBUG_PERSISTENCY
+
+void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
+{
+
+ void* oldPtr = pt.m_userPersistentData;
+ if (oldPtr)
+ {
+#ifdef DEBUG_PERSISTENCY
+ int i;
+ int occurance = 0;
+ for (i=0;i<m_cachedPoints;i++)
+ {
+ if (m_pointCache[i].m_userPersistentData == oldPtr)
+ {
+ occurance++;
+ if (occurance>1)
+ printf("error in clearUserCache\n");
+ }
+ }
+ assert(occurance<=0);
+#endif //DEBUG_PERSISTENCY
+
+ if (pt.m_userPersistentData && gContactDestroyedCallback)
+ {
+ (*gContactDestroyedCallback)(pt.m_userPersistentData);
+ pt.m_userPersistentData = 0;
+ }
+
+#ifdef DEBUG_PERSISTENCY
+ DebugPersistency();
+#endif
+ }
+
+
+}
+
+
+int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt)
+{
+
+ //calculate 4 possible cases areas, and take biggest area
+ //also need to keep 'deepest'
+
+ int maxPenetrationIndex = -1;
+#define KEEP_DEEPEST_POINT 1
+#ifdef KEEP_DEEPEST_POINT
+ float maxPenetration = pt.getDistance();
+ for (int i=0;i<4;i++)
+ {
+ if (m_pointCache[i].getDistance() < maxPenetration)
+ {
+ maxPenetrationIndex = i;
+ maxPenetration = m_pointCache[i].getDistance();
+ }
+ }
+#endif //KEEP_DEEPEST_POINT
+
+ btScalar res0(0.f),res1(0.f),res2(0.f),res3(0.f);
+ if (maxPenetrationIndex != 0)
+ {
+ btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
+ btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+ btVector3 cross = a0.cross(b0);
+ res0 = cross.length2();
+ }
+ if (maxPenetrationIndex != 1)
+ {
+ btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+ btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
+ btVector3 cross = a1.cross(b1);
+ res1 = cross.length2();
+ }
+
+ if (maxPenetrationIndex != 2)
+ {
+ btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+ btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
+ btVector3 cross = a2.cross(b2);
+ res2 = cross.length2();
+ }
+
+ if (maxPenetrationIndex != 3)
+ {
+ btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
+ btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
+ btVector3 cross = a3.cross(b3);
+ res3 = cross.length2();
+ }
+
+ btVector4 maxvec(res0,res1,res2,res3);
+ int biggestarea = maxvec.closestAxis4();
+ return biggestarea;
+}
+
+
+int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
+{
+ btScalar shortestDist = getContactBreakingTreshold() * getContactBreakingTreshold();
+ int size = getNumContacts();
+ int nearestPoint = -1;
+ for( int i = 0; i < size; i++ )
+ {
+ const btManifoldPoint &mp = m_pointCache[i];
+
+ btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA;
+ const btScalar distToManiPoint = diffA.dot(diffA);
+ if( distToManiPoint < shortestDist )
+ {
+ shortestDist = distToManiPoint;
+ nearestPoint = i;
+ }
+ }
+ return nearestPoint;
+}
+
+void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint)
+{
+ assert(validContactDistance(newPoint));
+
+ int insertIndex = getNumContacts();
+ if (insertIndex == MANIFOLD_CACHE_SIZE)
+ {
+#if MANIFOLD_CACHE_SIZE >= 4
+ //sort cache so best points come first, based on area
+ insertIndex = sortCachedPoints(newPoint);
+#else
+ insertIndex = 0;
+#endif
+
+
+ } else
+ {
+ m_cachedPoints++;
+
+
+ }
+ replaceContactPoint(newPoint,insertIndex);
+}
+
+float btPersistentManifold::getContactBreakingTreshold() const
+{
+ return gContactBreakingTreshold;
+}
+
+void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
+{
+ int i;
+
+ /// first refresh worldspace positions and distance
+ for (i=getNumContacts()-1;i>=0;i--)
+ {
+ btManifoldPoint &manifoldPoint = m_pointCache[i];
+ manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
+ manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
+ manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
+ manifoldPoint.m_lifeTime++;
+ }
+
+ /// then
+ btScalar distance2d;
+ btVector3 projectedDifference,projectedPoint;
+ for (i=getNumContacts()-1;i>=0;i--)
+ {
+
+ btManifoldPoint &manifoldPoint = m_pointCache[i];
+ //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
+ if (!validContactDistance(manifoldPoint))
+ {
+ removeContactPoint(i);
+ } else
+ {
+ //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
+ projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
+ projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
+ distance2d = projectedDifference.dot(projectedDifference);
+ if (distance2d > getContactBreakingTreshold()*getContactBreakingTreshold() )
+ {
+ removeContactPoint(i);
+ }
+ }
+ }
+#ifdef DEBUG_PERSISTENCY
+ DebugPersistency();
+#endif //
+}
+
+
+
+
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
new file mode 100644
index 00000000000..d0cc2577fb0
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
@@ -0,0 +1,140 @@
+/*
+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 PERSISTENT_MANIFOLD_H
+#define PERSISTENT_MANIFOLD_H
+
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
+#include "btManifoldPoint.h"
+
+struct btCollisionResult;
+
+///contact breaking and merging treshold
+extern float gContactBreakingTreshold;
+
+typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
+extern ContactDestroyedCallback gContactDestroyedCallback;
+
+
+
+
+#define MANIFOLD_CACHE_SIZE 4
+
+///btPersistentManifold maintains contact points, and reduces them to 4.
+///It does contact filtering/contact reduction.
+class btPersistentManifold
+{
+
+ btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE];
+
+ /// this two body pointers can point to the physics rigidbody class.
+ /// void* will allow any rigidbody class
+ void* m_body0;
+ void* m_body1;
+ int m_cachedPoints;
+
+
+ /// sort cached points so most isolated points come first
+ int sortCachedPoints(const btManifoldPoint& pt);
+
+ int findContactPoint(const btManifoldPoint* unUsed, int numUnused,const btManifoldPoint& pt);
+
+public:
+
+ int m_index1;
+
+ btPersistentManifold();
+
+ btPersistentManifold(void* body0,void* body1)
+ : m_body0(body0),m_body1(body1),m_cachedPoints(0)
+ {
+ }
+
+ inline void* getBody0() { return m_body0;}
+ inline void* getBody1() { return m_body1;}
+
+ inline const void* getBody0() const { return m_body0;}
+ inline const void* getBody1() const { return m_body1;}
+
+ void setBodies(void* body0,void* body1)
+ {
+ m_body0 = body0;
+ m_body1 = body1;
+ }
+
+ void clearUserCache(btManifoldPoint& pt);
+
+#ifdef DEBUG_PERSISTENCY
+ void DebugPersistency();
+#endif //
+
+ inline int getNumContacts() const { return m_cachedPoints;}
+
+ inline const btManifoldPoint& getContactPoint(int index) const
+ {
+ ASSERT(index < m_cachedPoints);
+ return m_pointCache[index];
+ }
+
+ inline btManifoldPoint& getContactPoint(int index)
+ {
+ ASSERT(index < m_cachedPoints);
+ return m_pointCache[index];
+ }
+
+ /// todo: get this margin from the current physics / collision environment
+ float getContactBreakingTreshold() const;
+
+ int getCacheEntry(const btManifoldPoint& newPoint) const;
+
+ void AddManifoldPoint( const btManifoldPoint& newPoint);
+
+ void removeContactPoint (int index)
+ {
+ clearUserCache(m_pointCache[index]);
+
+ int lastUsedIndex = getNumContacts() - 1;
+ m_pointCache[index] = m_pointCache[lastUsedIndex];
+ //get rid of duplicated userPersistentData pointer
+ m_pointCache[lastUsedIndex].m_userPersistentData = 0;
+ m_cachedPoints--;
+ }
+ void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex)
+ {
+ assert(validContactDistance(newPoint));
+
+ clearUserCache(m_pointCache[insertIndex]);
+
+ m_pointCache[insertIndex] = newPoint;
+ }
+
+ bool validContactDistance(const btManifoldPoint& pt) const
+ {
+ return pt.m_distance1 <= getContactBreakingTreshold();
+ }
+ /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
+ void refreshContactPoints( const btTransform& trA,const btTransform& trB);
+
+ void clearManifold();
+
+
+
+};
+
+
+
+#endif //PERSISTENT_MANIFOLD_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h
new file mode 100644
index 00000000000..a51e2d5e13b
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h
@@ -0,0 +1,57 @@
+/*
+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 POINT_COLLECTOR_H
+#define POINT_COLLECTOR_H
+
+#include "btDiscreteCollisionDetectorInterface.h"
+
+
+
+struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result
+{
+
+
+ btVector3 m_normalOnBInWorld;
+ btVector3 m_pointInWorld;
+ btScalar m_distance;//negative means penetration
+
+ bool m_hasResult;
+
+ btPointCollector ()
+ : m_distance(1e30f),m_hasResult(false)
+ {
+ }
+
+ virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)
+ {
+ //??
+ }
+
+ virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
+ {
+ if (depth< m_distance)
+ {
+ m_hasResult = true;
+ m_normalOnBInWorld = normalOnBInWorld;
+ m_pointInWorld = pointInWorld;
+ //negative means penetration
+ m_distance = depth;
+ }
+ }
+};
+
+#endif //POINT_COLLECTOR_H
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
new file mode 100644
index 00000000000..88ee005792c
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
@@ -0,0 +1,101 @@
+/*
+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 "btRaycastCallback.h"
+
+btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to)
+ :
+ m_from(from),
+ m_to(to),
+ m_hitFraction(1.f)
+{
+
+}
+
+
+
+void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
+{
+
+
+ const btVector3 &vert0=triangle[0];
+ const btVector3 &vert1=triangle[1];
+ const btVector3 &vert2=triangle[2];
+
+ btVector3 v10; v10 = vert1 - vert0 ;
+ btVector3 v20; v20 = vert2 - vert0 ;
+
+ btVector3 triangleNormal; triangleNormal = v10.cross( v20 );
+
+ const float dist = vert0.dot(triangleNormal);
+ float dist_a = triangleNormal.dot(m_from) ;
+ dist_a-= dist;
+ float dist_b = triangleNormal.dot(m_to);
+ dist_b -= dist;
+
+ if ( dist_a * dist_b >= 0.0f)
+ {
+ return ; // same sign
+ }
+
+ const float proj_length=dist_a-dist_b;
+ const float distance = (dist_a)/(proj_length);
+ // Now we have the intersection point on the plane, we'll see if it's inside the triangle
+ // Add an epsilon as a tolerance for the raycast,
+ // in case the ray hits exacly on the edge of the triangle.
+ // It must be scaled for the triangle size.
+
+ if(distance < m_hitFraction)
+ {
+
+
+ float edge_tolerance =triangleNormal.length2();
+ edge_tolerance *= -0.0001f;
+ btVector3 point; point.setInterpolate3( m_from, m_to, distance);
+ {
+ btVector3 v0p; v0p = vert0 - point;
+ btVector3 v1p; v1p = vert1 - point;
+ btVector3 cp0; cp0 = v0p.cross( v1p );
+
+ if ( (float)(cp0.dot(triangleNormal)) >=edge_tolerance)
+ {
+
+
+ btVector3 v2p; v2p = vert2 - point;
+ btVector3 cp1;
+ cp1 = v1p.cross( v2p);
+ if ( (float)(cp1.dot(triangleNormal)) >=edge_tolerance)
+ {
+ btVector3 cp2;
+ cp2 = v2p.cross(v0p);
+
+ if ( (float)(cp2.dot(triangleNormal)) >=edge_tolerance)
+ {
+
+ if ( dist_a > 0 )
+ {
+ m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
+ }
+ else
+ {
+ m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex);
+ }
+ }
+ }
+ }
+ }
+ }
+}
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
new file mode 100644
index 00000000000..fbb51d30522
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
@@ -0,0 +1,42 @@
+/*
+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 RAYCAST_TRI_CALLBACK_H
+#define RAYCAST_TRI_CALLBACK_H
+
+#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
+struct btBroadphaseProxy;
+
+
+class btTriangleRaycastCallback: public btTriangleCallback
+{
+public:
+
+ //input
+ btVector3 m_from;
+ btVector3 m_to;
+
+ float m_hitFraction;
+
+ btTriangleRaycastCallback(const btVector3& from,const btVector3& to);
+
+ virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
+
+ virtual float reportHit(const btVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) = 0;
+
+};
+
+#endif //RAYCAST_TRI_CALLBACK_H
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
new file mode 100644
index 00000000000..cf65f46505b
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h
@@ -0,0 +1,64 @@
+/*
+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 SIMPLEX_SOLVER_INTERFACE_H
+#define SIMPLEX_SOLVER_INTERFACE_H
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btPoint3.h"
+
+#define NO_VIRTUAL_INTERFACE 1
+#ifdef NO_VIRTUAL_INTERFACE
+#include "btVoronoiSimplexSolver.h"
+#define btSimplexSolverInterface btVoronoiSimplexSolver
+#else
+
+/// btSimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices
+/// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on
+/// voronoi regions or barycentric coordinates
+class btSimplexSolverInterface
+{
+ public:
+ virtual ~btSimplexSolverInterface() {};
+
+ virtual void reset() = 0;
+
+ virtual void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q) = 0;
+
+ virtual bool closest(btVector3& v) = 0;
+
+ virtual btScalar maxVertex() = 0;
+
+ virtual bool fullSimplex() const = 0;
+
+ virtual int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const = 0;
+
+ virtual bool inSimplex(const btVector3& w) = 0;
+
+ virtual void backup_closest(btVector3& v) = 0;
+
+ virtual bool emptySimplex() const = 0;
+
+ virtual void compute_points(btPoint3& p1, btPoint3& p2) = 0;
+
+ virtual int numVertices() const =0;
+
+
+};
+#endif
+#endif //SIMPLEX_SOLVER_INTERFACE_H
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
new file mode 100644
index 00000000000..dc995ca1f72
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
@@ -0,0 +1,133 @@
+/*
+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 "btSubSimplexConvexCast.h"
+#include "BulletCollision/CollisionShapes/btConvexShape.h"
+#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
+#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
+
+
+btSubsimplexConvexCast::btSubsimplexConvexCast (btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)
+:m_simplexSolver(simplexSolver),
+m_convexA(convexA),m_convexB(convexB)
+{
+}
+
+///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
+///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
+#define MAX_ITERATIONS 32
+
+bool btSubsimplexConvexCast::calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result)
+{
+
+ btMinkowskiSumShape combi(m_convexA,m_convexB);
+ btMinkowskiSumShape* convex = &combi;
+
+ btTransform rayFromLocalA;
+ btTransform rayToLocalA;
+
+ rayFromLocalA = fromA.inverse()* fromB;
+ rayToLocalA = toA.inverse()* toB;
+
+
+ m_simplexSolver->reset();
+
+ convex->setTransformB(btTransform(rayFromLocalA.getBasis()));
+
+ //float radius = 0.01f;
+
+ btScalar lambda = 0.f;
+ //todo: need to verify this:
+ //because of minkowski difference, we need the inverse direction
+
+ btVector3 s = -rayFromLocalA.getOrigin();
+ btVector3 r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin());
+ btVector3 x = s;
+ btVector3 v;
+ btVector3 arbitraryPoint = convex->localGetSupportingVertex(r);
+
+ v = x - arbitraryPoint;
+
+ int maxIter = MAX_ITERATIONS;
+
+ btVector3 n;
+ n.setValue(0.f,0.f,0.f);
+ bool hasResult = false;
+ btVector3 c;
+
+ float lastLambda = lambda;
+
+
+ float dist2 = v.length2();
+ float epsilon = 0.0001f;
+
+ btVector3 w,p;
+ float VdotR;
+
+ while ( (dist2 > epsilon) && maxIter--)
+ {
+ p = convex->localGetSupportingVertex( v);
+ w = x - p;
+
+ float VdotW = v.dot(w);
+
+ if ( VdotW > 0.f)
+ {
+ VdotR = v.dot(r);
+
+ if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
+ return false;
+ else
+ {
+ lambda = lambda - VdotW / VdotR;
+ x = s + lambda * r;
+ m_simplexSolver->reset();
+ //check next line
+ w = x-p;
+ lastLambda = lambda;
+ n = v;
+ hasResult = true;
+ }
+ }
+ m_simplexSolver->addVertex( w, x , p);
+ if (m_simplexSolver->closest(v))
+ {
+ dist2 = v.length2();
+ hasResult = true;
+ //printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
+ //printf("DIST2=%f\n",dist2);
+ //printf("numverts = %i\n",m_simplexSolver->numVertices());
+ } else
+ {
+ dist2 = 0.f;
+ }
+ }
+
+ //int numiter = MAX_ITERATIONS - maxIter;
+// printf("number of iterations: %d", numiter);
+ result.m_fraction = lambda;
+ result.m_normal = n;
+
+ return true;
+}
+
+
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
new file mode 100644
index 00000000000..a2a3193b090
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
@@ -0,0 +1,50 @@
+/*
+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 SUBSIMPLEX_CONVEX_CAST_H
+#define SUBSIMPLEX_CONVEX_CAST_H
+
+#include "btConvexCast.h"
+#include "btSimplexSolverInterface.h"
+class btConvexShape;
+
+/// btSubsimplexConvexCast implements Gino van den Bergens' paper
+///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
+/// GJK based Ray Cast, optimized version
+/// Objects should not start in overlap, otherwise results are not defined.
+class btSubsimplexConvexCast : public btConvexCast
+{
+ btSimplexSolverInterface* m_simplexSolver;
+ btConvexShape* m_convexA;
+ btConvexShape* m_convexB;
+
+public:
+
+ btSubsimplexConvexCast (btConvexShape* shapeA,btConvexShape* shapeB,btSimplexSolverInterface* simplexSolver);
+
+ //virtual ~btSubsimplexConvexCast();
+ ///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
+ ///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
+ virtual bool calcTimeOfImpact(
+ const btTransform& fromA,
+ const btTransform& toA,
+ const btTransform& fromB,
+ const btTransform& toB,
+ CastResult& result);
+
+};
+
+#endif //SUBSIMPLEX_CONVEX_CAST_H
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
new file mode 100644
index 00000000000..23d66a3bbc8
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
@@ -0,0 +1,598 @@
+
+/*
+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.
+
+ Elsevier CDROM license agreements grants nonexclusive license to use the software
+ for any purpose, commercial or non-commercial as long as the following credit is included
+ identifying the original source of the software:
+
+ Parts of the source are "from the book Real-Time Collision Detection by
+ Christer Ericson, published by Morgan Kaufmann Publishers,
+ (c) 2005 Elsevier Inc."
+
+*/
+
+
+#include "btVoronoiSimplexSolver.h"
+#include <assert.h>
+#include <stdio.h>
+
+#define VERTA 0
+#define VERTB 1
+#define VERTC 2
+#define VERTD 3
+
+#define CATCH_DEGENERATE_TETRAHEDRON 1
+void btVoronoiSimplexSolver::removeVertex(int index)
+{
+
+ assert(m_numVertices>0);
+ m_numVertices--;
+ m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
+ m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
+ m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
+}
+
+void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts)
+{
+ if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
+ removeVertex(3);
+
+ if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
+ removeVertex(2);
+
+ if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
+ removeVertex(1);
+
+ if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
+ removeVertex(0);
+
+}
+
+
+
+
+
+//clear the simplex, remove all the vertices
+void btVoronoiSimplexSolver::reset()
+{
+ m_cachedValidClosest = false;
+ m_numVertices = 0;
+ m_needsUpdate = true;
+ m_lastW = btVector3(1e30f,1e30f,1e30f);
+ m_cachedBC.reset();
+}
+
+
+
+ //add a vertex
+void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
+{
+ m_lastW = w;
+ m_needsUpdate = true;
+
+ m_simplexVectorW[m_numVertices] = w;
+ m_simplexPointsP[m_numVertices] = p;
+ m_simplexPointsQ[m_numVertices] = q;
+
+ m_numVertices++;
+}
+
+bool btVoronoiSimplexSolver::updateClosestVectorAndPoints()
+{
+
+ if (m_needsUpdate)
+ {
+ m_cachedBC.reset();
+
+ m_needsUpdate = false;
+
+ switch (numVertices())
+ {
+ case 0:
+ m_cachedValidClosest = false;
+ break;
+ case 1:
+ {
+ m_cachedP1 = m_simplexPointsP[0];
+ m_cachedP2 = m_simplexPointsQ[0];
+ m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
+ m_cachedBC.reset();
+ m_cachedBC.setBarycentricCoordinates(1.f,0.f,0.f,0.f);
+ m_cachedValidClosest = m_cachedBC.isValid();
+ break;
+ };
+ case 2:
+ {
+ //closest point origin from line segment
+ const btVector3& from = m_simplexVectorW[0];
+ const btVector3& to = m_simplexVectorW[1];
+ btVector3 nearest;
+
+ btVector3 p (0.f,0.f,0.f);
+ btVector3 diff = p - from;
+ btVector3 v = to - from;
+ float t = v.dot(diff);
+
+ if (t > 0) {
+ float dotVV = v.dot(v);
+ if (t < dotVV) {
+ t /= dotVV;
+ diff -= t*v;
+ m_cachedBC.m_usedVertices.usedVertexA = true;
+ m_cachedBC.m_usedVertices.usedVertexB = true;
+ } else {
+ t = 1;
+ diff -= v;
+ //reduce to 1 point
+ m_cachedBC.m_usedVertices.usedVertexB = true;
+ }
+ } else
+ {
+ t = 0;
+ //reduce to 1 point
+ m_cachedBC.m_usedVertices.usedVertexA = true;
+ }
+ m_cachedBC.setBarycentricCoordinates(1-t,t);
+ nearest = from + t*v;
+
+ m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
+ m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
+ m_cachedV = m_cachedP1 - m_cachedP2;
+
+ reduceVertices(m_cachedBC.m_usedVertices);
+
+ m_cachedValidClosest = m_cachedBC.isValid();
+ break;
+ }
+ case 3:
+ {
+ //closest point origin from triangle
+ btVector3 p (0.f,0.f,0.f);
+
+ const btVector3& a = m_simplexVectorW[0];
+ const btVector3& b = m_simplexVectorW[1];
+ const btVector3& c = m_simplexVectorW[2];
+
+ closestPtPointTriangle(p,a,b,c,m_cachedBC);
+ m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+ m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+ m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
+ m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
+
+ m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+ m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+ m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
+ m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
+
+ m_cachedV = m_cachedP1-m_cachedP2;
+
+ reduceVertices (m_cachedBC.m_usedVertices);
+ m_cachedValidClosest = m_cachedBC.isValid();
+
+ break;
+ }
+ case 4:
+ {
+
+
+ btVector3 p (0.f,0.f,0.f);
+
+ const btVector3& a = m_simplexVectorW[0];
+ const btVector3& b = m_simplexVectorW[1];
+ const btVector3& c = m_simplexVectorW[2];
+ const btVector3& d = m_simplexVectorW[3];
+
+ bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
+
+ if (hasSeperation)
+ {
+
+ m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
+ m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
+ m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
+ m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
+
+ m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
+ m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
+ m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
+ m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
+
+ m_cachedV = m_cachedP1-m_cachedP2;
+ reduceVertices (m_cachedBC.m_usedVertices);
+ } else
+ {
+// printf("sub distance got penetration\n");
+
+ if (m_cachedBC.m_degenerate)
+ {
+ m_cachedValidClosest = false;
+ } else
+ {
+ m_cachedValidClosest = true;
+ //degenerate case == false, penetration = true + zero
+ m_cachedV.setValue(0.f,0.f,0.f);
+ }
+ break;
+ }
+
+ m_cachedValidClosest = m_cachedBC.isValid();
+
+ //closest point origin from tetrahedron
+ break;
+ }
+ default:
+ {
+ m_cachedValidClosest = false;
+ }
+ };
+ }
+
+ return m_cachedValidClosest;
+
+}
+
+//return/calculate the closest vertex
+bool btVoronoiSimplexSolver::closest(btVector3& v)
+{
+ bool succes = updateClosestVectorAndPoints();
+ v = m_cachedV;
+ return succes;
+}
+
+
+
+btScalar btVoronoiSimplexSolver::maxVertex()
+{
+ int i, numverts = numVertices();
+ btScalar maxV = 0.f;
+ for (i=0;i<numverts;i++)
+ {
+ btScalar curLen2 = m_simplexVectorW[i].length2();
+ if (maxV < curLen2)
+ maxV = curLen2;
+ }
+ return maxV;
+}
+
+
+
+ //return the current simplex
+int btVoronoiSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
+{
+ int i;
+ for (i=0;i<numVertices();i++)
+ {
+ yBuf[i] = m_simplexVectorW[i];
+ pBuf[i] = m_simplexPointsP[i];
+ qBuf[i] = m_simplexPointsQ[i];
+ }
+ return numVertices();
+}
+
+
+
+
+bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
+{
+ bool found = false;
+ int i, numverts = numVertices();
+ //btScalar maxV = 0.f;
+
+ //w is in the current (reduced) simplex
+ for (i=0;i<numverts;i++)
+ {
+ if (m_simplexVectorW[i] == w)
+ found = true;
+ }
+
+ //check in case lastW is already removed
+ if (w == m_lastW)
+ return true;
+
+ return found;
+}
+
+void btVoronoiSimplexSolver::backup_closest(btVector3& v)
+{
+ v = m_cachedV;
+}
+
+
+bool btVoronoiSimplexSolver::emptySimplex() const
+{
+ return (numVertices() == 0);
+
+}
+
+void btVoronoiSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2)
+{
+ updateClosestVectorAndPoints();
+ p1 = m_cachedP1;
+ p2 = m_cachedP2;
+
+}
+
+
+
+
+bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result)
+{
+ result.m_usedVertices.reset();
+
+ // Check if P in vertex region outside A
+ btVector3 ab = b - a;
+ btVector3 ac = c - a;
+ btVector3 ap = p - a;
+ float d1 = ab.dot(ap);
+ float d2 = ac.dot(ap);
+ if (d1 <= 0.0f && d2 <= 0.0f)
+ {
+ result.m_closestPointOnSimplex = a;
+ result.m_usedVertices.usedVertexA = true;
+ result.setBarycentricCoordinates(1,0,0);
+ return true;// a; // barycentric coordinates (1,0,0)
+ }
+
+ // Check if P in vertex region outside B
+ btVector3 bp = p - b;
+ float d3 = ab.dot(bp);
+ float d4 = ac.dot(bp);
+ if (d3 >= 0.0f && d4 <= d3)
+ {
+ result.m_closestPointOnSimplex = b;
+ result.m_usedVertices.usedVertexB = true;
+ result.setBarycentricCoordinates(0,1,0);
+
+ return true; // b; // barycentric coordinates (0,1,0)
+ }
+ // Check if P in edge region of AB, if so return projection of P onto AB
+ float vc = d1*d4 - d3*d2;
+ if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
+ float v = d1 / (d1 - d3);
+ result.m_closestPointOnSimplex = a + v * ab;
+ result.m_usedVertices.usedVertexA = true;
+ result.m_usedVertices.usedVertexB = true;
+ result.setBarycentricCoordinates(1-v,v,0);
+ return true;
+ //return a + v * ab; // barycentric coordinates (1-v,v,0)
+ }
+
+ // Check if P in vertex region outside C
+ btVector3 cp = p - c;
+ float d5 = ab.dot(cp);
+ float d6 = ac.dot(cp);
+ if (d6 >= 0.0f && d5 <= d6)
+ {
+ result.m_closestPointOnSimplex = c;
+ result.m_usedVertices.usedVertexC = true;
+ result.setBarycentricCoordinates(0,0,1);
+ return true;//c; // barycentric coordinates (0,0,1)
+ }
+
+ // Check if P in edge region of AC, if so return projection of P onto AC
+ float vb = d5*d2 - d1*d6;
+ if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
+ float w = d2 / (d2 - d6);
+ result.m_closestPointOnSimplex = a + w * ac;
+ result.m_usedVertices.usedVertexA = true;
+ result.m_usedVertices.usedVertexC = true;
+ result.setBarycentricCoordinates(1-w,0,w);
+ return true;
+ //return a + w * ac; // barycentric coordinates (1-w,0,w)
+ }
+
+ // Check if P in edge region of BC, if so return projection of P onto BC
+ float va = d3*d6 - d5*d4;
+ if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
+ float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+
+ result.m_closestPointOnSimplex = b + w * (c - b);
+ result.m_usedVertices.usedVertexB = true;
+ result.m_usedVertices.usedVertexC = true;
+ result.setBarycentricCoordinates(0,1-w,w);
+ return true;
+ // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
+ }
+
+ // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
+ float denom = 1.0f / (va + vb + vc);
+ float v = vb * denom;
+ float w = vc * denom;
+
+ result.m_closestPointOnSimplex = a + ab * v + ac * w;
+ result.m_usedVertices.usedVertexA = true;
+ result.m_usedVertices.usedVertexB = true;
+ result.m_usedVertices.usedVertexC = true;
+ result.setBarycentricCoordinates(1-v-w,v,w);
+
+ return true;
+// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0f - v - w
+
+}
+
+
+
+
+
+/// Test if point p and d lie on opposite sides of plane through abc
+int btVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d)
+{
+ btVector3 normal = (b-a).cross(c-a);
+
+ float signp = (p - a).dot(normal); // [AP AB AC]
+ float signd = (d - a).dot( normal); // [AD AB AC]
+
+#ifdef CATCH_DEGENERATE_TETRAHEDRON
+ if (signd * signd < (1e-4f * 1e-4f))
+ {
+// printf("affine dependent/degenerate\n");//
+ return -1;
+ }
+#endif
+ // Points on opposite sides if expression signs are opposite
+ return signp * signd < 0.f;
+}
+
+
+bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult)
+{
+ btSubSimplexClosestResult tempResult;
+
+ // Start out assuming point inside all halfspaces, so closest to itself
+ finalResult.m_closestPointOnSimplex = p;
+ finalResult.m_usedVertices.reset();
+ finalResult.m_usedVertices.usedVertexA = true;
+ finalResult.m_usedVertices.usedVertexB = true;
+ finalResult.m_usedVertices.usedVertexC = true;
+ finalResult.m_usedVertices.usedVertexD = true;
+
+ int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
+ int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
+ int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
+ int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
+
+ if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
+ {
+ finalResult.m_degenerate = true;
+ return false;
+ }
+
+ if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
+ {
+ return false;
+ }
+
+
+ float bestSqDist = FLT_MAX;
+ // If point outside face abc then compute closest point on abc
+ if (pointOutsideABC)
+ {
+ closestPtPointTriangle(p, a, b, c,tempResult);
+ btPoint3 q = tempResult.m_closestPointOnSimplex;
+
+ float sqDist = (q - p).dot( q - p);
+ // Update best closest point if (squared) distance is less than current best
+ if (sqDist < bestSqDist) {
+ bestSqDist = sqDist;
+ finalResult.m_closestPointOnSimplex = q;
+ //convert result bitmask!
+ finalResult.m_usedVertices.reset();
+ finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+ finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
+ finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+ finalResult.setBarycentricCoordinates(
+ tempResult.m_barycentricCoords[VERTA],
+ tempResult.m_barycentricCoords[VERTB],
+ tempResult.m_barycentricCoords[VERTC],
+ 0
+ );
+
+ }
+ }
+
+
+ // Repeat test for face acd
+ if (pointOutsideACD)
+ {
+ closestPtPointTriangle(p, a, c, d,tempResult);
+ btPoint3 q = tempResult.m_closestPointOnSimplex;
+ //convert result bitmask!
+
+ float sqDist = (q - p).dot( q - p);
+ if (sqDist < bestSqDist)
+ {
+ bestSqDist = sqDist;
+ finalResult.m_closestPointOnSimplex = q;
+ finalResult.m_usedVertices.reset();
+ finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+ finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
+ finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
+ finalResult.setBarycentricCoordinates(
+ tempResult.m_barycentricCoords[VERTA],
+ 0,
+ tempResult.m_barycentricCoords[VERTB],
+ tempResult.m_barycentricCoords[VERTC]
+ );
+
+ }
+ }
+ // Repeat test for face adb
+
+
+ if (pointOutsideADB)
+ {
+ closestPtPointTriangle(p, a, d, b,tempResult);
+ btPoint3 q = tempResult.m_closestPointOnSimplex;
+ //convert result bitmask!
+
+ float sqDist = (q - p).dot( q - p);
+ if (sqDist < bestSqDist)
+ {
+ bestSqDist = sqDist;
+ finalResult.m_closestPointOnSimplex = q;
+ finalResult.m_usedVertices.reset();
+ finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
+ finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+ finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
+ finalResult.setBarycentricCoordinates(
+ tempResult.m_barycentricCoords[VERTA],
+ tempResult.m_barycentricCoords[VERTC],
+ 0,
+ tempResult.m_barycentricCoords[VERTB]
+ );
+
+ }
+ }
+ // Repeat test for face bdc
+
+
+ if (pointOutsideBDC)
+ {
+ closestPtPointTriangle(p, b, d, c,tempResult);
+ btPoint3 q = tempResult.m_closestPointOnSimplex;
+ //convert result bitmask!
+ float sqDist = (q - p).dot( q - p);
+ if (sqDist < bestSqDist)
+ {
+ bestSqDist = sqDist;
+ finalResult.m_closestPointOnSimplex = q;
+ finalResult.m_usedVertices.reset();
+ finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
+ finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
+ finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
+
+ finalResult.setBarycentricCoordinates(
+ 0,
+ tempResult.m_barycentricCoords[VERTA],
+ tempResult.m_barycentricCoords[VERTC],
+ tempResult.m_barycentricCoords[VERTB]
+ );
+
+ }
+ }
+
+ //help! we ended up full !
+
+ if (finalResult.m_usedVertices.usedVertexA &&
+ finalResult.m_usedVertices.usedVertexB &&
+ finalResult.m_usedVertices.usedVertexC &&
+ finalResult.m_usedVertices.usedVertexD)
+ {
+ return true;
+ }
+
+ return true;
+}
+
diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
new file mode 100644
index 00000000000..8b743996915
--- /dev/null
+++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h
@@ -0,0 +1,157 @@
+/*
+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 btVoronoiSimplexSolver_H
+#define btVoronoiSimplexSolver_H
+
+#include "btSimplexSolverInterface.h"
+
+
+
+#define VORONOI_SIMPLEX_MAX_VERTS 5
+
+struct btUsageBitfield{
+ btUsageBitfield()
+ {
+ reset();
+ }
+
+ void reset()
+ {
+ usedVertexA = false;
+ usedVertexB = false;
+ usedVertexC = false;
+ usedVertexD = false;
+ }
+ unsigned short usedVertexA : 1;
+ unsigned short usedVertexB : 1;
+ unsigned short usedVertexC : 1;
+ unsigned short usedVertexD : 1;
+ unsigned short unused1 : 1;
+ unsigned short unused2 : 1;
+ unsigned short unused3 : 1;
+ unsigned short unused4 : 1;
+};
+
+
+struct btSubSimplexClosestResult
+{
+ btPoint3 m_closestPointOnSimplex;
+ //MASK for m_usedVertices
+ //stores the simplex vertex-usage, using the MASK,
+ // if m_usedVertices & MASK then the related vertex is used
+ btUsageBitfield m_usedVertices;
+ float m_barycentricCoords[4];
+ bool m_degenerate;
+
+ void reset()
+ {
+ m_degenerate = false;
+ setBarycentricCoordinates();
+ m_usedVertices.reset();
+ }
+ bool isValid()
+ {
+ bool valid = (m_barycentricCoords[0] >= 0.f) &&
+ (m_barycentricCoords[1] >= 0.f) &&
+ (m_barycentricCoords[2] >= 0.f) &&
+ (m_barycentricCoords[3] >= 0.f);
+
+
+ return valid;
+ }
+ void setBarycentricCoordinates(float a=0.f,float b=0.f,float c=0.f,float d=0.f)
+ {
+ m_barycentricCoords[0] = a;
+ m_barycentricCoords[1] = b;
+ m_barycentricCoords[2] = c;
+ m_barycentricCoords[3] = d;
+ }
+
+};
+
+/// btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin.
+/// Can be used with GJK, as an alternative to Johnson distance algorithm.
+#ifdef NO_VIRTUAL_INTERFACE
+class btVoronoiSimplexSolver
+#else
+class btVoronoiSimplexSolver : public btSimplexSolverInterface
+#endif
+{
+public:
+
+ int m_numVertices;
+
+ btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];
+ btPoint3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];
+ btPoint3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];
+
+
+
+ btPoint3 m_cachedP1;
+ btPoint3 m_cachedP2;
+ btVector3 m_cachedV;
+ btVector3 m_lastW;
+ bool m_cachedValidClosest;
+
+ btSubSimplexClosestResult m_cachedBC;
+
+ bool m_needsUpdate;
+
+ void removeVertex(int index);
+ void reduceVertices (const btUsageBitfield& usedVerts);
+ bool updateClosestVectorAndPoints();
+
+ bool closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult);
+ int pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d);
+ bool closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result);
+
+public:
+
+ void reset();
+
+ void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
+
+
+ bool closest(btVector3& v);
+
+ btScalar maxVertex();
+
+ bool fullSimplex() const
+ {
+ return (m_numVertices == 4);
+ }
+
+ int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const;
+
+ bool inSimplex(const btVector3& w);
+
+ void backup_closest(btVector3& v) ;
+
+ bool emptySimplex() const ;
+
+ void compute_points(btPoint3& p1, btPoint3& p2) ;
+
+ int numVertices() const
+ {
+ return m_numVertices;
+ }
+
+
+};
+
+#endif //VoronoiSimplexSolver