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