BGE patch: new force field constraint actuator

A new type of constraint actuator is available: Force field.
It provides a very similar service to the Fh material feature
but with some specificities:
- It is defined at the object level: each object can have 
  different settings and you don't need to use material.
- It can be applied in all 6 directions and not just -Z.
- It can be enabled/disabled easily (it's an actuator).
- You can have multiple force fields active at the same time
  on the same object in different direction (think of a 
  space ship in a tunnel with a repulsive force field
  on each wall).
- You can have a different damping for the rotation.

Besides that it provides the same dynamic behavior and the 
parameters are self explanatory.
It works by adapting the linear and angular velocity: the
dynamic is independent of the mass. It is compatible with
all other motion actuators.

Note: linear and anysotropic friction is not yet implemented,
the only friction will come from the object damping parameters. 
Support for friction will be added in a future revision.

3 files changed, 152 insertions, 4 deletions

diff --git a/source/gameengine/Converter/KX_ConvertActuators.cpp b/source/gameengine/Converter/KX_ConvertActuators.cpp
index 78791c53d7c..b739e3b2094 100644
--- a/source/gameengine/Converter/KX_ConvertActuators.cpp
+++ b/source/gameengine/Converter/KX_ConvertActuators.cpp
@@ -719,6 +719,40 @@ void BL_ConvertActuators(char* maggiename,
 						break;
 					}
 					prop = conact->matprop;
+				} else if (conact->type == ACT_CONST_TYPE_FH) {
+					switch (conact->mode) {
+					case ACT_CONST_DIRPX:
+						locrot = KX_ConstraintActuator::KX_ACT_CONSTRAINT_FHPX;
+						min = conact->minloc[0];
+						max = conact->maxloc[0];
+						break;
+					case ACT_CONST_DIRPY:
+						locrot = KX_ConstraintActuator::KX_ACT_CONSTRAINT_FHPY;
+						min = conact->minloc[1];
+						max = conact->maxloc[1];
+						break;
+					case ACT_CONST_DIRPZ:
+						locrot = KX_ConstraintActuator::KX_ACT_CONSTRAINT_FHPZ;
+						min = conact->minloc[2];
+						max = conact->maxloc[2];
+						break;
+					case ACT_CONST_DIRNX:
+						locrot = KX_ConstraintActuator::KX_ACT_CONSTRAINT_FHNX;
+						min = conact->minloc[0];
+						max = conact->maxloc[0];
+						break;
+					case ACT_CONST_DIRNY:
+						locrot = KX_ConstraintActuator::KX_ACT_CONSTRAINT_FHNY;
+						min = conact->minloc[1];
+						max = conact->maxloc[1];
+						break;
+					case ACT_CONST_DIRNZ:
+						locrot = KX_ConstraintActuator::KX_ACT_CONSTRAINT_FHNZ;
+						min = conact->minloc[2];
+						max = conact->maxloc[2];
+						break;
+					}
+					prop = conact->matprop;
 				} else {
 					switch (conact->flag) {
 					case ACT_CONST_LOCX:
diff --git a/source/gameengine/Ketsji/KX_ConstraintActuator.cpp b/source/gameengine/Ketsji/KX_ConstraintActuator.cpp
index 49534ccbd4a..0210f78425e 100644
--- a/source/gameengine/Ketsji/KX_ConstraintActuator.cpp
+++ b/source/gameengine/Ketsji/KX_ConstraintActuator.cpp
@@ -112,7 +112,7 @@ KX_ConstraintActuator::~KX_ConstraintActuator()
 bool KX_ConstraintActuator::RayHit(KX_ClientObjectInfo* client, KX_RayCast* result, void * const data)
 {
 
-	KX_GameObject* hitKXObj = client->m_gameobject;
+	m_hitObject = client->m_gameobject;
 	
 	bool bFound = false;
 
@@ -131,7 +131,7 @@ bool KX_ConstraintActuator::RayHit(KX_ClientObjectInfo* client, KX_RayCast* resu
 		}
 		else
 		{
-			bFound = hitKXObj->GetProperty(m_property) != NULL;
+			bFound = m_hitObject->GetProperty(m_property) != NULL;
 		}
 	}
 	// update the hit status
@@ -372,7 +372,7 @@ bool KX_ConstraintActuator::Update(double curtime, bool frame)
 						// logically we should cancel the speed along the ray direction as we set the
 						// position along that axis
 						spc = obj->GetPhysicsController();
-						if (spc) {
+						if (spc && spc->IsDyna()) {
 							MT_Vector3 linV = spc->GetLinearVelocity();
 							// cancel the projection along the ray direction
 							MT_Scalar fallspeed = linV.dot(direction);
@@ -390,6 +390,110 @@ bool KX_ConstraintActuator::Update(double curtime, bool frame)
 				}
 			}
 			break; 
+		case KX_ACT_CONSTRAINT_FHPX:
+		case KX_ACT_CONSTRAINT_FHPY:
+		case KX_ACT_CONSTRAINT_FHPZ:
+		case KX_ACT_CONSTRAINT_FHNX:
+		case KX_ACT_CONSTRAINT_FHNY:
+		case KX_ACT_CONSTRAINT_FHNZ:
+			switch (m_locrot) {
+			case KX_ACT_CONSTRAINT_FHPX:
+				normal[0] = -rotation[0][0];
+				normal[1] = -rotation[1][0];
+				normal[2] = -rotation[2][0];
+				direction = MT_Vector3(1.0,0.0,0.0);
+				break;
+			case KX_ACT_CONSTRAINT_FHPY:
+				normal[0] = -rotation[0][1];
+				normal[1] = -rotation[1][1];
+				normal[2] = -rotation[2][1];
+				direction = MT_Vector3(0.0,1.0,0.0);
+				break;
+			case KX_ACT_CONSTRAINT_FHPZ:
+				normal[0] = -rotation[0][2];
+				normal[1] = -rotation[1][2];
+				normal[2] = -rotation[2][2];
+				direction = MT_Vector3(0.0,0.0,1.0);
+				break;
+			case KX_ACT_CONSTRAINT_FHNX:
+				normal[0] = rotation[0][0];
+				normal[1] = rotation[1][0];
+				normal[2] = rotation[2][0];
+				direction = MT_Vector3(-1.0,0.0,0.0);
+				break;
+			case KX_ACT_CONSTRAINT_FHNY:
+				normal[0] = rotation[0][1];
+				normal[1] = rotation[1][1];
+				normal[2] = rotation[2][1];
+				direction = MT_Vector3(0.0,-1.0,0.0);
+				break;
+			case KX_ACT_CONSTRAINT_FHNZ:
+				normal[0] = rotation[0][2];
+				normal[1] = rotation[1][2];
+				normal[2] = rotation[2][2];
+				direction = MT_Vector3(0.0,0.0,-1.0);
+				break;
+			}
+			normal.normalize();
+			{
+				PHY_IPhysicsEnvironment* pe = obj->GetPhysicsEnvironment();
+				KX_IPhysicsController *spc = obj->GetPhysicsController();
+
+				if (!pe) {
+					std::cout << "WARNING: Constraint actuator " << GetName() << ":  There is no physics environment!" << std::endl;
+					goto CHECK_TIME;
+				}	 
+				if (!spc || !spc->IsDyna()) {
+					// the object is not dynamic, it won't support setting speed
+					goto CHECK_TIME;
+				}
+				m_hitObject = NULL;
+				// distance of Fh area is stored in m_minimum
+				MT_Point3 topoint = position + (m_minimumBound+spc->GetRadius()) * direction;
+				KX_RayCast::Callback<KX_ConstraintActuator> callback(this,spc);
+				result = KX_RayCast::RayTest(pe, position, topoint, callback);
+				// we expect a hit object
+				if (!m_hitObject)
+					result = false;
+				if (result)	
+				{
+					MT_Vector3 newnormal = callback.m_hitNormal;
+					// compute new position & orientation
+					MT_Scalar distance = (callback.m_hitPoint-position).length()-spc->GetRadius(); 
+					// estimate the velocity of the hit point
+					MT_Point3 relativeHitPoint;
+					relativeHitPoint = (callback.m_hitPoint-m_hitObject->NodeGetWorldPosition());
+					MT_Vector3 velocityHitPoint = m_hitObject->GetVelocity(relativeHitPoint);
+					MT_Vector3 relativeVelocity = spc->GetLinearVelocity() - velocityHitPoint;
+					MT_Scalar relativeVelocityRay = direction.dot(relativeVelocity);
+					MT_Scalar springExtent = 1.0 - distance/m_minimumBound;
+					// Fh force is stored in m_maximum
+					MT_Scalar springForce = springExtent * m_maximumBound;
+					// damping is stored in m_refDirection [0] = damping, [1] = rot damping
+					MT_Scalar springDamp = relativeVelocityRay * m_refDirection[0];
+					MT_Vector3 newVelocity = spc->GetLinearVelocity()-(springForce+springDamp)*direction;
+					if (m_option & KX_ACT_CONSTRAINT_NORMAL)
+					{
+						newVelocity+=(springForce+springDamp)*(newnormal-newnormal.dot(direction)*direction);
+					}
+					spc->SetLinearVelocity(newVelocity, false);
+					if (m_option & KX_ACT_CONSTRAINT_DOROTFH)
+					{
+						MT_Vector3 angSpring = (normal.cross(newnormal))*m_maximumBound;
+						MT_Vector3 angVelocity = spc->GetAngularVelocity();
+						// remove component that is parallel to normal
+						angVelocity -= angVelocity.dot(newnormal)*newnormal;
+						MT_Vector3 angDamp = angVelocity * ((m_refDirection[1]>MT_EPSILON)?m_refDirection[1]:m_refDirection[0]);
+						spc->SetAngularVelocity(spc->GetAngularVelocity()+(angSpring-angDamp), false);
+					}
+				} else if (m_option & KX_ACT_CONSTRAINT_PERMANENT) {
+					// no contact but still keep running
+					result = true;
+				}
+				// don't set the position with this constraint
+				goto CHECK_TIME;
+			}
+			break; 
 		case KX_ACT_CONSTRAINT_LOCX:
 		case KX_ACT_CONSTRAINT_LOCY:
 		case KX_ACT_CONSTRAINT_LOCZ:
diff --git a/source/gameengine/Ketsji/KX_ConstraintActuator.h b/source/gameengine/Ketsji/KX_ConstraintActuator.h
index b2ef0dae553..28b9b1e6a0b 100644
--- a/source/gameengine/Ketsji/KX_ConstraintActuator.h
+++ b/source/gameengine/Ketsji/KX_ConstraintActuator.h
@@ -38,6 +38,7 @@
 #include "KX_ClientObjectInfo.h"
 
 class KX_RayCast;
+class KX_GameObject;
 
 class KX_ConstraintActuator : public SCA_IActuator
 {
@@ -65,6 +66,8 @@ protected:
 	int m_option;
 	// property to check
 	char m_property[32];
+	// hit object
+	KX_GameObject* m_hitObject;
 
 	/**
 	 * Clamp <var> to <min>, <max>. Borders are included (in as far as
@@ -92,6 +95,12 @@ protected:
 		KX_ACT_CONSTRAINT_ORIX,
 		KX_ACT_CONSTRAINT_ORIY,
 		KX_ACT_CONSTRAINT_ORIZ,
+		KX_ACT_CONSTRAINT_FHPX,
+		KX_ACT_CONSTRAINT_FHPY,
+		KX_ACT_CONSTRAINT_FHPZ,
+		KX_ACT_CONSTRAINT_FHNX,
+		KX_ACT_CONSTRAINT_FHNY,
+		KX_ACT_CONSTRAINT_FHNZ,
 		KX_ACT_CONSTRAINT_MAX
 	};
 	// match ACT_CONST_... values from BIF_interface.h
@@ -100,7 +109,8 @@ protected:
 		KX_ACT_CONSTRAINT_MATERIAL = 128,
 		KX_ACT_CONSTRAINT_PERMANENT = 256,
 		KX_ACT_CONSTRAINT_DISTANCE = 512,
-		KX_ACT_CONSTRAINT_LOCAL = 1024
+		KX_ACT_CONSTRAINT_LOCAL = 1024,
+		KX_ACT_CONSTRAINT_DOROTFH = 2048
 	};
 	bool IsValidMode(KX_CONSTRAINTTYPE m); 
 	bool RayHit(KX_ClientObjectInfo* client, KX_RayCast* result, void * const data);