Speed up the physics engine: hook the SOLID broad phase, so we can either reject...
[blender.git] / source / gameengine / Physics / Sumo / Fuzzics / src / SM_Object.cpp
1 /**
2  * $Id$
3  * Copyright (C) 2001 NaN Technologies B.V.
4  * The basic physics object.
5  *
6  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
7  *
8  * This program is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * as published by the Free Software Foundation; either version 2
11  * of the License, or (at your option) any later version. The Blender
12  * Foundation also sells licenses for use in proprietary software under
13  * the Blender License.  See http://www.blender.org/BL/ for information
14  * about this.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program; if not, write to the Free Software Foundation,
23  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
24  *
25  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
26  * All rights reserved.
27  *
28  * The Original Code is: all of this file.
29  *
30  * Contributor(s): none yet.
31  *
32  * ***** END GPL/BL DUAL LICENSE BLOCK *****
33  */
34
35 #ifdef HAVE_CONFIG_H
36 #include <config.h>
37 #endif
38
39 #ifdef WIN32
40 // This warning tells us about truncation of __long__ stl-generated names.
41 // It can occasionally cause DevStudio to have internal compiler warnings.
42 #pragma warning( disable : 4786 )     
43 #endif
44
45 #include "SM_Object.h"
46 #include "SM_Scene.h"
47 #include "SM_FhObject.h"
48 #include "SM_Debug.h"
49
50 #include "MT_MinMax.h"
51
52 MT_Scalar SM_Object::ImpulseThreshold = -10.;
53
54 SM_Object::SM_Object(
55         DT_ShapeHandle shape, 
56         const SM_MaterialProps *materialProps,
57         const SM_ShapeProps *shapeProps,
58         SM_Object *dynamicParent)  :
59         
60         m_dynamicParent(dynamicParent),
61         m_client_object(0),
62         
63         m_shape(shape),
64         m_materialProps(materialProps),
65         m_materialPropsBackup(0),
66         m_shapeProps(shapeProps),
67         m_shapePropsBackup(0),
68         m_object(DT_CreateObject(this, shape)),
69         m_margin(0.0),
70         m_scaling(1.0, 1.0, 1.0),
71         m_reaction_impulse(0.0, 0.0, 0.0),
72         m_reaction_force(0.0, 0.0, 0.0),
73         m_kinematic(false),
74         m_prev_kinematic(false),
75         m_is_rigid_body(false),
76         m_lin_mom(0.0, 0.0, 0.0),
77         m_ang_mom(0.0, 0.0, 0.0),
78         m_force(0.0, 0.0, 0.0),
79         m_torque(0.0, 0.0, 0.0),
80         m_error(0.0, 0.0, 0.0),
81         m_combined_lin_vel (0.0, 0.0, 0.0),
82         m_combined_ang_vel (0.0, 0.0, 0.0),
83         m_fh_object(0),
84         m_inv_mass(0.0),
85         m_inv_inertia(0., 0., 0.)
86 {
87         m_xform.setIdentity();
88         m_xform.getValue(m_ogl_matrix);
89         if (shapeProps)
90         { 
91                 if (shapeProps->m_do_fh || shapeProps->m_do_rot_fh) 
92                 {
93                         DT_Vector3 zero = {0., 0., 0.}, ray = {0.0, 0.0, -10.0};
94                         m_fh_object = new SM_FhObject(DT_NewLineSegment(zero, ray), MT_Vector3(ray), this);
95                         //printf("SM_Object:: WARNING! fh disabled.\n");
96                 }
97                 m_inv_mass = 1. / shapeProps->m_mass;
98                 m_inv_inertia = MT_Vector3(1./shapeProps->m_inertia[0], 1./shapeProps->m_inertia[1], 1./shapeProps->m_inertia[2]);
99         }
100         updateInvInertiaTensor();
101         m_suspended = false;
102 }
103
104
105         void 
106 SM_Object::
107 integrateForces(
108         MT_Scalar timeStep
109 ){
110         if (!m_suspended) {
111                 m_prev_state = *this;
112                 m_prev_state.setLinearVelocity(actualLinVelocity());
113                 m_prev_state.setAngularVelocity(actualAngVelocity());
114                 if (isDynamic()) {
115                         // Integrate momentum (forward Euler)
116                         m_lin_mom += m_force * timeStep;
117                         m_ang_mom += m_torque * timeStep;
118                         // Drain momentum because of air/water resistance
119                         m_lin_mom *= pow(m_shapeProps->m_lin_drag, timeStep);
120                         m_ang_mom *= pow(m_shapeProps->m_ang_drag, timeStep);
121                         // Set velocities according momentum
122                         m_lin_vel = m_lin_mom * m_inv_mass;
123                         m_ang_vel = m_inv_inertia_tensor * m_ang_mom;
124                 }
125         }       
126
127 };
128
129         void 
130 SM_Object::
131 integrateMomentum(
132         MT_Scalar timeStep
133 ){
134         // Integrate position and orientation
135
136         // only do it for objects with linear and/or angular velocity
137         // else clients with hierarchies may get into trouble
138         if (!actualLinVelocity().fuzzyZero() || !actualAngVelocity().fuzzyZero()) 
139         {
140
141         // those MIDPOINT and BACKWARD integration methods are
142         // in this form not ok with some testfiles ! 
143         // For a release build please use forward euler unless completely tested
144
145 //#define MIDPOINT
146 //#define BACKWARD
147 #ifdef  MIDPOINT
148 // Midpoint rule
149                 m_pos += (m_prev_state.getLinearVelocity() + actualLinVelocity()) * (timeStep * 0.5);
150                 m_orn += (m_prev_state.getAngularVelocity() * m_prev_state.getOrientation() + actualAngVelocity() * m_orn) * (timeStep * 0.25);
151 #elif defined BACKWARD
152 // Backward Euler
153                 m_pos += actualLinVelocity() * timeStep;
154                 m_orn += actualAngVelocity() * m_orn * (timeStep * 0.5);
155 #else 
156 // Forward Euler
157
158                 m_pos += m_prev_state.getLinearVelocity() * timeStep;
159                 m_orn += m_prev_state.getAngularVelocity() * m_orn * (timeStep * 0.5);
160 #endif
161                 m_orn.normalize(); // I might not be necessary to do this every call
162
163                 calcXform();
164                 notifyClient();         
165
166         }
167 }
168
169 /**
170  * dynamicCollision computes the response to a collision.
171  *
172  * @param local2 the contact point in local coordinates.
173  * @param normal the contact normal.
174  * @param dist the penetration depth of the contact. (unused)
175  * @param rel_vel the relative velocity of the objects
176  * @param restitution the amount of momentum conserved in the collision. Range: 0.0 - 1.0
177  * @param friction_factor the amount of friction between the two surfaces.
178  * @param invMass the inverse mass of the collision objects (1.0 / mass)
179  */
180 void SM_Object::dynamicCollision(const MT_Point3 &local2, 
181         const MT_Vector3 &normal, 
182         MT_Scalar dist, 
183         const MT_Vector3 &rel_vel,
184         MT_Scalar restitution,
185         MT_Scalar friction_factor,
186         MT_Scalar invMass
187 )
188 {
189         /**
190          * rel_vel_normal is the relative velocity in the contact normal direction.
191          */
192         MT_Scalar  rel_vel_normal = normal.dot(rel_vel);
193                         
194         /**
195          * if rel_vel_normal > 0, the objects are moving apart! 
196          */
197         if (rel_vel_normal < 0.) {
198                 /**
199                  * if rel_vel_normal < ImpulseThreshold, scale the restitution down.
200                  * This should improve the simulation where the object is stacked.
201                  */
202                 restitution *= MT_min(MT_Scalar(1.0), rel_vel_normal/ImpulseThreshold);
203                                 
204                 MT_Scalar impulse = -(1.0 + restitution) * rel_vel_normal;
205                 
206                 if (isRigidBody())
207                 {
208                         MT_Vector3 temp = getInvInertiaTensor() * local2.cross(normal);
209                         impulse /= invMass + normal.dot(temp.cross(local2));
210                         
211                         /**
212                          * Apply impulse at the collision point.
213                          * Take rotational inertia into account.
214                          */
215                         applyImpulse(local2 + m_pos, impulse * normal);
216                 } else {
217                         /**
218                          * Apply impulse through object centre. (no rotation.)
219                          */
220                         impulse /= invMass;
221                         applyCenterImpulse( impulse * normal ); 
222                 }
223                 
224                 MT_Vector3 external = m_combined_lin_vel + m_combined_ang_vel.cross(local2);
225                 MT_Vector3 lateral =  rel_vel - external - normal * (rel_vel_normal - external.dot(normal));
226 #if 0
227                 // test - only do friction on the physics part of the 
228                 // velocity.
229                 vel1  -= obj1->m_combined_lin_vel;
230                 vel2  -= obj2->m_combined_lin_vel;
231
232                 // This should look familiar....
233                 rel_vel        = vel2 - vel1;
234                 rel_vel_normal = normal.dot(rel_vel);
235 #endif
236                 /**
237                  * The friction part starts here!!!!!!!!
238                  *
239                  * Compute the lateral component of the relative velocity
240                  * lateral actually points in the opposite direction, i.e.,
241                  * into the direction of the friction force.
242                  */
243                 if (m_shapeProps->m_do_anisotropic) {
244
245                         /**
246                          * For anisotropic friction we scale the lateral component,
247                          * rather than compute a direction-dependent fricition 
248                          * factor. For this the lateral component is transformed to
249                          * local coordinates.
250                          */
251
252                         MT_Matrix3x3 lcs(m_orn);
253                         
254                         /**
255                          * We cannot use m_xform.getBasis() for the matrix, since 
256                          * it might contain a non-uniform scaling. 
257                          * OPT: it's a bit daft to compute the matrix since the 
258                          * quaternion itself can be used to do the transformation.
259                          */
260                         MT_Vector3 loc_lateral = lateral * lcs;
261                         
262                         /**
263                          * lcs is orthogonal so lcs.inversed() == lcs.transposed(),
264                          * and lcs.transposed() * lateral == lateral * lcs.
265                          */
266                         const MT_Vector3& friction_scaling = 
267                                 m_shapeProps->m_friction_scaling; 
268
269                         // Scale the local lateral...
270                         loc_lateral.scale(friction_scaling[0], 
271                                                         friction_scaling[1], 
272                                                         friction_scaling[2]);
273                         // ... and transform it back to global coordinates
274                         lateral = lcs * loc_lateral;
275                 }
276                         
277                 /**
278                  * A tiny Coulomb friction primer:
279                  * The Coulomb friction law states that the magnitude of the
280                  * maximum possible friction force depends linearly on the 
281                  * magnitude of the normal force.
282                  *
283                  * \f[
284                      F_max_friction = friction_factor * F_normal 
285                    \f]
286                  *
287                  * (NB: independent of the contact area!!)
288                  *
289                  * The friction factor depends on the material. 
290                  * We use impulses rather than forces but let us not be 
291                  * bothered by this. 
292                  */
293                 MT_Scalar  rel_vel_lateral = lateral.length();
294
295                 if (rel_vel_lateral > MT_EPSILON) {
296                         lateral /= rel_vel_lateral;
297
298                         // Compute the maximum friction impulse
299                         MT_Scalar max_friction = 
300                                 friction_factor * MT_max(MT_Scalar(0.0), impulse);
301
302                         // I guess the GEN_max is not necessary, so let's check it
303
304                         assert(impulse >= 0.0);
305
306                         /**
307                          * Here's the trick. We compute the impulse to make the
308                          * lateral velocity zero. (Make the objects stick together
309                          * at the contact point. If this impulse is larger than
310                          * the maximum possible friction impulse, then shrink its
311                          * magnitude to the maximum friction.
312                          */
313
314                         if (isRigidBody()) {
315                                         
316                                 /**
317                                  * For rigid bodies we take the inertia into account, 
318                                  * since the friction impulse is going to change the
319                                  * angular momentum as well.
320                                  */
321                                 MT_Vector3 temp = getInvInertiaTensor() * local2.cross(lateral);
322                                 MT_Scalar impulse_lateral = rel_vel_lateral /
323                                         (invMass + lateral.dot(temp.cross(local2)));
324
325                                 MT_Scalar friction = MT_min(impulse_lateral, max_friction);
326                                 applyImpulse(local2 + m_pos, -lateral * friction);
327                         }
328                         else {
329                                 MT_Scalar impulse_lateral = rel_vel_lateral / invMass;
330
331                                 MT_Scalar friction = MT_min(impulse_lateral, max_friction);
332                                 applyCenterImpulse( -friction * lateral);
333                         }
334                                 
335
336                 }       
337
338                 //calcXform();
339                 //notifyClient();
340
341         }
342 }
343
344 static void AddCallback(SM_Scene *scene, SM_Object *obj1, SM_Object *obj2)
345 {
346         // If we have callbacks on either of the client objects, do a collision test
347         // and add a callback if they intersect.
348         DT_Vector3 v;
349         if ((obj1->getClientObject() && obj1->getClientObject()->hasCollisionCallback()) || 
350             (obj2->getClientObject() && obj2->getClientObject()->hasCollisionCallback()) &&
351              DT_GetIntersect(obj1->getObjectHandle(), obj2->getObjectHandle(), v))
352                 scene->addPair(obj1, obj2);
353 }
354
355 DT_Bool SM_Object::boing(
356         void *client_data,  
357         void *object1,
358         void *object2,
359         const DT_CollData *coll_data
360 ){
361         SM_Scene  *scene = (SM_Scene *)client_data; 
362         SM_Object *obj1  = (SM_Object *)object1;  
363         SM_Object *obj2  = (SM_Object *)object2;  
364         
365         // at this point it is unknown whether we are really intersecting (broad phase)
366         
367         DT_Vector3 p1, p2;
368         if (!obj2->isDynamic()) {
369                 std::swap(obj1, obj2);
370         }
371         
372         // If one of the objects is a ghost then ignore it for the dynamics
373         if (obj1->isGhost() || obj2->isGhost()) {
374                 AddCallback(scene, obj1, obj2);
375                 return DT_CONTINUE;
376         }
377
378         // Objects do not collide with parent objects
379         if (obj1->getDynamicParent() == obj2 || obj2->getDynamicParent() == obj1) {
380                 AddCallback(scene, obj1, obj2);
381                 return DT_CONTINUE;
382         }
383         
384         if (!obj2->isDynamic()) {
385                 AddCallback(scene, obj1, obj2);
386                 return DT_CONTINUE;
387         }
388
389         // Get collision data from SOLID
390         if (!DT_GetPenDepth(obj1->getObjectHandle(), obj2->getObjectHandle(), p1, p2))
391                 return DT_CONTINUE;
392         
393         MT_Point3 local1(p1), local2(p2);
394         MT_Vector3 normal(local2 - local1);
395         MT_Scalar dist = normal.length();
396         
397         if (dist < MT_EPSILON)
398                 return DT_CONTINUE;
399                 
400         // Now we are definately intersecting.
401
402         // Set callbacks for game engine.
403         if ((obj1->getClientObject() && obj1->getClientObject()->hasCollisionCallback()) || 
404             (obj2->getClientObject() && obj2->getClientObject()->hasCollisionCallback()))
405                 scene->addPair(obj1, obj2);
406         
407         local1 -= obj1->m_pos;
408         local2 -= obj2->m_pos;
409         
410         // Calculate collision parameters
411         MT_Vector3 rel_vel        = obj1->getVelocity(local1) - obj2->getVelocity(local2);
412         
413         MT_Scalar restitution = 
414                 MT_min(obj1->getMaterialProps()->m_restitution,
415                                 obj2->getMaterialProps()->m_restitution);
416         
417         MT_Scalar friction_factor = 
418                 MT_min(obj1->getMaterialProps()->m_friction, 
419                                 obj2->getMaterialProps()->m_friction);
420                                 
421         MT_Scalar invMass = obj1->getInvMass() + obj2->getInvMass();
422         
423         normal /= dist;
424         
425         // Calculate reactions
426         if (obj1->isDynamic())
427                 obj1->dynamicCollision(local1, normal, dist, rel_vel, restitution, friction_factor, invMass);
428                 
429         if (obj2->isDynamic())
430                 obj2->dynamicCollision(local2, -normal, dist, -rel_vel, restitution, friction_factor, invMass);
431         
432         return DT_CONTINUE;
433 }
434
435 DT_Bool SM_Object::fix(
436         void *client_data,
437         void *object1,
438         void *object2,
439         const DT_CollData *coll_data
440 ){
441         SM_Scene  *scene = (SM_Scene *)client_data; 
442         SM_Object *obj1  = (SM_Object *)object1;  
443         SM_Object *obj2  = (SM_Object *)object2;  
444         
445         // If one of the objects is a ghost then ignore it for the dynamics
446         if (obj1->isGhost() || obj2->isGhost()) {
447                 return DT_CONTINUE;
448         }
449
450         if (obj1->getDynamicParent() == obj2 || obj2->getDynamicParent() == obj1) {
451                 return DT_CONTINUE;
452         }
453         
454         if (!obj2->isDynamic()) {
455                 std::swap(obj1, obj2);
456         }
457
458         if (!obj2->isDynamic()) {
459                 return DT_CONTINUE;
460         }
461
462         // obj1 points to a dynamic object
463         DT_Vector3 p1, p2;
464         if (!DT_GetPenDepth(obj1->getObjectHandle(), obj2->getObjectHandle(), p1, p2))
465                 return DT_CONTINUE;
466         MT_Point3 local1(p1), local2(p2);
467         // Get collision data from SOLID
468         MT_Vector3 normal(local2 - local1);
469         
470         MT_Scalar dist = normal.dot(normal);
471         if (dist < MT_EPSILON || dist > obj2->m_shapeProps->m_radius*obj2->m_shapeProps->m_radius)
472                 return DT_CONTINUE;
473                 
474         // This distinction between dynamic and non-dynamic objects should not be 
475         // necessary. Non-dynamic objects are assumed to have infinite mass.
476         if (obj1->isDynamic()) {
477                 MT_Vector3 error = normal * 0.5f;
478                 obj1->m_error += error;
479                 obj2->m_error -= error;
480         }
481         else {
482                 // Same again but now obj1 is non-dynamic
483                 obj2->m_error -= normal;
484         }
485         
486         return DT_CONTINUE;
487 }
488
489 void SM_Object::relax(void)
490
491         if (m_error.fuzzyZero())
492                 return;
493         //std::cout << "SM_Object::relax: { " << m_error << " }" << std::endl;
494         
495         m_pos += m_error; 
496         m_error.setValue(0., 0., 0.); 
497         calcXform();
498         notifyClient();
499 }
500         
501 SM_Object::SM_Object() :
502         m_dynamicParent(0),
503         m_client_object(0),
504         
505         m_shape(0),
506         m_materialProps(0),
507         m_materialPropsBackup(0),
508         m_shapeProps(0),
509         m_shapePropsBackup(0),
510         m_object(0),
511         m_margin(0.0),
512         m_scaling(1.0, 1.0, 1.0),
513         m_reaction_impulse(0.0, 0.0, 0.0),
514         m_reaction_force(0.0, 0.0, 0.0),
515         m_kinematic(false),
516         m_prev_kinematic(false),
517         m_is_rigid_body(false),
518         m_lin_mom(0.0, 0.0, 0.0),
519         m_ang_mom(0.0, 0.0, 0.0),
520         m_force(0.0, 0.0, 0.0),
521         m_torque(0.0, 0.0, 0.0),
522         m_error(0.0, 0.0, 0.0),
523         m_combined_lin_vel (0.0, 0.0, 0.0),
524         m_combined_ang_vel (0.0, 0.0, 0.0),
525         m_fh_object(0) 
526 {
527         // warning no initialization of variables done by moto.
528 }
529
530 SM_Object::
531 ~SM_Object() { 
532         if (m_fh_object)
533                 delete m_fh_object;
534         
535         DT_DestroyObject(m_object);
536         m_object = NULL;
537 }
538
539         bool 
540 SM_Object::
541 isDynamic(
542 ) const {
543         return m_shapeProps != 0; 
544
545
546 /* nzc experimental. There seem to be two places where kinematics
547  * are evaluated: proceedKinematic (called from SM_Scene) and
548  * proceed() in this object. I'll just try and bunge these out for
549  * now.  */
550         void 
551 SM_Object::
552 suspend(
553 ){
554         if (!m_suspended) {
555                 m_suspended = true;
556                 suspendDynamics();
557         }
558 }
559
560         void 
561 SM_Object::
562 resume(
563 ) {
564         if (m_suspended) {
565                 m_suspended = false;
566                 restoreDynamics();
567         }
568 }
569
570         void 
571 SM_Object::
572 suspendDynamics(
573 ) {
574         if (m_shapeProps) {
575                 m_shapePropsBackup = m_shapeProps;
576                 m_shapeProps = 0;
577         }
578 }
579
580         void 
581 SM_Object::
582 restoreDynamics(
583 ) {
584         if (m_shapePropsBackup) {
585                 m_shapeProps = m_shapePropsBackup;
586                 m_shapePropsBackup = 0;
587         }
588 }
589
590         bool 
591 SM_Object::
592 isGhost(
593 ) const {
594         return m_materialProps == 0;
595
596
597         void 
598 SM_Object::
599 suspendMaterial(
600 ) {
601         if (m_materialProps) {
602                 m_materialPropsBackup = m_materialProps;
603                 m_materialProps = 0;
604         }
605 }
606
607         void 
608 SM_Object::
609 restoreMaterial(
610 ) {
611         if (m_materialPropsBackup) {
612                 m_materialProps = m_materialPropsBackup;
613                 m_materialPropsBackup = 0;
614         }
615 }
616
617         SM_FhObject *
618 SM_Object::
619 getFhObject(
620 ) const {
621         return m_fh_object;
622
623
624         void 
625 SM_Object::
626 registerCallback(
627         SM_Callback& callback
628 ) {
629         m_callbackList.push_back(&callback);
630 }
631
632 // Set the local coordinate system according to the current state 
633         void 
634 SM_Object::
635 calcXform() {
636 #ifdef SM_DEBUG_XFORM
637         printf("SM_Object::calcXform m_pos = { %-0.5f, %-0.5f, %-0.5f }\n",
638                 m_pos[0], m_pos[1], m_pos[2]);
639         printf("                     m_orn = { %-0.5f, %-0.5f, %-0.5f, %-0.5f }\n",
640                 m_orn[0], m_orn[1], m_orn[2], m_orn[3]);
641         printf("                 m_scaling = { %-0.5f, %-0.5f, %-0.5f }\n",
642                 m_scaling[0], m_scaling[1], m_scaling[2]);
643 #endif
644         m_xform.setOrigin(m_pos);
645         m_xform.setBasis(MT_Matrix3x3(m_orn, m_scaling));
646         m_xform.getValue(m_ogl_matrix);
647         
648         /* Blender has been known to crash here.
649            This usually means SM_Object *this has been deleted more than once. */
650         DT_SetMatrixd(m_object, m_ogl_matrix);
651         if (m_fh_object) {
652                 m_fh_object->setPosition(m_pos);
653                 m_fh_object->calcXform();
654         }
655         updateInvInertiaTensor();
656 #ifdef SM_DEBUG_XFORM
657         printf("\n               | %-0.5f %-0.5f %-0.5f %-0.5f |\n",
658                 m_ogl_matrix[0], m_ogl_matrix[4], m_ogl_matrix[ 8], m_ogl_matrix[12]);
659         printf(  "               | %-0.5f %-0.5f %-0.5f %-0.5f |\n",
660                 m_ogl_matrix[1], m_ogl_matrix[5], m_ogl_matrix[ 9], m_ogl_matrix[13]);
661         printf(  "m_ogl_matrix = | %-0.5f %-0.5f %-0.5f %-0.5f |\n",
662                 m_ogl_matrix[2], m_ogl_matrix[6], m_ogl_matrix[10], m_ogl_matrix[14]);
663         printf(  "               | %-0.5f %-0.5f %-0.5f %-0.5f |\n\n",
664                 m_ogl_matrix[3], m_ogl_matrix[7], m_ogl_matrix[11], m_ogl_matrix[15]);
665 #endif
666 }
667
668         void 
669 SM_Object::updateInvInertiaTensor() 
670 {
671         m_inv_inertia_tensor = m_xform.getBasis().scaled(m_inv_inertia[0], m_inv_inertia[1], m_inv_inertia[2]) * m_xform.getBasis().transposed();
672 }
673
674 // Call callbacks to notify the client of a change of placement
675         void 
676 SM_Object::
677 notifyClient() {
678         T_CallbackList::iterator i;
679         for (i = m_callbackList.begin(); i != m_callbackList.end(); ++i) {
680                 (*i)->do_me();
681         }
682 }
683
684
685 // Save the current state information for use in the velocity computation in the next frame.  
686         void 
687 SM_Object::
688 proceedKinematic(
689         MT_Scalar timeStep
690 ) {
691         /* nzc: need to bunge this for the logic bubbling as well? */
692         if (!m_suspended) {
693                 m_prev_kinematic = m_kinematic;              
694                 if (m_kinematic) {
695                         m_prev_xform = m_xform;
696                         m_timeStep = timeStep;
697                         calcXform();
698                         m_kinematic  = false;
699                 }
700         }
701 }
702
703         void 
704 SM_Object::
705 saveReactionForce(
706         MT_Scalar timeStep
707 ) {
708         if (isDynamic()) {
709                 m_reaction_force   = m_reaction_impulse / timeStep;
710                 m_reaction_impulse.setValue(0.0, 0.0, 0.0);
711         }
712 }
713
714         void 
715 SM_Object::
716 clearForce(
717 ) {
718         m_force.setValue(0.0, 0.0, 0.0);
719         m_torque.setValue(0.0, 0.0, 0.0);
720 }
721
722         void 
723 SM_Object::
724 clearMomentum(
725 ) {
726         m_lin_mom.setValue(0.0, 0.0, 0.0);
727         m_ang_mom.setValue(0.0, 0.0, 0.0);
728 }
729
730         void 
731 SM_Object::
732 setMargin(
733         MT_Scalar margin
734 ) {
735         m_margin = margin;
736         DT_SetMargin(m_object, margin);
737 }
738
739         MT_Scalar 
740 SM_Object::
741 getMargin(
742 ) const {
743     return m_margin;
744 }
745
746 const 
747         SM_MaterialProps *
748 SM_Object::
749 getMaterialProps(
750 ) const {
751     return m_materialProps;
752 }
753
754 const 
755         SM_ShapeProps *
756 SM_Object::
757 getShapeProps(
758 ) const {
759     return m_shapeProps;
760 }
761
762         void 
763 SM_Object::
764 setPosition(
765         const MT_Point3& pos
766 ){
767         m_kinematic = true;
768         m_pos = pos;
769 }
770
771         void 
772 SM_Object::
773 setOrientation(
774         const MT_Quaternion& orn
775 ){
776         assert(!orn.fuzzyZero());
777         m_kinematic = true;
778         m_orn = orn;
779 }
780
781         void 
782 SM_Object::
783 setScaling(
784         const MT_Vector3& scaling
785 ){
786         m_kinematic = true;
787         m_scaling = scaling;
788 }
789
790 /**
791  * Functions to handle linear velocity
792  */
793
794         void 
795 SM_Object::
796 setExternalLinearVelocity(
797         const MT_Vector3& lin_vel
798 ) {
799         m_combined_lin_vel=lin_vel;
800 }
801
802         void 
803 SM_Object::
804 addExternalLinearVelocity(
805         const MT_Vector3& lin_vel
806 ) {
807         m_combined_lin_vel+=lin_vel;
808 }
809
810         void 
811 SM_Object::
812 addLinearVelocity(
813         const MT_Vector3& lin_vel
814 ){
815         m_lin_vel += lin_vel;
816         if (m_shapeProps) {
817                 m_lin_mom = m_lin_vel * m_shapeProps->m_mass;
818         }
819 }
820
821         void 
822 SM_Object::
823 setLinearVelocity(
824         const MT_Vector3& lin_vel
825 ){
826         m_lin_vel = lin_vel;
827         if (m_shapeProps) {
828                 m_lin_mom = m_lin_vel * m_shapeProps->m_mass;
829         }
830 }
831
832 /**
833  * Functions to handle angular velocity
834  */
835
836         void 
837 SM_Object::
838 setExternalAngularVelocity(
839         const MT_Vector3& ang_vel
840 ) {
841         m_combined_ang_vel = ang_vel;
842 }
843
844         void
845 SM_Object::
846 addExternalAngularVelocity(
847         const MT_Vector3& ang_vel
848 ) {
849         m_combined_ang_vel += ang_vel;
850 }
851
852         void 
853 SM_Object::
854 setAngularVelocity(
855         const MT_Vector3& ang_vel
856 ) {
857         m_ang_vel = ang_vel;
858         if (m_shapeProps) {
859                 m_ang_mom = m_ang_vel * m_shapeProps->m_inertia;
860         }
861 }
862
863         void
864 SM_Object::
865 addAngularVelocity(
866         const MT_Vector3& ang_vel
867 ) {
868         m_ang_vel += ang_vel;
869         if (m_shapeProps) {
870                 m_ang_mom = m_ang_vel * m_shapeProps->m_inertia;
871         }
872 }
873
874
875         void 
876 SM_Object::
877 clearCombinedVelocities(
878 ) {
879         m_combined_lin_vel = MT_Vector3(0,0,0);
880         m_combined_ang_vel = MT_Vector3(0,0,0);
881 }
882
883         void 
884 SM_Object::
885 resolveCombinedVelocities(
886         const MT_Vector3 & lin_vel,
887         const MT_Vector3 & ang_vel
888 ) {
889
890         // Different behaviours for dynamic and non-dynamic 
891         // objects. For non-dynamic we just set the velocity to 
892         // zero. For dynmic the physics velocity has to be 
893         // taken into account. We must make an arbitrary decision
894         // on how to resolve the 2 velocities. Choices are
895         // Add the physics velocity to the linear velocity. Objects
896         // will just keep on moving in the direction they were
897         // last set in - untill external forces affect them.
898         // Set the combinbed linear and physics velocity to zero.
899         // Set the physics velocity in the direction of the set velocity
900         // zero.
901         if (isDynamic()) {              
902
903 #if 1
904                 m_lin_vel += lin_vel;
905                 m_ang_vel += ang_vel;
906 #else
907
908                 //compute the component of the physics velocity in the 
909                 // direction of the set velocity and set it to zero.
910                 MT_Vector3 lin_vel_norm = lin_vel.normalized();
911
912                 m_lin_vel -= (m_lin_vel.dot(lin_vel_norm) * lin_vel_norm);
913 #endif
914                 m_lin_mom = m_lin_vel * m_shapeProps->m_mass;
915                 m_ang_mom = m_ang_vel * m_shapeProps->m_inertia;
916                 clearCombinedVelocities();
917
918         }
919
920 }               
921
922
923         MT_Scalar 
924 SM_Object::
925 getInvMass(
926 ) const { 
927         return m_inv_mass;
928         // OPT: cache the result of this division rather than compute it each call
929 }
930
931         const MT_Vector3&
932 SM_Object::
933 getInvInertia(
934 ) const { 
935         return m_inv_inertia;
936         // OPT: cache the result of this division rather than compute it each call
937 }
938
939         const MT_Matrix3x3&
940 SM_Object::
941 getInvInertiaTensor(
942 ) const { 
943         return m_inv_inertia_tensor; 
944 }
945
946         void 
947 SM_Object::
948 applyForceField(
949         const MT_Vector3& accel
950 ) {
951         if (m_shapeProps) {
952                 m_force += m_shapeProps->m_mass * accel;  // F = m * a
953         }
954 }
955
956         void 
957 SM_Object::
958 applyCenterForce(
959         const MT_Vector3& force
960 ) {
961         m_force += force;
962 }
963
964         void 
965 SM_Object::
966 applyTorque(
967         const MT_Vector3& torque
968 ) {
969         m_torque += torque;
970 }
971
972         void 
973 SM_Object::
974 applyImpulse(
975         const MT_Point3& attach, const MT_Vector3& impulse
976 ) {
977         applyCenterImpulse(impulse);                          // Change in linear momentum
978         applyAngularImpulse((attach - m_pos).cross(impulse)); // Change in angular momentump
979 }
980
981         void 
982 SM_Object::
983 applyCenterImpulse(
984         const MT_Vector3& impulse
985 ) {
986         if (m_shapeProps) {
987                 m_lin_mom          += impulse;
988                 m_reaction_impulse += impulse;
989                 m_lin_vel           = m_lin_mom * m_inv_mass;
990
991                 // The linear velocity is immedialtely updated since otherwise
992                 // simultaneous collisions will get a double impulse. 
993         }
994 }
995
996         void 
997 SM_Object::
998 applyAngularImpulse(
999         const MT_Vector3& impulse
1000 ) {
1001         if (m_shapeProps) {
1002                 m_ang_mom += impulse;
1003                 m_ang_vel = m_inv_inertia_tensor * m_ang_mom;
1004         }
1005 }
1006
1007         MT_Point3 
1008 SM_Object::
1009 getWorldCoord(
1010         const MT_Point3& local
1011 ) const {
1012     return m_xform(local);
1013 }
1014
1015         MT_Vector3 
1016 SM_Object::
1017 getVelocity(
1018         const MT_Point3& local
1019 ) const {
1020         if (m_prev_kinematic && !isDynamic())
1021         {
1022                 // For displaced objects the velocity is faked using the previous state. 
1023                 // Dynamic objects get their own velocity, not the faked velocity.
1024                 // (Dynamic objects shouldn't be displaced in the first place!!)
1025                 return (m_xform(local) - m_prev_xform(local)) / m_timeStep;
1026         }
1027         
1028         // NB: m_xform.getBasis() * local == m_xform(local) - m_xform.getOrigin()
1029         return actualLinVelocity() + actualAngVelocity().cross(local);
1030 }
1031
1032
1033 const 
1034         MT_Vector3& 
1035 SM_Object::
1036 getReactionForce(
1037 ) const {
1038         return m_reaction_force;
1039 }
1040
1041         void 
1042 SM_Object::
1043 getMatrix(
1044         double *m
1045 ) const {
1046     std::copy(&m_ogl_matrix[0], &m_ogl_matrix[16], &m[0]);
1047 }
1048
1049 const 
1050         double *
1051 SM_Object::
1052 getMatrix(
1053 ) const {
1054         return m_ogl_matrix;
1055 }
1056
1057 // Still need this???
1058 const 
1059         MT_Transform&  
1060 SM_Object::
1061 getScaledTransform(
1062 ) const {
1063         return m_xform;
1064 }
1065
1066         DT_ObjectHandle 
1067 SM_Object::
1068 getObjectHandle(
1069 ) const {
1070         return m_object;
1071 }
1072
1073         DT_ShapeHandle 
1074 SM_Object::
1075 getShapeHandle(
1076 ) const { 
1077         return m_shape;
1078 }
1079
1080         SM_Object *
1081 SM_Object::
1082 getDynamicParent(
1083 ) {
1084         return m_dynamicParent;
1085 }
1086
1087         void 
1088 SM_Object::
1089 setRigidBody(
1090         bool is_rigid_body
1091 ) { 
1092         m_is_rigid_body = is_rigid_body;
1093
1094
1095         bool 
1096 SM_Object::
1097 isRigidBody(
1098 ) const {
1099         return m_is_rigid_body;
1100 }
1101
1102 const 
1103         MT_Vector3
1104 SM_Object::
1105 actualLinVelocity(
1106 ) const {
1107         return m_combined_lin_vel + m_lin_vel;
1108 };
1109
1110 const 
1111         MT_Vector3
1112 SM_Object::
1113 actualAngVelocity(
1114 ) const {
1115         return m_combined_ang_vel + m_ang_vel;
1116 };
1117
1118
1119
1120
1121