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