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