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