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