1 files changed, 250 insertions, 0 deletions

diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp
new file mode 100644
index 00000000000..42387ba271e
--- /dev/null
+++ b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp
@@ -0,0 +1,250 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability 
+ * of this software for any purpose.  
+ * It is provided "as is" without express or implied warranty.
+*/
+#include "ConvexConvexAlgorithm.h"
+
+#include <stdio.h>
+#include "NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h"
+#include "BroadphaseCollision/BroadphaseInterface.h"
+#include "Dynamics/RigidBody.h"
+#include "CollisionShapes/ConvexShape.h"
+#include "NarrowPhaseCollision/GjkPairDetector.h"
+#include "BroadphaseCollision/BroadphaseProxy.h"
+#include "BroadphaseCollision/CollisionDispatcher.h"
+#include "CollisionShapes/BoxShape.h"
+#include "CollisionDispatch/ManifoldResult.h"
+
+#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h"
+#include "NarrowPhaseCollision/ContinuousConvexCollision.h"
+#include "NarrowPhaseCollision/SubsimplexConvexCast.h"
+#include "NarrowPhaseCollision/GjkConvexCast.h"
+
+
+
+#include "CollisionShapes/MinkowskiSumShape.h"
+#include "NarrowPhaseCollision/VoronoiSimplexSolver.h"
+#include "CollisionShapes/SphereShape.h"
+
+
+#ifdef WIN32
+void DrawRasterizerLine(const float* from,const float* to,int color);
+#endif
+
+
+
+
+//#define PROCESS_SINGLE_CONTACT
+#ifdef WIN32
+bool gForceBoxBox = false;//false;//true;
+
+#else
+bool gForceBoxBox = false;//false;//true;
+#endif
+bool gBoxBoxUseGjk = true;//true;//false;
+bool gDisableConvexCollision = false;
+
+
+
+
+ConvexConvexAlgorithm::ConvexConvexAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
+: CollisionAlgorithm(ci),
+m_gjkPairDetector(0,0,&m_simplexSolver,&m_penetrationDepthSolver),
+m_box0(*proxy0),
+m_box1(*proxy1),
+m_collisionImpulse(0.f),
+m_ownManifold (false),
+m_manifoldPtr(mf),
+m_lowLevelOfDetail(false)
+
+{
+
+	
+	RigidBody* body0 = (RigidBody*)m_box0.m_clientObject;
+	RigidBody* body1 = (RigidBody*)m_box1.m_clientObject;
+
+	if ((body0->getInvMass() != 0.f) || 
+		(body1->getInvMass() != 0.f))
+	{
+		if (!m_manifoldPtr)
+		{
+			m_manifoldPtr = m_dispatcher->GetNewManifold(proxy0->m_clientObject,proxy1->m_clientObject);
+			m_ownManifold = true;
+		}
+	}
+
+}
+
+
+
+ConvexConvexAlgorithm::~ConvexConvexAlgorithm()
+{
+	if (m_ownManifold)
+	{
+		if (m_manifoldPtr)
+			m_dispatcher->ReleaseManifold(m_manifoldPtr);
+	}
+}
+
+void	ConvexConvexAlgorithm ::SetLowLevelOfDetail(bool useLowLevel)
+{
+	m_lowLevelOfDetail = useLowLevel;
+}
+
+float	ConvexConvexAlgorithm::GetCollisionImpulse() const
+{
+	if (m_manifoldPtr)
+		return m_manifoldPtr->GetCollisionImpulse();
+	
+	return 0.f;
+}
+
+
+class FlippedContactResult : public DiscreteCollisionDetectorInterface::Result
+{
+	DiscreteCollisionDetectorInterface::Result* m_org;
+
+public:
+
+	FlippedContactResult(DiscreteCollisionDetectorInterface::Result* org)
+		: m_org(org)
+	{
+
+	}
+
+	virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
+	{
+		SimdVector3 flippedNormal = -normalOnBInWorld;
+
+		m_org->AddContactPoint(flippedNormal,pointInWorld,depth);
+	}
+
+};
+
+bool extra = false;
+
+float gFriction = 0.5f;
+//
+// box-box collision algorithm, for simplicity also applies resolution-impulse
+//
+void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,float timeStep,int stepCount, bool useContinuous)
+{
+	
+//	printf("ConvexConvexAlgorithm::ProcessCollision\n");
+m_collisionImpulse = 0.f;
+	
+	RigidBody* body0 = (RigidBody*)m_box0.m_clientObject;
+	RigidBody* body1 = (RigidBody*)m_box1.m_clientObject;
+
+	if (!m_manifoldPtr)
+		return;
+
+	if ((body0->getInvMass() == 0.f) && 
+		(body1->getInvMass() == 0.f))
+	{
+		return;
+	}
+
+	ManifoldResult output(body0,body1,m_manifoldPtr);
+	
+	ConvexShape* min0 = static_cast<ConvexShape*>(body0->GetCollisionShape());
+	ConvexShape* min1 = static_cast<ConvexShape*>(body1->GetCollisionShape());	
+	GjkPairDetector::ClosestPointInput input;
+
+	SphereShape	sphere(0.2f);
+	MinkowskiSumShape	expanded0(min0,&sphere);
+	MinkowskiSumShape	expanded1(min1,&sphere);
+
+	if (useContinuous)
+	{
+		m_gjkPairDetector.SetMinkowskiA(&expanded0);
+		m_gjkPairDetector.SetMinkowskiB(&expanded1);
+		input.m_maximumDistanceSquared = expanded0.GetMargin()+expanded1.GetMargin();
+		input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
+	}
+	else
+	{
+		m_gjkPairDetector.SetMinkowskiA(min0);
+		m_gjkPairDetector.SetMinkowskiB(min1);
+		input.m_maximumDistanceSquared = min0->GetMargin() + min1->GetMargin() + m_manifoldPtr->GetManifoldMargin();
+		input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
+	}
+	
+	input.m_transformA = body0->getCenterOfMassTransform();
+	input.m_transformB = body1->getCenterOfMassTransform();
+    
+	m_gjkPairDetector.GetClosestPoints(input,output);
+
+}
+bool disableCcd = false;
+float	ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,float timeStep,int stepCount)
+{
+
+	m_collisionImpulse = 0.f;
+	
+	RigidBody* body0 = (RigidBody*)m_box0.m_clientObject;
+	RigidBody* body1 = (RigidBody*)m_box1.m_clientObject;
+
+	if (!m_manifoldPtr)
+		return 1.f;
+
+	if ((body0->getInvMass() == 0.f) && 
+		(body1->getInvMass() == 0.f))
+	{
+		return 1.f;
+	}
+
+
+	ConvexShape* min0 = static_cast<ConvexShape*>(body0->GetCollisionShape());
+	ConvexShape* min1 = static_cast<ConvexShape*>(body1->GetCollisionShape());	
+	
+	GjkPairDetector::ClosestPointInput input;
+	input.m_transformA = body0->getCenterOfMassTransform();
+	input.m_transformB = body1->getCenterOfMassTransform();
+	SimdTransform predictA,predictB;
+
+	body0->predictIntegratedTransform(timeStep,predictA);
+	body1->predictIntegratedTransform(timeStep,predictB);
+
+
+	ConvexCast::CastResult result;
+
+
+	VoronoiSimplexSolver voronoiSimplex;
+	//SubsimplexConvexCast ccd(&voronoiSimplex);
+	//GjkConvexCast ccd(&voronoiSimplex);
+	
+	ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,&m_penetrationDepthSolver);
+
+	if (disableCcd)
+		return 1.f;
+
+	if (ccd.calcTimeOfImpact(input.m_transformA,predictA,input.m_transformB,predictB,result))
+	{
+	
+		//store result.m_fraction in both bodies
+		int i;
+		i=0;
+		
+//		if (result.m_fraction< 0.1f)
+//			result.m_fraction = 0.1f;
+
+		if (body0->m_hitFraction > result.m_fraction)
+			body0->m_hitFraction  = result.m_fraction;
+
+		if (body1->m_hitFraction > result.m_fraction)
+			body1->m_hitFraction  = result.m_fraction;
+
+		return result.m_fraction;
+	}
+
+	return 1.f;
+
+
+}