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