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