diff options
Diffstat (limited to 'source/gameengine/Physics/Sumo/Fuzzics/src/SM_FhObject.cpp')
| -rw-r--r-- | source/gameengine/Physics/Sumo/Fuzzics/src/SM_FhObject.cpp | 115 |
1 files changed, 115 insertions, 0 deletions
diff --git a/source/gameengine/Physics/Sumo/Fuzzics/src/SM_FhObject.cpp b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_FhObject.cpp new file mode 100644 index 00000000000..f8be49aa126 --- /dev/null +++ b/source/gameengine/Physics/Sumo/Fuzzics/src/SM_FhObject.cpp @@ -0,0 +1,115 @@ +#include "SM_FhObject.h" +#include "MT_MinMax.h" + +void SM_FhObject::ray_hit(void *client_data, + void *client_object1, + void *client_object2, + const DT_CollData *coll_data) { + + SM_Object *hit_object = (SM_Object *)client_object1; + const SM_MaterialProps *matProps = hit_object->getMaterialProps(); + + if ((matProps == 0) || (matProps->m_fh_distance < MT_EPSILON)) { + return; + } + + SM_FhObject *fh_object = (SM_FhObject *)client_object2; + SM_Object *cl_object = fh_object->getClientObject(); + + if (hit_object == cl_object) { + // Shot myself in the foot... + return; + } + + const SM_ShapeProps *shapeProps = cl_object->getShapeProps(); + + // Exit if the client object is not dynamic. + if (shapeProps == 0) { + return; + } + + MT_Point3 lspot; + MT_Vector3 normal; + + if (DT_ObjectRayTest(hit_object->getObjectHandle(), + fh_object->getPosition().getValue(), + fh_object->getSpot().getValue(), + lspot.getValue(), normal.getValue())) { + + const MT_Vector3& ray_dir = fh_object->getRayDirection(); + MT_Scalar dist = MT_distance(fh_object->getPosition(), + hit_object->getWorldCoord(lspot)) - + cl_object->getMargin(); + + normal.normalize(); + + if (dist < matProps->m_fh_distance) { + + if (shapeProps->m_do_fh) { + MT_Vector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocity(lspot); + MT_Scalar rel_vel_ray = ray_dir.dot(rel_vel); + MT_Scalar spring_extent = 1.0 - dist / matProps->m_fh_distance; + + MT_Scalar i_spring = spring_extent * matProps->m_fh_spring; + MT_Scalar i_damp = rel_vel_ray * matProps->m_fh_damping; + + cl_object->addLinearVelocity(-(i_spring + i_damp) * ray_dir); + if (matProps->m_fh_normal) { + cl_object->addLinearVelocity( + (i_spring + i_damp) * + (normal - normal.dot(ray_dir) * ray_dir)); + } + + MT_Vector3 lateral = rel_vel - rel_vel_ray * ray_dir; + const SM_ShapeProps *shapeProps = cl_object->getShapeProps(); + + if (shapeProps->m_do_anisotropic) { + MT_Matrix3x3 lcs(cl_object->getOrientation()); + MT_Vector3 loc_lateral = lateral * lcs; + const MT_Vector3& friction_scaling = + shapeProps->m_friction_scaling; + + loc_lateral.scale(friction_scaling[0], + friction_scaling[1], + friction_scaling[2]); + lateral = lcs * loc_lateral; + } + + + MT_Scalar rel_vel_lateral = lateral.length(); + + if (rel_vel_lateral > MT_EPSILON) { + MT_Scalar friction_factor = matProps->m_friction; + MT_Scalar max_friction = friction_factor * MT_max(0.0, i_spring); + + MT_Scalar rel_mom_lateral = rel_vel_lateral / + cl_object->getInvMass(); + + MT_Vector3 friction = + (rel_mom_lateral > max_friction) ? + -lateral * (max_friction / rel_vel_lateral) : + -lateral; + + cl_object->applyCenterImpulse(friction); + } + } + + if (shapeProps->m_do_rot_fh) { + const double *ogl_mat = cl_object->getMatrix(); + MT_Vector3 up(&ogl_mat[8]); + MT_Vector3 t_spring = up.cross(normal) * matProps->m_fh_spring; + MT_Vector3 ang_vel = cl_object->getAngularVelocity(); + + // only rotations that tilt relative to the normal are damped + ang_vel -= ang_vel.dot(normal) * normal; + + MT_Vector3 t_damp = ang_vel * matProps->m_fh_damping; + + cl_object->addAngularVelocity(t_spring - t_damp); + } + } + } +} + + + |