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