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