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