Rigid body physics for non spherical bounding objects.

If your simulation becomes unstable, crank up the 'Form' control.

Removed Solid from class SumoPhysicsEnvironment (since it wasn't actually used.)

source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp

@@ -80,7 +80,7 @@
 GEN_Map<GEN_HashedPtr,DT_ShapeHandle> map_gamemesh_to_sumoshape;
 
 // forward declarations
-void	BL_RegisterSumoObject(KX_GameObject* gameobj,class SM_Scene* sumoScene,DT_SceneHandle solidscene,class SM_Object* sumoObj,const STR_String& matname,bool isDynamic,bool isActor);
+void	BL_RegisterSumoObject(KX_GameObject* gameobj,class SM_Scene* sumoScene,class SM_Object* sumoObj,const STR_String& matname,bool isDynamic,bool isActor);
 DT_ShapeHandle CreateShapeFromMesh(RAS_MeshObject* meshobj);
 
 
@@ -102,7 +102,7 @@ void	KX_ConvertSumoObject(	KX_GameObject* gameobj,
 	smprop->m_friction_scaling[0]  = kxshapeprops->m_friction_scaling[0];
 	smprop->m_friction_scaling[1]  = kxshapeprops->m_friction_scaling[1];
 	smprop->m_friction_scaling[2]  = kxshapeprops->m_friction_scaling[2];
-	smprop->m_inertia = kxshapeprops->m_inertia;
+	smprop->m_inertia = MT_Vector3(1., 1., 1.) * kxshapeprops->m_inertia;
 	smprop->m_lin_drag = kxshapeprops->m_lin_drag;
 	smprop->m_mass = kxshapeprops->m_mass;
 	smprop->m_radius = objprop->m_radius;
@@ -131,6 +131,8 @@ void	KX_ConvertSumoObject(	KX_GameObject* gameobj,
 		{
 			case KX_BOUNDBOX:
 				shape = DT_NewBox(objprop->m_boundobject.box.m_extends[0], objprop->m_boundobject.box.m_extends[1], objprop->m_boundobject.box.m_extends[2]);
+				smprop->m_inertia.scale(objprop->m_boundobject.box.m_extends[0], objprop->m_boundobject.box.m_extends[1], objprop->m_boundobject.box.m_extends[2]);
+				smprop->m_inertia /= (objprop->m_boundobject.box.m_extends[0] + objprop->m_boundobject.box.m_extends[1] + objprop->m_boundobject.box.m_extends[2]) / 3.;
 				break;
 			case KX_BOUNDCYLINDER:
 				shape = DT_NewCylinder(objprop->m_radius, objprop->m_boundobject.c.m_height);
@@ -158,7 +160,7 @@ void	KX_ConvertSumoObject(	KX_GameObject* gameobj,
 		sumoObj->setRigidBody(objprop->m_angular_rigidbody?true:false);
 		
 		objprop->m_isactor = objprop->m_dyna = true;
-		BL_RegisterSumoObject(gameobj,sceneptr,sumoEnv->GetSolidScene(),sumoObj,"",true, true);
+		BL_RegisterSumoObject(gameobj,sceneptr,sumoObj,"",true, true);
 		
 	} 
 	else {
@@ -228,7 +230,7 @@ void	KX_ConvertSumoObject(	KX_GameObject* gameobj,
 
 					
 					BL_RegisterSumoObject(gameobj,sceneptr,
-						sumoEnv->GetSolidScene(),sumoObj,
+						sumoObj,
 						matname,
 						objprop->m_dyna,
 						objprop->m_isactor);
@@ -254,21 +256,15 @@ void	KX_ConvertSumoObject(	KX_GameObject* gameobj,
 void	BL_RegisterSumoObject(
 	KX_GameObject* gameobj,
 	class SM_Scene* sumoScene,
-	DT_SceneHandle solidscene,
 	class SM_Object* sumoObj,
 	const STR_String& matname,
 	bool isDynamic,
 	bool isActor) 
 {
-
-
-
-		//gameobj->SetDynamic(isDynamic);
-
 		PHY_IMotionState* motionstate = new KX_MotionState(gameobj->GetSGNode());
 
 		// need easy access, not via 'node' etc.
-		KX_SumoPhysicsController* physicscontroller = new KX_SumoPhysicsController(sumoScene,solidscene,sumoObj,motionstate,isDynamic);
+		KX_SumoPhysicsController* physicscontroller = new KX_SumoPhysicsController(sumoScene,sumoObj,motionstate,isDynamic);
 		gameobj->SetPhysicsController(physicscontroller);
 		physicscontroller->setClientInfo(gameobj);
 		
@@ -279,16 +275,12 @@ void	BL_RegisterSumoObject(
 		gameobj->GetSGNode()->AddSGController(physicscontroller);
 
 		gameobj->getClientInfo()->m_type = (isActor ? KX_ClientObjectInfo::ACTOR : KX_ClientObjectInfo::STATIC);
-		//gameobj->GetClientInfo()->m_clientobject = gameobj;
 
 		// store materialname in auxinfo, needed for touchsensors
 		gameobj->getClientInfo()->m_auxilary_info = (matname.Length() ? (void*)(matname.ReadPtr()+2) : NULL);
 
 		physicscontroller->SetObject(gameobj->GetSGNode());
-		
+}
-		//gameobj->SetDynamicsScaling(MT_Vector3(1.0, 1.0, 1.0));
-
-};
 
 DT_ShapeHandle CreateShapeFromMesh(RAS_MeshObject* meshobj)
 {

source/gameengine/Ketsji/KX_SumoPhysicsController.h

@@ -54,12 +54,12 @@ class KX_SumoPhysicsController : public KX_IPhysicsController,
 public:
 	KX_SumoPhysicsController(
 		class SM_Scene* sumoScene,
-		DT_SceneHandle solidscene,
+/*		DT_SceneHandle solidscene, */
 		class SM_Object* sumoObj,	
 		class PHY_IMotionState* motionstate
 		,bool dyna) 
 		: KX_IPhysicsController(dyna,NULL) ,
-		  SumoPhysicsController(sumoScene,solidscene,sumoObj,motionstate,dyna)
+		  SumoPhysicsController(sumoScene,/*solidscene,*/sumoObj,motionstate,dyna)
 	{
 	};
 	virtual ~KX_SumoPhysicsController();

source/gameengine/Physics/Sumo/Fuzzics/include/SM_Object.h

@@ -47,7 +47,7 @@ class SM_FhObject;
 struct SM_ShapeProps {
 	MT_Scalar  m_mass;                  // Total mass
 	MT_Scalar  m_radius;                // Bound sphere size
-	MT_Scalar  m_inertia;               // Inertia, should be a tensor some time 
+	MT_Vector3 m_inertia;               // Inertia, should be a tensor some time 
 	MT_Scalar  m_lin_drag;              // Linear drag (air, water) 0 = concrete, 1 = vacuum 
 	MT_Scalar  m_ang_drag;              // Angular drag
 	MT_Scalar  m_friction_scaling[3];   // Scaling for anisotropic friction. Component in range [0, 1]   
@@ -106,6 +106,8 @@ public:
 
 	void calcXform();
 	void notifyClient();
+	void updateInvInertiaTensor();
+
     
 	// Save the current state information for use in the 
 	// velocity computation in the next frame.  
@@ -181,7 +183,9 @@ public:
 
 	MT_Scalar getInvMass() const;
 
-	MT_Scalar getInvInertia() const ;
+	const MT_Vector3& getInvInertia() const ;
+	
+	const MT_Matrix3x3& getInvInertiaTensor() const;
 
 	void applyForceField(const MT_Vector3& accel) ;
 	
@@ -334,6 +338,11 @@ private:
 
 	SM_FhObject            *m_fh_object;             // The ray object used for Fh
 	bool                    m_suspended;             // Is this object frozen?
+	
+	// Mass properties
+	MT_Scalar               m_inv_mass;              // 1/mass
+	MT_Vector3              m_inv_inertia;           // [1/inertia_x, 1/inertia_y, 1/inertia_z]
+	MT_Matrix3x3            m_inv_inertia_tensor;    // Inverse Inertia Tensor
 };
 
 #endif

source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp

@@ -83,16 +83,24 @@ SM_Object::SM_Object(
 	m_error(0.0, 0.0, 0.0),
 	m_combined_lin_vel (0.0, 0.0, 0.0),
 	m_combined_ang_vel (0.0, 0.0, 0.0),
-	m_fh_object(0)
+	m_fh_object(0),
+	m_inv_mass(0.0),
+	m_inv_inertia(0., 0., 0.)
 {
 	m_xform.setIdentity();
 	m_xform.getValue(m_ogl_matrix);
-	if (shapeProps && 
+	if (shapeProps)
-		(shapeProps->m_do_fh || shapeProps->m_do_rot_fh)) {
+	{ 
+		if (shapeProps->m_do_fh || shapeProps->m_do_rot_fh) 
+		{
 			DT_Vector3 zero = {0., 0., 0.}, ray = {0.0, 0.0, -10.0};
 			m_fh_object = new SM_FhObject(DT_NewLineSegment(zero, ray), MT_Vector3(ray), this);
 			//printf("SM_Object:: WARNING! fh disabled.\n");
 		}
+		m_inv_mass = 1. / shapeProps->m_mass;
+		m_inv_inertia = MT_Vector3(1./shapeProps->m_inertia[0], 1./shapeProps->m_inertia[1], 1./shapeProps->m_inertia[2]);
+	}
+	updateInvInertiaTensor();
 	m_suspended = false;
 }
 
@@ -113,8 +121,8 @@ integrateForces(
 			m_lin_mom *= pow(m_shapeProps->m_lin_drag, timeStep);
 			m_ang_mom *= pow(m_shapeProps->m_ang_drag, timeStep);
 			// Set velocities according momentum
-			m_lin_vel = m_lin_mom / m_shapeProps->m_mass;
+			m_lin_vel = m_lin_mom * m_inv_mass;
-			m_ang_vel = m_ang_mom / m_shapeProps->m_inertia;
+			m_ang_vel = m_inv_inertia_tensor * m_ang_mom;
 		}
 	}	
 
@@ -182,8 +190,15 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2,
 			restitution = 0.0;
 		}
 				
-		MT_Scalar impulse = -(1.0 + restitution) * rel_vel_normal / invMass;
+		MT_Scalar impulse = -(1.0 + restitution) * rel_vel_normal;
-		applyCenterImpulse( impulse * normal); 
+		
+		if (isRigidBody())
+		{
+			MT_Vector3 temp = getInvInertiaTensor() * local2.cross(normal);  
+			applyImpulse(local2 + m_pos, (impulse / (invMass + normal.dot(temp.cross(local2)))) * normal);
+		} else {
+			applyCenterImpulse( ( impulse / invMass ) * normal ); 
+		}
 	   	
 		// The friction part starts here!!!!!!!!
 
@@ -276,7 +291,7 @@ void SM_Object::dynamicCollision(const MT_Point3 &local2,
 				// For rigid bodies we take the inertia into account, 
 				// since the friction impulse is going to change the
 				// angular momentum as well.
-				MT_Vector3 temp = getInvInertia() * local2.cross(lateral);
+				MT_Vector3 temp = getInvInertiaTensor() * local2.cross(lateral);
 				MT_Scalar impulse_lateral = rel_vel_lateral /
 					(invMass + lateral.dot(temp.cross(local2)));
 
@@ -621,6 +636,7 @@ calcXform() {
 		m_fh_object->setPosition(m_pos);
 		m_fh_object->calcXform();
 	}
+	updateInvInertiaTensor();
 #ifdef SM_DEBUG_XFORM
 	printf("\n               | %-0.5f %-0.5f %-0.5f %-0.5f |\n",
 		m_ogl_matrix[0], m_ogl_matrix[4], m_ogl_matrix[ 8], m_ogl_matrix[12]);
@@ -633,6 +649,12 @@ calcXform() {
 #endif
 }
 
+	void 
+SM_Object::updateInvInertiaTensor() 
+{
+	m_inv_inertia_tensor = m_xform.getBasis().scaled(m_inv_inertia[0], m_inv_inertia[1], m_inv_inertia[2]) * m_xform.getBasis().transposed();
+}
+
 // Call callbacks to notify the client of a change of placement
 	void 
 SM_Object::
@@ -886,18 +908,25 @@ resolveCombinedVelocities(
 SM_Object::
 getInvMass(
 ) const { 
-	return m_shapeProps ? 1.0 / m_shapeProps->m_mass : 0.0;
+	return m_inv_mass;
 	// OPT: cache the result of this division rather than compute it each call
 }
 
-	MT_Scalar 
+	const MT_Vector3&
 SM_Object::
 getInvInertia(
 ) const { 
-	return m_shapeProps ? 1.0 / m_shapeProps->m_inertia : 0.0;
+	return m_inv_inertia;
 	// OPT: cache the result of this division rather than compute it each call
 }
 
+	const MT_Matrix3x3&
+SM_Object::
+getInvInertiaTensor(
+) const { 
+	return m_inv_inertia_tensor; 
+}
+
 	void 
 SM_Object::
 applyForceField(
@@ -941,7 +970,7 @@ applyCenterImpulse(
 	if (m_shapeProps) {
 		m_lin_mom          += impulse;
 		m_reaction_impulse += impulse;
-		m_lin_vel           = m_lin_mom / m_shapeProps->m_mass;
+		m_lin_vel           = m_lin_mom * m_inv_mass;
 
 		// The linear velocity is immedialtely updated since otherwise
 		// simultaneous collisions will get a double impulse. 
@@ -955,7 +984,7 @@ applyAngularImpulse(
 ) {
 	if (m_shapeProps) {
 		m_ang_mom += impulse;
-		m_ang_vel = m_ang_mom / m_shapeProps->m_inertia;
+		m_ang_vel = m_inv_inertia_tensor * m_ang_mom;
 	}
 }
 

source/gameengine/Physics/Sumo/SumoPhysicsController.cpp

@@ -41,7 +41,6 @@
 
 SumoPhysicsController::SumoPhysicsController(
 				class SM_Scene* sumoScene,
-				DT_SceneHandle solidscene,
 				class SM_Object* sumoObj,
 				class PHY_IMotionState* motionstate,
 
@@ -49,7 +48,6 @@ SumoPhysicsController::SumoPhysicsController(
 		: 
 		m_sumoObj(sumoObj) , 
 		m_sumoScene(sumoScene),
-		m_solidscene(solidscene),
 		m_bFirstTime(true),
 		m_bDyna(dyna),
 		m_MotionState(motionstate)
@@ -155,8 +153,6 @@ void SumoPhysicsController::GetWorldScaling(MT_Vector3& scale)
 		// kinematic methods
 void		SumoPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
 {
-
-	
 	if (m_sumoObj)
 	{
 		MT_Matrix3x3 mat;
@@ -172,7 +168,6 @@ void		SumoPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlo
 }
 void		SumoPhysicsController::RelativeRotate(const float drot[12],bool local)
 {
-
 	if (m_sumoObj )
 	{
 		MT_Matrix3x3 drotmat(drot);
@@ -186,10 +181,7 @@ void		SumoPhysicsController::RelativeRotate(const float drot[12],bool local)
 }
 void		SumoPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
 {
-//	float orn [4]={quatImag0,quatImag1,quatImag2,quatReal};
-//	MT_Quaternion quat;
 	m_sumoObj->setOrientation(MT_Quaternion(quatImag0,quatImag1,quatImag2,quatReal));
-	
 }
 
 void		SumoPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
@@ -205,6 +197,7 @@ void		SumoPhysicsController::setPosition(float posX,float posY,float posZ)
 {
 	m_sumoObj->setPosition(MT_Point3(posX,posY,posZ));
 }
+
 void		SumoPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
 {
 	m_sumoObj->setScaling(MT_Vector3(scaleX,scaleY,scaleZ));
@@ -223,7 +216,6 @@ void		SumoPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqu
 			orn * torque :
 			torque);
 	}
-
 }
 
 void		SumoPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
@@ -238,9 +230,7 @@ void		SumoPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,b
 		m_sumoObj->applyCenterForce(local ?
 						orn * force :
 						force);
-		
 	}
-
 }
 
 void		SumoPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
@@ -255,7 +245,6 @@ void		SumoPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,fl
 		m_sumoObj->setAngularVelocity(local ?
 						orn * ang_vel :
 						ang_vel);
-		
 	}
 }
 	
@@ -315,8 +304,6 @@ void		SumoPhysicsController::RestoreDynamics()
 	{
 		m_sumoObj->restoreDynamics();
 	}
-
-
 }
 
 
@@ -340,6 +327,7 @@ void		SumoPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float&
 		linvZ = 0.f;
 	}
 }
+
 /** 
 	GetVelocity parameters are in geometric coordinates (Origin is not center of mass!).
 */
@@ -363,22 +351,19 @@ void		SumoPhysicsController::GetVelocity(const float posX,const float posY,const
 	}
 }
 
-
 void		SumoPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
 {
 	const MT_Vector3& force = m_sumoObj->getReactionForce();
 	forceX = force[0];
 	forceY = force[1];
 	forceZ = force[2];
-
-
 }
+
 void		SumoPhysicsController::setRigidBody(bool rigid)
 {
 	m_sumoObj->setRigidBody(rigid);
 }
 
-
 void		SumoPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
 {
 	m_MotionState = motionstate;
@@ -425,13 +410,11 @@ void	SumoPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
 
 void SumoPhysicsController::do_me()
 {
-
 	const MT_Point3& pos = m_sumoObj->getPosition();
 	const MT_Quaternion& orn = m_sumoObj->getOrientation();
 
 	m_MotionState->setWorldPosition(pos[0],pos[1],pos[2]);
 	m_MotionState->setWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
-
 }
 
 

source/gameengine/Physics/Sumo/SumoPhysicsController.h

@@ -52,7 +52,6 @@ class SumoPhysicsController : public PHY_IPhysicsController , public SM_Callback
 public:
 	SumoPhysicsController(
 				class SM_Scene* sumoScene,
-				DT_SceneHandle solidscene,
 				class SM_Object* sumoObj,
 				class PHY_IMotionState* motionstate,
 				bool dyna); 
@@ -141,7 +140,6 @@ public:
 private:
 	class SM_Object*		m_sumoObj;
 	class SM_Scene*			m_sumoScene; // needed for replication
-	DT_SceneHandle		m_solidscene;
 	bool				m_bFirstTime;
 	bool				m_bDyna;
 

source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.cpp

@@ -38,7 +38,7 @@
 #include <config.h>
 #endif
 
-#include <SOLID/SOLID.h>
+//#include <SOLID/SOLID.h>
 
 const MT_Scalar UpperBoundForFuzzicsIntegrator = 0.01;
 // At least 100Hz (isn't this CPU hungry ?)
@@ -46,9 +46,6 @@ const MT_Scalar UpperBoundForFuzzicsIntegrator = 0.01;
 
 SumoPhysicsEnvironment::SumoPhysicsEnvironment()
 {
-	// seperate collision scene for events
-	m_solidScene = DT_CreateScene();
-
 	m_sumoScene = new SM_Scene();
 }
 
@@ -56,10 +53,7 @@ SumoPhysicsEnvironment::SumoPhysicsEnvironment()
 
 SumoPhysicsEnvironment::~SumoPhysicsEnvironment()
 {
-	std::cout << "delete m_sumoScene " << m_sumoScene << std::endl;
 	delete m_sumoScene;
-	DT_DestroyScene(m_solidScene);
-	//DT_DestroyRespTable(m_respTable);
 }
 
 void SumoPhysicsEnvironment::proceed(double timeStep)
@@ -70,20 +64,17 @@ void SumoPhysicsEnvironment::proceed(double timeStep)
 void SumoPhysicsEnvironment::setGravity(float x,float y,float z)
 {
 	m_sumoScene->setForceField(MT_Vector3(x,y,z));
-
 }
 
-
+int SumoPhysicsEnvironment::createConstraint(
-
+	class PHY_IPhysicsController* ctrl,
-
+	class PHY_IPhysicsController* ctrl2,
-
+	PHY_ConstraintType type,
-int			SumoPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type,
+	float pivotX,float pivotY,float pivotZ,
-		float pivotX,float pivotY,float pivotZ,float axisX,float axisY,float axisZ)
+	float axisX,float axisY,float axisZ)
 {
-
 	int constraintid = 0;
 	return constraintid;
-
 }
 
 void SumoPhysicsEnvironment::removeConstraint(int constraintid)

source/gameengine/Physics/Sumo/SumoPhysicsEnvironment.h

@@ -33,7 +33,6 @@
 #define _SUMOPhysicsEnvironment
 
 #include "PHY_IPhysicsEnvironment.h"
-#include <SOLID/SOLID.h>
 
 /**
 *	Physics Environment takes care of stepping the simulation and is a container for physics entities (rigidbodies,constraints, materials etc.)
@@ -44,8 +43,6 @@ class SumoPhysicsEnvironment : public PHY_IPhysicsEnvironment
 
 	class SM_Scene*	m_sumoScene;
 
-	DT_SceneHandle		m_solidScene;
-
 public:
 	SumoPhysicsEnvironment();
 	virtual		~SumoPhysicsEnvironment();
@@ -67,11 +64,6 @@ public:
 		return m_sumoScene;
 	}
 
-	DT_SceneHandle		GetSolidScene()
-	{
-		return m_solidScene;
-	}
-
 private: