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