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