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