More warning cleanups...
[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_Object *obj1  = (SM_Object *)object1;  
530         SM_Object *obj2  = (SM_Object *)object2;  
531         
532         // If one of the objects is a ghost then ignore it for the dynamics
533         if (obj1->isGhost() || obj2->isGhost()) {
534                 return DT_CONTINUE;
535         }
536
537         if (obj1->getDynamicParent() == obj2 || obj2->getDynamicParent() == obj1) {
538                 return DT_CONTINUE;
539         }
540         
541         if (!obj2->isDynamic()) {
542                 std::swap(obj1, obj2);
543         }
544
545         if (!obj2->isDynamic()) {
546                 return DT_CONTINUE;
547         }
548
549         // obj1 points to a dynamic object
550         DT_Vector3 p1, p2;
551         if (!DT_GetPenDepth(obj1->getObjectHandle(), obj2->getObjectHandle(), p1, p2))
552                 return DT_CONTINUE;
553         MT_Point3 local1(p1), local2(p2);
554         // Get collision data from SOLID
555         MT_Vector3 normal(local2 - local1);
556         
557         MT_Scalar dist = normal.dot(normal);
558         if (dist < MT_EPSILON || dist > obj2->m_shapeProps->m_radius*obj2->m_shapeProps->m_radius)
559                 return DT_CONTINUE;
560                 
561                 
562         if ((obj1->m_static || !obj1->isDynamic()) && obj1->m_static < obj2->m_static)
563         {
564                 obj2->m_static = obj1->m_static + 1;
565         } else if (obj2->m_static && obj2->m_static < obj1->m_static)
566         {
567                 obj1->m_static = obj2->m_static + 1;
568         }
569         
570         contacts.insert(new Contact(obj1, obj2, normal, MT_Point3(local1 + 0.5*(local2 - local1))));
571         
572         
573         return DT_CONTINUE;
574 }
575
576 void SM_Object::relax(void)
577 {
578         for (Contact::Set::iterator csit = contacts.begin() ; csit != contacts.end(); ++csit)
579         {
580                 (*csit)->resolve();
581                 delete (*csit);
582         }
583                 
584         contacts.clear();
585         if (m_error.fuzzyZero())
586                 return;
587         //std::cout << "SM_Object::relax: { " << m_error << " }" << std::endl;
588         
589         getNextFrame().setPosition(getNextFrame().getPosition() + m_error); 
590         m_error.setValue(0., 0., 0.); 
591         //calcXform();
592         //notifyClient();
593 }
594         
595 SM_Object::SM_Object() :
596         m_dynamicParent(0),
597         m_client_object(0),
598         
599         m_shape(0),
600         m_materialProps(0),
601         m_materialPropsBackup(0),
602         m_shapeProps(0),
603         m_shapePropsBackup(0),
604         m_object(0),
605         m_margin(0.0),
606         m_scaling(1.0, 1.0, 1.0),
607         m_reaction_impulse(0.0, 0.0, 0.0),
608         m_reaction_force(0.0, 0.0, 0.0),
609         m_lin_mom(0.0, 0.0, 0.0),
610         m_ang_mom(0.0, 0.0, 0.0),
611         m_force(0.0, 0.0, 0.0),
612         m_torque(0.0, 0.0, 0.0),
613         m_error(0.0, 0.0, 0.0),
614         m_combined_lin_vel (0.0, 0.0, 0.0),
615         m_combined_ang_vel (0.0, 0.0, 0.0),
616         m_fh_object(0),
617         m_kinematic(false),
618         m_prev_kinematic(false),
619         m_is_rigid_body(false)
620 {
621         // warning no initialization of variables done by moto.
622 }
623
624 SM_Object::
625 ~SM_Object() { 
626         if (m_fh_object)
627                 delete m_fh_object;
628         
629         DT_DestroyObject(m_object);
630         m_object = NULL;
631 }
632
633         bool 
634 SM_Object::
635 isDynamic(
636 ) const {
637         return m_shapeProps != 0; 
638
639
640 /* nzc experimental. There seem to be two places where kinematics
641  * are evaluated: proceedKinematic (called from SM_Scene) and
642  * proceed() in this object. I'll just try and bunge these out for
643  * now.  */
644         void 
645 SM_Object::
646 suspend(
647 ){
648         if (!m_suspended) {
649                 m_suspended = true;
650                 suspendDynamics();
651         }
652 }
653
654         void 
655 SM_Object::
656 resume(
657 ) {
658         if (m_suspended) {
659                 m_suspended = false;
660                 restoreDynamics();
661         }
662 }
663
664         void 
665 SM_Object::
666 suspendDynamics(
667 ) {
668         if (m_shapeProps) {
669                 m_shapePropsBackup = m_shapeProps;
670                 m_shapeProps = 0;
671         }
672 }
673
674         void 
675 SM_Object::
676 restoreDynamics(
677 ) {
678         if (m_shapePropsBackup) {
679                 m_shapeProps = m_shapePropsBackup;
680                 m_shapePropsBackup = 0;
681         }
682 }
683
684         bool 
685 SM_Object::
686 isGhost(
687 ) const {
688         return m_materialProps == 0;
689
690
691         void 
692 SM_Object::
693 suspendMaterial(
694 ) {
695         if (m_materialProps) {
696                 m_materialPropsBackup = m_materialProps;
697                 m_materialProps = 0;
698         }
699 }
700
701         void 
702 SM_Object::
703 restoreMaterial(
704 ) {
705         if (m_materialPropsBackup) {
706                 m_materialProps = m_materialPropsBackup;
707                 m_materialPropsBackup = 0;
708         }
709 }
710
711         SM_FhObject *
712 SM_Object::
713 getFhObject(
714 ) const {
715         return m_fh_object;
716
717
718         void 
719 SM_Object::
720 registerCallback(
721         SM_Callback& callback
722 ) {
723         m_callbackList.push_back(&callback);
724 }
725
726 // Set the local coordinate system according to the current state 
727         void 
728 SM_Object::
729 calcXform() {
730 #ifdef SM_DEBUG_XFORM
731         printf("SM_Object::calcXform m_pos = { %-0.5f, %-0.5f, %-0.5f }\n",
732                 m_pos[0], m_pos[1], m_pos[2]);
733         printf("                     m_orn = { %-0.5f, %-0.5f, %-0.5f, %-0.5f }\n",
734                 m_orn[0], m_orn[1], m_orn[2], m_orn[3]);
735         printf("                 m_scaling = { %-0.5f, %-0.5f, %-0.5f }\n",
736                 m_scaling[0], m_scaling[1], m_scaling[2]);
737 #endif
738         m_xform.setOrigin(getNextFrame().getPosition());
739         m_xform.setBasis(MT_Matrix3x3(getNextFrame().getOrientation(), m_scaling));
740         m_xform.getValue(m_ogl_matrix);
741         
742         /* Blender has been known to crash here.
743            This usually means SM_Object *this has been deleted more than once. */
744         DT_SetMatrixd(m_object, m_ogl_matrix);
745         if (m_fh_object) {
746                 m_fh_object->setPosition(getNextFrame().getPosition());
747                 m_fh_object->calcXform();
748         }
749         updateInvInertiaTensor();
750 #ifdef SM_DEBUG_XFORM
751         printf("\n               | %-0.5f %-0.5f %-0.5f %-0.5f |\n",
752                 m_ogl_matrix[0], m_ogl_matrix[4], m_ogl_matrix[ 8], m_ogl_matrix[12]);
753         printf(  "               | %-0.5f %-0.5f %-0.5f %-0.5f |\n",
754                 m_ogl_matrix[1], m_ogl_matrix[5], m_ogl_matrix[ 9], m_ogl_matrix[13]);
755         printf(  "m_ogl_matrix = | %-0.5f %-0.5f %-0.5f %-0.5f |\n",
756                 m_ogl_matrix[2], m_ogl_matrix[6], m_ogl_matrix[10], m_ogl_matrix[14]);
757         printf(  "               | %-0.5f %-0.5f %-0.5f %-0.5f |\n\n",
758                 m_ogl_matrix[3], m_ogl_matrix[7], m_ogl_matrix[11], m_ogl_matrix[15]);
759 #endif
760 }
761
762         void 
763 SM_Object::updateInvInertiaTensor() 
764 {
765         m_inv_inertia_tensor = m_xform.getBasis().scaled(m_inv_inertia[0], m_inv_inertia[1], m_inv_inertia[2]) * m_xform.getBasis().transposed();
766 }
767
768 // Call callbacks to notify the client of a change of placement
769         void 
770 SM_Object::
771 notifyClient() {
772         T_CallbackList::iterator i;
773         for (i = m_callbackList.begin(); i != m_callbackList.end(); ++i) {
774                 (*i)->do_me();
775         }
776 }
777
778
779 // Save the current state information for use in the velocity computation in the next frame.  
780         void 
781 SM_Object::
782 proceedKinematic(
783         MT_Scalar timeStep
784 ) {
785         /* nzc: need to bunge this for the logic bubbling as well? */
786         if (!m_suspended) {
787                 m_prev_kinematic = m_kinematic;              
788                 if (m_kinematic) {
789                         m_prev_xform = m_xform;
790                         m_timeStep = timeStep;
791                         calcXform();
792                         m_kinematic  = false;
793                 }
794         }
795 }
796
797         void 
798 SM_Object::
799 saveReactionForce(
800         MT_Scalar timeStep
801 ) {
802         if (isDynamic()) {
803                 m_reaction_force   = m_reaction_impulse / timeStep;
804                 m_reaction_impulse.setValue(0.0, 0.0, 0.0);
805         }
806 }
807
808         void 
809 SM_Object::
810 clearForce(
811 ) {
812         m_force.setValue(0.0, 0.0, 0.0);
813         m_torque.setValue(0.0, 0.0, 0.0);
814 }
815
816         void 
817 SM_Object::
818 clearMomentum(
819 ) {
820         m_lin_mom.setValue(0.0, 0.0, 0.0);
821         m_ang_mom.setValue(0.0, 0.0, 0.0);
822 }
823
824         void 
825 SM_Object::
826 setMargin(
827         MT_Scalar margin
828 ) {
829         m_margin = margin;
830         DT_SetMargin(m_object, margin);
831 }
832
833         MT_Scalar 
834 SM_Object::
835 getMargin(
836 ) const {
837     return m_margin;
838 }
839
840 const 
841         SM_MaterialProps *
842 SM_Object::
843 getMaterialProps(
844 ) const {
845     return m_materialProps;
846 }
847
848 const 
849         SM_ShapeProps *
850 SM_Object::
851 getShapeProps(
852 ) const {
853     return m_shapeProps;
854 }
855
856         void 
857 SM_Object::
858 setPosition(
859         const MT_Point3& pos
860 ){
861         m_kinematic = true;
862         getNextFrame().setPosition(pos);
863         endFrame();
864 }
865         
866         void 
867 SM_Object::
868 setOrientation(
869         const MT_Quaternion& orn
870 ){
871         assert(!orn.fuzzyZero());
872         m_kinematic = true;
873         getNextFrame().setOrientation(orn);
874         endFrame();
875 }
876
877         void 
878 SM_Object::
879 setScaling(
880         const MT_Vector3& scaling
881 ){
882         m_kinematic = true;
883         m_scaling = scaling;
884 }
885
886 /**
887  * Functions to handle linear velocity
888  */
889
890         void 
891 SM_Object::
892 setExternalLinearVelocity(
893         const MT_Vector3& lin_vel
894 ) {
895         m_combined_lin_vel=lin_vel;
896 }
897
898         void 
899 SM_Object::
900 addExternalLinearVelocity(
901         const MT_Vector3& lin_vel
902 ) {
903         m_combined_lin_vel+=lin_vel;
904 }
905
906         void 
907 SM_Object::
908 addLinearVelocity(
909         const MT_Vector3& lin_vel
910 ){
911         setLinearVelocity(getNextFrame().getLinearVelocity() + lin_vel);
912 }
913
914         void 
915 SM_Object::
916 setLinearVelocity(
917         const MT_Vector3& lin_vel
918 ){
919         getNextFrame().setLinearVelocity(lin_vel);
920         if (m_shapeProps) {
921                 m_lin_mom = getNextFrame().getLinearVelocity() * m_shapeProps->m_mass;
922         }
923 }
924
925 /**
926  * Functions to handle angular velocity
927  */
928
929         void 
930 SM_Object::
931 setExternalAngularVelocity(
932         const MT_Vector3& ang_vel
933 ) {
934         m_combined_ang_vel = ang_vel;
935 }
936
937         void
938 SM_Object::
939 addExternalAngularVelocity(
940         const MT_Vector3& ang_vel
941 ) {
942         m_combined_ang_vel += ang_vel;
943 }
944
945         void 
946 SM_Object::
947 setAngularVelocity(
948         const MT_Vector3& ang_vel
949 ) {
950         getNextFrame().setAngularVelocity(ang_vel);
951         if (m_shapeProps) {
952                 m_ang_mom = getNextFrame().getAngularVelocity() * m_shapeProps->m_inertia;
953         }
954 }
955
956         void
957 SM_Object::
958 addAngularVelocity(
959         const MT_Vector3& ang_vel
960 ) {
961         setAngularVelocity(getNextFrame().getAngularVelocity() + ang_vel);
962 }
963
964
965         void 
966 SM_Object::
967 clearCombinedVelocities(
968 ) {
969         m_combined_lin_vel = MT_Vector3(0,0,0);
970         m_combined_ang_vel = MT_Vector3(0,0,0);
971 }
972
973         void 
974 SM_Object::
975 resolveCombinedVelocities(
976         const MT_Vector3 & lin_vel,
977         const MT_Vector3 & ang_vel
978 ) {
979
980         // Different behaviours for dynamic and non-dynamic 
981         // objects. For non-dynamic we just set the velocity to 
982         // zero. For dynmic the physics velocity has to be 
983         // taken into account. We must make an arbitrary decision
984         // on how to resolve the 2 velocities. Choices are
985         // Add the physics velocity to the linear velocity. Objects
986         // will just keep on moving in the direction they were
987         // last set in - untill external forces affect them.
988         // Set the combinbed linear and physics velocity to zero.
989         // Set the physics velocity in the direction of the set velocity
990         // zero.
991         if (isDynamic()) {              
992
993 #if 1
994                 getNextFrame().setLinearVelocity(getNextFrame().getLinearVelocity() + lin_vel);
995                 getNextFrame().setAngularVelocity(getNextFrame().getAngularVelocity() + ang_vel);
996 #else
997
998                 //compute the component of the physics velocity in the 
999                 // direction of the set velocity and set it to zero.
1000                 MT_Vector3 lin_vel_norm = lin_vel.normalized();
1001
1002                 setLinearVelocity(getNextFrame().getLinearVelocity() - (getNextFrame().getLinearVelocity().dot(lin_vel_norm) * lin_vel_norm));
1003 #endif
1004                 m_lin_mom = getNextFrame().getLinearVelocity() * m_shapeProps->m_mass;
1005                 m_ang_mom = getNextFrame().getAngularVelocity() * m_shapeProps->m_inertia;
1006                 clearCombinedVelocities();
1007
1008         }
1009
1010 }               
1011
1012
1013         MT_Scalar 
1014 SM_Object::
1015 getInvMass(
1016 ) const { 
1017         return m_inv_mass;
1018         // OPT: cache the result of this division rather than compute it each call
1019 }
1020
1021         const MT_Vector3&
1022 SM_Object::
1023 getInvInertia(
1024 ) const { 
1025         return m_inv_inertia;
1026         // OPT: cache the result of this division rather than compute it each call
1027 }
1028
1029         const MT_Matrix3x3&
1030 SM_Object::
1031 getInvInertiaTensor(
1032 ) const { 
1033         return m_inv_inertia_tensor; 
1034 }
1035
1036         void 
1037 SM_Object::
1038 applyForceField(
1039         const MT_Vector3& accel
1040 ) {
1041         if (m_shapeProps) {
1042                 m_force += m_shapeProps->m_mass * accel;  // F = m * a
1043         }
1044 }
1045
1046         void 
1047 SM_Object::
1048 applyCenterForce(
1049         const MT_Vector3& force
1050 ) {
1051         m_force += force;
1052 }
1053
1054         void 
1055 SM_Object::
1056 applyTorque(
1057         const MT_Vector3& torque
1058 ) {
1059         m_torque += torque;
1060 }
1061
1062         void 
1063 SM_Object::
1064 applyImpulse(
1065         const MT_Point3& attach, const MT_Vector3& impulse
1066 ) {
1067         applyCenterImpulse(impulse);                          // Change in linear momentum
1068         applyAngularImpulse((attach - getNextFrame().getPosition()).cross(impulse)); // Change in angular momentump
1069 }
1070
1071         void 
1072 SM_Object::
1073 applyCenterImpulse(
1074         const MT_Vector3& impulse
1075 ) {
1076         if (m_shapeProps) {
1077                 m_lin_mom          += impulse;
1078                 m_reaction_impulse += impulse;
1079                 getNextFrame().setLinearVelocity(m_lin_mom * m_inv_mass);
1080
1081                 // The linear velocity is immedialtely updated since otherwise
1082                 // simultaneous collisions will get a double impulse. 
1083         }
1084 }
1085
1086         void 
1087 SM_Object::
1088 applyAngularImpulse(
1089         const MT_Vector3& impulse
1090 ) {
1091         if (m_shapeProps) {
1092                 m_ang_mom += impulse;
1093                 getNextFrame().setAngularVelocity( m_inv_inertia_tensor * m_ang_mom);
1094         }
1095 }
1096
1097         MT_Point3 
1098 SM_Object::
1099 getWorldCoord(
1100         const MT_Point3& local
1101 ) const {
1102     return m_xform(local);
1103 }
1104
1105         MT_Vector3 
1106 SM_Object::
1107 getVelocity(
1108         const MT_Point3& local
1109 ) const {
1110         if (m_prev_kinematic && !isDynamic())
1111         {
1112                 // For displaced objects the velocity is faked using the previous state. 
1113                 // Dynamic objects get their own velocity, not the faked velocity.
1114                 // (Dynamic objects shouldn't be displaced in the first place!!)
1115                 return (m_xform(local) - m_prev_xform(local)) / m_timeStep;
1116         }
1117         
1118         // NB: m_xform.getBasis() * local == m_xform(local) - m_xform.getOrigin()
1119         return actualLinVelocity() + actualAngVelocity().cross(local);
1120 }
1121
1122
1123 const 
1124         MT_Vector3& 
1125 SM_Object::
1126 getReactionForce(
1127 ) const {
1128         return m_reaction_force;
1129 }
1130
1131         void 
1132 SM_Object::
1133 getMatrix(
1134         double *m
1135 ) const {
1136     std::copy(&m_ogl_matrix[0], &m_ogl_matrix[16], &m[0]);
1137 }
1138
1139 const 
1140         double *
1141 SM_Object::
1142 getMatrix(
1143 ) const {
1144         return m_ogl_matrix;
1145 }
1146
1147 // Still need this???
1148 const 
1149         MT_Transform&  
1150 SM_Object::
1151 getScaledTransform(
1152 ) const {
1153         return m_xform;
1154 }
1155
1156         DT_ObjectHandle 
1157 SM_Object::
1158 getObjectHandle(
1159 ) const {
1160         return m_object;
1161 }
1162
1163         DT_ShapeHandle 
1164 SM_Object::
1165 getShapeHandle(
1166 ) const { 
1167         return m_shape;
1168 }
1169
1170         SM_Object *
1171 SM_Object::
1172 getDynamicParent(
1173 ) {
1174         return m_dynamicParent;
1175 }
1176
1177         void 
1178 SM_Object::
1179 setRigidBody(
1180         bool is_rigid_body
1181 ) { 
1182         m_is_rigid_body = is_rigid_body;
1183
1184
1185         bool 
1186 SM_Object::
1187 isRigidBody(
1188 ) const {
1189         return m_is_rigid_body;
1190 }
1191
1192 const 
1193         MT_Vector3
1194 SM_Object::
1195 actualLinVelocity(
1196 ) const {
1197         return m_combined_lin_vel + getNextFrame().getLinearVelocity();
1198 };
1199
1200 const 
1201         MT_Vector3
1202 SM_Object::
1203 actualAngVelocity(
1204 ) const {
1205         return m_combined_ang_vel + getNextFrame().getAngularVelocity();
1206 }
1207
1208
1209 SM_MotionState&
1210 SM_Object::
1211 getCurrentFrame()
1212 {
1213         return m_frames[1];
1214 }
1215
1216 SM_MotionState&
1217 SM_Object::
1218 getPreviousFrame()
1219 {
1220         return m_frames[0];
1221 }
1222
1223 SM_MotionState &
1224 SM_Object::
1225 getNextFrame()
1226 {
1227         return m_frames[2];
1228 }
1229
1230 const SM_MotionState &
1231 SM_Object::
1232 getCurrentFrame() const
1233 {
1234         return m_frames[1];
1235 }
1236
1237 const SM_MotionState &
1238 SM_Object::
1239 getPreviousFrame() const
1240 {
1241         return m_frames[0];
1242 }
1243
1244 const SM_MotionState &
1245 SM_Object::
1246 getNextFrame() const
1247 {
1248         return m_frames[2];
1249 }
1250
1251
1252 const MT_Point3&     
1253 SM_Object::
1254 getPosition()        const
1255 {
1256         return m_frames[1].getPosition();
1257 }
1258
1259 const MT_Quaternion& 
1260 SM_Object::
1261 getOrientation()     const
1262 {
1263         return m_frames[1].getOrientation();
1264 }
1265
1266 const MT_Vector3&    
1267 SM_Object::
1268 getLinearVelocity()  const
1269 {
1270         return m_frames[1].getLinearVelocity();
1271 }
1272
1273 const MT_Vector3&    
1274 SM_Object::
1275 getAngularVelocity() const
1276 {
1277         return m_frames[1].getAngularVelocity();
1278 }
1279
1280 void
1281 SM_Object::
1282 interpolate(MT_Scalar timeStep)
1283 {
1284         if (!actualLinVelocity().fuzzyZero() || !actualAngVelocity().fuzzyZero()) 
1285         {
1286                 getCurrentFrame().setTime(timeStep);
1287                 getCurrentFrame().lerp(getPreviousFrame(), getNextFrame());
1288                 notifyClient();
1289         }
1290 }
1291
1292 void
1293 SM_Object::
1294 endFrame()
1295 {
1296         getPreviousFrame() = getNextFrame();
1297         getCurrentFrame() = getNextFrame();
1298         m_static = 0;
1299 }