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