BGE patch: new Physics button and margin parameter in Logic panel. Change subversion.
[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 #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
19 #include "PHY_IMotionState.h"
20 #include "CcdPhysicsEnvironment.h"
21 #include "RAS_MeshObject.h"
22
23
24 class BP_Proxy;
25
26 ///todo: fill all the empty CcdPhysicsController methods, hook them up to the btRigidBody class
27
28 //'temporarily' global variables
29 //float gDeactivationTime = 2.f;
30 //bool  gDisableDeactivation = false;
31 extern float gDeactivationTime;
32 extern bool gDisableDeactivation;
33
34
35 float gLinearSleepingTreshold = 0.8f;
36 float gAngularSleepingTreshold = 1.0f;
37
38
39 btVector3 startVel(0,0,0);//-10000);
40
41 CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
42 :m_cci(ci)
43 {
44         m_collisionDelay = 0;
45         m_newClientInfo = 0;
46         m_registerCount = 0;
47                 
48         // copy pointers locally to allow smart release
49         m_MotionState = ci.m_MotionState;
50         m_collisionShape = ci.m_collisionShape;
51         // apply scaling before creating rigid body
52         m_collisionShape->setLocalScaling(m_cci.m_scaling);
53         if (m_cci.m_mass)
54                 m_collisionShape->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
55         // shape info is shared, increment ref count
56         m_shapeInfo = ci.m_shapeInfo;
57         if (m_shapeInfo)
58                 m_shapeInfo->AddRef();
59         
60         m_bulletMotionState = 0;
61         
62         
63         CreateRigidbody();
64         
65
66         
67         #ifdef WIN32
68         if (m_body->getInvMass())
69                 m_body->setLinearVelocity(startVel);
70         #endif
71
72 }
73
74 btTransform     CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
75 {
76         btTransform trans;
77         float tmp[3];
78         motionState->getWorldPosition(tmp[0],tmp[1],tmp[2]);
79         trans.setOrigin(btVector3(tmp[0],tmp[1],tmp[2]));
80
81         btQuaternion orn;
82         motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
83         trans.setRotation(orn);
84         return trans;
85
86 }
87
88 class   BlenderBulletMotionState : public btMotionState
89 {
90         PHY_IMotionState*       m_blenderMotionState;
91
92 public:
93
94         BlenderBulletMotionState(PHY_IMotionState* bms)
95                 :m_blenderMotionState(bms)
96         {
97
98         }
99
100         virtual void    getWorldTransform(btTransform& worldTrans ) const
101         {
102                 float pos[3];
103                 float quatOrn[4];
104
105                 m_blenderMotionState->getWorldPosition(pos[0],pos[1],pos[2]);
106                 m_blenderMotionState->getWorldOrientation(quatOrn[0],quatOrn[1],quatOrn[2],quatOrn[3]);
107                 worldTrans.setOrigin(btVector3(pos[0],pos[1],pos[2]));
108                 worldTrans.setBasis(btMatrix3x3(btQuaternion(quatOrn[0],quatOrn[1],quatOrn[2],quatOrn[3])));
109         }
110
111         virtual void    setWorldTransform(const btTransform& worldTrans)
112         {
113                 m_blenderMotionState->setWorldPosition(worldTrans.getOrigin().getX(),worldTrans.getOrigin().getY(),worldTrans.getOrigin().getZ());
114                 btQuaternion rotQuat = worldTrans.getRotation();
115                 m_blenderMotionState->setWorldOrientation(rotQuat[0],rotQuat[1],rotQuat[2],rotQuat[3]);
116                 m_blenderMotionState->calculateWorldTransformations();
117         }
118
119 };
120
121
122 void CcdPhysicsController::CreateRigidbody()
123 {
124
125         btTransform trans = GetTransformFromMotionState(m_MotionState);
126
127         m_bulletMotionState = new BlenderBulletMotionState(m_MotionState);
128
129         btRigidBody::btRigidBodyConstructionInfo rbci(m_cci.m_mass,m_bulletMotionState,m_collisionShape,m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
130         rbci.m_linearDamping = m_cci.m_linearDamping;
131         rbci.m_angularDamping = m_cci.m_angularDamping;
132         rbci.m_friction = m_cci.m_friction;
133         rbci.m_restitution = m_cci.m_restitution;
134         
135         m_body = new btRigidBody(rbci);
136         
137         //
138         // init the rigidbody properly
139         //
140         
141         //setMassProps this also sets collisionFlags
142         //convert collision flags!
143         //special case: a near/radar sensor controller should not be defined static or it will
144         //generate loads of static-static collision messages on the console
145         if ((m_cci.m_collisionFilterGroup & CcdConstructionInfo::SensorFilter) != 0)
146         {
147                 // reset the flags that have been set so far
148                 m_body->setCollisionFlags(0);
149         }
150         m_body->setCollisionFlags(m_body->getCollisionFlags() | m_cci.m_collisionFlags);
151         m_body->setGravity( m_cci.m_gravity);
152         m_body->setDamping(m_cci.m_linearDamping, m_cci.m_angularDamping);
153
154         if (!m_cci.m_bRigid)
155         {
156                 m_body->setAngularFactor(0.f);
157         }
158 }
159
160 static void DeleteBulletShape(btCollisionShape* shape)
161 {
162         if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
163         {
164                 // shapes based on meshes use an interface that contains the vertices.
165                 btTriangleMeshShape* meshShape = static_cast<btTriangleMeshShape*>(shape);
166                 btStridingMeshInterface* meshInterface = meshShape->getMeshInterface();
167                 if (meshInterface)
168                         delete meshInterface;
169         }
170         delete shape;
171 }
172
173 CcdPhysicsController::~CcdPhysicsController()
174 {
175         //will be reference counted, due to sharing
176         if (m_cci.m_physicsEnv)
177                 m_cci.m_physicsEnv->removeCcdPhysicsController(this);
178
179         if (m_MotionState)
180                 delete m_MotionState;
181         if (m_bulletMotionState)
182                 delete m_bulletMotionState;
183         delete m_body;
184
185         if (m_collisionShape)
186         {
187                 // collision shape is always unique to the controller, can delete it here
188                 if (m_collisionShape->isCompound())
189                 {
190                         // bullet does not delete the child shape, must do it here
191                         btCompoundShape* compoundShape = (btCompoundShape*)m_collisionShape;
192                         int numChild = compoundShape->getNumChildShapes();
193                         for (int i=numChild-1 ; i >= 0; i--)
194                         {
195                                 btCollisionShape* childShape = compoundShape->getChildShape(i);
196                                 DeleteBulletShape(childShape);
197                         }
198                 }
199                 DeleteBulletShape(m_collisionShape);
200         }
201         if (m_shapeInfo)
202         {
203                 m_shapeInfo->Release();
204         }
205 }
206
207
208                 /**
209                         SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
210                 */
211 bool            CcdPhysicsController::SynchronizeMotionStates(float time)
212 {
213         //sync non-static to motionstate, and static from motionstate (todo: add kinematic etc.)
214
215         if (!m_body->isStaticObject())
216         {
217                 const btVector3& worldPos = m_body->getCenterOfMassPosition();
218                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
219                 
220                 const btQuaternion& worldquat = m_body->getOrientation();
221                 m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
222
223                 m_MotionState->calculateWorldTransformations();
224
225                 float scale[3];
226                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
227                 btVector3 scaling(scale[0],scale[1],scale[2]);
228                 GetCollisionShape()->setLocalScaling(scaling);
229         } else
230         {
231                 btVector3 worldPos;
232                 btQuaternion worldquat;
233
234 /*              m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
235                 m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
236                 btTransform oldTrans = m_body->getCenterOfMassTransform();
237                 btTransform newTrans(worldquat,worldPos);
238                                 
239                 m_body->setCenterOfMassTransform(newTrans);
240                 //need to keep track of previous position for friction effects...
241                 
242                 m_MotionState->calculateWorldTransformations();
243 */
244                 float scale[3];
245                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
246                 btVector3 scaling(scale[0],scale[1],scale[2]);
247                 GetCollisionShape()->setLocalScaling(scaling);
248         }
249         return true;
250
251 }
252
253                 /**
254                         WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
255                 */
256                 
257 void            CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
258 {
259
260 }
261 void            CcdPhysicsController::WriteDynamicsToMotionState()
262 {
263 }
264                 // controller replication
265 void            CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
266 {
267         m_MotionState = motionstate;
268         m_registerCount = 0;
269         m_collisionShape = NULL;
270
271         // always create a new shape to avoid scaling bug
272         if (m_shapeInfo)
273         {
274                 m_shapeInfo->AddRef();
275                 m_collisionShape = m_shapeInfo->CreateBulletShape();
276
277                 if (m_collisionShape)
278                 {
279                         // new shape has no scaling, apply initial scaling
280                         m_collisionShape->setMargin(m_cci.m_margin);
281                         m_collisionShape->setLocalScaling(m_cci.m_scaling);
282                         
283                         if (m_cci.m_mass)
284                                 m_collisionShape->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
285                 }
286         }
287
288         m_body = 0;
289         CreateRigidbody();
290
291         if (m_body)
292         {
293                 if (m_cci.m_mass)
294                 {
295                         m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
296                 } 
297         }                       
298         m_cci.m_physicsEnv->addCcdPhysicsController(this);
299
300
301 /*      SM_Object* dynaparent=0;
302         SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
303         
304         if (sumoparentctrl)
305         {
306                 dynaparent = sumoparentctrl->GetSumoObject();
307         }
308         
309         SM_Object* orgsumoobject = m_sumoObj;
310         
311         
312         m_sumoObj       =       new SM_Object(
313                 orgsumoobject->getShapeHandle(), 
314                 orgsumoobject->getMaterialProps(),                      
315                 orgsumoobject->getShapeProps(),
316                 dynaparent);
317         
318         m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
319         
320         m_sumoObj->setMargin(orgsumoobject->getMargin());
321         m_sumoObj->setPosition(orgsumoobject->getPosition());
322         m_sumoObj->setOrientation(orgsumoobject->getOrientation());
323         //if it is a dyna, register for a callback
324         m_sumoObj->registerCallback(*this);
325         
326         m_sumoScene->add(* (m_sumoObj));
327         */
328
329
330
331 }
332
333                 // kinematic methods
334 void            CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
335 {
336         if (m_body)
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
345                 btVector3 dloc(dlocX,dlocY,dlocZ);
346                 btTransform xform = m_body->getCenterOfMassTransform();
347
348                 if (local)
349                 {
350                         dloc = xform.getBasis()*dloc;
351                 }
352
353                 xform.setOrigin(xform.getOrigin() + dloc);
354                 m_body->setCenterOfMassTransform(xform);
355         }
356
357 }
358
359 void            CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
360 {
361         if (m_body)
362         {
363                 m_body->activate(true);
364                 if (m_body->isStaticObject())
365                 {
366                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
367                 }
368
369                 btMatrix3x3 drotmat(    rotval[0],rotval[4],rotval[8],
370                                                                 rotval[1],rotval[5],rotval[9],
371                                                                 rotval[2],rotval[6],rotval[10]);
372
373
374                 btMatrix3x3 currentOrn;
375                 GetWorldOrientation(currentOrn);
376
377                 btTransform xform = m_body->getCenterOfMassTransform();
378
379                 xform.setBasis(xform.getBasis()*(local ? 
380                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
381
382                 m_body->setCenterOfMassTransform(xform);
383         }
384
385 }
386
387 void CcdPhysicsController::GetWorldOrientation(btMatrix3x3& mat)
388 {
389         float orn[4];
390         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
391         btQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
392         mat.setRotation(quat);
393 }
394
395 void            CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
396 {
397         btQuaternion q = m_body->getCenterOfMassTransform().getRotation();
398         quatImag0 = q[0];
399         quatImag1 = q[1];
400         quatImag2 = q[2];
401         quatReal = q[3];
402 }
403 void            CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
404 {
405         if (m_body)
406         {
407                 m_body->activate(true);
408                 if (m_body->isStaticObject())
409                 {
410                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
411                 }
412                 // not required
413                 //m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
414                 btTransform xform  = m_body->getCenterOfMassTransform();
415                 xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
416                 m_body->setCenterOfMassTransform(xform);
417                 // not required
418                 //m_bulletMotionState->setWorldTransform(xform);
419         }
420
421 }
422
423 void CcdPhysicsController::setWorldOrientation(const btMatrix3x3& orn)
424 {
425         if (m_body)
426         {
427                 m_body->activate(true);
428                 if (m_body->isStaticObject())
429                 {
430                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
431                 }
432                 // not required
433                 //m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
434                 btTransform xform  = m_body->getCenterOfMassTransform();
435                 xform.setBasis(orn);
436                 m_body->setCenterOfMassTransform(xform);
437                 // not required
438                 //m_bulletMotionState->setWorldTransform(xform);
439         }
440
441 }
442
443 void            CcdPhysicsController::setPosition(float posX,float posY,float posZ)
444 {
445         if (m_body)
446         {
447                 m_body->activate(true);
448                 if (m_body->isStaticObject())
449                 {
450                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
451                 }
452                 // not required, this function is only used to update the physic controller
453                 //m_MotionState->setWorldPosition(posX,posY,posZ);
454                 btTransform xform  = m_body->getCenterOfMassTransform();
455                 xform.setOrigin(btVector3(posX,posY,posZ));
456                 m_body->setCenterOfMassTransform(xform);
457                 // not required
458                 //m_bulletMotionState->setWorldTransform(xform);
459         }
460
461
462 }
463 void            CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
464 {
465 }
466
467 void            CcdPhysicsController::getPosition(PHY__Vector3& pos) const
468 {
469         const btTransform& xform = m_body->getCenterOfMassTransform();
470         pos[0] = xform.getOrigin().x();
471         pos[1] = xform.getOrigin().y();
472         pos[2] = xform.getOrigin().z();
473 }
474
475 void            CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
476 {
477         if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
478                 !btFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
479                 !btFuzzyZero(m_cci.m_scaling.z()-scaleZ))
480         {
481                 m_cci.m_scaling = btVector3(scaleX,scaleY,scaleZ);
482
483                 if (m_body && m_body->getCollisionShape())
484                 {
485                         m_body->getCollisionShape()->setLocalScaling(m_cci.m_scaling);
486                         
487                         //printf("no inertia recalc for fixed objects with mass=0\n");
488                         if (m_cci.m_mass)
489                         {
490                                 m_body->getCollisionShape()->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
491                                 m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
492                         } 
493                         
494                 }
495         }
496 }
497                 
498                 // physics methods
499 void            CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
500 {
501         btVector3 torque(torqueX,torqueY,torqueZ);
502         btTransform xform = m_body->getCenterOfMassTransform();
503         if (m_body && torque.length2() > (SIMD_EPSILON*SIMD_EPSILON))
504         {
505                 m_body->activate();
506                 if (m_body->isStaticObject())
507                 {
508                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
509                 }
510                 if (local)
511                 {
512                         torque  = xform.getBasis()*torque;
513                 }
514                 m_body->applyTorque(torque);
515         }
516 }
517
518 void            CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
519 {
520         btVector3 force(forceX,forceY,forceZ);
521         
522         if (m_body && force.length2() > (SIMD_EPSILON*SIMD_EPSILON))
523         {
524                 m_body->activate();
525                 if (m_body->isStaticObject())
526                 {
527                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
528                 }
529                 {
530                         btTransform xform = m_body->getCenterOfMassTransform();
531                         if (local)
532                         {       
533                                 force   = xform.getBasis()*force;
534                         }
535                 }
536                 m_body->applyCentralForce(force);
537         }
538 }
539 void            CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
540 {
541         btVector3 angvel(ang_velX,ang_velY,ang_velZ);
542         if (m_body && angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
543         {
544                 m_body->activate(true);
545                 if (m_body->isStaticObject())
546                 {
547                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
548                 }
549                 {
550                         btTransform xform = m_body->getCenterOfMassTransform();
551                         if (local)
552                         {
553                                 angvel  = xform.getBasis()*angvel;
554                         }
555                 }
556                 m_body->setAngularVelocity(angvel);
557         }
558
559 }
560 void            CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
561 {
562
563         btVector3 linVel(lin_velX,lin_velY,lin_velZ);
564         if (m_body && linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
565         {
566                 m_body->activate(true);
567                 if (m_body->isStaticObject())
568                 {
569                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
570                 }
571
572                 {
573                         btTransform xform = m_body->getCenterOfMassTransform();
574                         if (local)
575                         {
576                                 linVel  = xform.getBasis()*linVel;
577                         }
578                 }
579                 m_body->setLinearVelocity(linVel);
580         }
581 }
582 void            CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
583 {
584         btVector3 impulse(impulseX,impulseY,impulseZ);
585
586         if (m_body && impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
587         {
588                 m_body->activate();
589                 if (m_body->isStaticObject())
590                 {
591                         m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
592                 }
593                 
594                 btVector3 pos(attachX,attachY,attachZ);
595
596                 m_body->applyImpulse(impulse,pos);
597         }
598
599 }
600 void            CcdPhysicsController::SetActive(bool active)
601 {
602 }
603                 // reading out information from physics
604 void            CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
605 {
606         const btVector3& linvel = this->m_body->getLinearVelocity();
607         linvX = linvel.x();
608         linvY = linvel.y();
609         linvZ = linvel.z();
610
611 }
612
613 void            CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
614 {
615         const btVector3& angvel= m_body->getAngularVelocity();
616         angVelX = angvel.x();
617         angVelY = angvel.y();
618         angVelZ = angvel.z();
619 }
620
621 void            CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
622 {
623         btVector3 pos(posX,posY,posZ);
624         btVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
625         btVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
626         linvX = linvel.x();
627         linvY = linvel.y();
628         linvZ = linvel.z();
629 }
630 void            CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
631 {
632 }
633
634                 // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
635 void            CcdPhysicsController::setRigidBody(bool rigid)
636 {
637         if (!rigid)
638         {
639                 //fake it for now
640                 btVector3 inertia = m_body->getInvInertiaDiagLocal();
641                 inertia[1] = 0.f;
642                 m_body->setInvInertiaDiagLocal(inertia);
643                 m_body->updateInertiaTensor();
644         }
645 }
646
647                 // clientinfo for raycasts for example
648 void*           CcdPhysicsController::getNewClientInfo()
649 {
650         return m_newClientInfo;
651 }
652 void            CcdPhysicsController::setNewClientInfo(void* clientinfo)
653 {
654         m_newClientInfo = clientinfo;
655 }
656
657
658 void    CcdPhysicsController::UpdateDeactivation(float timeStep)
659 {
660         m_body->updateDeactivation( timeStep);
661 }
662
663 bool CcdPhysicsController::wantsSleeping()
664 {
665
666         return m_body->wantsSleeping();
667 }
668
669 PHY_IPhysicsController* CcdPhysicsController::GetReplica()
670 {
671         // This is used only to replicate Near and Radar sensor controllers
672         // The replication of object physics controller is done in KX_BulletPhysicsController::GetReplica()
673         CcdConstructionInfo cinfo = m_cci;
674         if (m_shapeInfo)
675         {
676                 // This situation does not normally happen
677                 cinfo.m_collisionShape = m_shapeInfo->CreateBulletShape();
678         } 
679         else if (m_collisionShape)
680         {
681                 switch (m_collisionShape->getShapeType())
682                 {
683                 case SPHERE_SHAPE_PROXYTYPE:
684                         {
685                                 btSphereShape* orgShape = (btSphereShape*)m_collisionShape;
686                                 cinfo.m_collisionShape = new btSphereShape(*orgShape);
687                                 break;
688                         }
689
690                 case CONE_SHAPE_PROXYTYPE:
691                         {
692                                 btConeShape* orgShape = (btConeShape*)m_collisionShape;
693                                 cinfo.m_collisionShape = new btConeShape(*orgShape);
694                                 break;
695                         }
696
697                 default:
698                         {
699                                 return 0;
700                         }
701                 }
702         }
703
704         cinfo.m_MotionState = new DefaultMotionState();
705         cinfo.m_shapeInfo = m_shapeInfo;
706
707         CcdPhysicsController* replica = new CcdPhysicsController(cinfo);
708         return replica;
709 }
710
711 ///////////////////////////////////////////////////////////
712 ///A small utility class, DefaultMotionState
713 ///
714 ///////////////////////////////////////////////////////////
715
716 DefaultMotionState::DefaultMotionState()
717 {
718         m_worldTransform.setIdentity();
719         m_localScaling.setValue(1.f,1.f,1.f);
720 }
721
722
723 DefaultMotionState::~DefaultMotionState()
724 {
725
726 }
727
728 void    DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
729 {
730         posX = m_worldTransform.getOrigin().x();
731         posY = m_worldTransform.getOrigin().y();
732         posZ = m_worldTransform.getOrigin().z();
733 }
734
735 void    DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
736 {
737         scaleX = m_localScaling.getX();
738         scaleY = m_localScaling.getY();
739         scaleZ = m_localScaling.getZ();
740 }
741
742 void    DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
743 {
744         quatIma0 = m_worldTransform.getRotation().x();
745         quatIma1 = m_worldTransform.getRotation().y();
746         quatIma2 = m_worldTransform.getRotation().z();
747         quatReal = m_worldTransform.getRotation()[3];
748 }
749                 
750 void    DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
751 {
752         btPoint3 pos(posX,posY,posZ);
753         m_worldTransform.setOrigin( pos );
754 }
755
756 void    DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
757 {
758         btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
759         m_worldTransform.setRotation( orn );
760 }
761                 
762 void    DefaultMotionState::calculateWorldTransformations()
763 {
764
765 }
766
767 // Shape constructor
768 bool CcdShapeConstructionInfo::SetMesh(RAS_MeshObject* meshobj, bool polytope)
769 {
770         // assume no shape information
771         m_shapeType = PHY_SHAPE_NONE;
772         m_vertexArray.clear();
773         m_polygonIndexArray.clear();
774         m_meshObject = NULL;
775
776         if (!meshobj)
777                 return false;
778
779         // Mesh has no polygons!
780         int numpolys = meshobj->NumPolygons();
781         if (!numpolys)
782         {
783                 return false;
784         }
785
786         // check that we have at least one colliding polygon
787         int numvalidpolys = 0;
788
789         for (int p=0; p<numpolys; p++)
790         {
791                 RAS_Polygon* poly = meshobj->GetPolygon(p);
792
793                 // only add polygons that have the collisionflag set
794                 if (poly->IsCollider())
795                 {
796                         numvalidpolys++;
797                         break;
798                 }
799         }
800
801         // No collision polygons
802         if (numvalidpolys < 1)
803                 return false;
804
805         m_shapeType = (polytope) ? PHY_SHAPE_POLYTOPE : PHY_SHAPE_MESH;
806
807         numvalidpolys = 0;
808
809         for (int p2=0; p2<numpolys; p2++)
810         {
811                 RAS_Polygon* poly = meshobj->GetPolygon(p2);
812
813                 // only add polygons that have the collisionflag set
814                 if (poly->IsCollider())
815                 {   
816                         //Bullet can raycast any shape, so
817                         if (polytope)
818                         {
819                                 for (int i=0;i<poly->VertexCount();i++)
820                                 {
821                                         const float* vtx = poly->GetVertex(i)->getXYZ();
822                                         btPoint3 point(vtx[0],vtx[1],vtx[2]);
823                                         m_vertexArray.push_back(point);
824                                         numvalidpolys++;
825                                 }
826                         } else
827                         {
828                                 {
829                                         const float* vtx = poly->GetVertex(2)->getXYZ();
830                                         btPoint3 vertex0(vtx[0],vtx[1],vtx[2]);
831
832                                         vtx = poly->GetVertex(1)->getXYZ();
833                                         btPoint3 vertex1(vtx[0],vtx[1],vtx[2]);
834
835                                         vtx = poly->GetVertex(0)->getXYZ();
836                                         btPoint3 vertex2(vtx[0],vtx[1],vtx[2]);
837
838                                         m_vertexArray.push_back(vertex0);
839                                         m_vertexArray.push_back(vertex1);
840                                         m_vertexArray.push_back(vertex2);
841                                         m_polygonIndexArray.push_back(p2);
842                                         numvalidpolys++;
843                                 }
844                                 if (poly->VertexCount() == 4)
845                                 {
846                                         const float* vtx = poly->GetVertex(3)->getXYZ();
847                                         btPoint3 vertex0(vtx[0],vtx[1],vtx[2]);
848
849                                         vtx = poly->GetVertex(2)->getXYZ();
850                                         btPoint3 vertex1(vtx[0],vtx[1],vtx[2]);
851
852                                         vtx = poly->GetVertex(0)->getXYZ();
853                                         btPoint3 vertex2(vtx[0],vtx[1],vtx[2]);
854
855                                         m_vertexArray.push_back(vertex0);
856                                         m_vertexArray.push_back(vertex1);
857                                         m_vertexArray.push_back(vertex2);
858                                         m_polygonIndexArray.push_back(p2);
859                                         numvalidpolys++;
860                                 }
861                         }               
862                 }
863         }
864
865         if (!numvalidpolys)
866         {
867                 // should not happen
868                 m_shapeType = PHY_SHAPE_NONE;
869                 return false;
870         }
871         m_meshObject = meshobj;
872         return true;
873 }
874
875 btCollisionShape* CcdShapeConstructionInfo::CreateBulletShape()
876 {
877         btCollisionShape* collisionShape = 0;
878         btTriangleMeshShape* concaveShape = 0;
879         btTriangleMesh* collisionMeshData = 0;
880         btCompoundShape* compoundShape = 0;
881         CcdShapeConstructionInfo* nextShapeInfo;
882
883         switch (m_shapeType) 
884         {
885         case PHY_SHAPE_NONE:
886                 break;
887
888         case PHY_SHAPE_BOX:
889                 collisionShape = new btBoxShape(m_halfExtend);
890                 break;
891
892         case PHY_SHAPE_SPHERE:
893                 collisionShape = new btSphereShape(m_radius);
894                 break;
895
896         case PHY_SHAPE_CYLINDER:
897                 collisionShape = new btCylinderShapeZ(m_halfExtend);
898                 break;
899
900         case PHY_SHAPE_CONE:
901                 collisionShape = new btConeShapeZ(m_radius, m_height);
902                 break;
903
904         case PHY_SHAPE_POLYTOPE:
905                 collisionShape = new btConvexHullShape(&m_vertexArray.begin()->getX(), m_vertexArray.size());
906                 break;
907
908         case PHY_SHAPE_MESH:
909                 // Let's use the latest btScaledBvhTriangleMeshShape: it allows true sharing of 
910                 // triangle mesh information between duplicates => drastic performance increase when 
911                 // duplicating complex mesh objects. 
912                 // BUT it causes a small performance decrease when sharing is not required: 
913                 // 9 multiplications/additions and one function call for each triangle that passes the mid phase filtering
914                 // One possible optimization is to use directly the btBvhTriangleMeshShape when the scale is 1,1,1
915                 // and btScaledBvhTriangleMeshShape otherwise.
916                 if (!m_unscaledShape)
917                 {
918                         collisionMeshData = new btTriangleMesh();
919                         // m_vertexArray is necessarily a multiple of 3
920                         for (std::vector<btPoint3>::iterator it=m_vertexArray.begin(); it != m_vertexArray.end(); )
921                         {
922                                 collisionMeshData->addTriangle(*it++,*it++,*it++);
923                         }
924                         // this shape will be shared and not deleted until shapeInfo is deleted
925                         m_unscaledShape = new btBvhTriangleMeshShape( collisionMeshData, true );
926                         m_unscaledShape->recalcLocalAabb();
927                 }
928                 collisionShape = new btScaledBvhTriangleMeshShape(m_unscaledShape, btVector3(1.0f,1.0f,1.0f));
929                 break;
930
931         case PHY_SHAPE_COMPOUND:
932                 if (m_nextShape)
933                 {
934                         compoundShape = new btCompoundShape();
935                         for (nextShapeInfo=m_nextShape; nextShapeInfo; nextShapeInfo = nextShapeInfo->m_nextShape)
936                         {
937                                 collisionShape = nextShapeInfo->CreateBulletShape();
938                                 if (collisionShape)
939                                 {
940                                         collisionShape->setLocalScaling(nextShapeInfo->m_childScale);
941                                         compoundShape->addChildShape(nextShapeInfo->m_childTrans, collisionShape);
942                                 }
943                         }
944                         collisionShape = compoundShape;
945                 }
946         }
947         return collisionShape;
948 }
949
950 void CcdShapeConstructionInfo::AddShape(CcdShapeConstructionInfo* shapeInfo)
951 {
952         CcdShapeConstructionInfo* nextShape = this;
953         while (nextShape->m_nextShape != NULL)
954                 nextShape = nextShape->m_nextShape;
955         nextShape->m_nextShape = shapeInfo;
956 }
957
958 CcdShapeConstructionInfo::~CcdShapeConstructionInfo()
959 {
960         CcdShapeConstructionInfo* childShape = m_nextShape;
961
962         while (childShape)
963         {
964                 CcdShapeConstructionInfo* nextShape = childShape->m_nextShape;
965                 childShape->m_nextShape = NULL;
966                 childShape->Release();
967                 childShape = nextShape;
968         }
969         if (m_unscaledShape)
970         {
971                 DeleteBulletShape(m_unscaledShape);
972         }
973         m_vertexArray.clear();
974 }
975
976