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