Code:

void Hovertank::ApplyThrust()
{
Matrix4x4 trans,rot;
trans.SetTranslation(&CurrentState.mPosition);
Vector3 *euler_orientation = &CurrentState.mEuler_Orientation;
// Vector3 deg_orientation( RAD2DEG(euler_orientation->x), RAD2DEG(euler_orientation->y), RAD2DEG(euler_orientation->z) );
rot.InitFromRadians(euler_orientation->x,euler_orientation->y,euler_orientation->z);
Matrix4x4 temp_mat = trans * rot;
Thruster temp_thrusters[NUM_THRUSTERS];
float rest_height = mRadius + 1.0f;
for(int i = 0; i < NUM_THRUSTERS; i++)
{
temp_thrusters[i].Position = temp_mat * mThrusters[i].Position;
temp_thrusters[i].Orientation = CurrentState.mMat_Orientation * mThrusters[i].Orientation;
Vector3 end = temp_thrusters[i].Position + (temp_thrusters[i].Orientation * 10000.0f);
Vector3 start = temp_thrusters[i].Position;
gpGLRenderer->AddDebugLineToRenderer(start.x,start.y,start.z,end.x,end.y,end.z,1,0,0,2.0f);
gpGLRenderer->AddDebugSphereToRenderer(start.x,start.y,start.z,0,0,0,1,1,1,2.0f);
end = temp_thrusters[i].Position + (temp_thrusters[i].Orientation * rest_height);
HitInfo CGTrace = TraceSphereToBSPFaces(gpBSP,temp_thrusters[i].Position,end,5.0f,gpPhysicsManager->physics_epsilon);
//spring force = k * x
//k is some constant
//x is the amount of compression
if(CGTrace.Time < 1.0f)
{
float dist_to_point = (CGTrace.HitPoint - temp_thrusters[i].Position).GetLength(); //dist from collision point to cg
float compression = fabs(rest_height - dist_to_point);
float force_magnitude(0);
if(dist_to_point <= rest_height)
force_magnitude = (mMass * gpPhysicsManager->mGravAccel) * compression;
else
{
force_magnitude = (mMass * gpPhysicsManager->mGravAccel) / compression;
}
Vector3 force_vector = temp_thrusters[i].Orientation * force_magnitude * -1.0f;
// dampen the velocity
Vector3 local_contact = temp_thrusters[i].Position - CurrentState.mPosition;
Vector3 angular_part = CrossProduct(&CurrentState.mAngularVelocity,&local_contact);
float velocity_magnitude = DotProduct(&temp_thrusters[i].Orientation,&CurrentState.mLinearVelocity) +
DotProduct(&temp_thrusters[i].Orientation,&angular_part);
Vector3 dampen_velocity_force = temp_thrusters[i].Orientation * 100.0f * velocity_magnitude * -1.0f;
force_vector += dampen_velocity_force;
#if 0
// dampen the acceleration
Vector3 angular_accel = CrossProduct(&(mJInverseGlobal * mMoments),&local_contact);
float acceleration_magnitude = DotProduct(&temp_thrusters[i].Orientation,&(mForces * mMassInv)) +
DotProduct(&temp_thrusters[i].Orientation,&angular_accel);
Vector3 dampen_acceleration_force = temp_thrusters[i].Orientation * acceleration_magnitude * -5.0f;
force_vector += dampen_acceleration_force;
#endif
// DebugPrintFloat("force_applied",force_magnitude,gpTextManager);
AddForce(force_vector,local_contact);
gpGLRenderer->AddDebugSphereToRenderer(CGTrace.HitPoint.x,CGTrace.HitPoint.y,CGTrace.HitPoint.z,0,0,0,1,1,1,5.0f);
}
else
{
// gpTextManager->PushLineOfText("No force applied",-1024,1,1,0);
}
}
}