Add Fh/Rot Fh to Bullet.

6 files changed, 231 insertions, 5 deletions

diff --git a/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp b/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
index d7bd1e9c7cd..3d20ce8a3a5 100644
--- a/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
+++ b/source/gameengine/Ketsji/KX_BulletPhysicsController.cpp
@@ -211,6 +211,9 @@ SG_Controller*	KX_BulletPhysicsController::GetReplica(class SG_Node* destnode)
 	//parentcontroller is here be able to avoid collisions between parent/child
 
 	PHY_IPhysicsController* parentctrl = NULL;
+	KX_BulletPhysicsController* parentKxCtrl = NULL;
+	CcdPhysicsController* ccdParent = NULL;
+
 	
 	if (destnode != destnode->GetRootSGParent())
 	{
@@ -230,12 +233,15 @@ SG_Controller*	KX_BulletPhysicsController::GetReplica(class SG_Node* destnode)
 				KX_GameObject *clientgameobj = static_cast<KX_GameObject*>( (*childit)->GetSGClientObject());
 				if (clientgameobj)
 				{
-					parentctrl = (KX_BulletPhysicsController*)clientgameobj->GetPhysicsController();
+					parentKxCtrl = (KX_BulletPhysicsController*)clientgameobj->GetPhysicsController();
+					parentctrl = parentKxCtrl;
+					ccdParent = parentKxCtrl;
 				}
 			}
 		}
 	}
 
+	physicsreplica->setParentCtrl(ccdParent);
 	physicsreplica->PostProcessReplica(motionstate,parentctrl);
 	physicsreplica->m_userdata = (PHY_IPhysicsController*)physicsreplica;
 	return physicsreplica;
diff --git a/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp b/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp
index 68e0997aec0..0e7a6d92ec1 100644
--- a/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp
+++ b/source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp
@@ -1028,10 +1028,16 @@ void	KX_ConvertBulletObject(	class	KX_GameObject* gameobj,
 	ci.m_do_anisotropic = shapeprops->m_do_anisotropic;
 	ci.m_anisotropicFriction.setValue(shapeprops->m_friction_scaling[0],shapeprops->m_friction_scaling[1],shapeprops->m_friction_scaling[2]);
 
-	//smprop->m_do_fh = kxshapeprops->m_do_fh;
-	//smprop->m_do_rot_fh = kxshapeprops->m_do_rot_fh ;
 
-	
+//////////
+	//do Fh, do Rot Fh
+	ci.m_do_fh = shapeprops->m_do_fh;
+	ci.m_do_rot_fh = shapeprops->m_do_rot_fh ;
+	ci.m_fh_damping = smmaterial->m_fh_damping;
+	ci.m_fh_distance = smmaterial->m_fh_distance;
+	ci.m_fh_normal = smmaterial->m_fh_normal;
+	ci.m_fh_spring = smmaterial->m_fh_spring;
+	ci.m_radius = objprop->m_radius;
 	
 	
 	///////////////////
@@ -1098,6 +1104,10 @@ void	KX_ConvertBulletObject(	class	KX_GameObject* gameobj,
 		if (rbody && objprop->m_disableSleeping)
 			rbody->setActivationState(DISABLE_DEACTIVATION);
 	}
+
+	CcdPhysicsController* parentCtrl = objprop->m_dynamic_parent ? (KX_BulletPhysicsController*)objprop->m_dynamic_parent->GetPhysicsController() : 0;
+	physicscontroller->setParentCtrl(parentCtrl);
+
 	
 	//Now done directly in ci.m_collisionFlags so that it propagates to replica
 	//if (objprop->m_ghost)
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
index 8ec040e89ee..7196a1ae095 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
@@ -54,7 +54,7 @@ CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
 	m_newClientInfo = 0;
 	m_registerCount = 0;
 	m_softBodyTransformInitialized = false;
-
+	m_parentCtrl = 0;
 	// copy pointers locally to allow smart release
 	m_MotionState = ci.m_MotionState;
 	m_collisionShape = ci.m_collisionShape;
@@ -628,6 +628,7 @@ void		CcdPhysicsController::WriteDynamicsToMotionState()
 		// controller replication
 void		CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
 {
+	
 	m_softBodyTransformInitialized=false;
 	m_MotionState = motionstate;
 	m_registerCount = 0;
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.h b/source/gameengine/Physics/Bullet/CcdPhysicsController.h
index f001d6043cb..a1245f6a3e9 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsController.h
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.h
@@ -264,6 +264,14 @@ struct CcdConstructionInfo
 	bool	m_do_anisotropic;
 	btVector3 m_anisotropicFriction;
 
+	bool		m_do_fh;                 ///< Should the object have a linear Fh spring?
+	bool		m_do_rot_fh;             ///< Should the object have an angular Fh spring?
+	btScalar	m_fh_spring;             ///< Spring constant (both linear and angular)
+	btScalar	m_fh_damping;            ///< Damping factor (linear and angular) in range [0, 1]
+	btScalar	m_fh_distance;           ///< The range above the surface where Fh is active.    
+	bool		m_fh_normal;             ///< Should the object slide off slopes?
+	float		m_radius;//for fh backwards compatibility
+
 };
 
 
@@ -295,6 +303,9 @@ class CcdPhysicsController : public PHY_IPhysicsController
 	void*		m_newClientInfo;
 	int			m_registerCount;	// needed when multiple sensors use the same controller
 	CcdConstructionInfo	m_cci;//needed for replication
+
+	CcdPhysicsController* m_parentCtrl;
+
 	void GetWorldOrientation(btMatrix3x3& mat);
 
 	void CreateRigidbody();
@@ -318,6 +329,15 @@ class CcdPhysicsController : public PHY_IPhysicsController
 
 		virtual ~CcdPhysicsController();
 
+		CcdConstructionInfo& getConstructionInfo()
+		{
+			return m_cci;
+		}
+		const CcdConstructionInfo& getConstructionInfo() const
+		{
+			return m_cci;
+		}
+
 
 		btRigidBody* GetRigidBody();
 		btCollisionObject*	GetCollisionObject();
@@ -422,6 +442,23 @@ class CcdPhysicsController : public PHY_IPhysicsController
 		{
 			return m_cci.m_physicsEnv;
 		}
+
+		void	setParentCtrl(CcdPhysicsController* parentCtrl)
+		{
+			m_parentCtrl = parentCtrl;
+		}
+
+		CcdPhysicsController*	getParentCtrl()
+		{
+			return m_parentCtrl;
+		}
+
+		const CcdPhysicsController*	getParentCtrl() const
+		{
+			return m_parentCtrl;
+		}
+
+
 		
 };
 
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
index 763fc148a04..483f829c2bd 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
@@ -558,6 +558,8 @@ bool	CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 		(*it)->SynchronizeMotionStates(timeStep);
 	}
 
+	processFhSprings(curTime,timeStep);
+
 	float subStep = timeStep / float(m_numTimeSubSteps);
 	for (i=0;i<m_numTimeSubSteps;i++)
 	{
@@ -570,6 +572,11 @@ bool	CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 		(*it)->SynchronizeMotionStates(timeStep);
 	}
 
+	for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
+	{
+		(*it)->SynchronizeMotionStates(timeStep);
+	}
+
 	for (i=0;i<m_wrapperVehicles.size();i++)
 	{
 		WrapperVehicle* veh = m_wrapperVehicles[i];
@@ -585,6 +592,170 @@ bool	CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
 	return true;
 }
 
+class ClosestRayResultCallbackNotMe : public btCollisionWorld::ClosestRayResultCallback
+{
+	btCollisionObject* m_owner;
+	btCollisionObject* m_parent;
+
+public:
+	ClosestRayResultCallbackNotMe(const btVector3& rayFromWorld,const btVector3& rayToWorld,btCollisionObject* owner,btCollisionObject* parent)
+		:btCollisionWorld::ClosestRayResultCallback(rayFromWorld,rayToWorld),
+		m_owner(owner)
+	{
+
+	}
+
+	virtual bool needsCollision(btBroadphaseProxy* proxy0) const
+	{
+		//don't collide with self
+		if (proxy0->m_clientObject == m_owner)
+			return false;
+
+		if (proxy0->m_clientObject == m_parent)
+			return false;
+
+		return btCollisionWorld::ClosestRayResultCallback::needsCollision(proxy0);
+	}
+
+};
+
+void	CcdPhysicsEnvironment::processFhSprings(double curTime,float timeStep)
+{
+	std::set<CcdPhysicsController*>::iterator it;
+	
+	for (it=m_controllers.begin(); it!=m_controllers.end(); it++)
+	{
+		CcdPhysicsController* ctrl = (*it);
+		if (ctrl->GetRigidBody() && ctrl->getConstructionInfo().m_do_fh || ctrl->getConstructionInfo().m_do_rot_fh)
+		{
+			//printf("has Fh or RotFh\n");
+			//re-implement SM_FhObject.cpp using btCollisionWorld::rayTest and info from ctrl->getConstructionInfo()
+			//send a ray from {0.0, 0.0, 0.0} towards {0.0, 0.0, -10.0}, in local coordinates
+
+			btRigidBody* body = ctrl->GetRigidBody();
+			CcdPhysicsController* parentCtrl = ctrl->getParentCtrl();
+			btRigidBody* parentBody = parentCtrl?parentCtrl->GetRigidBody() : 0;
+			btRigidBody* cl_object = parentBody ? parentBody : body;
+
+			if (body->isStaticOrKinematicObject())
+				continue;
+
+			btVector3 rayDirLocal(0,0,-10);
+
+			//m_dynamicsWorld
+			//ctrl->GetRigidBody();
+			btVector3 rayFromWorld = body->getCenterOfMassPosition();
+			//btVector3	rayToWorld = rayFromWorld + body->getCenterOfMassTransform().getBasis() * rayDirLocal;
+			//ray always points down the z axis in world space...
+			btVector3	rayToWorld = rayFromWorld + rayDirLocal;
+
+			ClosestRayResultCallbackNotMe	resultCallback(rayFromWorld,rayToWorld,body,parentBody);
+
+			m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,resultCallback);
+			if (resultCallback.hasHit())
+			{
+				//we hit this one: resultCallback.m_collisionObject;
+				CcdPhysicsController* controller = static_cast<CcdPhysicsController*>(resultCallback.m_collisionObject->getUserPointer());
+
+				if (controller)
+				{
+					if (controller->getConstructionInfo().m_fh_distance < SIMD_EPSILON)
+						continue;
+
+					btRigidBody* hit_object = controller->GetRigidBody();
+					if (!hit_object)
+						continue;
+
+					CcdConstructionInfo& hitObjShapeProps = controller->getConstructionInfo();
+
+					float distance = resultCallback.m_closestHitFraction*rayDirLocal.length()-ctrl->getConstructionInfo().m_radius;
+					if (distance >= hitObjShapeProps.m_fh_distance)
+						continue;
+					
+					
+
+					//btVector3 ray_dir = cl_object->getCenterOfMassTransform().getBasis()* rayDirLocal.normalized();
+					btVector3 ray_dir = rayDirLocal.normalized();
+					btVector3 normal = resultCallback.m_hitNormalWorld;
+					normal.normalize();
+
+					
+					if (ctrl->getConstructionInfo().m_do_fh) 
+					{
+						btVector3 lspot = cl_object->getCenterOfMassPosition()
+							+ rayDirLocal * resultCallback.m_closestHitFraction;
+
+
+							
+
+						lspot -= hit_object->getCenterOfMassPosition();
+						btVector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocityInLocalPoint(lspot);
+						btScalar rel_vel_ray = ray_dir.dot(rel_vel);
+						btScalar spring_extent = 1.0 - distance / hitObjShapeProps.m_fh_distance; 
+						
+						btScalar i_spring = spring_extent * hitObjShapeProps.m_fh_spring;
+						btScalar i_damp =   rel_vel_ray * hitObjShapeProps.m_fh_damping;
+						
+						cl_object->setLinearVelocity(cl_object->getLinearVelocity() + (-(i_spring + i_damp) * ray_dir)); 
+						if (hitObjShapeProps.m_fh_normal) 
+						{
+							cl_object->setLinearVelocity(cl_object->getLinearVelocity()+(i_spring + i_damp) *(normal - normal.dot(ray_dir) * ray_dir));
+						}
+						
+						btVector3 lateral = rel_vel - rel_vel_ray * ray_dir;
+						
+						
+						if (ctrl->getConstructionInfo().m_do_anisotropic) {
+							//Bullet basis contains no scaling/shear etc.
+							const btMatrix3x3& lcs = cl_object->getCenterOfMassTransform().getBasis();
+							btVector3 loc_lateral = lateral * lcs;
+							const btVector3& friction_scaling = cl_object->getAnisotropicFriction();
+							loc_lateral *= friction_scaling;
+							lateral = lcs * loc_lateral;
+						}
+
+						btScalar rel_vel_lateral = lateral.length();
+						
+						if (rel_vel_lateral > SIMD_EPSILON) {
+							btScalar friction_factor = hit_object->getFriction();//cl_object->getFriction();
+
+							btScalar max_friction = friction_factor * btMax(btScalar(0.0), i_spring);
+							
+							btScalar rel_mom_lateral = rel_vel_lateral / cl_object->getInvMass();
+							
+							btVector3 friction = (rel_mom_lateral > max_friction) ?
+								-lateral * (max_friction / rel_vel_lateral) :
+								-lateral;
+							
+								cl_object->applyCentralImpulse(friction);
+						}
+					}
+
+					
+					if (ctrl->getConstructionInfo().m_do_rot_fh) {
+						btVector3 up2 = cl_object->getWorldTransform().getBasis().getColumn(2);
+
+						btVector3 t_spring = up2.cross(normal) * hitObjShapeProps.m_fh_spring;
+						btVector3 ang_vel = cl_object->getAngularVelocity();
+						
+						// only rotations that tilt relative to the normal are damped
+						ang_vel -= ang_vel.dot(normal) * normal;
+						
+						btVector3 t_damp = ang_vel * hitObjShapeProps.m_fh_damping;  
+						
+						cl_object->setAngularVelocity(cl_object->getAngularVelocity() + (t_spring - t_damp));
+					}
+
+				}
+
+
+			}
+
+
+		}
+	}
+	
+}
 
 void		CcdPhysicsEnvironment::setDebugMode(int debugMode)
 {
diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
index 56a639509c5..3d9d5442b8f 100644
--- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
+++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
@@ -74,6 +74,7 @@ protected:
 
 	btContactSolverInfo	m_solverInfo;
 	
+	void	processFhSprings(double curTime,float timeStep);
 
 	public:
 		CcdPhysicsEnvironment(btDispatcher* dispatcher=0, btOverlappingPairCache* pairCache=0);