Removed old Blender/extern/bullet, and upgraded game engine to use Bullet 2.x
[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();
249
250                 btVector3 dloc(dlocX,dlocY,dlocZ);
251                 btTransform xform = m_body->getCenterOfMassTransform();
252
253                 if (local)
254                 {
255                         dloc = xform.getBasis()*dloc;
256                 }
257
258                 xform.setOrigin(xform.getOrigin() + dloc);
259                 m_body->setCenterOfMassTransform(xform);
260         }
261
262 }
263
264 void            CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
265 {
266         if (m_body )
267         {
268                 m_body->activate();
269
270                 btMatrix3x3 drotmat(    rotval[0],rotval[1],rotval[2],
271                                                                 rotval[4],rotval[5],rotval[6],
272                                                                 rotval[8],rotval[9],rotval[10]);
273
274
275                 btMatrix3x3 currentOrn;
276                 GetWorldOrientation(currentOrn);
277
278                 btTransform xform = m_body->getCenterOfMassTransform();
279
280                 xform.setBasis(xform.getBasis()*(local ? 
281                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
282
283                 m_body->setCenterOfMassTransform(xform);
284         }
285
286 }
287
288 void CcdPhysicsController::GetWorldOrientation(btMatrix3x3& mat)
289 {
290         float orn[4];
291         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
292         btQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
293         mat.setRotation(quat);
294 }
295
296 void            CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
297 {
298         btQuaternion q = m_body->getCenterOfMassTransform().getRotation();
299         quatImag0 = q[0];
300         quatImag1 = q[1];
301         quatImag2 = q[2];
302         quatReal = q[3];
303 }
304 void            CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
305 {
306         m_body->activate();
307         m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
308         btTransform xform  = m_body->getCenterOfMassTransform();
309         xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
310         m_body->setCenterOfMassTransform(xform);
311
312 }
313
314 void            CcdPhysicsController::setPosition(float posX,float posY,float posZ)
315 {
316         m_body->activate();
317         m_MotionState->setWorldPosition(posX,posY,posZ);
318         btTransform xform  = m_body->getCenterOfMassTransform();
319         xform.setOrigin(btVector3(posX,posY,posZ));
320         m_body->setCenterOfMassTransform(xform);
321
322 }
323 void            CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
324 {
325 }
326
327 void            CcdPhysicsController::getPosition(PHY__Vector3& pos) const
328 {
329         const btTransform& xform = m_body->getCenterOfMassTransform();
330         pos[0] = xform.getOrigin().x();
331         pos[1] = xform.getOrigin().y();
332         pos[2] = xform.getOrigin().z();
333 }
334
335 void            CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
336 {
337         if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
338                 !btFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
339                 !btFuzzyZero(m_cci.m_scaling.z()-scaleZ))
340         {
341                 m_cci.m_scaling = btVector3(scaleX,scaleY,scaleZ);
342
343                 if (m_body && m_body->getCollisionShape())
344                 {
345                         m_body->getCollisionShape()->setLocalScaling(m_cci.m_scaling);
346                         m_body->getCollisionShape()->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
347                         m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
348                 }
349         }
350 }
351                 
352                 // physics methods
353 void            CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
354 {
355         btVector3 torque(torqueX,torqueY,torqueZ);
356         btTransform xform = m_body->getCenterOfMassTransform();
357         if (local)
358         {
359                 torque  = xform.getBasis()*torque;
360         }
361         m_body->applyTorque(torque);
362 }
363
364 void            CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
365 {
366         btVector3 force(forceX,forceY,forceZ);
367         btTransform xform = m_body->getCenterOfMassTransform();
368         if (local)
369         {
370                 force   = xform.getBasis()*force;
371         }
372         m_body->applyCentralForce(force);
373 }
374 void            CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
375 {
376         btVector3 angvel(ang_velX,ang_velY,ang_velZ);
377         if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
378         {
379                 m_body->activate();
380         }
381
382         {
383                 btTransform xform = m_body->getCenterOfMassTransform();
384                 if (local)
385                 {
386                         angvel  = xform.getBasis()*angvel;
387                 }
388
389                 m_body->setAngularVelocity(angvel);
390         }
391
392 }
393 void            CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
394 {
395
396         btVector3 linVel(lin_velX,lin_velY,lin_velZ);
397         if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
398         {
399                 m_body->activate();
400         }
401         
402         {
403                 btTransform xform = m_body->getCenterOfMassTransform();
404                 if (local)
405                 {
406                         linVel  = xform.getBasis()*linVel;
407                 }
408                 m_body->setLinearVelocity(linVel);
409         }
410 }
411 void            CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
412 {
413         btVector3 impulse(impulseX,impulseY,impulseZ);
414
415         if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
416         {
417                 m_body->activate();
418                 
419                 btVector3 pos(attachX,attachY,attachZ);
420
421                 m_body->activate();
422
423                 m_body->applyImpulse(impulse,pos);
424         }
425
426 }
427 void            CcdPhysicsController::SetActive(bool active)
428 {
429 }
430                 // reading out information from physics
431 void            CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
432 {
433         const btVector3& linvel = this->m_body->getLinearVelocity();
434         linvX = linvel.x();
435         linvY = linvel.y();
436         linvZ = linvel.z();
437
438 }
439
440 void            CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
441 {
442         const btVector3& angvel= m_body->getAngularVelocity();
443         angVelX = angvel.x();
444         angVelY = angvel.y();
445         angVelZ = angvel.z();
446 }
447
448 void            CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
449 {
450         btVector3 pos(posX,posY,posZ);
451         btVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
452         btVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
453         linvX = linvel.x();
454         linvY = linvel.y();
455         linvZ = linvel.z();
456 }
457 void            CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
458 {
459 }
460
461                 // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
462 void            CcdPhysicsController::setRigidBody(bool rigid)
463 {
464         if (!rigid)
465         {
466                 //fake it for now
467                 btVector3 inertia = m_body->getInvInertiaDiagLocal();
468                 inertia[1] = 0.f;
469                 m_body->setInvInertiaDiagLocal(inertia);
470                 m_body->updateInertiaTensor();
471         }
472 }
473
474                 // clientinfo for raycasts for example
475 void*           CcdPhysicsController::getNewClientInfo()
476 {
477         return m_newClientInfo;
478 }
479 void            CcdPhysicsController::setNewClientInfo(void* clientinfo)
480 {
481         m_newClientInfo = clientinfo;
482 }
483
484
485 void    CcdPhysicsController::UpdateDeactivation(float timeStep)
486 {
487         m_body->updateDeactivation( timeStep);
488 }
489
490 bool CcdPhysicsController::wantsSleeping()
491 {
492
493         return m_body->wantsSleeping();
494 }
495
496 PHY_IPhysicsController* CcdPhysicsController::GetReplica()
497 {
498         //very experimental, shape sharing is not implemented yet.
499         //just support btSphereShape/ConeShape for now
500
501         CcdConstructionInfo cinfo = m_cci;
502         if (cinfo.m_collisionShape)
503         {
504                 switch (cinfo.m_collisionShape->getShapeType())
505                 {
506                 case SPHERE_SHAPE_PROXYTYPE:
507                         {
508                                 btSphereShape* orgShape = (btSphereShape*)cinfo.m_collisionShape;
509                                 cinfo.m_collisionShape = new btSphereShape(*orgShape);
510                                 break;
511                         }
512
513                         case CONE_SHAPE_PROXYTYPE:
514                         {
515                                 btConeShape* orgShape = (btConeShape*)cinfo.m_collisionShape;
516                                 cinfo.m_collisionShape = new btConeShape(*orgShape);
517                                 break;
518                         }
519
520
521                 default:
522                         {
523                                 return 0;
524                         }
525                 }
526         }
527
528         cinfo.m_MotionState = new DefaultMotionState();
529
530         CcdPhysicsController* replica = new CcdPhysicsController(cinfo);
531         return replica;
532 }
533
534 ///////////////////////////////////////////////////////////
535 ///A small utility class, DefaultMotionState
536 ///
537 ///////////////////////////////////////////////////////////
538
539 DefaultMotionState::DefaultMotionState()
540 {
541         m_worldTransform.setIdentity();
542         m_localScaling.setValue(1.f,1.f,1.f);
543 }
544
545
546 DefaultMotionState::~DefaultMotionState()
547 {
548
549 }
550
551 void    DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
552 {
553         posX = m_worldTransform.getOrigin().x();
554         posY = m_worldTransform.getOrigin().y();
555         posZ = m_worldTransform.getOrigin().z();
556 }
557
558 void    DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
559 {
560         scaleX = m_localScaling.getX();
561         scaleY = m_localScaling.getY();
562         scaleZ = m_localScaling.getZ();
563 }
564
565 void    DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
566 {
567         quatIma0 = m_worldTransform.getRotation().x();
568         quatIma1 = m_worldTransform.getRotation().y();
569         quatIma2 = m_worldTransform.getRotation().z();
570         quatReal = m_worldTransform.getRotation()[3];
571 }
572                 
573 void    DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
574 {
575         btPoint3 pos(posX,posY,posZ);
576         m_worldTransform.setOrigin( pos );
577 }
578
579 void    DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
580 {
581         btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
582         m_worldTransform.setRotation( orn );
583 }
584                 
585 void    DefaultMotionState::calculateWorldTransformations()
586 {
587
588 }
589