fix, static objects were not dynamically added at the right position.
[blender-staging.git] / source / gameengine / Physics / Bullet / CcdPhysicsController.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "CcdPhysicsController.h"
17 #include "btBulletDynamicsCommon.h"
18
19 #include "PHY_IMotionState.h"
20 #include "CcdPhysicsEnvironment.h"
21
22
23 class BP_Proxy;
24
25 ///todo: fill all the empty CcdPhysicsController methods, hook them up to the btRigidBody class
26
27 //'temporarily' global variables
28 //float gDeactivationTime = 2.f;
29 //bool  gDisableDeactivation = false;
30 extern float gDeactivationTime;
31 extern bool gDisableDeactivation;
32
33
34 float gLinearSleepingTreshold = 0.8f;
35 float gAngularSleepingTreshold = 1.0f;
36
37
38 btVector3 startVel(0,0,0);//-10000);
39 CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
40 :m_cci(ci)
41 {
42         m_collisionDelay = 0;
43         m_newClientInfo = 0;
44         
45         m_MotionState = ci.m_MotionState;
46         m_bulletMotionState = 0;
47         
48         
49         CreateRigidbody();
50         
51
52         
53         #ifdef WIN32
54         if (m_body->getInvMass())
55                 m_body->setLinearVelocity(startVel);
56         #endif
57
58 }
59
60 btTransform     CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
61 {
62         btTransform trans;
63         float tmp[3];
64         motionState->getWorldPosition(tmp[0],tmp[1],tmp[2]);
65         trans.setOrigin(btVector3(tmp[0],tmp[1],tmp[2]));
66
67         btQuaternion orn;
68         motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
69         trans.setRotation(orn);
70         return trans;
71
72 }
73
74 class   BlenderBulletMotionState : public btMotionState
75 {
76         PHY_IMotionState*       m_blenderMotionState;
77
78 public:
79
80         BlenderBulletMotionState(PHY_IMotionState* bms)
81                 :m_blenderMotionState(bms)
82         {
83
84         }
85
86         virtual void    getWorldTransform(btTransform& worldTrans ) const
87         {
88                 float pos[3];
89                 float quatOrn[4];
90
91                 m_blenderMotionState->getWorldPosition(pos[0],pos[1],pos[2]);
92                 m_blenderMotionState->getWorldOrientation(quatOrn[0],quatOrn[1],quatOrn[2],quatOrn[3]);
93                 worldTrans.setOrigin(btVector3(pos[0],pos[1],pos[2]));
94                 worldTrans.setBasis(btMatrix3x3(btQuaternion(quatOrn[0],quatOrn[1],quatOrn[2],quatOrn[3])));
95         }
96
97         virtual void    setWorldTransform(const btTransform& worldTrans)
98         {
99                 m_blenderMotionState->setWorldPosition(worldTrans.getOrigin().getX(),worldTrans.getOrigin().getY(),worldTrans.getOrigin().getZ());
100                 btQuaternion rotQuat = worldTrans.getRotation();
101                 m_blenderMotionState->setWorldOrientation(rotQuat[0],rotQuat[1],rotQuat[2],rotQuat[3]);
102                 m_blenderMotionState->calculateWorldTransformations();
103         }
104
105 };
106
107 void CcdPhysicsController::CreateRigidbody()
108 {
109
110         btTransform trans = GetTransformFromMotionState(m_cci.m_MotionState);
111
112         m_bulletMotionState = new BlenderBulletMotionState(m_cci.m_MotionState);
113
114         m_body = new btRigidBody(m_cci.m_mass,
115                 m_bulletMotionState,
116                 m_cci.m_collisionShape,
117                 m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor,
118                 m_cci.m_linearDamping,m_cci.m_angularDamping,
119                 m_cci.m_friction,m_cci.m_restitution);
120
121         
122
123         //
124         // init the rigidbody properly
125         //
126         
127         //setMassProps this also sets collisionFlags
128         //convert collision flags!
129
130         m_body->setCollisionFlags(m_body->getCollisionFlags() | m_cci.m_collisionFlags);
131         
132         m_body->setGravity( m_cci.m_gravity);
133         m_body->setDamping(m_cci.m_linearDamping, m_cci.m_angularDamping);
134
135 }
136
137 CcdPhysicsController::~CcdPhysicsController()
138 {
139         //will be reference counted, due to sharing
140         if (m_cci.m_physicsEnv)
141                 m_cci.m_physicsEnv->removeCcdPhysicsController(this);
142
143         delete m_MotionState;
144         if (m_bulletMotionState)
145                 delete m_bulletMotionState;
146         delete m_body;
147 }
148
149                 /**
150                         SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
151                 */
152 bool            CcdPhysicsController::SynchronizeMotionStates(float time)
153 {
154         //sync non-static to motionstate, and static from motionstate (todo: add kinematic etc.)
155
156         if (!m_body->isStaticObject())
157         {
158                 const btVector3& worldPos = m_body->getCenterOfMassPosition();
159                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
160                 
161                 const btQuaternion& worldquat = m_body->getOrientation();
162                 m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
163
164                 m_MotionState->calculateWorldTransformations();
165
166                 float scale[3];
167                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
168                 btVector3 scaling(scale[0],scale[1],scale[2]);
169                 GetCollisionShape()->setLocalScaling(scaling);
170         } else
171         {
172                 btVector3 worldPos;
173                 btQuaternion worldquat;
174
175 /*              m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
176                 m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
177                 btTransform oldTrans = m_body->getCenterOfMassTransform();
178                 btTransform newTrans(worldquat,worldPos);
179                                 
180                 m_body->setCenterOfMassTransform(newTrans);
181                 //need to keep track of previous position for friction effects...
182                 
183                 m_MotionState->calculateWorldTransformations();
184 */
185                 float scale[3];
186                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
187                 btVector3 scaling(scale[0],scale[1],scale[2]);
188                 GetCollisionShape()->setLocalScaling(scaling);
189         }
190         return true;
191
192 }
193
194                 /**
195                         WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
196                 */
197                 
198 void            CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
199 {
200
201 }
202 void            CcdPhysicsController::WriteDynamicsToMotionState()
203 {
204 }
205                 // controller replication
206 void            CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
207 {
208         m_MotionState = motionstate;
209
210         
211
212         m_body = 0;
213         CreateRigidbody();
214         
215         m_cci.m_physicsEnv->addCcdPhysicsController(this);
216
217
218 /*      SM_Object* dynaparent=0;
219         SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
220         
221         if (sumoparentctrl)
222         {
223                 dynaparent = sumoparentctrl->GetSumoObject();
224         }
225         
226         SM_Object* orgsumoobject = m_sumoObj;
227         
228         
229         m_sumoObj       =       new SM_Object(
230                 orgsumoobject->getShapeHandle(), 
231                 orgsumoobject->getMaterialProps(),                      
232                 orgsumoobject->getShapeProps(),
233                 dynaparent);
234         
235         m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
236         
237         m_sumoObj->setMargin(orgsumoobject->getMargin());
238         m_sumoObj->setPosition(orgsumoobject->getPosition());
239         m_sumoObj->setOrientation(orgsumoobject->getOrientation());
240         //if it is a dyna, register for a callback
241         m_sumoObj->registerCallback(*this);
242         
243         m_sumoScene->add(* (m_sumoObj));
244         */
245
246
247
248 }
249
250                 // kinematic methods
251 void            CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
252 {
253         if (m_body)
254         {
255                 m_body->activate(true);
256                 if (m_body->isStaticObject())
257                 {
258                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
259                 }
260
261
262                 btVector3 dloc(dlocX,dlocY,dlocZ);
263                 btTransform xform = m_body->getCenterOfMassTransform();
264
265                 if (local)
266                 {
267                         dloc = xform.getBasis()*dloc;
268                 }
269
270                 xform.setOrigin(xform.getOrigin() + dloc);
271                 m_body->setCenterOfMassTransform(xform);
272         }
273
274 }
275
276 void            CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
277 {
278         if (m_body )
279         {
280                 m_body->activate(true);
281                 if (m_body->isStaticObject())
282                 {
283                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
284                 }
285
286                 btMatrix3x3 drotmat(    rotval[0],rotval[1],rotval[2],
287                                                                 rotval[4],rotval[5],rotval[6],
288                                                                 rotval[8],rotval[9],rotval[10]);
289
290
291                 btMatrix3x3 currentOrn;
292                 GetWorldOrientation(currentOrn);
293
294                 btTransform xform = m_body->getCenterOfMassTransform();
295
296                 xform.setBasis(xform.getBasis()*(local ? 
297                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
298
299                 m_body->setCenterOfMassTransform(xform);
300         }
301
302 }
303
304 void CcdPhysicsController::GetWorldOrientation(btMatrix3x3& mat)
305 {
306         float orn[4];
307         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
308         btQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
309         mat.setRotation(quat);
310 }
311
312 void            CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
313 {
314         btQuaternion q = m_body->getCenterOfMassTransform().getRotation();
315         quatImag0 = q[0];
316         quatImag1 = q[1];
317         quatImag2 = q[2];
318         quatReal = q[3];
319 }
320 void            CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
321 {
322         m_body->activate(true);
323         if (m_body->isStaticObject())
324         {
325                 m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
326         }
327
328         m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
329         btTransform xform  = m_body->getCenterOfMassTransform();
330         xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
331         m_body->setCenterOfMassTransform(xform);
332         m_bulletMotionState->setWorldTransform(xform);
333
334 }
335
336 void            CcdPhysicsController::setPosition(float posX,float posY,float posZ)
337 {
338         m_body->activate(true);
339         if (m_body->isStaticObject())
340         {
341                 m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
342         }
343         
344         m_MotionState->setWorldPosition(posX,posY,posZ);
345         btTransform xform  = m_body->getCenterOfMassTransform();
346         xform.setOrigin(btVector3(posX,posY,posZ));
347         m_body->setCenterOfMassTransform(xform);
348         m_bulletMotionState->setWorldTransform(xform);
349
350
351 }
352 void            CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
353 {
354 }
355
356 void            CcdPhysicsController::getPosition(PHY__Vector3& pos) const
357 {
358         const btTransform& xform = m_body->getCenterOfMassTransform();
359         pos[0] = xform.getOrigin().x();
360         pos[1] = xform.getOrigin().y();
361         pos[2] = xform.getOrigin().z();
362 }
363
364 void            CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
365 {
366         if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
367                 !btFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
368                 !btFuzzyZero(m_cci.m_scaling.z()-scaleZ))
369         {
370                 m_cci.m_scaling = btVector3(scaleX,scaleY,scaleZ);
371
372                 if (m_body && m_body->getCollisionShape())
373                 {
374                         m_body->getCollisionShape()->setLocalScaling(m_cci.m_scaling);
375                         
376                         //printf("no inertia recalc for fixed objects with mass=0\n");
377                         if (m_cci.m_mass)
378                         {
379                                 m_body->getCollisionShape()->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
380                                 m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
381                         } 
382                         
383                 }
384         }
385 }
386                 
387                 // physics methods
388 void            CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
389 {
390         btVector3 torque(torqueX,torqueY,torqueZ);
391         btTransform xform = m_body->getCenterOfMassTransform();
392         if (torque.length2() > (SIMD_EPSILON*SIMD_EPSILON))
393         {
394                 m_body->activate();
395         }
396         if (local)
397         {
398                 torque  = xform.getBasis()*torque;
399         }
400         m_body->applyTorque(torque);
401 }
402
403 void            CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
404 {
405         btVector3 force(forceX,forceY,forceZ);
406         
407         if (force.length2() > (SIMD_EPSILON*SIMD_EPSILON))
408         {
409                 m_body->activate();
410         }
411
412
413         btTransform xform = m_body->getCenterOfMassTransform();
414         if (local)
415         {
416                 force   = xform.getBasis()*force;
417         }
418         m_body->applyCentralForce(force);
419 }
420 void            CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
421 {
422         btVector3 angvel(ang_velX,ang_velY,ang_velZ);
423         if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
424         {
425                 m_body->activate(true);
426         }
427
428         {
429                 btTransform xform = m_body->getCenterOfMassTransform();
430                 if (local)
431                 {
432                         angvel  = xform.getBasis()*angvel;
433                 }
434
435                 m_body->setAngularVelocity(angvel);
436         }
437
438 }
439 void            CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
440 {
441
442         btVector3 linVel(lin_velX,lin_velY,lin_velZ);
443         if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
444         {
445                 m_body->activate(true);
446         }
447         
448         {
449                 btTransform xform = m_body->getCenterOfMassTransform();
450                 if (local)
451                 {
452                         linVel  = xform.getBasis()*linVel;
453                 }
454                 m_body->setLinearVelocity(linVel);
455         }
456 }
457 void            CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
458 {
459         btVector3 impulse(impulseX,impulseY,impulseZ);
460
461         if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
462         {
463                 m_body->activate();
464                 
465                 btVector3 pos(attachX,attachY,attachZ);
466
467                 m_body->activate();
468
469                 m_body->applyImpulse(impulse,pos);
470         }
471
472 }
473 void            CcdPhysicsController::SetActive(bool active)
474 {
475 }
476                 // reading out information from physics
477 void            CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
478 {
479         const btVector3& linvel = this->m_body->getLinearVelocity();
480         linvX = linvel.x();
481         linvY = linvel.y();
482         linvZ = linvel.z();
483
484 }
485
486 void            CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
487 {
488         const btVector3& angvel= m_body->getAngularVelocity();
489         angVelX = angvel.x();
490         angVelY = angvel.y();
491         angVelZ = angvel.z();
492 }
493
494 void            CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
495 {
496         btVector3 pos(posX,posY,posZ);
497         btVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
498         btVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
499         linvX = linvel.x();
500         linvY = linvel.y();
501         linvZ = linvel.z();
502 }
503 void            CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
504 {
505 }
506
507                 // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
508 void            CcdPhysicsController::setRigidBody(bool rigid)
509 {
510         if (!rigid)
511         {
512                 //fake it for now
513                 btVector3 inertia = m_body->getInvInertiaDiagLocal();
514                 inertia[1] = 0.f;
515                 m_body->setInvInertiaDiagLocal(inertia);
516                 m_body->updateInertiaTensor();
517         }
518 }
519
520                 // clientinfo for raycasts for example
521 void*           CcdPhysicsController::getNewClientInfo()
522 {
523         return m_newClientInfo;
524 }
525 void            CcdPhysicsController::setNewClientInfo(void* clientinfo)
526 {
527         m_newClientInfo = clientinfo;
528 }
529
530
531 void    CcdPhysicsController::UpdateDeactivation(float timeStep)
532 {
533         m_body->updateDeactivation( timeStep);
534 }
535
536 bool CcdPhysicsController::wantsSleeping()
537 {
538
539         return m_body->wantsSleeping();
540 }
541
542 PHY_IPhysicsController* CcdPhysicsController::GetReplica()
543 {
544         //very experimental, shape sharing is not implemented yet.
545         //just support btSphereShape/ConeShape for now
546
547         CcdConstructionInfo cinfo = m_cci;
548         if (cinfo.m_collisionShape)
549         {
550                 switch (cinfo.m_collisionShape->getShapeType())
551                 {
552                 case SPHERE_SHAPE_PROXYTYPE:
553                         {
554                                 btSphereShape* orgShape = (btSphereShape*)cinfo.m_collisionShape;
555                                 cinfo.m_collisionShape = new btSphereShape(*orgShape);
556                                 break;
557                         }
558
559                         case CONE_SHAPE_PROXYTYPE:
560                         {
561                                 btConeShape* orgShape = (btConeShape*)cinfo.m_collisionShape;
562                                 cinfo.m_collisionShape = new btConeShape(*orgShape);
563                                 break;
564                         }
565
566
567                 default:
568                         {
569                                 return 0;
570                         }
571                 }
572         }
573
574         cinfo.m_MotionState = new DefaultMotionState();
575
576         CcdPhysicsController* replica = new CcdPhysicsController(cinfo);
577         return replica;
578 }
579
580 ///////////////////////////////////////////////////////////
581 ///A small utility class, DefaultMotionState
582 ///
583 ///////////////////////////////////////////////////////////
584
585 DefaultMotionState::DefaultMotionState()
586 {
587         m_worldTransform.setIdentity();
588         m_localScaling.setValue(1.f,1.f,1.f);
589 }
590
591
592 DefaultMotionState::~DefaultMotionState()
593 {
594
595 }
596
597 void    DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
598 {
599         posX = m_worldTransform.getOrigin().x();
600         posY = m_worldTransform.getOrigin().y();
601         posZ = m_worldTransform.getOrigin().z();
602 }
603
604 void    DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
605 {
606         scaleX = m_localScaling.getX();
607         scaleY = m_localScaling.getY();
608         scaleZ = m_localScaling.getZ();
609 }
610
611 void    DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
612 {
613         quatIma0 = m_worldTransform.getRotation().x();
614         quatIma1 = m_worldTransform.getRotation().y();
615         quatIma2 = m_worldTransform.getRotation().z();
616         quatReal = m_worldTransform.getRotation()[3];
617 }
618                 
619 void    DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
620 {
621         btPoint3 pos(posX,posY,posZ);
622         m_worldTransform.setOrigin( pos );
623 }
624
625 void    DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
626 {
627         btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
628         m_worldTransform.setRotation( orn );
629 }
630                 
631 void    DefaultMotionState::calculateWorldTransformations()
632 {
633
634 }
635