Initial revision
[blender.git] / source / gameengine / Physics / Sumo / Fuzzics / src / SM_FhObject.cpp
1 #include "SM_FhObject.h"
2 #include "MT_MinMax.h"
3
4 void SM_FhObject::ray_hit(void *client_data,
5                                                   void *client_object1,
6                                                   void *client_object2,
7                                                   const DT_CollData *coll_data) {
8
9         SM_Object   *hit_object = (SM_Object *)client_object1;
10         const SM_MaterialProps *matProps = hit_object->getMaterialProps();
11
12         if ((matProps == 0) || (matProps->m_fh_distance < MT_EPSILON)) {
13                 return;
14         }
15
16         SM_FhObject         *fh_object  = (SM_FhObject *)client_object2;
17         SM_Object           *cl_object  = fh_object->getClientObject();
18
19         if (hit_object == cl_object) {
20                 // Shot myself in the foot...
21                 return;
22         }
23
24         const SM_ShapeProps *shapeProps = cl_object->getShapeProps();
25
26         // Exit if the client object is not dynamic.
27         if (shapeProps == 0) {
28                 return;
29         }
30
31         MT_Point3 lspot; 
32         MT_Vector3 normal; 
33         
34         if (DT_ObjectRayTest(hit_object->getObjectHandle(), 
35                                                  fh_object->getPosition().getValue(), 
36                                                  fh_object->getSpot().getValue(),
37                                                  lspot.getValue(), normal.getValue())) {
38                 
39                 const MT_Vector3& ray_dir = fh_object->getRayDirection();
40                 MT_Scalar dist = MT_distance(fh_object->getPosition(), 
41                                                                          hit_object->getWorldCoord(lspot)) - 
42                         cl_object->getMargin();
43
44                 normal.normalize();
45         
46                 if (dist < matProps->m_fh_distance) {
47                         
48                         if (shapeProps->m_do_fh) {
49                                 MT_Vector3 rel_vel = cl_object->getLinearVelocity()     - hit_object->getVelocity(lspot);
50                                 MT_Scalar rel_vel_ray = ray_dir.dot(rel_vel);
51                                 MT_Scalar spring_extent = 1.0 - dist / matProps->m_fh_distance; 
52                                 
53                                 MT_Scalar i_spring = spring_extent * matProps->m_fh_spring;
54                                 MT_Scalar i_damp =   rel_vel_ray * matProps->m_fh_damping;
55                                 
56                                 cl_object->addLinearVelocity(-(i_spring + i_damp) * ray_dir); 
57                                 if (matProps->m_fh_normal) {
58                                         cl_object->addLinearVelocity(
59                                                 (i_spring + i_damp) *
60                                                 (normal - normal.dot(ray_dir) * ray_dir));
61                                 }
62                                 
63                                 MT_Vector3 lateral = rel_vel - rel_vel_ray * ray_dir;
64                                 const SM_ShapeProps *shapeProps = cl_object->getShapeProps();
65                                 
66                                 if (shapeProps->m_do_anisotropic) {
67                                         MT_Matrix3x3 lcs(cl_object->getOrientation());
68                                         MT_Vector3 loc_lateral = lateral * lcs;
69                                         const MT_Vector3& friction_scaling = 
70                                                 shapeProps->m_friction_scaling; 
71                                         
72                                         loc_lateral.scale(friction_scaling[0], 
73                                                                           friction_scaling[1], 
74                                                                           friction_scaling[2]);
75                                         lateral = lcs * loc_lateral;
76                                 }
77                                 
78
79                                 MT_Scalar rel_vel_lateral = lateral.length();
80                                 
81                                 if (rel_vel_lateral > MT_EPSILON) {
82                                         MT_Scalar friction_factor = matProps->m_friction;
83                                         MT_Scalar max_friction = friction_factor * MT_max(0.0, i_spring);
84                                         
85                                         MT_Scalar rel_mom_lateral = rel_vel_lateral / 
86                                                 cl_object->getInvMass();
87                                         
88                                         MT_Vector3 friction =
89                                                 (rel_mom_lateral > max_friction) ?
90                                                 -lateral * (max_friction / rel_vel_lateral) :
91                                                 -lateral;
92                                         
93                                         cl_object->applyCenterImpulse(friction);
94                                 }
95                         }
96                         
97                         if (shapeProps->m_do_rot_fh) {
98                                 const double *ogl_mat = cl_object->getMatrix();
99                                 MT_Vector3 up(&ogl_mat[8]);
100                                 MT_Vector3 t_spring = up.cross(normal) * matProps->m_fh_spring;
101                                 MT_Vector3 ang_vel = cl_object->getAngularVelocity();
102                                 
103                                 // only rotations that tilt relative to the normal are damped
104                                 ang_vel -= ang_vel.dot(normal) * normal;
105                                 
106                                 MT_Vector3 t_damp = ang_vel * matProps->m_fh_damping;  
107                                 
108                                 cl_object->addAngularVelocity(t_spring - t_damp);
109                         }
110                 }
111         }       
112 }
113
114
115