Delete all SOLID shapes at the end of the game engine. They were all being leaked!
[blender-staging.git] / source / gameengine / Physics / Sumo / Fuzzics / src / SM_FhObject.cpp
1 /**
2  * $Id$
3  *
4  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version. The Blender
10  * Foundation also sells licenses for use in proprietary software under
11  * the Blender License.  See http://www.blender.org/BL/ for information
12  * about this.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program; if not, write to the Free Software Foundation,
21  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
22  *
23  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
24  * All rights reserved.
25  *
26  * The Original Code is: all of this file.
27  *
28  * Contributor(s): none yet.
29  *
30  * ***** END GPL/BL DUAL LICENSE BLOCK *****
31  */
32 #include "SM_FhObject.h"
33 #include "MT_MinMax.h"
34
35 #ifdef HAVE_CONFIG_H
36 #include <config.h>
37 #endif
38
39 SM_FhObject::SM_FhObject(DT_ShapeHandle rayshape, MT_Vector3 ray, SM_Object *parent_object) :
40                 SM_Object(rayshape, NULL, NULL, NULL),
41                 m_ray(ray),
42                 m_ray_direction(ray.normalized()),
43                 m_parent_object(parent_object) 
44 {
45 }
46
47 SM_FhObject::~SM_FhObject()
48 {
49         DT_DeleteShape(getShapeHandle());
50 }
51
52 DT_Bool SM_FhObject::ray_hit(void *client_data,
53                                                   void *client_object1,
54                                                   void *client_object2,
55                                                   const DT_CollData *coll_data) 
56 {
57
58         SM_FhObject *fh_object  = dynamic_cast<SM_FhObject *>((SM_Object *)client_object2);
59         if (!fh_object)
60         {
61                 std::swap(client_object1, client_object2);
62                 fh_object  = dynamic_cast<SM_FhObject *>((SM_Object *)client_object2);
63         }
64         
65         SM_Object   *hit_object = (SM_Object *)client_object1;
66         const SM_MaterialProps *matProps = hit_object->getMaterialProps();
67         
68         if ((matProps == 0) || (matProps->m_fh_distance < MT_EPSILON)) {
69                 return DT_CONTINUE;
70         }
71
72         SM_Object           *cl_object  = fh_object->getParentObject();
73         
74         assert(fh_object);
75
76         if (hit_object == cl_object) {
77                 // Shot myself in the foot...
78                 return DT_CONTINUE;
79         }
80
81         const SM_ShapeProps *shapeProps = cl_object->getShapeProps();
82
83         // Exit if the client object is not dynamic.
84         if (shapeProps == 0) {
85                 return DT_CONTINUE;
86         }
87
88         MT_Point3 lspot; 
89         MT_Vector3 normal; 
90         
91         DT_Vector3 from, to, dnormal;
92         DT_Scalar dlspot;
93         fh_object->getPosition().getValue(from);
94         fh_object->getSpot().getValue(to);
95         
96         
97         if (DT_ObjectRayCast(hit_object->getObjectHandle(), 
98                                                  from, 
99                                                  to,
100                                                  1.,
101                                                  &dlspot, 
102                                                  dnormal)) {
103                 
104                 lspot = fh_object->getPosition() + (fh_object->getSpot() - fh_object->getPosition()) * dlspot;
105                 const MT_Vector3& ray_dir = fh_object->getRayDirection();
106                 MT_Scalar dist = MT_distance(fh_object->getPosition(), lspot) - 
107                         cl_object->getMargin() - shapeProps->m_radius;
108
109                 normal = MT_Vector3(dnormal).safe_normalized();
110                 
111                 if (dist < matProps->m_fh_distance) {
112                         
113                         if (shapeProps->m_do_fh) {
114                                 lspot -= hit_object->getPosition();
115                                 MT_Vector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocity(lspot);
116                                 MT_Scalar rel_vel_ray = ray_dir.dot(rel_vel);
117                                 MT_Scalar spring_extent = 1.0 - dist / matProps->m_fh_distance; 
118                                 
119                                 MT_Scalar i_spring = spring_extent * matProps->m_fh_spring;
120                                 MT_Scalar i_damp =   rel_vel_ray * matProps->m_fh_damping;
121                                 
122                                 cl_object->addLinearVelocity(-(i_spring + i_damp) * ray_dir); 
123                                 if (matProps->m_fh_normal) {
124                                         cl_object->addLinearVelocity(
125                                                 (i_spring + i_damp) *
126                                                 (normal - normal.dot(ray_dir) * ray_dir));
127                                 }
128                                 
129                                 MT_Vector3 lateral = rel_vel - rel_vel_ray * ray_dir;
130                                 const SM_ShapeProps *shapeProps = cl_object->getShapeProps();
131                                 
132                                 if (shapeProps->m_do_anisotropic) {
133                                         MT_Matrix3x3 lcs(cl_object->getOrientation());
134                                         MT_Vector3 loc_lateral = lateral * lcs;
135                                         const MT_Vector3& friction_scaling = 
136                                                 shapeProps->m_friction_scaling; 
137                                         
138                                         loc_lateral.scale(friction_scaling[0], 
139                                                                           friction_scaling[1], 
140                                                                           friction_scaling[2]);
141                                         lateral = lcs * loc_lateral;
142                                 }
143                                 
144
145                                 MT_Scalar rel_vel_lateral = lateral.length();
146                                 
147                                 if (rel_vel_lateral > MT_EPSILON) {
148                                         MT_Scalar friction_factor = matProps->m_friction;
149                                         MT_Scalar max_friction = friction_factor * MT_max(MT_Scalar(0.0), i_spring);
150                                         
151                                         MT_Scalar rel_mom_lateral = rel_vel_lateral / 
152                                                 cl_object->getInvMass();
153                                         
154                                         MT_Vector3 friction =
155                                                 (rel_mom_lateral > max_friction) ?
156                                                 -lateral * (max_friction / rel_vel_lateral) :
157                                                 -lateral;
158                                         
159                                         cl_object->applyCenterImpulse(friction);
160                                 }
161                         }
162                         
163                         if (shapeProps->m_do_rot_fh) {
164                                 const double *ogl_mat = cl_object->getMatrix();
165                                 MT_Vector3 up(&ogl_mat[8]);
166                                 MT_Vector3 t_spring = up.cross(normal) * matProps->m_fh_spring;
167                                 MT_Vector3 ang_vel = cl_object->getAngularVelocity();
168                                 
169                                 // only rotations that tilt relative to the normal are damped
170                                 ang_vel -= ang_vel.dot(normal) * normal;
171                                 
172                                 MT_Vector3 t_damp = ang_vel * matProps->m_fh_damping;  
173                                 
174                                 cl_object->addAngularVelocity(t_spring - t_damp);
175                         }
176                 }
177         }       
178         
179         return DT_CONTINUE;
180 }
181
182
183