Reverted incorrect merge (missing files)
[blender.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
20 #include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
21
22 #include "PHY_IMotionState.h"
23 #include "CcdPhysicsEnvironment.h"
24 #include "RAS_MeshObject.h"
25 #include "BulletSoftBody/btSoftBody.h"
26 #include "BulletSoftBody//btSoftBodyInternals.h"
27 #include "BulletSoftBody/btSoftBodyHelpers.h"
28 #include "LinearMath/btConvexHull.h"
29 #include "BulletCollision/Gimpact/btGImpactShape.h"
30 #include "BulletCollision/Gimpact/btGImpactShape.h"
31
32
33 #include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
34
35 #include "DNA_mesh_types.h"
36 #include "DNA_meshdata_types.h"
37
38 extern "C"{
39 #include "BKE_cdderivedmesh.h"
40 }
41
42 class BP_Proxy;
43
44 ///todo: fill all the empty CcdPhysicsController methods, hook them up to the btRigidBody class
45
46 //'temporarily' global variables
47 //float gDeactivationTime = 2.f;
48 //bool  gDisableDeactivation = false;
49 extern float gDeactivationTime;
50 extern bool gDisableDeactivation;
51
52
53 float gLinearSleepingTreshold = 0.8f;
54 float gAngularSleepingTreshold = 1.0f;
55
56
57 btVector3 startVel(0,0,0);//-10000);
58
59 CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
60 :m_cci(ci)
61 {
62         m_prototypeTransformInitialized = false;
63         m_softbodyMappingDone = false;
64         m_collisionDelay = 0;
65         m_newClientInfo = 0;
66         m_registerCount = 0;
67         m_softBodyTransformInitialized = false;
68         m_parentCtrl = 0;
69         // copy pointers locally to allow smart release
70         m_MotionState = ci.m_MotionState;
71         m_collisionShape = ci.m_collisionShape;
72         // apply scaling before creating rigid body
73         m_collisionShape->setLocalScaling(m_cci.m_scaling);
74         if (m_cci.m_mass)
75                 m_collisionShape->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
76         // shape info is shared, increment ref count
77         m_shapeInfo = ci.m_shapeInfo;
78         if (m_shapeInfo)
79                 m_shapeInfo->AddRef();
80         
81         m_bulletMotionState = 0;
82         
83         
84         CreateRigidbody();
85         
86
87 ///???
88 /*#ifdef WIN32
89         if (GetRigidBody() && !GetRigidBody()->isStaticObject())
90                 GetRigidBody()->setLinearVelocity(startVel);
91 #endif*/
92
93 }
94
95 btTransform&    CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
96 {
97         static btTransform trans;
98         btVector3 tmp;
99         motionState->getWorldPosition(tmp.m_floats[0], tmp.m_floats[1], tmp.m_floats[2]);
100         trans.setOrigin(tmp);
101
102         float ori[12];
103         motionState->getWorldOrientation(ori);
104         trans.getBasis().setFromOpenGLSubMatrix(ori);
105         //btQuaternion orn;
106         //motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
107         //trans.setRotation(orn);
108         return trans;
109
110 }
111
112 class   BlenderBulletMotionState : public btMotionState
113 {
114         PHY_IMotionState*       m_blenderMotionState;
115
116 public:
117
118         BlenderBulletMotionState(PHY_IMotionState* bms)
119                 :m_blenderMotionState(bms)
120         {
121
122         }
123
124         void    getWorldTransform(btTransform& worldTrans ) const
125         {
126                 btVector3 pos;
127                 float ori[12];
128
129                 m_blenderMotionState->getWorldPosition(pos.m_floats[0],pos.m_floats[1],pos.m_floats[2]);
130                 m_blenderMotionState->getWorldOrientation(ori);
131                 worldTrans.setOrigin(pos);
132                 worldTrans.getBasis().setFromOpenGLSubMatrix(ori);
133         }
134
135         void    setWorldTransform(const btTransform& worldTrans)
136         {
137                 m_blenderMotionState->setWorldPosition(worldTrans.getOrigin().getX(),worldTrans.getOrigin().getY(),worldTrans.getOrigin().getZ());
138                 btQuaternion rotQuat = worldTrans.getRotation();
139                 m_blenderMotionState->setWorldOrientation(rotQuat[0],rotQuat[1],rotQuat[2],rotQuat[3]);
140                 m_blenderMotionState->calculateWorldTransformations();
141         }
142
143 };
144
145
146 btRigidBody* CcdPhysicsController::GetRigidBody()
147 {
148         return btRigidBody::upcast(m_object);
149 }
150 btCollisionObject*      CcdPhysicsController::GetCollisionObject()
151 {
152         return m_object;
153 }
154 btSoftBody* CcdPhysicsController::GetSoftBody()
155 {
156         return btSoftBody::upcast(m_object);
157 }
158
159 #include "BulletSoftBody/btSoftBodyHelpers.h"
160
161
162
163 void CcdPhysicsController::CreateRigidbody()
164 {
165
166         //btTransform trans = GetTransformFromMotionState(m_MotionState);
167         m_bulletMotionState = new BlenderBulletMotionState(m_MotionState);
168
169         ///either create a btCollisionObject, btRigidBody or btSoftBody
170
171         //create a collision object
172
173         int shapeType = m_cci.m_collisionShape ? m_cci.m_collisionShape->getShapeType() : 0;
174
175         //disable soft body until first sneak preview is ready
176         if (m_cci.m_bSoft && m_cci.m_collisionShape && 
177                 (shapeType == CONVEX_HULL_SHAPE_PROXYTYPE)|
178                 (shapeType == TRIANGLE_MESH_SHAPE_PROXYTYPE) |
179                 (shapeType == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE))
180         {
181                 btRigidBody::btRigidBodyConstructionInfo rbci(m_cci.m_mass,m_bulletMotionState,m_collisionShape,m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
182                 rbci.m_linearDamping = m_cci.m_linearDamping;
183                 rbci.m_angularDamping = m_cci.m_angularDamping;
184                 rbci.m_friction = m_cci.m_friction;
185                 rbci.m_restitution = m_cci.m_restitution;
186
187                 
188
189                 
190                 
191                 
192                 btVector3 p(0,0,0);// = getOrigin();
193
194                 
195                 btSoftRigidDynamicsWorld* softDynaWorld = (btSoftRigidDynamicsWorld*)m_cci.m_physicsEnv->getDynamicsWorld();
196
197                 PHY__Vector3    grav;
198                 grav[0] = softDynaWorld->getGravity().getX();
199                 grav[1] = softDynaWorld->getGravity().getY();
200                 grav[2] = softDynaWorld->getGravity().getZ();
201                 softDynaWorld->getWorldInfo().m_gravity.setValue(grav[0],grav[1],grav[2]); //??
202
203         
204                 //btSoftBody*   psb=btSoftBodyHelpers::CreateRope(sbi,  btVector3(-10,0,i*0.25),btVector3(10,0,i*0.25), 16,1+2);
205
206                 btSoftBody* psb  = 0;
207
208                 if (m_cci.m_collisionShape->getShapeType() == CONVEX_HULL_SHAPE_PROXYTYPE)
209                 {
210                         btConvexHullShape* convexHull = (btConvexHullShape* )m_cci.m_collisionShape;
211
212                         //psb = btSoftBodyHelpers::CreateFromConvexHull(sbi,&transformedVertices[0],convexHull->getNumPoints());
213
214                         {
215                                 int nvertices = convexHull->getNumPoints();
216                                 const btVector3* vertices = convexHull->getPoints();
217                                 btSoftBodyWorldInfo& worldInfo = softDynaWorld->getWorldInfo();
218
219                                 HullDesc                hdsc(QF_TRIANGLES,nvertices,vertices);
220                                 HullResult              hres;
221                                 HullLibrary             hlib;/*??*/ 
222                                 hdsc.mMaxVertices=nvertices;
223                                 hlib.CreateConvexHull(hdsc,hres);
224                                 
225                                 psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
226                                         &hres.m_OutputVertices[0],0);
227                                 for(int i=0;i<(int)hres.mNumFaces;++i)
228                                 {
229                                         const int idx[]={       hres.m_Indices[i*3+0],
230                                                 hres.m_Indices[i*3+1],
231                                                 hres.m_Indices[i*3+2]};
232                                         if(idx[0]<idx[1]) psb->appendLink(      idx[0],idx[1]);
233                                         if(idx[1]<idx[2]) psb->appendLink(      idx[1],idx[2]);
234                                         if(idx[2]<idx[0]) psb->appendLink(      idx[2],idx[0]);
235                                         psb->appendFace(idx[0],idx[1],idx[2]);
236                                 }
237                                 
238                                 
239
240                                 hlib.ReleaseResult(hres);
241
242                                 
243                         }
244
245
246
247
248
249
250                 } else
251                 {
252                         
253                         btSoftBodyWorldInfo& sbi= softDynaWorld->getWorldInfo();
254
255                         if (m_cci.m_collisionShape->getShapeType() ==SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
256                         {
257                                 btScaledBvhTriangleMeshShape* scaledtrimeshshape = (btScaledBvhTriangleMeshShape*) m_cci.m_collisionShape;
258                                 btBvhTriangleMeshShape* trimeshshape = scaledtrimeshshape->getChildShape();
259
260                                 ///only deal with meshes that have 1 sub part/component, for now
261                                 if (trimeshshape->getMeshInterface()->getNumSubParts()==1)
262                                 {
263                                         unsigned char* vertexBase;
264                                         PHY_ScalarType vertexType;
265                                         int numverts;
266                                         int vertexstride;
267                                         unsigned char* indexbase;
268                                         int indexstride;
269                                         int numtris;
270                                         PHY_ScalarType indexType;
271                                         trimeshshape->getMeshInterface()->getLockedVertexIndexBase(&vertexBase,numverts,vertexType,vertexstride,&indexbase,indexstride,numtris,indexType);
272                                         
273                                         psb = btSoftBodyHelpers::CreateFromTriMesh(sbi,(const btScalar*)vertexBase,(const int*)indexbase,numtris);
274                                 }
275                         } else
276                         {
277                                 btBvhTriangleMeshShape* trimeshshape = (btBvhTriangleMeshShape*) m_cci.m_collisionShape;
278                                 ///only deal with meshes that have 1 sub part/component, for now
279                                 if (trimeshshape->getMeshInterface()->getNumSubParts()==1)
280                                 {
281                                         unsigned char* vertexBase;
282                                         PHY_ScalarType vertexType;
283                                         int numverts;
284                                         int vertexstride;
285                                         unsigned char* indexbase;
286                                         int indexstride;
287                                         int numtris;
288                                         PHY_ScalarType indexType;
289                                         trimeshshape->getMeshInterface()->getLockedVertexIndexBase(&vertexBase,numverts,vertexType,vertexstride,&indexbase,indexstride,numtris,indexType);
290                                         
291                                         psb = btSoftBodyHelpers::CreateFromTriMesh(sbi,(const btScalar*)vertexBase,(const int*)indexbase,numtris);
292                                 }
293                         
294
295                                 //psb = btSoftBodyHelpers::CreateFromTriMesh(sbi,&pts[0].getX(),triangles,numtriangles);
296                         }
297
298                 }
299                 if (m_cci.m_margin > 0.f)
300                 {
301                         psb->getCollisionShape()->setMargin(m_cci.m_margin);
302                         psb->updateBounds();
303                 }
304         
305                 
306                 m_object = psb;
307
308                 //psb->m_cfg.collisions =       btSoftBody::fCollision::SDF_RS;//btSoftBody::fCollision::CL_SS+ btSoftBody::fCollision::CL_RS;
309                 
310                 //psb->m_cfg.collisions =       btSoftBody::fCollision::SDF_RS + btSoftBody::fCollision::VF_SS;//CL_SS;
311                 
312                 
313                 //btSoftBody::Material* pm=psb->appendMaterial();
314                 btSoftBody::Material*   pm=psb->m_materials[0];
315                 pm->m_kLST                              =       m_cci.m_soft_linStiff;
316                 pm->m_kAST                              =       m_cci.m_soft_angStiff;
317                 pm->m_kVST                              =       m_cci.m_soft_volume;
318                 psb->m_cfg.collisions = 0;
319
320                 if (m_cci.m_soft_collisionflags & CCD_BSB_COL_CL_RS)
321                 {
322                         psb->m_cfg.collisions   +=      btSoftBody::fCollision::CL_RS;
323                 } else
324                 {
325                         psb->m_cfg.collisions   +=      btSoftBody::fCollision::SDF_RS;
326                 }
327                 if (m_cci.m_soft_collisionflags & CCD_BSB_COL_CL_SS)
328                 {
329                         psb->m_cfg.collisions += btSoftBody::fCollision::CL_SS;
330                 } else
331                 {
332                         psb->m_cfg.collisions += btSoftBody::fCollision::VF_SS;
333                 }
334
335
336                 psb->m_cfg.kSRHR_CL = m_cci.m_soft_kSRHR_CL;            /* Soft vs rigid hardness [0,1] (cluster only) */
337                 psb->m_cfg.kSKHR_CL = m_cci.m_soft_kSKHR_CL;            /* Soft vs kinetic hardness [0,1] (cluster only) */
338                 psb->m_cfg.kSSHR_CL = m_cci.m_soft_kSSHR_CL;            /* Soft vs soft hardness [0,1] (cluster only) */
339                 psb->m_cfg.kSR_SPLT_CL = m_cci.m_soft_kSR_SPLT_CL;      /* Soft vs rigid impulse split [0,1] (cluster only) */
340
341                 psb->m_cfg.kSK_SPLT_CL = m_cci.m_soft_kSK_SPLT_CL;      /* Soft vs rigid impulse split [0,1] (cluster only) */
342                 psb->m_cfg.kSS_SPLT_CL = m_cci.m_soft_kSS_SPLT_CL;      /* Soft vs rigid impulse split [0,1] (cluster only) */
343                 psb->m_cfg.kVCF = m_cci.m_soft_kVCF;                    /* Velocities correction factor (Baumgarte) */
344                 psb->m_cfg.kDP = m_cci.m_soft_kDP;                      /* Damping coefficient [0,1] */
345
346                 psb->m_cfg.kDG = m_cci.m_soft_kDG;                      /* Drag coefficient [0,+inf] */
347                 psb->m_cfg.kLF = m_cci.m_soft_kLF;                      /* Lift coefficient [0,+inf] */
348                 psb->m_cfg.kPR = m_cci.m_soft_kPR;                      /* Pressure coefficient [-inf,+inf] */
349                 psb->m_cfg.kVC = m_cci.m_soft_kVC;                      /* Volume conversation coefficient [0,+inf] */
350
351                 psb->m_cfg.kDF = m_cci.m_soft_kDF;                      /* Dynamic friction coefficient [0,1] */
352                 psb->m_cfg.kMT = m_cci.m_soft_kMT;                      /* Pose matching coefficient [0,1] */
353                 psb->m_cfg.kCHR = m_cci.m_soft_kCHR;                    /* Rigid contacts hardness [0,1] */
354                 psb->m_cfg.kKHR = m_cci.m_soft_kKHR;                    /* Kinetic contacts hardness [0,1] */
355
356                 psb->m_cfg.kSHR = m_cci.m_soft_kSHR;                    /* Soft contacts hardness [0,1] */
357                 psb->m_cfg.kAHR = m_cci.m_soft_kAHR;                    /* Anchors hardness [0,1] */
358
359
360
361                 if (m_cci.m_gamesoftFlag & CCD_BSB_BENDING_CONSTRAINTS)//OB_SB_GOAL)
362                 {
363                         psb->generateBendingConstraints(2,pm);
364                 }
365
366                 psb->m_cfg.piterations = m_cci.m_soft_piterations;
367                 psb->m_cfg.viterations = m_cci.m_soft_viterations;
368                 psb->m_cfg.diterations = m_cci.m_soft_diterations;
369                 psb->m_cfg.citerations = m_cci.m_soft_citerations;
370
371                 if (m_cci.m_gamesoftFlag & CCD_BSB_SHAPE_MATCHING)//OB_SB_GOAL)
372                 {
373                         psb->setPose(false,true);//
374                 } else
375                 {
376                         psb->setPose(true,false);
377                 }
378
379
380                 
381                 psb->randomizeConstraints();
382
383                 if (m_cci.m_soft_collisionflags & (CCD_BSB_COL_CL_RS+CCD_BSB_COL_CL_SS))
384                 {
385                         psb->generateClusters(m_cci.m_soft_numclusteriterations);
386                 }
387
388 //              psb->activate();
389 //              psb->setActivationState(1);
390 //              psb->setDeactivationTime(1.f);
391                 
392                 //psb->m_materials[0]->m_kLST   =       0.1+(i/(btScalar)(n-1))*0.9;
393                 psb->setTotalMass(m_cci.m_mass);
394                 
395                 psb->setCollisionFlags(0);
396
397                 ///create a mapping between graphics mesh vertices and soft body vertices
398                 {
399                         RAS_MeshObject* rasMesh= GetShapeInfo()->GetMesh();
400
401                         if (rasMesh && !m_softbodyMappingDone)
402                         {
403                                 
404                                 //printf("apply\n");
405                                 RAS_MeshSlot::iterator it;
406                                 RAS_MeshMaterial *mmat;
407                                 RAS_MeshSlot *slot;
408                                 size_t i;
409
410                                 //for each material
411                                 for (int m=0;m<rasMesh->NumMaterials();m++)
412                                 {
413                                         // The vertex cache can only be updated for this deformer:
414                                         // Duplicated objects with more than one ploymaterial (=multiple mesh slot per object)
415                                         // share the same mesh (=the same cache). As the rendering is done per polymaterial
416                                         // cycling through the objects, the entire mesh cache cannot be updated in one shot.
417                                         mmat = rasMesh->GetMeshMaterial(m);
418
419                                         slot = mmat->m_baseslot;
420                                         for(slot->begin(it); !slot->end(it); slot->next(it))
421                                         {
422                                                 int index = 0;
423                                                 for(i=it.startvertex; i<it.endvertex; i++,index++) 
424                                                 {
425                                                         RAS_TexVert* vertex = &it.vertex[i];
426                                                         
427
428                                                         //search closest index, and store it in vertex
429                                                         vertex->setSoftBodyIndex(0);
430                                                         btScalar maxDistSqr = 1e30;
431                                                         btSoftBody::tNodeArray&   nodes(psb->m_nodes);
432                                                         btVector3 xyz = btVector3(vertex->getXYZ()[0],vertex->getXYZ()[1],vertex->getXYZ()[2]);
433                                                         for (int n=0;n<nodes.size();n++)
434                                                         {
435                                                                 btScalar distSqr = (nodes[n].m_x - xyz).length2();
436                                                                 if (distSqr<maxDistSqr)
437                                                                 {
438                                                                         maxDistSqr = distSqr;
439                                                                         
440                                                                         vertex->setSoftBodyIndex(n);
441                                                                 }
442                                                         }
443                                                 }
444                                         }
445                                 }
446                         }
447                 }
448                 
449                 m_softbodyMappingDone = true;
450
451
452
453
454
455
456 //              m_object->setCollisionShape(rbci.m_collisionShape);
457                 btTransform startTrans;
458
459                 if (rbci.m_motionState)
460                 {
461                         rbci.m_motionState->getWorldTransform(startTrans);
462                 } else
463                 {
464                         startTrans = rbci.m_startWorldTransform;
465                 }
466                 //startTrans.setIdentity();
467
468                 //m_object->setWorldTransform(startTrans);
469                 //m_object->setInterpolationWorldTransform(startTrans);
470                 m_MotionState->setWorldPosition(startTrans.getOrigin().getX(),startTrans.getOrigin().getY(),startTrans.getOrigin().getZ());
471                 m_MotionState->setWorldOrientation(0,0,0,1);
472
473                 if (!m_prototypeTransformInitialized)
474                 {
475                         m_prototypeTransformInitialized = true;
476                         m_softBodyTransformInitialized = true;
477                         GetSoftBody()->transform(startTrans);
478                 }
479
480 //              btVector3 wp = m_softBody->getWorldTransform().getOrigin();
481 //              MT_Point3 center(wp.getX(),wp.getY(),wp.getZ());
482 //              m_gameobj->NodeSetWorldPosition(center);
483
484
485         } else
486         {
487                 btRigidBody::btRigidBodyConstructionInfo rbci(m_cci.m_mass,m_bulletMotionState,m_collisionShape,m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
488                 rbci.m_linearDamping = m_cci.m_linearDamping;
489                 rbci.m_angularDamping = m_cci.m_angularDamping;
490                 rbci.m_friction = m_cci.m_friction;
491                 rbci.m_restitution = m_cci.m_restitution;
492                 m_object = new btRigidBody(rbci);
493         }
494         
495         //
496         // init the rigidbody properly
497         //
498         
499         //setMassProps this also sets collisionFlags
500         //convert collision flags!
501         //special case: a near/radar sensor controller should not be defined static or it will
502         //generate loads of static-static collision messages on the console
503         if (m_cci.m_bSensor)
504         {
505                 // reset the flags that have been set so far
506                 GetCollisionObject()->setCollisionFlags(0);
507                 // sensor must never go to sleep: they need to detect continously
508                 GetCollisionObject()->setActivationState(DISABLE_DEACTIVATION);
509         }
510         GetCollisionObject()->setCollisionFlags(m_object->getCollisionFlags() | m_cci.m_collisionFlags);
511         btRigidBody* body = GetRigidBody();
512
513         if (body)
514         {
515                 body->setGravity( m_cci.m_gravity);
516                 body->setDamping(m_cci.m_linearDamping, m_cci.m_angularDamping);
517
518                 if (!m_cci.m_bRigid)
519                 {
520                         body->setAngularFactor(0.f);
521                 }
522                 body->setContactProcessingThreshold(m_cci.m_contactProcessingThreshold);
523
524         }
525         if (m_object && m_cci.m_do_anisotropic)
526         {
527                 m_object->setAnisotropicFriction(m_cci.m_anisotropicFriction);
528         }
529                 
530 }
531
532 static void DeleteBulletShape(btCollisionShape* shape)
533 {
534         if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
535         {
536                 // shapes based on meshes use an interface that contains the vertices.
537                 btTriangleMeshShape* meshShape = static_cast<btTriangleMeshShape*>(shape);
538                 btStridingMeshInterface* meshInterface = meshShape->getMeshInterface();
539                 if (meshInterface)
540                         delete meshInterface;
541         }
542         delete shape;
543 }
544
545 CcdPhysicsController::~CcdPhysicsController()
546 {
547         //will be reference counted, due to sharing
548         if (m_cci.m_physicsEnv)
549                 m_cci.m_physicsEnv->removeCcdPhysicsController(this);
550
551         if (m_MotionState)
552                 delete m_MotionState;
553         if (m_bulletMotionState)
554                 delete m_bulletMotionState;
555         delete m_object;
556
557         if (m_collisionShape)
558         {
559                 // collision shape is always unique to the controller, can delete it here
560                 if (m_collisionShape->isCompound())
561                 {
562                         // bullet does not delete the child shape, must do it here
563                         btCompoundShape* compoundShape = (btCompoundShape*)m_collisionShape;
564                         int numChild = compoundShape->getNumChildShapes();
565                         for (int i=numChild-1 ; i >= 0; i--)
566                         {
567                                 btCollisionShape* childShape = compoundShape->getChildShape(i);
568                                 DeleteBulletShape(childShape);
569                         }
570                 }
571                 DeleteBulletShape(m_collisionShape);
572         }
573         if (m_shapeInfo)
574         {
575                 m_shapeInfo->Release();
576         }
577 }
578
579
580                 /**
581                         SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
582                 */
583 bool            CcdPhysicsController::SynchronizeMotionStates(float time)
584 {
585         //sync non-static to motionstate, and static from motionstate (todo: add kinematic etc.)
586
587         btSoftBody* sb = GetSoftBody();
588         if (sb)
589         {
590                 if (sb->m_pose.m_bframe) 
591                 {
592                         btVector3 worldPos = sb->m_pose.m_com;
593                         btQuaternion worldquat;
594                         btMatrix3x3     trs = sb->m_pose.m_rot*sb->m_pose.m_scl;
595                         trs.getRotation(worldquat);
596                         m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
597                         m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
598                 }
599                 else 
600                 {
601                         btVector3 aabbMin,aabbMax;
602                         sb->getAabb(aabbMin,aabbMax);
603                         btVector3 worldPos  = (aabbMax+aabbMin)*0.5f;
604                         m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
605                 }
606                 m_MotionState->calculateWorldTransformations();
607                 return true;
608         }
609
610         btRigidBody* body = GetRigidBody();
611
612         if (body && !body->isStaticObject())
613         {
614                 
615                 if ((m_cci.m_clamp_vel_max>0.0) || (m_cci.m_clamp_vel_min>0.0))
616                 {
617                         const btVector3& linvel = body->getLinearVelocity();
618                         float len= linvel.length();
619                         
620                         if((m_cci.m_clamp_vel_max>0.0) && (len > m_cci.m_clamp_vel_max))
621                                         body->setLinearVelocity(linvel * (m_cci.m_clamp_vel_max / len));
622                         
623                         else if ((m_cci.m_clamp_vel_min>0.0) && btFuzzyZero(len)==0 && (len < m_cci.m_clamp_vel_min))
624                                 body->setLinearVelocity(linvel * (m_cci.m_clamp_vel_min / len));
625                 }
626                 
627                 const btTransform& xform = body->getCenterOfMassTransform();
628                 const btMatrix3x3& worldOri = xform.getBasis();
629                 const btVector3& worldPos = xform.getOrigin();
630                 float ori[12];
631                 worldOri.getOpenGLSubMatrix(ori);
632                 m_MotionState->setWorldOrientation(ori);
633                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
634                 m_MotionState->calculateWorldTransformations();
635
636                 float scale[3];
637                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
638                 btVector3 scaling(scale[0],scale[1],scale[2]);
639                 GetCollisionShape()->setLocalScaling(scaling);
640         } else
641         {
642                 btVector3 worldPos;
643                 btQuaternion worldquat;
644
645 /*              m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
646                 m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
647                 btTransform oldTrans = m_body->getCenterOfMassTransform();
648                 btTransform newTrans(worldquat,worldPos);
649                                 
650                 SetCenterOfMassTransform(newTrans);
651                 //need to keep track of previous position for friction effects...
652                 
653                 m_MotionState->calculateWorldTransformations();
654 */
655                 float scale[3];
656                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
657                 btVector3 scaling(scale[0],scale[1],scale[2]);
658                 GetCollisionShape()->setLocalScaling(scaling);
659         }
660         return true;
661
662 }
663
664                 /**
665                         WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
666                 */
667                 
668 void            CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
669 {
670         btTransform& xform = CcdPhysicsController::GetTransformFromMotionState(m_MotionState);
671         SetCenterOfMassTransform(xform);
672 }
673
674 void            CcdPhysicsController::WriteDynamicsToMotionState()
675 {
676 }
677                 // controller replication
678 void            CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
679 {
680         
681         m_softBodyTransformInitialized=false;
682         m_MotionState = motionstate;
683         m_registerCount = 0;
684         m_collisionShape = NULL;
685
686         // always create a new shape to avoid scaling bug
687         if (m_shapeInfo)
688         {
689                 m_shapeInfo->AddRef();
690                 m_collisionShape = m_shapeInfo->CreateBulletShape(m_cci.m_margin);
691
692                 if (m_collisionShape)
693                 {
694                         // new shape has no scaling, apply initial scaling
695                         //m_collisionShape->setMargin(m_cci.m_margin);
696                         m_collisionShape->setLocalScaling(m_cci.m_scaling);
697                         
698                         if (m_cci.m_mass)
699                                 m_collisionShape->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
700                 }
701         }
702
703         // load some characterists that are not 
704         btRigidBody* oldbody = GetRigidBody();
705         m_object = 0;
706         CreateRigidbody();
707         btRigidBody* body = GetRigidBody();
708         if (body)
709         {
710                 if (m_cci.m_mass)
711                 {
712                         body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
713                 }
714
715                 if (oldbody)
716                 {
717                         body->setLinearFactor(oldbody->getLinearFactor());
718                         body->setAngularFactor(oldbody->getAngularFactor());
719                         if (oldbody->getActivationState() == DISABLE_DEACTIVATION)
720                                 body->setActivationState(DISABLE_DEACTIVATION);
721                 }
722         }       
723         // sensor object are added when needed
724         if (!m_cci.m_bSensor)
725                 m_cci.m_physicsEnv->addCcdPhysicsController(this);
726
727
728 /*      SM_Object* dynaparent=0;
729         SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
730         
731         if (sumoparentctrl)
732         {
733                 dynaparent = sumoparentctrl->GetSumoObject();
734         }
735         
736         SM_Object* orgsumoobject = m_sumoObj;
737         
738         
739         m_sumoObj       =       new SM_Object(
740                 orgsumoobject->getShapeHandle(), 
741                 orgsumoobject->getMaterialProps(),                      
742                 orgsumoobject->getShapeProps(),
743                 dynaparent);
744         
745         m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
746         
747         m_sumoObj->setMargin(orgsumoobject->getMargin());
748         m_sumoObj->setPosition(orgsumoobject->getPosition());
749         m_sumoObj->setOrientation(orgsumoobject->getOrientation());
750         //if it is a dyna, register for a callback
751         m_sumoObj->registerCallback(*this);
752         
753         m_sumoScene->add(* (m_sumoObj));
754         */
755
756
757
758 }
759
760
761 void    CcdPhysicsController::SetCenterOfMassTransform(btTransform& xform)
762 {
763         btRigidBody* body = GetRigidBody();
764         if (body)
765         {
766                 body->setCenterOfMassTransform(xform);
767         } else
768         {
769                 //either collision object or soft body?
770                 if (GetSoftBody())
771                 {
772
773                 } else
774                 {
775
776                         if (m_object->isStaticOrKinematicObject())
777                         {
778                                 m_object->setInterpolationWorldTransform(m_object->getWorldTransform());
779                         } else
780                         {
781                                 m_object->setInterpolationWorldTransform(xform);
782                         }
783                         if (body)
784                         {
785                                 body->setInterpolationLinearVelocity(body->getLinearVelocity());
786                                 body->setInterpolationAngularVelocity(body->getAngularVelocity());
787                                 body->updateInertiaTensor();
788                         }
789                         m_object->setWorldTransform(xform);
790                 }
791         }
792 }
793
794                 // kinematic methods
795 void            CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
796 {
797         if (m_object)
798         {
799                 m_object->activate(true);
800                 if (m_object->isStaticObject())
801                 {
802                         if (!m_cci.m_bSensor)
803                                 m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
804                         // kinematic object should not set the transform, it disturbs the velocity interpolation
805                         return;
806                 }
807
808                 // btRigidBody* body = GetRigidBody(); // not used anymore
809
810                 btVector3 dloc(dlocX,dlocY,dlocZ);
811                 btTransform xform = m_object->getWorldTransform();
812         
813                 if (local)
814                 {
815                         dloc = xform.getBasis()*dloc;
816                 }
817
818                 xform.setOrigin(xform.getOrigin() + dloc);
819                 SetCenterOfMassTransform(xform);
820         }
821
822 }
823
824 void            CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
825 {
826         if (m_object)
827         {
828                 m_object->activate(true);
829                 if (m_object->isStaticObject())
830                 {
831                         if (!m_cci.m_bSensor)
832                                 m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
833                         // kinematic object should not set the transform, it disturbs the velocity interpolation
834                         return;
835                 }
836
837                 btMatrix3x3 drotmat(    rotval[0],rotval[4],rotval[8],
838                                                                 rotval[1],rotval[5],rotval[9],
839                                                                 rotval[2],rotval[6],rotval[10]);
840
841
842                 btMatrix3x3 currentOrn;
843                 GetWorldOrientation(currentOrn);
844
845                 btTransform xform = m_object->getWorldTransform();
846                 
847                 xform.setBasis(xform.getBasis()*(local ? 
848                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
849
850                 SetCenterOfMassTransform(xform);
851         }
852 }
853
854
855 void CcdPhysicsController::GetWorldOrientation(btMatrix3x3& mat)
856 {
857         float ori[12];
858         m_MotionState->getWorldOrientation(ori);
859         mat.setFromOpenGLSubMatrix(ori);
860 }
861
862 void            CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
863 {
864         btQuaternion q = m_object->getWorldTransform().getRotation();
865         quatImag0 = q[0];
866         quatImag1 = q[1];
867         quatImag2 = q[2];
868         quatReal = q[3];
869 }
870 void            CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
871 {
872         if (m_object)
873         {
874                 m_object->activate(true);
875                 if (m_object->isStaticObject())
876                 {
877                         if (!m_cci.m_bSensor)
878                                 m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
879                         // kinematic object should not set the transform, it disturbs the velocity interpolation
880                         return;
881                 }
882                 // not required
883                 //m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
884                 btTransform xform  = m_object->getWorldTransform();
885                 xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
886                 SetCenterOfMassTransform(xform);
887                 // not required
888                 //m_bulletMotionState->setWorldTransform(xform);
889                 
890                 
891
892         }
893
894 }
895
896 void CcdPhysicsController::setWorldOrientation(const btMatrix3x3& orn)
897 {
898         if (m_object)
899         {
900                 m_object->activate(true);
901                 if (m_object->isStaticObject() && !m_cci.m_bSensor)
902                 {
903                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
904                 }
905                 // not required
906                 //m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
907                 btTransform xform  = m_object->getWorldTransform();
908                 xform.setBasis(orn);
909                 SetCenterOfMassTransform(xform);
910                 // not required
911                 //m_bulletMotionState->setWorldTransform(xform);
912                 //only once!
913                 if (!m_softBodyTransformInitialized && GetSoftBody())
914                 {
915                         m_softbodyStartTrans.setBasis(orn);
916                         xform.setOrigin(m_softbodyStartTrans.getOrigin());
917                         GetSoftBody()->transform(xform);
918                         m_softBodyTransformInitialized = true;
919                 }
920
921         }
922
923 }
924
925 void            CcdPhysicsController::setPosition(float posX,float posY,float posZ)
926 {
927         if (m_object)
928         {
929                 m_object->activate(true);
930                 if (m_object->isStaticObject())
931                 {
932                         if (!m_cci.m_bSensor)
933                                 m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
934                         // kinematic object should not set the transform, it disturbs the velocity interpolation
935                         return;
936                 }
937                 // not required, this function is only used to update the physic controller
938                 //m_MotionState->setWorldPosition(posX,posY,posZ);
939                 btTransform xform  = m_object->getWorldTransform();
940                 xform.setOrigin(btVector3(posX,posY,posZ));
941                 SetCenterOfMassTransform(xform);
942                 if (!m_softBodyTransformInitialized)
943                         m_softbodyStartTrans.setOrigin(xform.getOrigin());
944                 // not required
945                 //m_bulletMotionState->setWorldTransform(xform);
946         }
947 }
948
949 void CcdPhysicsController::forceWorldTransform(const btMatrix3x3& mat, const btVector3& pos)
950 {
951         if (m_object)
952         {
953                 btTransform& xform = m_object->getWorldTransform();
954                 xform.setBasis(mat);
955                 xform.setOrigin(pos);
956         }
957 }
958
959
960 void            CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
961 {
962 }
963
964 void            CcdPhysicsController::getPosition(PHY__Vector3& pos) const
965 {
966         const btTransform& xform = m_object->getWorldTransform();
967         pos[0] = xform.getOrigin().x();
968         pos[1] = xform.getOrigin().y();
969         pos[2] = xform.getOrigin().z();
970 }
971
972 void            CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
973 {
974         if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
975                 !btFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
976                 !btFuzzyZero(m_cci.m_scaling.z()-scaleZ))
977         {
978                 m_cci.m_scaling = btVector3(scaleX,scaleY,scaleZ);
979
980                 if (m_object && m_object->getCollisionShape())
981                 {
982                         m_object->activate(true); // without this, sleeping objects scale wont be applied in bullet if python changes the scale - Campbell.
983                         m_object->getCollisionShape()->setLocalScaling(m_cci.m_scaling);
984                         
985                         //printf("no inertia recalc for fixed objects with mass=0\n");
986                         btRigidBody* body = GetRigidBody();
987                         if (body && m_cci.m_mass)
988                         {
989                                 body->getCollisionShape()->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
990                                 body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
991                         } 
992                         
993                 }
994         }
995 }
996                 
997                 // physics methods
998 void            CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
999 {
1000         btVector3 torque(torqueX,torqueY,torqueZ);
1001         btTransform xform = m_object->getWorldTransform();
1002         
1003
1004         if (m_object && torque.length2() > (SIMD_EPSILON*SIMD_EPSILON))
1005         {
1006                 btRigidBody* body = GetRigidBody();
1007                 m_object->activate();
1008                 if (m_object->isStaticObject())
1009                 {
1010                         if (!m_cci.m_bSensor)
1011                                 m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
1012                         return;
1013                 }
1014                 if (local)
1015                 {
1016                         torque  = xform.getBasis()*torque;
1017                 }
1018                 if (body)
1019                 {
1020                         if      (m_cci.m_bRigid)
1021                         {
1022                                 body->applyTorque(torque);
1023                         }
1024                         else
1025                         {
1026                                 //workaround for incompatibility between 'DYNAMIC' game object, and angular factor
1027                                 //a DYNAMIC object has some inconsistency: it has no angular effect due to collisions, but still has torque
1028                                 const btVector3& angFac = body->getAngularFactor();
1029                                 btVector3 tmpFac(0,0,1);
1030                                 body->setAngularFactor(tmpFac);
1031                                 body->applyTorque(torque);
1032                                 body->setAngularFactor(angFac);
1033                         } 
1034                 } 
1035         }
1036 }
1037
1038 void            CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
1039 {
1040         btVector3 force(forceX,forceY,forceZ);
1041         
1042
1043         if (m_object && force.length2() > (SIMD_EPSILON*SIMD_EPSILON))
1044         {
1045                 m_object->activate();
1046                 if (m_object->isStaticObject())
1047                 {
1048                         if (!m_cci.m_bSensor)
1049                                 m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
1050                         return;
1051                 }
1052                 btTransform xform = m_object->getWorldTransform();
1053                 
1054                 if (local)
1055                 {       
1056                         force   = xform.getBasis()*force;
1057                 }
1058                 btRigidBody* body = GetRigidBody();
1059                 if (body)
1060                         body->applyCentralForce(force);
1061                 btSoftBody* soft = GetSoftBody();
1062                 if (soft)
1063                 {
1064                         // the force is applied on each node, must reduce it in the same extend
1065                         if (soft->m_nodes.size() > 0)
1066                                 force /= soft->m_nodes.size();
1067                         soft->addForce(force);
1068                 }
1069         }
1070 }
1071 void            CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
1072 {
1073         btVector3 angvel(ang_velX,ang_velY,ang_velZ);
1074         if (m_object && angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
1075         {
1076                 m_object->activate(true);
1077                 if (m_object->isStaticObject())
1078                 {
1079                         if (!m_cci.m_bSensor)
1080                                 m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
1081                         return;
1082                 }
1083                 btTransform xform = m_object->getWorldTransform();
1084                 if (local)
1085                 {
1086                         angvel  = xform.getBasis()*angvel;
1087                 }
1088                 btRigidBody* body = GetRigidBody();
1089                 if (body)
1090                         body->setAngularVelocity(angvel);
1091         }
1092
1093 }
1094 void            CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
1095 {
1096
1097         btVector3 linVel(lin_velX,lin_velY,lin_velZ);
1098         if (m_object/* && linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON)*/)
1099         {
1100                 m_object->activate(true);
1101                 if (m_object->isStaticObject())
1102                 {
1103                         if (!m_cci.m_bSensor)
1104                                 m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
1105                         return;
1106                 }
1107                 
1108                 btSoftBody* soft = GetSoftBody();
1109                 if (soft)
1110                 {
1111                         if (local)
1112                         {
1113                                 linVel  = m_softbodyStartTrans.getBasis()*linVel;
1114                         }
1115                         soft->setVelocity(linVel);
1116                 } else
1117                 {
1118                         btTransform xform = m_object->getWorldTransform();
1119                         if (local)
1120                         {
1121                                 linVel  = xform.getBasis()*linVel;
1122                         }
1123                         btRigidBody* body = GetRigidBody();
1124                         if (body)
1125                                 body->setLinearVelocity(linVel);
1126                 }
1127         }
1128 }
1129 void            CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
1130 {
1131         btVector3 impulse(impulseX,impulseY,impulseZ);
1132
1133         if (m_object && impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
1134         {
1135                 m_object->activate();
1136                 if (m_object->isStaticObject())
1137                 {
1138                         if (!m_cci.m_bSensor)
1139                                 m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
1140                         return;
1141                 }
1142                 
1143                 btVector3 pos(attachX,attachY,attachZ);
1144                 btRigidBody* body = GetRigidBody();
1145                 if (body)
1146                         body->applyImpulse(impulse,pos);
1147                         
1148         }
1149
1150 }
1151 void            CcdPhysicsController::SetActive(bool active)
1152 {
1153 }
1154                 // reading out information from physics
1155 void            CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
1156 {
1157         btRigidBody* body = GetRigidBody();
1158         if (body)
1159         {
1160                 const btVector3& linvel = body->getLinearVelocity();
1161                 linvX = linvel.x();
1162                 linvY = linvel.y();
1163                 linvZ = linvel.z();
1164         } else
1165         {
1166                 linvX = 0.f;
1167                 linvY = 0.f;
1168                 linvZ = 0.f;
1169         }
1170
1171 }
1172
1173 void            CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
1174 {
1175         btRigidBody* body = GetRigidBody();
1176         if (body)
1177         {
1178                 const btVector3& angvel= body->getAngularVelocity();
1179                 angVelX = angvel.x();
1180                 angVelY = angvel.y();
1181                 angVelZ = angvel.z();
1182         } else
1183         {
1184                 angVelX = 0.f;
1185                 angVelY = 0.f;
1186                 angVelZ = 0.f;
1187         }
1188 }
1189
1190 void            CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
1191 {
1192         btVector3 pos(posX,posY,posZ);
1193         btRigidBody* body = GetRigidBody();
1194         if (body)
1195         {
1196                 btVector3 linvel = body->getVelocityInLocalPoint(pos);
1197                 linvX = linvel.x();
1198                 linvY = linvel.y();
1199                 linvZ = linvel.z();
1200         } else
1201         {
1202                 linvX = 0.f;
1203                 linvY = 0.f;
1204                 linvZ = 0.f;
1205         }
1206 }
1207 void            CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
1208 {
1209 }
1210
1211                 // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
1212 void            CcdPhysicsController::setRigidBody(bool rigid)
1213 {
1214         if (!rigid)
1215         {
1216                 btRigidBody* body = GetRigidBody();
1217                 if (body)
1218                 {
1219                         //fake it for now
1220                         btVector3 inertia = body->getInvInertiaDiagLocal();
1221                         inertia[1] = 0.f;
1222                         body->setInvInertiaDiagLocal(inertia);
1223                         body->updateInertiaTensor();
1224                 }
1225         }
1226 }
1227
1228                 // clientinfo for raycasts for example
1229 void*           CcdPhysicsController::getNewClientInfo()
1230 {
1231         return m_newClientInfo;
1232 }
1233 void            CcdPhysicsController::setNewClientInfo(void* clientinfo)
1234 {
1235         m_newClientInfo = clientinfo;
1236 }
1237
1238
1239 void    CcdPhysicsController::UpdateDeactivation(float timeStep)
1240 {
1241         btRigidBody* body = GetRigidBody();
1242         if (body)
1243         {
1244                 body->updateDeactivation( timeStep);
1245         }
1246 }
1247
1248 bool CcdPhysicsController::wantsSleeping()
1249 {
1250         btRigidBody* body = GetRigidBody();
1251         if (body)
1252         {
1253                 return body->wantsSleeping();
1254         }
1255         //check it out
1256         return true;
1257 }
1258
1259 PHY_IPhysicsController* CcdPhysicsController::GetReplica()
1260 {
1261         // This is used only to replicate Near and Radar sensor controllers
1262         // The replication of object physics controller is done in KX_BulletPhysicsController::GetReplica()
1263         CcdConstructionInfo cinfo = m_cci;
1264         if (m_shapeInfo)
1265         {
1266                 // This situation does not normally happen
1267                 cinfo.m_collisionShape = m_shapeInfo->CreateBulletShape(0.01);
1268         } 
1269         else if (m_collisionShape)
1270         {
1271                 switch (m_collisionShape->getShapeType())
1272                 {
1273                 case SPHERE_SHAPE_PROXYTYPE:
1274                         {
1275                                 btSphereShape* orgShape = (btSphereShape*)m_collisionShape;
1276                                 cinfo.m_collisionShape = new btSphereShape(*orgShape);
1277                                 break;
1278                         }
1279
1280                 case CONE_SHAPE_PROXYTYPE:
1281                         {
1282                                 btConeShape* orgShape = (btConeShape*)m_collisionShape;
1283                                 cinfo.m_collisionShape = new btConeShape(*orgShape);
1284                                 break;
1285                         }
1286
1287                 default:
1288                         {
1289                                 return 0;
1290                         }
1291                 }
1292         }
1293
1294         cinfo.m_MotionState = new DefaultMotionState();
1295         cinfo.m_shapeInfo = m_shapeInfo;
1296
1297         CcdPhysicsController* replica = new CcdPhysicsController(cinfo);
1298         return replica;
1299 }
1300
1301 ///////////////////////////////////////////////////////////
1302 ///A small utility class, DefaultMotionState
1303 ///
1304 ///////////////////////////////////////////////////////////
1305
1306 DefaultMotionState::DefaultMotionState()
1307 {
1308         m_worldTransform.setIdentity();
1309         m_localScaling.setValue(1.f,1.f,1.f);
1310 }
1311
1312
1313 DefaultMotionState::~DefaultMotionState()
1314 {
1315
1316 }
1317
1318 void    DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
1319 {
1320         posX = m_worldTransform.getOrigin().x();
1321         posY = m_worldTransform.getOrigin().y();
1322         posZ = m_worldTransform.getOrigin().z();
1323 }
1324
1325 void    DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
1326 {
1327         scaleX = m_localScaling.getX();
1328         scaleY = m_localScaling.getY();
1329         scaleZ = m_localScaling.getZ();
1330 }
1331
1332 void    DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
1333 {
1334         btQuaternion quat = m_worldTransform.getRotation();
1335         quatIma0 = quat.x();
1336         quatIma1 = quat.y();
1337         quatIma2 = quat.z();
1338         quatReal = quat[3];
1339 }
1340                 
1341 void    DefaultMotionState::getWorldOrientation(float* ori)
1342 {
1343         m_worldTransform.getBasis().getOpenGLSubMatrix(ori);
1344 }
1345
1346 void    DefaultMotionState::setWorldOrientation(const float* ori)
1347 {
1348         m_worldTransform.getBasis().setFromOpenGLSubMatrix(ori);
1349 }
1350 void    DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
1351 {
1352         btVector3 pos(posX,posY,posZ);
1353         m_worldTransform.setOrigin( pos );
1354 }
1355
1356 void    DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
1357 {
1358         btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
1359         m_worldTransform.setRotation( orn );
1360 }
1361                 
1362 void    DefaultMotionState::calculateWorldTransformations()
1363 {
1364
1365 }
1366
1367 // Shape constructor
1368 std::map<RAS_MeshObject*, CcdShapeConstructionInfo*> CcdShapeConstructionInfo::m_meshShapeMap;
1369
1370 CcdShapeConstructionInfo* CcdShapeConstructionInfo::FindMesh(RAS_MeshObject* mesh, struct DerivedMesh* dm, bool polytope, bool gimpact)
1371 {
1372         if (polytope || dm || gimpact)
1373                 // not yet supported
1374                 return NULL;
1375
1376         std::map<RAS_MeshObject*,CcdShapeConstructionInfo*>::const_iterator mit = m_meshShapeMap.find(mesh);
1377         if (mit != m_meshShapeMap.end())
1378                 return mit->second;
1379         return NULL;
1380 }
1381
1382 bool CcdShapeConstructionInfo::SetMesh(RAS_MeshObject* meshobj, DerivedMesh* dm, bool polytope,bool useGimpact)
1383 {
1384         int numpolys, numverts;
1385
1386         m_useGimpact = useGimpact;
1387
1388         // assume no shape information
1389         // no support for dynamic change of shape yet
1390         assert(IsUnused());
1391         m_shapeType = PHY_SHAPE_NONE;
1392         m_meshObject = NULL;
1393         bool free_dm = false;
1394
1395         // No mesh object or mesh has no polys
1396         if (!meshobj || meshobj->HasColliderPolygon()==false) {
1397                 m_vertexArray.clear();
1398                 m_polygonIndexArray.clear();
1399                 m_triFaceArray.clear();
1400                 return false;
1401         }
1402
1403         if (!dm) {
1404                 free_dm = true;
1405                 dm = CDDM_from_mesh(meshobj->GetMesh(), NULL);
1406         }
1407
1408         MVert *mvert = dm->getVertArray(dm);
1409         MFace *mface = dm->getFaceArray(dm);
1410         numpolys = dm->getNumFaces(dm);
1411         numverts = dm->getNumVerts(dm);
1412         int* index = (int*)dm->getFaceDataArray(dm, CD_ORIGINDEX);
1413
1414         m_shapeType = (polytope) ? PHY_SHAPE_POLYTOPE : PHY_SHAPE_MESH;
1415
1416         /* Convert blender geometry into bullet mesh, need these vars for mapping */
1417         vector<bool> vert_tag_array(numverts, false);
1418         unsigned int tot_bt_verts= 0;
1419
1420         if (polytope)
1421         {
1422                 // Tag verts we're using
1423                 for (int p2=0; p2<numpolys; p2++)
1424                 {
1425                         MFace* mf = &mface[p2];
1426                         RAS_Polygon* poly = meshobj->GetPolygon(index[p2]);
1427
1428                         // only add polygons that have the collision flag set
1429                         if (poly->IsCollider())
1430                         {
1431                                 if (vert_tag_array[mf->v1]==false) {vert_tag_array[mf->v1]= true;tot_bt_verts++;}
1432                                 if (vert_tag_array[mf->v2]==false) {vert_tag_array[mf->v2]= true;tot_bt_verts++;}
1433                                 if (vert_tag_array[mf->v3]==false) {vert_tag_array[mf->v3]= true;tot_bt_verts++;}
1434                                 if (mf->v4 && vert_tag_array[mf->v4]==false) {vert_tag_array[mf->v4]= true;tot_bt_verts++;}
1435                         }
1436                 }
1437
1438                 m_vertexArray.resize(tot_bt_verts*3);
1439
1440                 btScalar *bt= &m_vertexArray[0];
1441
1442                 for (int p2=0; p2<numpolys; p2++)
1443                 {
1444                         MFace* mf = &mface[p2];
1445                         RAS_Polygon* poly= meshobj->GetPolygon(index[p2]);
1446
1447                         // only add polygons that have the collisionflag set
1448                         if (poly->IsCollider())
1449                         {
1450                                 if (vert_tag_array[mf->v1]==true)
1451                                 {
1452                                         const float* vtx = mvert[mf->v1].co;
1453                                         vert_tag_array[mf->v1]= false;
1454                                         *bt++ = vtx[0];
1455                                         *bt++ = vtx[1];
1456                                         *bt++ = vtx[2];
1457                                 }
1458                                 if (vert_tag_array[mf->v2]==true)
1459                                 {
1460                                         const float* vtx = mvert[mf->v2].co;
1461                                         vert_tag_array[mf->v2]= false;
1462                                         *bt++ = vtx[0];
1463                                         *bt++ = vtx[1];
1464                                         *bt++ = vtx[2];
1465                                 }
1466                                 if (vert_tag_array[mf->v3]==true)
1467                                 {
1468                                         const float* vtx = mvert[mf->v3].co;
1469                                         vert_tag_array[mf->v3]= false;
1470                                         *bt++ = vtx[0];
1471                                         *bt++ = vtx[1];
1472                                         *bt++ = vtx[2];
1473                                 }
1474                                 if (mf->v4 && vert_tag_array[mf->v4]==true)
1475                                 {
1476                                         const float* vtx = mvert[mf->v4].co;
1477                                         vert_tag_array[mf->v4]= false;
1478                                         *bt++ = vtx[0];
1479                                         *bt++ = vtx[1];
1480                                         *bt++ = vtx[2];
1481                                 }
1482                         }
1483                 }
1484         }
1485         else {
1486                 unsigned int tot_bt_tris= 0;
1487                 vector<int> vert_remap_array(numverts, 0);
1488                 
1489                 // Tag verts we're using
1490                 for (int p2=0; p2<numpolys; p2++)
1491                 {
1492                         MFace* mf = &mface[p2];
1493                         RAS_Polygon* poly= meshobj->GetPolygon(index[p2]);
1494
1495                         // only add polygons that have the collision flag set
1496                         if (poly->IsCollider())
1497                         {
1498                                 if (vert_tag_array[mf->v1]==false)
1499                                         {vert_tag_array[mf->v1]= true;vert_remap_array[mf->v1]= tot_bt_verts;tot_bt_verts++;}
1500                                 if (vert_tag_array[mf->v2]==false)
1501                                         {vert_tag_array[mf->v2]= true;vert_remap_array[mf->v2]= tot_bt_verts;tot_bt_verts++;}
1502                                 if (vert_tag_array[mf->v3]==false)
1503                                         {vert_tag_array[mf->v3]= true;vert_remap_array[mf->v3]= tot_bt_verts;tot_bt_verts++;}
1504                                 if (mf->v4 && vert_tag_array[mf->v4]==false)
1505                                         {vert_tag_array[mf->v4]= true;vert_remap_array[mf->v4]= tot_bt_verts;tot_bt_verts++;}
1506                                 tot_bt_tris += (mf->v4 ? 2:1); /* a quad or a tri */
1507                         }
1508                 }
1509
1510                 m_vertexArray.resize(tot_bt_verts*3);
1511                 m_polygonIndexArray.resize(tot_bt_tris);
1512                 m_triFaceArray.resize(tot_bt_tris*3);
1513
1514                 btScalar *bt= &m_vertexArray[0];
1515                 int *poly_index_pt= &m_polygonIndexArray[0];
1516                 int *tri_pt= &m_triFaceArray[0];
1517
1518                 for (int p2=0; p2<numpolys; p2++)
1519                 {
1520                         MFace* mf = &mface[p2];
1521                         RAS_Polygon* poly= meshobj->GetPolygon(index[p2]);
1522
1523                         // only add polygons that have the collisionflag set
1524                         if (poly->IsCollider())
1525                         {
1526                                 MVert *v1= &mvert[mf->v1];
1527                                 MVert *v2= &mvert[mf->v2];
1528                                 MVert *v3= &mvert[mf->v3];
1529
1530                                 // the face indicies
1531                                 tri_pt[0]= vert_remap_array[mf->v1];
1532                                 tri_pt[1]= vert_remap_array[mf->v2];
1533                                 tri_pt[2]= vert_remap_array[mf->v3];
1534                                 tri_pt= tri_pt+3;
1535
1536                                 // m_polygonIndexArray
1537                                 *poly_index_pt= index[p2];
1538                                 poly_index_pt++;
1539
1540                                 // the vertex location
1541                                 if (vert_tag_array[mf->v1]==true) { /* *** v1 *** */
1542                                         vert_tag_array[mf->v1]= false;
1543                                         *bt++ = v1->co[0];
1544                                         *bt++ = v1->co[1];
1545                                         *bt++ = v1->co[2];
1546                                 }
1547                                 if (vert_tag_array[mf->v2]==true) { /* *** v2 *** */
1548                                         vert_tag_array[mf->v2]= false;
1549                                         *bt++ = v2->co[0];
1550                                         *bt++ = v2->co[1];
1551                                         *bt++ = v2->co[2];
1552                                 }
1553                                 if (vert_tag_array[mf->v3]==true) { /* *** v3 *** */
1554                                         vert_tag_array[mf->v3]= false;
1555                                         *bt++ = v3->co[0];      
1556                                         *bt++ = v3->co[1];
1557                                         *bt++ = v3->co[2];
1558                                 }
1559
1560                                 if (mf->v4)
1561                                 {
1562                                         MVert *v4= &mvert[mf->v4];
1563
1564                                         tri_pt[0]= vert_remap_array[mf->v1];
1565                                         tri_pt[1]= vert_remap_array[mf->v3];
1566                                         tri_pt[2]= vert_remap_array[mf->v4];
1567                                         tri_pt= tri_pt+3;
1568
1569                                         // m_polygonIndexArray
1570                                         *poly_index_pt= index[p2];
1571                                         poly_index_pt++;
1572
1573                                         // the vertex location
1574                                         if (vert_tag_array[mf->v4]==true) { /* *** v4 *** */
1575                                                 vert_tag_array[mf->v4]= false;
1576                                                 *bt++ = v4->co[0];
1577                                                 *bt++ = v4->co[1];      
1578                                                 *bt++ = v4->co[2];
1579                                         }
1580                                 }
1581                         }
1582                 }
1583
1584
1585                 /* If this ever gets confusing, print out an OBJ file for debugging */
1586 #if 0
1587                 printf("# vert count %d\n", m_vertexArray.size());
1588                 for(i=0; i<m_vertexArray.size(); i+=1) {
1589                         printf("v %.6f %.6f %.6f\n", m_vertexArray[i].x(), m_vertexArray[i].y(), m_vertexArray[i].z());
1590                 }
1591
1592                 printf("# face count %d\n", m_triFaceArray.size());
1593                 for(i=0; i<m_triFaceArray.size(); i+=3) {
1594                         printf("f %d %d %d\n", m_triFaceArray[i]+1, m_triFaceArray[i+1]+1, m_triFaceArray[i+2]+1);
1595                 }
1596 #endif
1597
1598         }
1599
1600 #if 0
1601         if (validpolys==false)
1602         {
1603                 // should not happen
1604                 m_shapeType = PHY_SHAPE_NONE;
1605                 return false;
1606         }
1607 #endif
1608         
1609         m_meshObject = meshobj;
1610         if (free_dm) {
1611                 dm->release(dm);
1612                 dm = NULL;
1613         }
1614
1615         // sharing only on static mesh at present, if you change that, you must also change in FindMesh
1616         if (!polytope && !dm && !useGimpact)
1617         {
1618                 // triangle shape can be shared, store the mesh object in the map
1619                 m_meshShapeMap.insert(std::pair<RAS_MeshObject*,CcdShapeConstructionInfo*>(meshobj,this));
1620         }
1621         return true;
1622 }
1623
1624 bool CcdShapeConstructionInfo::SetProxy(CcdShapeConstructionInfo* shapeInfo)
1625 {
1626         if (shapeInfo == NULL)
1627                 return false;
1628         // no support for dynamic change
1629         assert(IsUnused());
1630         m_shapeType = PHY_SHAPE_PROXY;
1631         m_shapeProxy = shapeInfo;
1632         return true;
1633 }
1634
1635 btCollisionShape* CcdShapeConstructionInfo::CreateBulletShape(btScalar margin)
1636 {
1637         btCollisionShape* collisionShape = 0;
1638         btCompoundShape* compoundShape = 0;     
1639
1640         if (m_shapeType == PHY_SHAPE_PROXY && m_shapeProxy != NULL)
1641                 return m_shapeProxy->CreateBulletShape(margin);
1642
1643         switch (m_shapeType) 
1644         {
1645         default:
1646                 break;
1647
1648         case PHY_SHAPE_BOX:
1649                 collisionShape = new btBoxShape(m_halfExtend);
1650                 collisionShape->setMargin(margin);
1651                 break;
1652
1653         case PHY_SHAPE_SPHERE:
1654                 collisionShape = new btSphereShape(m_radius);
1655                 collisionShape->setMargin(margin);
1656                 break;
1657
1658         case PHY_SHAPE_CYLINDER:
1659                 collisionShape = new btCylinderShapeZ(m_halfExtend);
1660                 collisionShape->setMargin(margin);
1661                 break;
1662
1663         case PHY_SHAPE_CONE:
1664                 collisionShape = new btConeShapeZ(m_radius, m_height);
1665                 collisionShape->setMargin(margin);
1666                 break;
1667
1668         case PHY_SHAPE_POLYTOPE:
1669                 collisionShape = new btConvexHullShape(&m_vertexArray[0], m_vertexArray.size()/3, 3*sizeof(btScalar));
1670                 collisionShape->setMargin(margin);
1671                 break;
1672
1673         case PHY_SHAPE_MESH:
1674                 // Let's use the latest btScaledBvhTriangleMeshShape: it allows true sharing of 
1675                 // triangle mesh information between duplicates => drastic performance increase when 
1676                 // duplicating complex mesh objects. 
1677                 // BUT it causes a small performance decrease when sharing is not required: 
1678                 // 9 multiplications/additions and one function call for each triangle that passes the mid phase filtering
1679                 // One possible optimization is to use directly the btBvhTriangleMeshShape when the scale is 1,1,1
1680                 // and btScaledBvhTriangleMeshShape otherwise.
1681                 if (m_useGimpact)
1682                 {                               
1683                                 btTriangleIndexVertexArray* indexVertexArrays = new btTriangleIndexVertexArray(
1684                                                 m_polygonIndexArray.size(),
1685                                                 &m_triFaceArray[0],
1686                                                 3*sizeof(int),
1687                                                 m_vertexArray.size()/3,
1688                                                 &m_vertexArray[0],
1689                                                 3*sizeof(btScalar)
1690                                 );
1691                                 
1692                                 btGImpactMeshShape* gimpactShape =  new btGImpactMeshShape(indexVertexArrays);
1693                                 gimpactShape->setMargin(margin);
1694                                 collisionShape = gimpactShape;
1695                                 gimpactShape->updateBound();
1696
1697                 } else
1698                 {
1699                         if (!m_unscaledShape)
1700                         {
1701                         
1702                                 btTriangleIndexVertexArray* indexVertexArrays = 0;
1703
1704                                 ///enable welding, only for the objects that need it (such as soft bodies)
1705                                 if (0.f != m_weldingThreshold1)
1706                                 {
1707                                         btTriangleMesh* collisionMeshData = new btTriangleMesh(true,false);
1708                                         collisionMeshData->m_weldingThreshold = m_weldingThreshold1;
1709                                         bool removeDuplicateVertices=true;
1710                                         // m_vertexArray not in multiple of 3 anymore, use m_triFaceArray
1711                                         for(unsigned int i=0; i<m_triFaceArray.size(); i+=3) {
1712                                                 btScalar *bt = &m_vertexArray[3*m_triFaceArray[i]];
1713                                                 btVector3 v1(bt[0], bt[1], bt[2]);
1714                                                 bt = &m_vertexArray[3*m_triFaceArray[i+1]];
1715                                                 btVector3 v2(bt[0], bt[1], bt[2]);
1716                                                 bt = &m_vertexArray[3*m_triFaceArray[i+2]];
1717                                                 btVector3 v3(bt[0], bt[1], bt[2]);
1718                                                 collisionMeshData->addTriangle(v1, v2, v3, removeDuplicateVertices);
1719                                         }
1720                                         indexVertexArrays = collisionMeshData;
1721
1722                                 } else
1723                                 {
1724                                         indexVertexArrays = new btTriangleIndexVertexArray(
1725                                                         m_polygonIndexArray.size(),
1726                                                         &m_triFaceArray[0],
1727                                                         3*sizeof(int),
1728                                                         m_vertexArray.size()/3,
1729                                                         &m_vertexArray[0],
1730                                                         3*sizeof(btScalar));
1731                                 }
1732                                 
1733                                 // this shape will be shared and not deleted until shapeInfo is deleted
1734                                 m_unscaledShape = new btBvhTriangleMeshShape( indexVertexArrays, true );
1735                                 m_unscaledShape->recalcLocalAabb();
1736                         }
1737                         collisionShape = new btScaledBvhTriangleMeshShape(m_unscaledShape, btVector3(1.0f,1.0f,1.0f));
1738                         collisionShape->setMargin(margin);
1739                 }
1740                 break;
1741
1742         case PHY_SHAPE_COMPOUND:
1743                 if (m_shapeArray.size() > 0)
1744                 {
1745                         compoundShape = new btCompoundShape();
1746                         for (std::vector<CcdShapeConstructionInfo*>::iterator sit = m_shapeArray.begin();
1747                                  sit != m_shapeArray.end();
1748                                  sit++)
1749                         {
1750                                 collisionShape = (*sit)->CreateBulletShape(margin);
1751                                 if (collisionShape)
1752                                 {
1753                                         collisionShape->setLocalScaling((*sit)->m_childScale);
1754                                         compoundShape->addChildShape((*sit)->m_childTrans, collisionShape);
1755                                 }
1756                         }
1757                         collisionShape = compoundShape;
1758                 }
1759         }
1760         return collisionShape;
1761 }
1762
1763 void CcdShapeConstructionInfo::AddShape(CcdShapeConstructionInfo* shapeInfo)
1764 {
1765         m_shapeArray.push_back(shapeInfo);
1766 }
1767
1768 CcdShapeConstructionInfo::~CcdShapeConstructionInfo()
1769 {
1770         for (std::vector<CcdShapeConstructionInfo*>::iterator sit = m_shapeArray.begin();
1771                  sit != m_shapeArray.end();
1772                  sit++)
1773         {
1774                 (*sit)->Release();
1775         }
1776         m_shapeArray.clear();
1777         if (m_unscaledShape)
1778         {
1779                 DeleteBulletShape(m_unscaledShape);
1780         }
1781         m_vertexArray.clear();
1782         if (m_shapeType == PHY_SHAPE_MESH && m_meshObject != NULL) 
1783         {
1784                 std::map<RAS_MeshObject*,CcdShapeConstructionInfo*>::iterator mit = m_meshShapeMap.find(m_meshObject);
1785                 if (mit != m_meshShapeMap.end() && mit->second == this)
1786                 {
1787                         m_meshShapeMap.erase(mit);
1788                 }
1789         }
1790         if (m_shapeType == PHY_SHAPE_PROXY && m_shapeProxy != NULL)
1791         {
1792                 m_shapeProxy->Release();
1793         }
1794 }
1795
1796