more Bullet 2.x upgrading. fair amount of functionality is now restored, not all...
[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
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,m_bulletMotionState,m_cci.m_collisionShape,m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor,m_cci.m_friction,m_cci.m_restitution);
115         
116
117         //
118         // init the rigidbody properly
119         //
120         
121         //setMassProps this also sets collisionFlags
122         //convert collision flags!
123 //      m_body->m_collisionFlags = m_cci.m_collisionFlags;
124         
125         m_body->setGravity( m_cci.m_gravity);
126         m_body->setDamping(m_cci.m_linearDamping, m_cci.m_angularDamping);
127
128 }
129
130 CcdPhysicsController::~CcdPhysicsController()
131 {
132         //will be reference counted, due to sharing
133         if (m_cci.m_physicsEnv)
134                 m_cci.m_physicsEnv->removeCcdPhysicsController(this);
135
136         delete m_MotionState;
137         if (m_bulletMotionState)
138                 delete m_bulletMotionState;
139         delete m_body;
140 }
141
142                 /**
143                         SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
144                 */
145 bool            CcdPhysicsController::SynchronizeMotionStates(float time)
146 {
147         //sync non-static to motionstate, and static from motionstate (todo: add kinematic etc.)
148
149         if (!m_body->isStaticObject())
150         {
151                 const btVector3& worldPos = m_body->getCenterOfMassPosition();
152                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
153                 
154                 const btQuaternion& worldquat = m_body->getOrientation();
155                 m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
156
157                 m_MotionState->calculateWorldTransformations();
158
159                 float scale[3];
160                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
161                 btVector3 scaling(scale[0],scale[1],scale[2]);
162                 GetCollisionShape()->setLocalScaling(scaling);
163         } else
164         {
165                 btVector3 worldPos;
166                 btQuaternion worldquat;
167
168 /*              m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
169                 m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
170                 btTransform oldTrans = m_body->getCenterOfMassTransform();
171                 btTransform newTrans(worldquat,worldPos);
172                                 
173                 m_body->setCenterOfMassTransform(newTrans);
174                 //need to keep track of previous position for friction effects...
175                 
176                 m_MotionState->calculateWorldTransformations();
177 */
178                 float scale[3];
179                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
180                 btVector3 scaling(scale[0],scale[1],scale[2]);
181                 GetCollisionShape()->setLocalScaling(scaling);
182         }
183         return true;
184
185 }
186
187                 /**
188                         WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
189                 */
190                 
191 void            CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
192 {
193
194 }
195 void            CcdPhysicsController::WriteDynamicsToMotionState()
196 {
197 }
198                 // controller replication
199 void            CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
200 {
201         m_MotionState = motionstate;
202
203         
204
205         m_body = 0;
206         CreateRigidbody();
207         
208         m_cci.m_physicsEnv->addCcdPhysicsController(this);
209
210
211 /*      SM_Object* dynaparent=0;
212         SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
213         
214         if (sumoparentctrl)
215         {
216                 dynaparent = sumoparentctrl->GetSumoObject();
217         }
218         
219         SM_Object* orgsumoobject = m_sumoObj;
220         
221         
222         m_sumoObj       =       new SM_Object(
223                 orgsumoobject->getShapeHandle(), 
224                 orgsumoobject->getMaterialProps(),                      
225                 orgsumoobject->getShapeProps(),
226                 dynaparent);
227         
228         m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
229         
230         m_sumoObj->setMargin(orgsumoobject->getMargin());
231         m_sumoObj->setPosition(orgsumoobject->getPosition());
232         m_sumoObj->setOrientation(orgsumoobject->getOrientation());
233         //if it is a dyna, register for a callback
234         m_sumoObj->registerCallback(*this);
235         
236         m_sumoScene->add(* (m_sumoObj));
237         */
238
239
240
241 }
242
243                 // kinematic methods
244 void            CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
245 {
246         if (m_body)
247         {
248                 m_body->activate(true);
249                 if (m_body->isStaticObject())
250                 {
251                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
252                 }
253
254
255                 btVector3 dloc(dlocX,dlocY,dlocZ);
256                 btTransform xform = m_body->getCenterOfMassTransform();
257
258                 if (local)
259                 {
260                         dloc = xform.getBasis()*dloc;
261                 }
262
263                 xform.setOrigin(xform.getOrigin() + dloc);
264                 m_body->setCenterOfMassTransform(xform);
265         }
266
267 }
268
269 void            CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
270 {
271         if (m_body )
272         {
273                 m_body->activate(true);
274                 if (m_body->isStaticObject())
275                 {
276                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
277                 }
278
279                 btMatrix3x3 drotmat(    rotval[0],rotval[1],rotval[2],
280                                                                 rotval[4],rotval[5],rotval[6],
281                                                                 rotval[8],rotval[9],rotval[10]);
282
283
284                 btMatrix3x3 currentOrn;
285                 GetWorldOrientation(currentOrn);
286
287                 btTransform xform = m_body->getCenterOfMassTransform();
288
289                 xform.setBasis(xform.getBasis()*(local ? 
290                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
291
292                 m_body->setCenterOfMassTransform(xform);
293         }
294
295 }
296
297 void CcdPhysicsController::GetWorldOrientation(btMatrix3x3& mat)
298 {
299         float orn[4];
300         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
301         btQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
302         mat.setRotation(quat);
303 }
304
305 void            CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
306 {
307         btQuaternion q = m_body->getCenterOfMassTransform().getRotation();
308         quatImag0 = q[0];
309         quatImag1 = q[1];
310         quatImag2 = q[2];
311         quatReal = q[3];
312 }
313 void            CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
314 {
315         m_body->activate(true);
316         if (m_body->isStaticObject())
317         {
318                 m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
319         }
320
321         m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
322         btTransform xform  = m_body->getCenterOfMassTransform();
323         xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
324         m_body->setCenterOfMassTransform(xform);
325
326 }
327
328 void            CcdPhysicsController::setPosition(float posX,float posY,float posZ)
329 {
330         m_body->activate(true);
331         if (m_body->isStaticObject())
332         {
333                 m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
334         }
335         m_MotionState->setWorldPosition(posX,posY,posZ);
336         btTransform xform  = m_body->getCenterOfMassTransform();
337         xform.setOrigin(btVector3(posX,posY,posZ));
338         m_body->setCenterOfMassTransform(xform);
339
340 }
341 void            CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
342 {
343 }
344
345 void            CcdPhysicsController::getPosition(PHY__Vector3& pos) const
346 {
347         const btTransform& xform = m_body->getCenterOfMassTransform();
348         pos[0] = xform.getOrigin().x();
349         pos[1] = xform.getOrigin().y();
350         pos[2] = xform.getOrigin().z();
351 }
352
353 void            CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
354 {
355         if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
356                 !btFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
357                 !btFuzzyZero(m_cci.m_scaling.z()-scaleZ))
358         {
359                 m_cci.m_scaling = btVector3(scaleX,scaleY,scaleZ);
360
361                 if (m_body && m_body->getCollisionShape())
362                 {
363                         m_body->getCollisionShape()->setLocalScaling(m_cci.m_scaling);
364                         m_body->getCollisionShape()->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
365                         m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
366                 }
367         }
368 }
369                 
370                 // physics methods
371 void            CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
372 {
373         btVector3 torque(torqueX,torqueY,torqueZ);
374         btTransform xform = m_body->getCenterOfMassTransform();
375         if (local)
376         {
377                 torque  = xform.getBasis()*torque;
378         }
379         m_body->applyTorque(torque);
380 }
381
382 void            CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
383 {
384         btVector3 force(forceX,forceY,forceZ);
385         btTransform xform = m_body->getCenterOfMassTransform();
386         if (local)
387         {
388                 force   = xform.getBasis()*force;
389         }
390         m_body->applyCentralForce(force);
391 }
392 void            CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
393 {
394         btVector3 angvel(ang_velX,ang_velY,ang_velZ);
395         if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
396         {
397                 m_body->activate(true);
398         }
399
400         {
401                 btTransform xform = m_body->getCenterOfMassTransform();
402                 if (local)
403                 {
404                         angvel  = xform.getBasis()*angvel;
405                 }
406
407                 m_body->setAngularVelocity(angvel);
408         }
409
410 }
411 void            CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
412 {
413
414         btVector3 linVel(lin_velX,lin_velY,lin_velZ);
415         if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
416         {
417                 m_body->activate(true);
418         }
419         
420         {
421                 btTransform xform = m_body->getCenterOfMassTransform();
422                 if (local)
423                 {
424                         linVel  = xform.getBasis()*linVel;
425                 }
426                 m_body->setLinearVelocity(linVel);
427         }
428 }
429 void            CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
430 {
431         btVector3 impulse(impulseX,impulseY,impulseZ);
432
433         if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
434         {
435                 m_body->activate();
436                 
437                 btVector3 pos(attachX,attachY,attachZ);
438
439                 m_body->activate();
440
441                 m_body->applyImpulse(impulse,pos);
442         }
443
444 }
445 void            CcdPhysicsController::SetActive(bool active)
446 {
447 }
448                 // reading out information from physics
449 void            CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
450 {
451         const btVector3& linvel = this->m_body->getLinearVelocity();
452         linvX = linvel.x();
453         linvY = linvel.y();
454         linvZ = linvel.z();
455
456 }
457
458 void            CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
459 {
460         const btVector3& angvel= m_body->getAngularVelocity();
461         angVelX = angvel.x();
462         angVelY = angvel.y();
463         angVelZ = angvel.z();
464 }
465
466 void            CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
467 {
468         btVector3 pos(posX,posY,posZ);
469         btVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
470         btVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
471         linvX = linvel.x();
472         linvY = linvel.y();
473         linvZ = linvel.z();
474 }
475 void            CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
476 {
477 }
478
479                 // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
480 void            CcdPhysicsController::setRigidBody(bool rigid)
481 {
482         if (!rigid)
483         {
484                 //fake it for now
485                 btVector3 inertia = m_body->getInvInertiaDiagLocal();
486                 inertia[1] = 0.f;
487                 m_body->setInvInertiaDiagLocal(inertia);
488                 m_body->updateInertiaTensor();
489         }
490 }
491
492                 // clientinfo for raycasts for example
493 void*           CcdPhysicsController::getNewClientInfo()
494 {
495         return m_newClientInfo;
496 }
497 void            CcdPhysicsController::setNewClientInfo(void* clientinfo)
498 {
499         m_newClientInfo = clientinfo;
500 }
501
502
503 void    CcdPhysicsController::UpdateDeactivation(float timeStep)
504 {
505         m_body->updateDeactivation( timeStep);
506 }
507
508 bool CcdPhysicsController::wantsSleeping()
509 {
510
511         return m_body->wantsSleeping();
512 }
513
514 PHY_IPhysicsController* CcdPhysicsController::GetReplica()
515 {
516         //very experimental, shape sharing is not implemented yet.
517         //just support btSphereShape/ConeShape for now
518
519         CcdConstructionInfo cinfo = m_cci;
520         if (cinfo.m_collisionShape)
521         {
522                 switch (cinfo.m_collisionShape->getShapeType())
523                 {
524                 case SPHERE_SHAPE_PROXYTYPE:
525                         {
526                                 btSphereShape* orgShape = (btSphereShape*)cinfo.m_collisionShape;
527                                 cinfo.m_collisionShape = new btSphereShape(*orgShape);
528                                 break;
529                         }
530
531                         case CONE_SHAPE_PROXYTYPE:
532                         {
533                                 btConeShape* orgShape = (btConeShape*)cinfo.m_collisionShape;
534                                 cinfo.m_collisionShape = new btConeShape(*orgShape);
535                                 break;
536                         }
537
538
539                 default:
540                         {
541                                 return 0;
542                         }
543                 }
544         }
545
546         cinfo.m_MotionState = new DefaultMotionState();
547
548         CcdPhysicsController* replica = new CcdPhysicsController(cinfo);
549         return replica;
550 }
551
552 ///////////////////////////////////////////////////////////
553 ///A small utility class, DefaultMotionState
554 ///
555 ///////////////////////////////////////////////////////////
556
557 DefaultMotionState::DefaultMotionState()
558 {
559         m_worldTransform.setIdentity();
560         m_localScaling.setValue(1.f,1.f,1.f);
561 }
562
563
564 DefaultMotionState::~DefaultMotionState()
565 {
566
567 }
568
569 void    DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
570 {
571         posX = m_worldTransform.getOrigin().x();
572         posY = m_worldTransform.getOrigin().y();
573         posZ = m_worldTransform.getOrigin().z();
574 }
575
576 void    DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
577 {
578         scaleX = m_localScaling.getX();
579         scaleY = m_localScaling.getY();
580         scaleZ = m_localScaling.getZ();
581 }
582
583 void    DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
584 {
585         quatIma0 = m_worldTransform.getRotation().x();
586         quatIma1 = m_worldTransform.getRotation().y();
587         quatIma2 = m_worldTransform.getRotation().z();
588         quatReal = m_worldTransform.getRotation()[3];
589 }
590                 
591 void    DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
592 {
593         btPoint3 pos(posX,posY,posZ);
594         m_worldTransform.setOrigin( pos );
595 }
596
597 void    DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
598 {
599         btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
600         m_worldTransform.setRotation( orn );
601 }
602                 
603 void    DefaultMotionState::calculateWorldTransformations()
604 {
605
606 }
607