Created a KX_SoftBodyDeformer for real-time soft bodies.
[blender.git] / source / gameengine / Physics / BlOde / OdePhysicsController.cpp
1 /**
2  * $Id$
3  *
4  * ***** BEGIN GPL LICENSE BLOCK *****
5  *
6  * The contents of this file may be used under the terms of either the GNU
7  * General Public License Version 2 or later (the "GPL", see
8  * http://www.gnu.org/licenses/gpl.html ), or the Blender License 1.0 or
9  * later (the "BL", see http://www.blender.org/BL/ ) which has to be
10  * bought from the Blender Foundation to become active, in which case the
11  * above mentioned GPL option does not apply.
12  *
13  * The Original Code is Copyright (C) 2002 by NaN Holding BV.
14  * All rights reserved.
15  *
16  * The Original Code is: all of this file.
17  *
18  * Contributor(s): none yet.
19  *
20  * ***** END GPL LICENSE BLOCK *****
21  */
22  
23 #define USE_ODE
24 #ifdef USE_ODE
25
26 #include "OdePhysicsController.h"
27 #include "PHY_IMotionState.h"
28
29 #include <ode/ode.h>
30
31 #ifdef HAVE_CONFIG_H
32 #include <config.h>
33 #endif
34
35 ///////////////////////////////////////////////////////////////////////////
36 //
37 // general to-do list for ODE physics. This is maintained in doxygen format.
38 //
39 /// \todo determine assignment time for bounding spheres.
40 ///
41 /// it appears you have to select "sphere" for bounding volume AND "draw bounds"
42 /// in order for a bounding sphere to be generated. otherwise a box is generated.
43 /// determine exactly when and how the bounding volumes are generated and make
44 /// this consistent.
45 /// }
46 /// 
47 /// \todo bounding sphere size incorrect
48 ///
49 /// it appears NOT to use the size of the shown bounding sphere (button "draw bounds").
50 /// it appears instead to use the size of the "size" dynamic parameter in the
51 /// gamebuttons but this "size" draws an incorrectly-sized circle on screen for the
52 /// bounding sphere (leftover skewed size calculation from sumo?) so figure out WHERE
53 /// its getting the radius from.
54 /// 
55 /// \todo ODE collisions must fire collision actuator
56 ///
57 /// See OdePhysicsEnvironment::OdeNearCallback. If a sensor was created to check
58 /// for the presence of this collision, then in the NearCallback you need to
59 /// take appropriate action regarding the sensor - something like checking its
60 /// controller and if needed firing its actuator. Need to find similar code in
61 /// Fuzzics which fires collision controllers/actuators.
62 /// 
63 /// \todo Are ghost collisions possible?
64 ///
65 /// How do ghost collisions work? Do they require collision detection through ODE
66 /// and NON-CREATION of contact-joint in OdeNearCallback? Currently OdeNearCallback
67 /// creates joints ALWAYS for collisions.
68 /// 
69 /// \todo Why is KX_GameObject::addLinearVelocity commented out?
70 ///
71 /// Try putting this code back in.
72 /// 
73 /// \todo Too many non-dynamic actors bogs down ODE physics
74 ///
75 /// Lots of "geoms" (ODE static geometry) probably slows down ode. Try a test file
76 /// with lots of static geometry - the game performance in Blender says it is
77 /// spending all its time in physics, and I bet all that time is in collision
78 /// detection. It's ode's non-hierarchical collision detection. 
79 /// try making a separate ode test program (not within blender) with 1000 geoms and
80 /// see how fast it is. if it is really slow, there is the culprit. 
81 /// isnt someone working on an improved ODE collision detector? check
82 /// ode mailing list.
83 /// 
84 /// 
85 /// \todo support collision of dynas with non-dynamic triangle meshes
86 ///
87 /// ODE has trimesh-collision support but only for trimeshes without a transform
88 /// matrix. update ODE tricollider to support a transform matrix. this will allow
89 /// moving trimeshes non-dynamically (e.g. through Ipos). then collide trimeshes
90 /// with dynas. this allows dynamic primitives (spheres, boxes) to collide with
91 /// non-dynamic or kinematically controlled tri-meshes. full dynamic trimesh to
92 /// dynamic trimesh support is hard because it requires (a) collision and penetration
93 /// depth for trimesh to trimesh and (hard to compute) (b) an intertia tensor
94 /// (easy to compute).
95 /// 
96 /// a triangle mesh collision geometry should be created when the blender
97 /// bounding volume (F9, EDITBUTTONS) is set to "polyheder", since this is
98 /// currently the place where sphere/box selection is made
99 /// 
100 /// \todo specify ODE ERP+CFM in blender interface
101 ///
102 /// when ODE physics selected, have to be able to set global cfm and erp.
103 /// per-joint erp/cfm could be handled in constraint window.
104 /// 
105 /// \todo moving infinite mass objects should impart extra impulse to objects they collide with
106 ///
107 /// currently ODE's ERP pushes them apart but doesn't account for their motion.
108 /// you have to detect if one body in a collision is a non-dyna. This
109 /// requires adding a new accessor method to
110 /// KX_IPhysicsInterfaceController to access the hidden m_isDyna variable,
111 /// currently it can only be written, not read). If one of the bodies in a
112 /// collision is a non-dyna, then impart an extra impulse based on the
113 /// motion of the static object (using its last 2 frames as an approximation
114 /// of its linear and angular velocity). Linear velocity is easy to
115 /// approximate, but angular? you have orientation at this frame and
116 /// orientation at previous frame. The question is what is the angular
117 /// velocity which would have taken you from the previous frame's orientation
118 /// to this frame's orientation?
119 /// 
120 /// \todo allow tweaking bounding volume size
121 ///
122 /// the scene converter currently uses the blender bounding volume of the selected
123 /// object as the geometry for ODE collision purposes. this is good and automatic
124 /// intuitive - lets you choose between cube, sphere, mesh. but you need to be able
125 /// to tweak this size for physics.
126 /// 
127 /// \todo off center meshes totally wrong for ode
128 /// 
129 /// ode uses x, y, z extents regradless of center. then places geom at center of object.
130 /// but visual geom is not necessarily at center. need to detect off-center situations.
131 /// then do what? treat it as an encapsulated off-center mass, or recenter it?
132 /// 
133 /// i.o.w. recalculate center, or recalculate mass distribution (using encapsulation)?
134 /// 
135 /// \todo allow off-center mass
136 /// 
137 /// using ode geometry encapsulators
138 /// 
139 /// \todo allow entering compound geoms for complex collision shapes specified as a union of simpler shapes
140 /// 
141 /// The collision shape for arbitrary triangle meshes can probably in general be
142 ///well approximated by a compound ODE geometry object, which is merely a combination
143 ///of many primitives (capsule, sphere, box). I eventually want to add the ability
144 ///to associate compound geometry objects with Blender gameobjects. I think one
145 ///way of doing this would be to add a new button in the GameButtons, "RigidBodyCompound".
146 ///If the object is "Dynamic" + "RigidBody", then the object's bounding volume (sphere,
147 ///box) is created. If an object is "Dynamic" + "RigidBodyCompound", then the object itself
148 ///will merely create a "wrapper" compound object, with the actual geometry objects
149 ///being created from the object's children in Blender. E.g. if I wanted to make a
150 ///compound collision object consisting of a sphere and 2 boxes, I would create a
151 ///parent gameobject with the actual triangle mesh, and set its GameButtons to
152 ///"RigidBodyCompound". I would then create 3 children of this object, 1 sphere and
153 ///2 boxes, and set the GameButtons for the children to be "RigidBody". Then at
154 ///scene conversion time, the scene converter sees "RigidBodyCompound" for the
155 ///top-level object, then appropriately traverses the children and creates the compound
156 ///collision geometry consisting of 2 boxes and a sphere. In this way, arbitrary
157 ///mesh-mesh collision becomes much less necessary - the artist can (or must,
158 ///depending on your point of view!) approximate the collision shape for arbitrary
159 ///meshes with a combination of one or more primitive shapes. I think using the
160 ///parent/child relationship in Blender and a new button "RigidBodyCompound" for the
161 ///parent object of a compound is a feasible way of doing this in Blender.
162 /// 
163 ///See ODE demo test_boxstack and look at the code when you drop a compound object
164 ///with the "X" key.
165 ///
166 /// \todo add visual specification of constraints
167 /// 
168 /// extend the armature constraint system. by using empties and constraining one empty
169 /// to "copy location" of another, you can get a p2p constraint between the two empties.
170 /// by making the two empties each a parent of a blender object, you effectively have
171 /// a p2p constraint between 2 blender bodies. the scene converter can detect these
172 /// empties, detect the constraint, and generate an ODE constraint.
173 /// 
174 /// then add a new constraint type "hinge" and "slider" to correspond to ODE joints.
175 /// e.g. a slider would be a constraint which restricts the axis of its object to lie
176 /// along the same line as another axis of a different object. e.g. you constrain x-axis
177 /// of one empty to lie along the same line as the z-axis of another empty; this gives
178 /// a slider joint.
179 ///
180 /// open questions: how to handle powered joints? to what extent should/must constraints
181 /// be enforced during modeling? use CCD-style algorithm in modeler to enforce constraints?
182 /// how about ODE powered constraints e.g. motors?
183 /// 
184 /// \todo enable suspension of bodies
185 /// ODE offers native support for suspending dynas. but what about suspending non-dynas
186 /// (e.g. geoms)? suspending geoms is also necessary to ease the load of ODE's (simple?)
187 /// collision detector. suspending dynas and geoms is important for the activity culling,
188 /// which apparently works at a simple level. perhaps suspension should actually
189 /// remove or insert geoms/dynas into the ODE space/world? is this operation (insertion/
190 /// removal) fast enough at run-time? test it. if fast enough, then suspension=remove from
191 /// ODE simulation, awakening=insertion into ODE simulation.
192 ///
193 /// \todo python interface for tweaking constraints via python
194 ///
195 /// \todo raytesting to support gameengine sensors that need it
196 ///
197 /// \todo investigate compatibility issues with old Blender 2.25 physics engine (sumo/fuzzics)
198 /// is it possible to have compatibility? how hard is it? how important is it?
199
200
201 ODEPhysicsController::ODEPhysicsController(bool dyna, bool fullRigidBody,
202                                            bool phantom, class PHY_IMotionState* motionstate, struct dxSpace* space,
203                                            struct dxWorld* world, float mass,float friction,float restitution,
204                                            bool implicitsphere,float center[3],float extents[3],float radius)
205   :     
206   m_OdeDyna(dyna),
207   m_firstTime(true),
208   m_bFullRigidBody(fullRigidBody),
209   m_bPhantom(phantom),
210   m_bKinematic(false),
211   m_bPrevKinematic(false),
212   m_MotionState(motionstate),
213   m_OdeSuspendDynamics(false),
214   m_space(space),
215   m_world(world),
216   m_mass(mass),
217   m_friction(friction),
218   m_restitution(restitution),
219   m_bodyId(0),
220   m_geomId(0),
221   m_implicitsphere(implicitsphere),
222   m_radius(radius)
223 {
224   m_center[0] = center[0];
225   m_center[1] = center[1];
226   m_center[2] = center[2];
227   m_extends[0] = extents[0];
228   m_extends[1] = extents[1];
229   m_extends[2] = extents[2];
230 };
231
232
233 ODEPhysicsController::~ODEPhysicsController()
234 {
235   if (m_geomId)
236     {
237       dGeomDestroy (m_geomId);
238     }
239 }
240
241 float ODEPhysicsController::getMass()
242 {
243   dMass mass;
244   dBodyGetMass(m_bodyId,&mass);
245   return mass.mass;
246 }
247
248 //////////////////////////////////////////////////////////////////////
249 /// \todo Impart some extra impulse to dynamic objects when they collide with kinematically controlled "static" objects (ODE geoms), by using last 2 frames as 1st order approximation to the linear/angular velocity, and computing an appropriate impulse. Sumo (old physics engine) did this, see for details.
250 /// \todo handle scaling of static ODE geoms or fail with error message if Ipo tries to change scale of a static geom object
251
252 bool ODEPhysicsController::SynchronizeMotionStates(float time)
253 {
254   /** 
255       'Late binding' of the rigidbody, because the World Scaling is not available until the scenegraph is traversed
256   */
257
258         
259   if (m_firstTime)
260     {
261       m_firstTime=false;
262
263       m_MotionState->calculateWorldTransformations();
264         
265       dQuaternion worldquat;
266       float worldpos[3];
267                 
268 #ifdef dDOUBLE
269       m_MotionState->getWorldOrientation((float)worldquat[1],
270          (float)worldquat[2],(float)worldquat[3],(float)worldquat[0]);
271 #else
272       m_MotionState->getWorldOrientation(worldquat[1],
273          worldquat[2],worldquat[3],worldquat[0]);
274 #endif
275       m_MotionState->getWorldPosition(worldpos[0],worldpos[1],worldpos[2]);
276         
277       float scaling[3];
278       m_MotionState->getWorldScaling(scaling[0],scaling[1],scaling[2]);
279                 
280       if (!m_bPhantom)
281         {
282           if (m_implicitsphere)
283             {
284               m_geomId = dCreateSphere (m_space,m_radius*scaling[0]);
285             } else
286               {
287                 m_geomId = dCreateBox (m_space, m_extends[0]*scaling[0],m_extends[1]*scaling[1],m_extends[2]*scaling[2]);
288               }
289         } else
290           {
291             m_geomId=0;
292           }
293
294       if (m_geomId)
295         dGeomSetData(m_geomId,this);
296
297       if (!this->m_OdeDyna)
298         {
299           if (!m_bPhantom)
300             {
301               dGeomSetPosition (this->m_geomId,worldpos[0],worldpos[1],worldpos[2]);
302               dMatrix3 R;
303               dQtoR (worldquat, R);
304               dGeomSetRotation (this->m_geomId,R);
305             }
306         } else
307           {
308             //it's dynamic, so create a 'model'
309             m_bodyId = dBodyCreate(this->m_world);
310             dBodySetPosition (m_bodyId,worldpos[0],worldpos[1],worldpos[2]);
311             dBodySetQuaternion (this->m_bodyId,worldquat);
312             //this contains both scalar mass and inertia tensor
313             dMass m; 
314             float length=1,width=1,height=1;
315             dMassSetBox (&m,1,m_extends[0]*scaling[0],m_extends[1]*scaling[1],m_extends[2]*scaling[2]);
316             dMassAdjust (&m,this->m_mass);
317             dBodySetMass (m_bodyId,&m);
318
319             if (!m_bPhantom)
320               {
321                 dGeomSetBody (m_geomId,m_bodyId);
322               }
323
324                         
325           } 
326                 
327       if (this->m_OdeDyna && !m_bFullRigidBody)
328         {
329           // ?? huh? what to do here?
330         }
331     }
332
333
334
335   if (m_OdeDyna)
336     {
337       if (this->m_OdeSuspendDynamics)
338         {
339           return false;
340         }
341
342       const float* worldPos = (float *)dBodyGetPosition(m_bodyId);
343       m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
344
345       const float* worldquat = (float *)dBodyGetQuaternion(m_bodyId);
346       m_MotionState->setWorldOrientation(worldquat[1],worldquat[2],worldquat[3],worldquat[0]);
347     }
348   else {
349     // not a dyna, so dynamics (i.e. this controller) has not updated
350     // anything. BUT! an Ipo or something else might have changed the
351     // position/orientation of this geometry.
352     // so update the static geom position
353
354     /// \todo impart some extra impulse to colliding objects!  
355       dQuaternion worldquat;
356       float worldpos[3];
357
358 #ifdef dDOUBLE
359       m_MotionState->getWorldOrientation((float)worldquat[1],
360         (float)worldquat[2],(float)worldquat[3],(float)worldquat[0]);
361 #else
362       m_MotionState->getWorldOrientation(worldquat[1],
363         worldquat[2],worldquat[3],worldquat[0]);
364 #endif
365       m_MotionState->getWorldPosition(worldpos[0],worldpos[1],worldpos[2]);
366         
367       float scaling[3];
368       m_MotionState->getWorldScaling(scaling[0],scaling[1],scaling[2]);
369
370       /// \todo handle scaling! what if Ipo changes scale of object?
371       // Must propagate to geom... is scaling geoms possible with ODE? Also
372       // what about scaling trimeshes, that is certainly difficult...
373       dGeomSetPosition (this->m_geomId,worldpos[0],worldpos[1],worldpos[2]);
374       dMatrix3 R;
375       dQtoR (worldquat, R);
376       dGeomSetRotation (this->m_geomId,R);
377   }
378
379   return false; //it update the worldpos
380 }
381  
382
383
384
385 // kinematic methods
386 void ODEPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
387 {
388
389 }
390 void ODEPhysicsController::RelativeRotate(const float drot[9],bool local)
391 {
392 }
393 void ODEPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
394 {
395
396   dQuaternion worldquat;
397   worldquat[0] = quatReal;
398   worldquat[1] = quatImag0;
399   worldquat[2] = quatImag1;
400   worldquat[3] = quatImag2;
401         
402   if (!this->m_OdeDyna)
403     {
404       dMatrix3 R;
405       dQtoR (worldquat, R);
406       dGeomSetRotation (this->m_geomId,R);
407     } else
408       {
409         dBodySetQuaternion (m_bodyId,worldquat);
410         this->m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
411       }
412
413 }
414
415 void ODEPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
416 {
417   float q[4];
418   this->m_MotionState->getWorldOrientation(q[0],q[1],q[2],q[3]);
419   quatImag0=q[0];
420   quatImag1=q[1];
421   quatImag2=q[2];
422   quatReal=q[3];
423 }
424
425 void            ODEPhysicsController::getPosition(PHY__Vector3& pos) const
426 {
427         m_MotionState->getWorldPosition(pos[0],pos[1],pos[2]);
428
429 }
430         
431 void ODEPhysicsController::setPosition(float posX,float posY,float posZ)
432 {
433   if (!m_bPhantom)
434     {
435       if (!this->m_OdeDyna)
436         {
437           dGeomSetPosition (m_geomId, posX, posY, posZ);
438         } else
439           {
440             dBodySetPosition (m_bodyId, posX, posY, posZ);
441           }
442     }
443 }
444 void ODEPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
445 {
446 }
447         
448 // physics methods
449 void ODEPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
450 {
451   if (m_OdeDyna) {
452     if(local) {
453       dBodyAddRelTorque(m_bodyId, torqueX, torqueY, torqueZ);
454     } else {
455       dBodyAddTorque (m_bodyId, torqueX, torqueY, torqueZ);
456     }
457   }
458 }
459
460 void ODEPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
461 {
462   if (m_OdeDyna) {
463     if(local) {
464       dBodyAddRelForce(m_bodyId, forceX, forceY, forceZ);
465     } else {
466       dBodyAddForce (m_bodyId, forceX, forceY, forceZ);
467     }
468   }
469 }
470
471 void ODEPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
472 {
473   if (m_OdeDyna) {
474     if(local) {
475       // TODO: translate angular vel into local frame, then apply
476     } else {
477       dBodySetAngularVel  (m_bodyId, ang_velX,ang_velY,ang_velZ);
478     }
479   }
480 }
481         
482 void ODEPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
483 {
484   if (m_OdeDyna)
485     {
486       dVector3 vel = {lin_velX,lin_velY,lin_velZ, 1.0};
487       if (local)
488         {
489           dMatrix3 worldmat;
490           dVector3 localvel;
491           dQuaternion worldquat;
492
493 #ifdef dDOUBLE
494           m_MotionState->getWorldOrientation((float)worldquat[1],
495                 (float)worldquat[2], (float)worldquat[3],(float)worldquat[0]);
496 #else
497           m_MotionState->getWorldOrientation(worldquat[1],worldquat[2],
498                 worldquat[3],worldquat[0]);
499 #endif
500           dQtoR (worldquat, worldmat);
501                         
502           dMULTIPLY0_331 (localvel,worldmat,vel);
503           dBodySetLinearVel  (m_bodyId, localvel[0],localvel[1],localvel[2]);
504
505         } else
506           {
507             dBodySetLinearVel  (m_bodyId, lin_velX,lin_velY,lin_velZ);
508           }
509     }
510 }
511
512 void ODEPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
513 {
514   if (m_OdeDyna)
515     {
516       //apply linear and angular effect
517       const dReal* linvel = dBodyGetLinearVel(m_bodyId);
518       float mass =      getMass();
519       if (mass >= 0.00001f)
520         {
521           float massinv = 1.f/mass;
522           float newvel[3];
523           newvel[0]=linvel[0]+impulseX*massinv;
524           newvel[1]=linvel[1]+impulseY*massinv;
525           newvel[2]=linvel[2]+impulseZ*massinv;
526           dBodySetLinearVel(m_bodyId,newvel[0],newvel[1],newvel[2]);
527
528           const float* worldPos = (float *)dBodyGetPosition(m_bodyId);
529
530           const float* angvelc = (float *)dBodyGetAngularVel(m_bodyId);
531           float angvel[3];
532           angvel[0]=angvelc[0];
533           angvel[1]=angvelc[1];
534           angvel[2]=angvelc[2];
535
536           dVector3 impulse;
537           impulse[0]=impulseX;
538           impulse[1]=impulseY;
539           impulse[2]=impulseZ;
540
541           dVector3 ap;
542           ap[0]=attachX-worldPos[0];
543           ap[1]=attachY-worldPos[1];
544           ap[2]=attachZ-worldPos[2];
545
546           dCROSS(angvel,+=,ap,impulse);
547           dBodySetAngularVel(m_bodyId,angvel[0],angvel[1],angvel[2]);
548
549         }
550                 
551     }
552
553 }
554
555 void ODEPhysicsController::SuspendDynamics()
556 {
557
558 }
559
560 void ODEPhysicsController::RestoreDynamics()
561 {
562
563 }
564
565
566 /**  
567      reading out information from physics
568 */
569 void ODEPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
570 {
571   if (m_OdeDyna)
572     {   
573       const float* vel = (float *)dBodyGetLinearVel(m_bodyId);
574       linvX = vel[0];
575       linvY = vel[1];
576       linvZ = vel[2];
577     } else
578       {
579         linvX = 0.f;
580         linvY = 0.f;
581         linvZ = 0.f;
582
583       }
584 }
585 /** 
586     GetVelocity parameters are in geometric coordinates (Origin is not center of mass!).
587 */
588 void ODEPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
589 {
590
591 }
592
593
594 void ODEPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
595 {
596
597 }
598 void ODEPhysicsController::setRigidBody(bool rigid)
599 {
600
601 }
602                 
603         
604 void ODEPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
605 {
606   m_MotionState = motionstate;
607   m_bKinematic = false;
608   m_bPrevKinematic = false;
609   m_firstTime = true;
610 }
611         
612
613 void ODEPhysicsController::SetSimulatedTime(float time)
614 {
615 }
616         
617         
618 void ODEPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
619 {
620
621 }
622 #endif