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