Bullet physics: fixed some accuracy problem (square length should be compared to square epsilons), and more collision sensor work + deactivation issues

12 files changed, 75 insertions, 44 deletions

diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp
index 565045b39a3..a1e132c0f60 100644
--- a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp
+++ b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp
@@ -33,13 +33,20 @@ void CollisionDispatcher::FindUnions()
 		{
 			const PersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
 			//static objects (invmass 0.f) don't merge !
-			if ((((CollisionObject*)manifold->GetBody0()) && (((CollisionObject*)manifold->GetBody0())->mergesSimulationIslands())) &&
-				(((CollisionObject*)manifold->GetBody1()) && (((CollisionObject*)manifold->GetBody1())->mergesSimulationIslands())))
-			{
 
-				m_unionFind.unite(((CollisionObject*)manifold->GetBody0())->m_islandTag1,
-					((CollisionObject*)manifold->GetBody1())->m_islandTag1);
-			}
+			 const  CollisionObject* colObj0 = static_cast<const CollisionObject*>(manifold->GetBody0());
+			 const  CollisionObject* colObj1 = static_cast<const CollisionObject*>(manifold->GetBody1());
+
+			 if (colObj0 && colObj1 && NeedsResponse(*colObj0,*colObj1))
+			 {
+				if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
+					((colObj1) && ((colObj1)->mergesSimulationIslands())))
+				{
+
+					m_unionFind.unite((colObj0)->m_islandTag1,
+						(colObj1)->m_islandTag1);
+				}
+			 }
 			
 			
 		}
@@ -119,7 +126,7 @@ void CollisionDispatcher::BuildAndProcessIslands(int numBodies, IslandCallback*
 			 CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
 			 CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
 
-			 if (NeedsResponse(*colObj0,*colObj1))
+			 
 			 {
 				if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) ||
 					((colObj1) && (colObj1)->m_islandTag1 == (islandId)))
@@ -224,11 +231,11 @@ CollisionAlgorithm* CollisionDispatcher::InternalFindAlgorithm(BroadphaseProxy&
 	
 }
 
-bool	CollisionDispatcher::NeedsResponse(CollisionObject& colObj0,CollisionObject& colObj1)
+bool	CollisionDispatcher::NeedsResponse(const  CollisionObject& colObj0,const CollisionObject& colObj1)
 {
 	//here you can do filtering
 	bool hasResponse = 
-		(!(colObj0.m_collisionFlags & CollisionObject::noContactResponse)) &
+		(!(colObj0.m_collisionFlags & CollisionObject::noContactResponse)) &&
 		(!(colObj1.m_collisionFlags & CollisionObject::noContactResponse));
 	return hasResponse;
 }
diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h
index 91cacccc52d..e3f3c09e76d 100644
--- a/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h
+++ b/extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h
@@ -124,7 +124,7 @@ public:
 	
 	virtual bool	NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1);
 	
-	virtual bool	NeedsResponse(CollisionObject& colObj0,CollisionObject& colObj1);
+	virtual bool	NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1);
 
 	virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;}
 	
diff --git a/extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp b/extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp
index 33c45179104..ab0fea21605 100644
--- a/extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp
+++ b/extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp
@@ -40,8 +40,11 @@ void CollisionObject::ForceActivationState(int newState)
 
 void CollisionObject::activate()
 {
+	if (!(m_collisionFlags & isStatic))
+	{
 		SetActivationState(1);
 		m_deactivationTime = 0.f;
+	}
 }
 
 bool CollisionObject::mergesSimulationIslands() const
diff --git a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp
index 443899e62a3..ad06b68aa92 100644
--- a/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp
+++ b/extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp
@@ -333,7 +333,7 @@ void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy
 		input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
 	}
 
-	input.m_maximumDistanceSquared = 1e30;//
+	input.m_maximumDistanceSquared = 1e30f;//
 	
 	input.m_transformA = col0->m_worldTransform;
 	input.m_transformB = col1->m_worldTransform;
diff --git a/extern/bullet/Bullet/CollisionShapes/BoxShape.h b/extern/bullet/Bullet/CollisionShapes/BoxShape.h
index f7c5bba7345..f47f982112d 100644
--- a/extern/bullet/Bullet/CollisionShapes/BoxShape.h
+++ b/extern/bullet/Bullet/CollisionShapes/BoxShape.h
@@ -59,7 +59,7 @@ public:
 		if ( GetMargin()!=0.f )
 		{
 			SimdVector3 vecnorm = vec;
-			if (vecnorm .length2() == 0.f)
+			if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
 			{
 				vecnorm.setValue(-1.f,-1.f,-1.f);
 			} 
diff --git a/extern/bullet/Bullet/CollisionShapes/ConeShape.cpp b/extern/bullet/Bullet/CollisionShapes/ConeShape.cpp
index 9543aa48139..fb99c2c4480 100644
--- a/extern/bullet/Bullet/CollisionShapes/ConeShape.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/ConeShape.cpp
@@ -87,7 +87,7 @@ SimdVector3	ConeShape::LocalGetSupportingVertex(const SimdVector3& vec)  const
 	if ( GetMargin()!=0.f )
 	{
 		SimdVector3 vecnorm = vec;
-		if (vecnorm .length2() == 0.f)
+		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
 		{
 			vecnorm.setValue(-1.f,-1.f,-1.f);
 		} 
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp
index 5de2ea3dcb8..e527dcd7e84 100644
--- a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp
@@ -97,7 +97,7 @@ SimdVector3	ConvexHullShape::LocalGetSupportingVertex(const SimdVector3& vec)con
 	if ( GetMargin()!=0.f )
 	{
 		SimdVector3 vecnorm = vec;
-		if (vecnorm .length2() == 0.f)
+		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
 		{
 			vecnorm.setValue(-1.f,-1.f,-1.f);
 		} 
diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexShape.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexShape.cpp
index 2ca3511a8da..951f7a07d3a 100644
--- a/extern/bullet/Bullet/CollisionShapes/ConvexShape.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/ConvexShape.cpp
@@ -60,7 +60,7 @@ SimdVector3	ConvexShape::LocalGetSupportingVertex(const SimdVector3& vec)const
 	if ( GetMargin()!=0.f )
 	{
 		SimdVector3 vecnorm = vec;
-		if (vecnorm .length2() == 0.f)
+		if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
 		{
 			vecnorm.setValue(-1.f,-1.f,-1.f);
 		} 
diff --git a/extern/bullet/Bullet/CollisionShapes/CylinderShape.h b/extern/bullet/Bullet/CollisionShapes/CylinderShape.h
index b0814f98066..2c0c8008d71 100644
--- a/extern/bullet/Bullet/CollisionShapes/CylinderShape.h
+++ b/extern/bullet/Bullet/CollisionShapes/CylinderShape.h
@@ -45,7 +45,7 @@ public:
 		if ( GetMargin()!=0.f )
 		{
 			SimdVector3 vecnorm = vec;
-			if (vecnorm .length2() == 0.f)
+			if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
 			{
 				vecnorm.setValue(-1.f,-1.f,-1.f);
 			} 
diff --git a/extern/bullet/Bullet/CollisionShapes/SphereShape.cpp b/extern/bullet/Bullet/CollisionShapes/SphereShape.cpp
index 6cc9fd09887..28c46a28ab7 100644
--- a/extern/bullet/Bullet/CollisionShapes/SphereShape.cpp
+++ b/extern/bullet/Bullet/CollisionShapes/SphereShape.cpp
@@ -44,7 +44,7 @@ SimdVector3	SphereShape::LocalGetSupportingVertex(const SimdVector3& vec)const
 	supVertex = LocalGetSupportingVertexWithoutMargin(vec);
 
 	SimdVector3 vecnorm = vec;
-	if (SimdFuzzyZero(vecnorm .length2()))
+	if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
 	{
 		vecnorm.setValue(-1.f,-1.f,-1.f);
 	} 
diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
index 57f29575569..cbe7961773d 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
@@ -195,16 +195,21 @@ void		CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionsta
 		// kinematic methods
 void		CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
 {
-	SimdVector3 dloc(dlocX,dlocY,dlocZ);
-	SimdTransform xform = m_body->getCenterOfMassTransform();
-
-	if (local)
+	if (m_body)
 	{
-		dloc = xform.getBasis()*dloc;
-	}
+		m_body->activate();
+
+		SimdVector3 dloc(dlocX,dlocY,dlocZ);
+		SimdTransform xform = m_body->getCenterOfMassTransform();
+
+		if (local)
+		{
+			dloc = xform.getBasis()*dloc;
+		}
 
-	xform.setOrigin(xform.getOrigin() + dloc);
-	this->m_body->setCenterOfMassTransform(xform);
+		xform.setOrigin(xform.getOrigin() + dloc);
+		m_body->setCenterOfMassTransform(xform);
+	}
 
 }
 
@@ -212,6 +217,8 @@ void		CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
 {
 	if (m_body )
 	{
+		m_body->activate();
+
 		SimdMatrix3x3 drotmat(	rotval[0],rotval[1],rotval[2],
 								rotval[4],rotval[5],rotval[6],
 								rotval[8],rotval[9],rotval[10]);
@@ -319,34 +326,47 @@ void		CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bo
 void		CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
 {
 	SimdVector3 angvel(ang_velX,ang_velY,ang_velZ);
-	SimdTransform xform = m_body->getCenterOfMassTransform();
-	if (local)
+	if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
 	{
-		angvel	= xform.getBasis()*angvel;
-	}
+		SimdTransform xform = m_body->getCenterOfMassTransform();
+		if (local)
+		{
+			angvel	= xform.getBasis()*angvel;
+		}
 
-	m_body->setAngularVelocity(angvel);
+		m_body->setAngularVelocity(angvel);
+	}
 
 }
 void		CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
 {
+
 	SimdVector3 linVel(lin_velX,lin_velY,lin_velZ);
-	SimdTransform xform = m_body->getCenterOfMassTransform();
-	if (local)
+	if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
 	{
-		linVel	= xform.getBasis()*linVel;
+		m_body->activate();
+		SimdTransform xform = m_body->getCenterOfMassTransform();
+		if (local)
+		{
+			linVel	= xform.getBasis()*linVel;
+		}
+		m_body->setLinearVelocity(linVel);
 	}
-	m_body->setLinearVelocity(linVel);
 }
 void		CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
 {
-	
 	SimdVector3 impulse(impulseX,impulseY,impulseZ);
-	SimdVector3 pos(attachX,attachY,attachZ);
 
-	m_body->activate();
+	if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
+	{
+		m_body->activate();
+		
+		SimdVector3 pos(attachX,attachY,attachZ);
+
+		m_body->activate();
 
-	m_body->applyImpulse(impulse,pos);
+		m_body->applyImpulse(impulse,pos);
+	}
 
 }
 void		CcdPhysicsController::SetActive(bool active)
diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
index 7f9bbd62ceb..cf9c4157475 100644
--- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
+++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
@@ -1462,15 +1462,15 @@ TypedConstraint*	CcdPhysicsEnvironment::getConstraintById(int constraintId)
 
 void CcdPhysicsEnvironment::addSensor(PHY_IPhysicsController* ctrl)
 {
-	printf("addSensor\n");
+	//printf("addSensor\n");
 }
 void CcdPhysicsEnvironment::removeSensor(PHY_IPhysicsController* ctrl)
 {
-	printf("removeSensor\n");
+	//printf("removeSensor\n");
 }
 void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCallback callback, void *user)
 {
-	printf("addTouchCallback\n(response class = %i)\n",response_class);
+/*	printf("addTouchCallback\n(response class = %i)\n",response_class);
 
 	//map PHY_ convention into SM_ convention
 	switch (response_class)
@@ -1494,6 +1494,7 @@ void CcdPhysicsEnvironment::addTouchCallback(int response_class, PHY_ResponseCal
 		assert(0);
 		return;
 	}
+*/
 
 	m_triggerCallbacks[response_class] = callback;
 	m_triggerCallbacksUserPtrs[response_class] = user;
@@ -1503,7 +1504,7 @@ void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctr
 {
 	CcdPhysicsController* ccdCtrl = static_cast<CcdPhysicsController*>(ctrl);
 
-	printf("requestCollisionCallback\n");
+	//printf("requestCollisionCallback\n");
 	m_triggerControllers.push_back(ccdCtrl);
 }
 
@@ -1514,7 +1515,7 @@ void	CcdPhysicsEnvironment::CallbackTriggers()
 
 	if (m_triggerCallbacks[PHY_OBJECT_RESPONSE])
 	{
-
+		//walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
 		int numManifolds = m_collisionWorld->GetDispatcher()->GetNumManifolds();
 		for (int i=0;i<numManifolds;i++)
 		{
@@ -1547,7 +1548,7 @@ void	CcdPhysicsEnvironment::CallbackTriggers()
 		
 
 	}
-	//walk over all overlapping pairs, and if
+
 }