166731459e7b1882516b464b5a0678c9e970f437
[blender-staging.git] / source / gameengine / Physics / Bullet / CcdPhysicsController.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "CcdPhysicsController.h"
17 #include "btBulletDynamicsCommon.h"
18 #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
19 #include "PHY_IMotionState.h"
20 #include "CcdPhysicsEnvironment.h"
21 #include "RAS_MeshObject.h"
22 #include "BulletSoftBody/btSoftBody.h"
23 #include "BulletSoftBody//btSoftBodyInternals.h"
24 #include "BulletSoftBody/btSoftBodyHelpers.h"
25 #include "LinearMath/btConvexHull.h"
26 #include "BulletCollision/Gimpact/btGImpactShape.h"
27
28
29 #include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
30
31 class BP_Proxy;
32
33 ///todo: fill all the empty CcdPhysicsController methods, hook them up to the btRigidBody class
34
35 //'temporarily' global variables
36 //float gDeactivationTime = 2.f;
37 //bool  gDisableDeactivation = false;
38 extern float gDeactivationTime;
39 extern bool gDisableDeactivation;
40
41
42 float gLinearSleepingTreshold = 0.8f;
43 float gAngularSleepingTreshold = 1.0f;
44
45
46 btVector3 startVel(0,0,0);//-10000);
47
48 CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
49 :m_cci(ci)
50 {
51         m_prototypeTransformInitialized = false;
52         m_softbodyMappingDone = false;
53         m_collisionDelay = 0;
54         m_newClientInfo = 0;
55         m_registerCount = 0;
56         m_softBodyTransformInitialized = false;
57
58         // copy pointers locally to allow smart release
59         m_MotionState = ci.m_MotionState;
60         m_collisionShape = ci.m_collisionShape;
61         // apply scaling before creating rigid body
62         m_collisionShape->setLocalScaling(m_cci.m_scaling);
63         if (m_cci.m_mass)
64                 m_collisionShape->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
65         // shape info is shared, increment ref count
66         m_shapeInfo = ci.m_shapeInfo;
67         if (m_shapeInfo)
68                 m_shapeInfo->AddRef();
69         
70         m_bulletMotionState = 0;
71         
72         
73         CreateRigidbody();
74         
75
76 ///???
77 #ifdef WIN32
78         if (GetRigidBody() && !GetRigidBody()->isStaticObject())
79                 GetRigidBody()->setLinearVelocity(startVel);
80 #endif
81
82 }
83
84 btTransform     CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
85 {
86         btTransform trans;
87         float tmp[3];
88         motionState->getWorldPosition(tmp[0],tmp[1],tmp[2]);
89         trans.setOrigin(btVector3(tmp[0],tmp[1],tmp[2]));
90
91         btQuaternion orn;
92         motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
93         trans.setRotation(orn);
94         return trans;
95
96 }
97
98 class   BlenderBulletMotionState : public btMotionState
99 {
100         PHY_IMotionState*       m_blenderMotionState;
101
102 public:
103
104         BlenderBulletMotionState(PHY_IMotionState* bms)
105                 :m_blenderMotionState(bms)
106         {
107
108         }
109
110         virtual void    getWorldTransform(btTransform& worldTrans ) const
111         {
112                 float pos[3];
113                 float quatOrn[4];
114
115                 m_blenderMotionState->getWorldPosition(pos[0],pos[1],pos[2]);
116                 m_blenderMotionState->getWorldOrientation(quatOrn[0],quatOrn[1],quatOrn[2],quatOrn[3]);
117                 worldTrans.setOrigin(btVector3(pos[0],pos[1],pos[2]));
118                 worldTrans.setBasis(btMatrix3x3(btQuaternion(quatOrn[0],quatOrn[1],quatOrn[2],quatOrn[3])));
119         }
120
121         virtual void    setWorldTransform(const btTransform& worldTrans)
122         {
123                 m_blenderMotionState->setWorldPosition(worldTrans.getOrigin().getX(),worldTrans.getOrigin().getY(),worldTrans.getOrigin().getZ());
124                 btQuaternion rotQuat = worldTrans.getRotation();
125                 m_blenderMotionState->setWorldOrientation(rotQuat[0],rotQuat[1],rotQuat[2],rotQuat[3]);
126                 m_blenderMotionState->calculateWorldTransformations();
127         }
128
129 };
130
131
132 btRigidBody* CcdPhysicsController::GetRigidBody()
133 {
134         return btRigidBody::upcast(m_object);
135 }
136 btCollisionObject*      CcdPhysicsController::GetCollisionObject()
137 {
138         return m_object;
139 }
140 btSoftBody* CcdPhysicsController::GetSoftBody()
141 {
142         return btSoftBody::upcast(m_object);
143 }
144
145 #include "BulletSoftBody/btSoftBodyHelpers.h"
146
147
148
149 void CcdPhysicsController::CreateRigidbody()
150 {
151
152         //btTransform trans = GetTransformFromMotionState(m_MotionState);
153         m_bulletMotionState = new BlenderBulletMotionState(m_MotionState);
154
155         ///either create a btCollisionObject, btRigidBody or btSoftBody
156
157         //create a collision object
158
159         int shapeType = m_cci.m_collisionShape ? m_cci.m_collisionShape->getShapeType() : 0;
160
161         //disable soft body until first sneak preview is ready
162         if (m_cci.m_bSoft && m_cci.m_collisionShape && 
163                 (shapeType == CONVEX_HULL_SHAPE_PROXYTYPE)|
164                 (shapeType == TRIANGLE_MESH_SHAPE_PROXYTYPE) |
165                 (shapeType == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE))
166         {
167                 btRigidBody::btRigidBodyConstructionInfo rbci(m_cci.m_mass,m_bulletMotionState,m_collisionShape,m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
168                 rbci.m_linearDamping = m_cci.m_linearDamping;
169                 rbci.m_angularDamping = m_cci.m_angularDamping;
170                 rbci.m_friction = m_cci.m_friction;
171                 rbci.m_restitution = m_cci.m_restitution;
172
173                 
174                 int nodecount = 0;
175                 
176                 int numtriangles = 1;
177                 
178                 btVector3 p(0,0,0);// = getOrigin();
179                 btScalar h = 1.f;
180                 
181                 btSoftRigidDynamicsWorld* softDynaWorld = (btSoftRigidDynamicsWorld*)m_cci.m_physicsEnv->getDynamicsWorld();
182
183                 PHY__Vector3    grav;
184                 grav[0] = softDynaWorld->getGravity().getX();
185                 grav[1] = softDynaWorld->getGravity().getY();
186                 grav[2] = softDynaWorld->getGravity().getZ();
187                 softDynaWorld->getWorldInfo().m_gravity.setValue(grav[0],grav[1],grav[2]); //??
188
189         
190                 //btSoftBody*   psb=btSoftBodyHelpers::CreateRope(sbi,  btVector3(-10,0,i*0.25),btVector3(10,0,i*0.25), 16,1+2);
191
192                 btSoftBody* psb  = 0;
193
194                 if (m_cci.m_collisionShape->getShapeType() == CONVEX_HULL_SHAPE_PROXYTYPE)
195                 {
196                         btConvexHullShape* convexHull = (btConvexHullShape* )m_cci.m_collisionShape;
197
198                         //psb = btSoftBodyHelpers::CreateFromConvexHull(sbi,&transformedVertices[0],convexHull->getNumPoints());
199
200                         {
201                                 int nvertices = convexHull->getNumPoints();
202                                 const btVector3* vertices = convexHull->getPoints();
203                                 btSoftBodyWorldInfo& worldInfo = softDynaWorld->getWorldInfo();
204
205                                 HullDesc                hdsc(QF_TRIANGLES,nvertices,vertices);
206                                 HullResult              hres;
207                                 HullLibrary             hlib;/*??*/ 
208                                 hdsc.mMaxVertices=nvertices;
209                                 hlib.CreateConvexHull(hdsc,hres);
210                                 
211                                 psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
212                                         &hres.m_OutputVertices[0],0);
213                                 for(int i=0;i<(int)hres.mNumFaces;++i)
214                                 {
215                                         const int idx[]={       hres.m_Indices[i*3+0],
216                                                 hres.m_Indices[i*3+1],
217                                                 hres.m_Indices[i*3+2]};
218                                         if(idx[0]<idx[1]) psb->appendLink(      idx[0],idx[1]);
219                                         if(idx[1]<idx[2]) psb->appendLink(      idx[1],idx[2]);
220                                         if(idx[2]<idx[0]) psb->appendLink(      idx[2],idx[0]);
221                                         psb->appendFace(idx[0],idx[1],idx[2]);
222                                 }
223                                 
224                                 
225
226                                 hlib.ReleaseResult(hres);
227
228                                 
229                         }
230
231
232
233
234
235
236                 } else
237                 {
238                         
239                         btSoftBodyWorldInfo& sbi= softDynaWorld->getWorldInfo();
240
241                         if (m_cci.m_collisionShape->getShapeType() ==SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE)
242                         {
243                                 btScaledBvhTriangleMeshShape* scaledtrimeshshape = (btScaledBvhTriangleMeshShape*) m_cci.m_collisionShape;
244                                 btBvhTriangleMeshShape* trimeshshape = scaledtrimeshshape->getChildShape();
245
246                                 ///only deal with meshes that have 1 sub part/component, for now
247                                 if (trimeshshape->getMeshInterface()->getNumSubParts()==1)
248                                 {
249                                         unsigned char* vertexBase;
250                                         PHY_ScalarType vertexType;
251                                         int numverts;
252                                         int vertexstride;
253                                         unsigned char* indexbase;
254                                         int indexstride;
255                                         int numtris;
256                                         PHY_ScalarType indexType;
257                                         trimeshshape->getMeshInterface()->getLockedVertexIndexBase(&vertexBase,numverts,vertexType,vertexstride,&indexbase,indexstride,numtris,indexType);
258                                         
259                                         psb = btSoftBodyHelpers::CreateFromTriMesh(sbi,(const btScalar*)vertexBase,(const int*)indexbase,numtris);
260                                 }
261                         } else
262                         {
263                                 btBvhTriangleMeshShape* trimeshshape = (btBvhTriangleMeshShape*) m_cci.m_collisionShape;
264                                 ///only deal with meshes that have 1 sub part/component, for now
265                                 if (trimeshshape->getMeshInterface()->getNumSubParts()==1)
266                                 {
267                                         unsigned char* vertexBase;
268                                         PHY_ScalarType vertexType;
269                                         int numverts;
270                                         int vertexstride;
271                                         unsigned char* indexbase;
272                                         int indexstride;
273                                         int numtris;
274                                         PHY_ScalarType indexType;
275                                         trimeshshape->getMeshInterface()->getLockedVertexIndexBase(&vertexBase,numverts,vertexType,vertexstride,&indexbase,indexstride,numtris,indexType);
276                                         
277                                         psb = btSoftBodyHelpers::CreateFromTriMesh(sbi,(const btScalar*)vertexBase,(const int*)indexbase,numtris);
278                                 }
279                         
280
281                                 //psb = btSoftBodyHelpers::CreateFromTriMesh(sbi,&pts[0].getX(),triangles,numtriangles);
282                         }
283
284                 }
285
286         
287                 
288                 m_object = psb;
289
290                 //psb->m_cfg.collisions =       btSoftBody::fCollision::SDF_RS;//btSoftBody::fCollision::CL_SS+ btSoftBody::fCollision::CL_RS;
291                 
292                 //psb->m_cfg.collisions =       btSoftBody::fCollision::SDF_RS + btSoftBody::fCollision::VF_SS;//CL_SS;
293                 
294                 
295                 //btSoftBody::Material* pm=psb->appendMaterial();
296                 btSoftBody::Material*   pm=psb->m_materials[0];
297                 pm->m_kLST                              =       m_cci.m_soft_linStiff;
298                 pm->m_kAST                              =       m_cci.m_soft_angStiff;
299                 pm->m_kVST                              =       m_cci.m_soft_volume;
300                 psb->m_cfg.collisions = 0;
301
302                 if (m_cci.m_soft_collisionflags & CCD_BSB_COL_CL_RS)
303                 {
304                         psb->m_cfg.collisions   +=      btSoftBody::fCollision::CL_RS;
305                 } else
306                 {
307                         psb->m_cfg.collisions   +=      btSoftBody::fCollision::SDF_RS;
308                 }
309                 if (m_cci.m_soft_collisionflags & CCD_BSB_COL_CL_SS)
310                 {
311                         psb->m_cfg.collisions += btSoftBody::fCollision::CL_SS;
312                 } else
313                 {
314                         psb->m_cfg.collisions += btSoftBody::fCollision::VF_SS;
315                 }
316
317
318                 psb->m_cfg.kSRHR_CL = m_cci.m_soft_kSRHR_CL;            /* Soft vs rigid hardness [0,1] (cluster only) */
319                 psb->m_cfg.kSKHR_CL = m_cci.m_soft_kSKHR_CL;            /* Soft vs kinetic hardness [0,1] (cluster only) */
320                 psb->m_cfg.kSSHR_CL = m_cci.m_soft_kSSHR_CL;            /* Soft vs soft hardness [0,1] (cluster only) */
321                 psb->m_cfg.kSR_SPLT_CL = m_cci.m_soft_kSR_SPLT_CL;      /* Soft vs rigid impulse split [0,1] (cluster only) */
322
323                 psb->m_cfg.kSK_SPLT_CL = m_cci.m_soft_kSK_SPLT_CL;      /* Soft vs rigid impulse split [0,1] (cluster only) */
324                 psb->m_cfg.kSS_SPLT_CL = m_cci.m_soft_kSS_SPLT_CL;      /* Soft vs rigid impulse split [0,1] (cluster only) */
325                 psb->m_cfg.kVCF = m_cci.m_soft_kVCF;                    /* Velocities correction factor (Baumgarte) */
326                 psb->m_cfg.kDP = m_cci.m_soft_kDP;                      /* Damping coefficient [0,1] */
327
328                 psb->m_cfg.kDG = m_cci.m_soft_kDG;                      /* Drag coefficient [0,+inf] */
329                 psb->m_cfg.kLF = m_cci.m_soft_kLF;                      /* Lift coefficient [0,+inf] */
330                 psb->m_cfg.kPR = m_cci.m_soft_kPR;                      /* Pressure coefficient [-inf,+inf] */
331                 psb->m_cfg.kVC = m_cci.m_soft_kVC;                      /* Volume conversation coefficient [0,+inf] */
332
333                 psb->m_cfg.kDF = m_cci.m_soft_kDF;                      /* Dynamic friction coefficient [0,1] */
334                 psb->m_cfg.kMT = m_cci.m_soft_kMT;                      /* Pose matching coefficient [0,1] */
335                 psb->m_cfg.kCHR = m_cci.m_soft_kCHR;                    /* Rigid contacts hardness [0,1] */
336                 psb->m_cfg.kKHR = m_cci.m_soft_kKHR;                    /* Kinetic contacts hardness [0,1] */
337
338                 psb->m_cfg.kSHR = m_cci.m_soft_kSHR;                    /* Soft contacts hardness [0,1] */
339                 psb->m_cfg.kAHR = m_cci.m_soft_kAHR;                    /* Anchors hardness [0,1] */
340
341
342
343                 if (m_cci.m_gamesoftFlag & CCD_BSB_BENDING_CONSTRAINTS)//OB_SB_GOAL)
344                 {
345                         psb->generateBendingConstraints(2,pm);
346                 }
347
348                 psb->m_cfg.piterations = m_cci.m_soft_piterations;
349                 psb->m_cfg.viterations = m_cci.m_soft_viterations;
350                 psb->m_cfg.diterations = m_cci.m_soft_diterations;
351                 psb->m_cfg.citerations = m_cci.m_soft_citerations;
352
353                 if (m_cci.m_gamesoftFlag & CCD_BSB_SHAPE_MATCHING)//OB_SB_GOAL)
354                 {
355                         psb->setPose(false,true);//
356                 } else
357                 {
358                         psb->setPose(true,false);
359                 }
360
361
362                 
363                 psb->randomizeConstraints();
364
365                 if (m_cci.m_soft_collisionflags & (CCD_BSB_COL_CL_RS+CCD_BSB_COL_CL_SS))
366                 {
367                         psb->generateClusters(m_cci.m_soft_numclusteriterations);
368                 }
369
370 //              psb->activate();
371 //              psb->setActivationState(1);
372 //              psb->setDeactivationTime(1.f);
373                 
374                 //psb->m_materials[0]->m_kLST   =       0.1+(i/(btScalar)(n-1))*0.9;
375                 psb->setTotalMass(m_cci.m_mass);
376                 
377                 psb->setCollisionFlags(0);
378
379                 ///create a mapping between graphics mesh vertices and soft body vertices
380                 {
381                         RAS_MeshObject* rasMesh= GetShapeInfo()->GetMesh();
382
383                         if (rasMesh && !m_softbodyMappingDone)
384                         {
385                                 
386                                 //printf("apply\n");
387                                 RAS_MeshSlot::iterator it;
388                                 RAS_MeshMaterial *mmat;
389                                 RAS_MeshSlot *slot;
390                                 size_t i;
391
392                                 //for each material
393                                 for (int m=0;m<rasMesh->NumMaterials();m++)
394                                 {
395                                         // The vertex cache can only be updated for this deformer:
396                                         // Duplicated objects with more than one ploymaterial (=multiple mesh slot per object)
397                                         // share the same mesh (=the same cache). As the rendering is done per polymaterial
398                                         // cycling through the objects, the entire mesh cache cannot be updated in one shot.
399                                         mmat = rasMesh->GetMeshMaterial(m);
400
401                                         slot = mmat->m_baseslot;
402                                         for(slot->begin(it); !slot->end(it); slot->next(it))
403                                         {
404                                                 int index = 0;
405                                                 for(i=it.startvertex; i<it.endvertex; i++,index++) 
406                                                 {
407                                                         RAS_TexVert* vertex = &it.vertex[i];
408                                                         
409
410                                                         //search closest index, and store it in vertex
411                                                         vertex->setSoftBodyIndex(0);
412                                                         btScalar maxDistSqr = 1e30;
413                                                         btSoftBody::tNodeArray&   nodes(psb->m_nodes);
414                                                         btVector3 xyz = btVector3(vertex->getXYZ()[0],vertex->getXYZ()[1],vertex->getXYZ()[2]);
415                                                         for (int n=0;n<nodes.size();n++)
416                                                         {
417                                                                 btScalar distSqr = (nodes[n].m_x - xyz).length2();
418                                                                 if (distSqr<maxDistSqr)
419                                                                 {
420                                                                         maxDistSqr = distSqr;
421                                                                         
422                                                                         vertex->setSoftBodyIndex(n);
423                                                                 }
424                                                         }
425                                                 }
426                                         }
427                                 }
428                         }
429                 }
430                 
431                 m_softbodyMappingDone = true;
432
433
434
435
436
437
438 //              m_object->setCollisionShape(rbci.m_collisionShape);
439                 btTransform startTrans;
440
441                 if (rbci.m_motionState)
442                 {
443                         rbci.m_motionState->getWorldTransform(startTrans);
444                 } else
445                 {
446                         startTrans = rbci.m_startWorldTransform;
447                 }
448                 //startTrans.setIdentity();
449
450                 //m_object->setWorldTransform(startTrans);
451                 //m_object->setInterpolationWorldTransform(startTrans);
452                 m_MotionState->setWorldPosition(startTrans.getOrigin().getX(),startTrans.getOrigin().getY(),startTrans.getOrigin().getZ());
453                 m_MotionState->setWorldOrientation(0,0,0,1);
454
455                 if (!m_prototypeTransformInitialized)
456                 {
457                         m_prototypeTransformInitialized = true;
458                         m_softBodyTransformInitialized = true;
459                         GetSoftBody()->transform(startTrans);
460                 }
461
462 //              btVector3 wp = m_softBody->getWorldTransform().getOrigin();
463 //              MT_Point3 center(wp.getX(),wp.getY(),wp.getZ());
464 //              m_gameobj->NodeSetWorldPosition(center);
465
466
467         } else
468         {
469                 btRigidBody::btRigidBodyConstructionInfo rbci(m_cci.m_mass,m_bulletMotionState,m_collisionShape,m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
470                 rbci.m_linearDamping = m_cci.m_linearDamping;
471                 rbci.m_angularDamping = m_cci.m_angularDamping;
472                 rbci.m_friction = m_cci.m_friction;
473                 rbci.m_restitution = m_cci.m_restitution;
474                 m_object = new btRigidBody(rbci);
475         }
476         
477         //
478         // init the rigidbody properly
479         //
480         
481         //setMassProps this also sets collisionFlags
482         //convert collision flags!
483         //special case: a near/radar sensor controller should not be defined static or it will
484         //generate loads of static-static collision messages on the console
485         if ((m_cci.m_collisionFilterGroup & CcdConstructionInfo::SensorFilter) != 0)
486         {
487                 // reset the flags that have been set so far
488                 GetCollisionObject()->setCollisionFlags(0);
489         }
490         GetCollisionObject()->setCollisionFlags(m_object->getCollisionFlags() | m_cci.m_collisionFlags);
491         btRigidBody* body = GetRigidBody();
492
493         if (body)
494         {
495                 body->setGravity( m_cci.m_gravity);
496                 body->setDamping(m_cci.m_linearDamping, m_cci.m_angularDamping);
497
498                 if (!m_cci.m_bRigid)
499                 {
500                         body->setAngularFactor(0.f);
501                 }
502         }
503 }
504
505 static void DeleteBulletShape(btCollisionShape* shape)
506 {
507         if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
508         {
509                 // shapes based on meshes use an interface that contains the vertices.
510                 btTriangleMeshShape* meshShape = static_cast<btTriangleMeshShape*>(shape);
511                 btStridingMeshInterface* meshInterface = meshShape->getMeshInterface();
512                 if (meshInterface)
513                         delete meshInterface;
514         }
515         delete shape;
516 }
517
518 CcdPhysicsController::~CcdPhysicsController()
519 {
520         //will be reference counted, due to sharing
521         if (m_cci.m_physicsEnv)
522                 m_cci.m_physicsEnv->removeCcdPhysicsController(this);
523
524         if (m_MotionState)
525                 delete m_MotionState;
526         if (m_bulletMotionState)
527                 delete m_bulletMotionState;
528         delete m_object;
529
530         if (m_collisionShape)
531         {
532                 // collision shape is always unique to the controller, can delete it here
533                 if (m_collisionShape->isCompound())
534                 {
535                         // bullet does not delete the child shape, must do it here
536                         btCompoundShape* compoundShape = (btCompoundShape*)m_collisionShape;
537                         int numChild = compoundShape->getNumChildShapes();
538                         for (int i=numChild-1 ; i >= 0; i--)
539                         {
540                                 btCollisionShape* childShape = compoundShape->getChildShape(i);
541                                 DeleteBulletShape(childShape);
542                         }
543                 }
544                 DeleteBulletShape(m_collisionShape);
545         }
546         if (m_shapeInfo)
547         {
548                 m_shapeInfo->Release();
549         }
550 }
551
552
553                 /**
554                         SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
555                 */
556 bool            CcdPhysicsController::SynchronizeMotionStates(float time)
557 {
558         //sync non-static to motionstate, and static from motionstate (todo: add kinematic etc.)
559
560         btSoftBody* sb = GetSoftBody();
561         if (sb)
562         {
563                 btVector3 aabbMin,aabbMax;
564                 sb->getAabb(aabbMin,aabbMax);
565                 btVector3 worldPos  = (aabbMax+aabbMin)*0.5f;
566                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
567                 m_MotionState->calculateWorldTransformations();
568                 return true;
569         }
570
571         btRigidBody* body = GetRigidBody();
572
573         if (body && !body->isStaticObject())
574         {
575
576                 const btVector3& worldPos = body->getCenterOfMassPosition();
577                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
578                 
579                 const btQuaternion& worldquat = body->getOrientation();
580                 m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
581
582                 m_MotionState->calculateWorldTransformations();
583
584                 float scale[3];
585                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
586                 btVector3 scaling(scale[0],scale[1],scale[2]);
587                 GetCollisionShape()->setLocalScaling(scaling);
588         } else
589         {
590                 btVector3 worldPos;
591                 btQuaternion worldquat;
592
593 /*              m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
594                 m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
595                 btTransform oldTrans = m_body->getCenterOfMassTransform();
596                 btTransform newTrans(worldquat,worldPos);
597                                 
598                 SetCenterOfMassTransform(newTrans);
599                 //need to keep track of previous position for friction effects...
600                 
601                 m_MotionState->calculateWorldTransformations();
602 */
603                 float scale[3];
604                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
605                 btVector3 scaling(scale[0],scale[1],scale[2]);
606                 GetCollisionShape()->setLocalScaling(scaling);
607         }
608         return true;
609
610 }
611
612                 /**
613                         WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
614                 */
615                 
616 void            CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
617 {
618
619 }
620 void            CcdPhysicsController::WriteDynamicsToMotionState()
621 {
622 }
623                 // controller replication
624 void            CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
625 {
626         m_softBodyTransformInitialized=false;
627         m_MotionState = motionstate;
628         m_registerCount = 0;
629         m_collisionShape = NULL;
630
631         // always create a new shape to avoid scaling bug
632         if (m_shapeInfo)
633         {
634                 m_shapeInfo->AddRef();
635                 m_collisionShape = m_shapeInfo->CreateBulletShape();
636
637                 if (m_collisionShape)
638                 {
639                         // new shape has no scaling, apply initial scaling
640                         m_collisionShape->setMargin(m_cci.m_margin);
641                         m_collisionShape->setLocalScaling(m_cci.m_scaling);
642                         
643                         if (m_cci.m_mass)
644                                 m_collisionShape->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
645                 }
646         }
647
648         m_object = 0;
649         CreateRigidbody();
650
651         btRigidBody* body = GetRigidBody();
652
653         if (body)
654         {
655                 if (m_cci.m_mass)
656                 {
657                         body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
658                 }
659         }                       
660         m_cci.m_physicsEnv->addCcdPhysicsController(this);
661
662
663 /*      SM_Object* dynaparent=0;
664         SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
665         
666         if (sumoparentctrl)
667         {
668                 dynaparent = sumoparentctrl->GetSumoObject();
669         }
670         
671         SM_Object* orgsumoobject = m_sumoObj;
672         
673         
674         m_sumoObj       =       new SM_Object(
675                 orgsumoobject->getShapeHandle(), 
676                 orgsumoobject->getMaterialProps(),                      
677                 orgsumoobject->getShapeProps(),
678                 dynaparent);
679         
680         m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
681         
682         m_sumoObj->setMargin(orgsumoobject->getMargin());
683         m_sumoObj->setPosition(orgsumoobject->getPosition());
684         m_sumoObj->setOrientation(orgsumoobject->getOrientation());
685         //if it is a dyna, register for a callback
686         m_sumoObj->registerCallback(*this);
687         
688         m_sumoScene->add(* (m_sumoObj));
689         */
690
691
692
693 }
694
695
696 void    CcdPhysicsController::SetCenterOfMassTransform(btTransform& xform)
697 {
698         btRigidBody* body = GetRigidBody();
699         if (body)
700         {
701                 body->setCenterOfMassTransform(xform);
702         } else
703         {
704                 //either collision object or soft body?
705                 if (GetSoftBody())
706                 {
707
708                 } else
709                 {
710
711                         if (m_object->isStaticOrKinematicObject())
712                         {
713                                 m_object->setInterpolationWorldTransform(m_object->getWorldTransform());
714                         } else
715                         {
716                                 m_object->setInterpolationWorldTransform(xform);
717                         }
718                         if (body)
719                         {
720                                 body->setInterpolationLinearVelocity(body->getLinearVelocity());
721                                 body->setInterpolationAngularVelocity(body->getAngularVelocity());
722                                 body->updateInertiaTensor();
723                         }
724                         m_object->setWorldTransform(xform);
725                 }
726         }
727 }
728
729                 // kinematic methods
730 void            CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
731 {
732         if (m_object)
733         {
734                 m_object->activate(true);
735                 if (m_object->isStaticObject())
736                 {
737                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
738                 }
739
740                 btRigidBody* body = GetRigidBody();
741
742                 btVector3 dloc(dlocX,dlocY,dlocZ);
743                 btTransform xform = m_object->getWorldTransform();
744         
745                 if (local)
746                 {
747                         dloc = xform.getBasis()*dloc;
748                 }
749
750                 xform.setOrigin(xform.getOrigin() + dloc);
751                 SetCenterOfMassTransform(xform);
752         }
753
754 }
755
756 void            CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
757 {
758         if (m_object)
759         {
760                 m_object->activate(true);
761                 if (m_object->isStaticObject())
762                 {
763                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
764                 }
765
766                 btMatrix3x3 drotmat(    rotval[0],rotval[4],rotval[8],
767                                                                 rotval[1],rotval[5],rotval[9],
768                                                                 rotval[2],rotval[6],rotval[10]);
769
770
771                 btMatrix3x3 currentOrn;
772                 GetWorldOrientation(currentOrn);
773
774                 btTransform xform = m_object->getWorldTransform();
775                 
776                 xform.setBasis(xform.getBasis()*(local ? 
777                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
778
779                 SetCenterOfMassTransform(xform);
780         }
781 }
782
783
784 void CcdPhysicsController::GetWorldOrientation(btMatrix3x3& mat)
785 {
786         float orn[4];
787         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
788         btQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
789         mat.setRotation(quat);
790 }
791
792 void            CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
793 {
794         btQuaternion q = m_object->getWorldTransform().getRotation();
795         quatImag0 = q[0];
796         quatImag1 = q[1];
797         quatImag2 = q[2];
798         quatReal = q[3];
799 }
800 void            CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
801 {
802         if (m_object)
803         {
804                 m_object->activate(true);
805                 if (m_object->isStaticObject())
806                 {
807                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
808                 }
809                 // not required
810                 //m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
811                 btTransform xform  = m_object->getWorldTransform();
812                 xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
813                 SetCenterOfMassTransform(xform);
814                 // not required
815                 //m_bulletMotionState->setWorldTransform(xform);
816                 
817                 
818
819         }
820
821 }
822
823 void CcdPhysicsController::setWorldOrientation(const btMatrix3x3& orn)
824 {
825         if (m_object)
826         {
827                 m_object->activate(true);
828                 if (m_object->isStaticObject())
829                 {
830                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
831                 }
832                 // not required
833                 //m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
834                 btTransform xform  = m_object->getWorldTransform();
835                 xform.setBasis(orn);
836                 SetCenterOfMassTransform(xform);
837                 // not required
838                 //m_bulletMotionState->setWorldTransform(xform);
839                 //only once!
840                 if (!m_softBodyTransformInitialized && GetSoftBody())
841                 {
842                         m_softbodyStartTrans.setBasis(orn);
843                         xform.setOrigin(m_softbodyStartTrans.getOrigin());
844                         GetSoftBody()->transform(xform);
845                         m_softBodyTransformInitialized = true;
846                 }
847
848         }
849
850 }
851
852 void            CcdPhysicsController::setPosition(float posX,float posY,float posZ)
853 {
854         if (m_object)
855         {
856                 m_object->activate(true);
857                 if (m_object->isStaticObject())
858                 {
859                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
860                 }
861                 // not required, this function is only used to update the physic controller
862                 //m_MotionState->setWorldPosition(posX,posY,posZ);
863                 btTransform xform  = m_object->getWorldTransform();
864                 xform.setOrigin(btVector3(posX,posY,posZ));
865                 SetCenterOfMassTransform(xform);
866                 if (!m_softBodyTransformInitialized)
867                         m_softbodyStartTrans.setOrigin(xform.getOrigin());
868                 // not required
869                 //m_bulletMotionState->setWorldTransform(xform);
870         }
871
872
873 }
874 void            CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
875 {
876 }
877
878 void            CcdPhysicsController::getPosition(PHY__Vector3& pos) const
879 {
880         const btTransform& xform = m_object->getWorldTransform();
881         pos[0] = xform.getOrigin().x();
882         pos[1] = xform.getOrigin().y();
883         pos[2] = xform.getOrigin().z();
884 }
885
886 void            CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
887 {
888         if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
889                 !btFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
890                 !btFuzzyZero(m_cci.m_scaling.z()-scaleZ))
891         {
892                 m_cci.m_scaling = btVector3(scaleX,scaleY,scaleZ);
893
894                 if (m_object && m_object->getCollisionShape())
895                 {
896                         m_object->getCollisionShape()->setLocalScaling(m_cci.m_scaling);
897                         
898                         //printf("no inertia recalc for fixed objects with mass=0\n");
899                         btRigidBody* body = GetRigidBody();
900                         if (body && m_cci.m_mass)
901                         {
902                                 body->getCollisionShape()->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
903                                 body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
904                         } 
905                         
906                 }
907         }
908 }
909                 
910                 // physics methods
911 void            CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
912 {
913         btVector3 torque(torqueX,torqueY,torqueZ);
914         btTransform xform = m_object->getWorldTransform();
915         
916
917         if (m_object && torque.length2() > (SIMD_EPSILON*SIMD_EPSILON))
918         {
919                 btRigidBody* body = GetRigidBody();
920                 m_object->activate();
921                 if (m_object->isStaticObject())
922                 {
923                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
924                 }
925                 if (local)
926                 {
927                         torque  = xform.getBasis()*torque;
928                 }
929                 if (body)
930                         body->applyTorque(torque);
931         }
932 }
933
934 void            CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
935 {
936         btVector3 force(forceX,forceY,forceZ);
937         
938
939         if (m_object && force.length2() > (SIMD_EPSILON*SIMD_EPSILON))
940         {
941                 m_object->activate();
942                 if (m_object->isStaticObject())
943                 {
944                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
945                 }
946
947                 {
948                         btTransform xform = m_object->getWorldTransform();
949                         
950                         if (local)
951                         {       
952                                 force   = xform.getBasis()*force;
953                         }
954                         btRigidBody* body = GetRigidBody();
955                         if (body)
956                                 body->applyCentralForce(force);
957                         btSoftBody* soft = GetSoftBody();
958                         if (soft)
959                                 soft->addForce(force);
960
961                 }
962         }
963 }
964 void            CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
965 {
966         btVector3 angvel(ang_velX,ang_velY,ang_velZ);
967         if (m_object && angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
968         {
969                 m_object->activate(true);
970                 if (m_object->isStaticObject())
971                 {
972                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
973                 }
974                 {
975                         btTransform xform = m_object->getWorldTransform();
976                         if (local)
977                         {
978                                 angvel  = xform.getBasis()*angvel;
979                         }
980                         btRigidBody* body = GetRigidBody();
981                         if (body)
982                                 body->setAngularVelocity(angvel);
983
984                 }
985         }
986
987 }
988 void            CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
989 {
990
991         btVector3 linVel(lin_velX,lin_velY,lin_velZ);
992         if (m_object/* && linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON)*/)
993         {
994                 m_object->activate(true);
995                 if (m_object->isStaticObject())
996                 {
997                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
998                 }
999                 
1000                 btSoftBody* soft = GetSoftBody();
1001                 if (soft)
1002                 {
1003                         if (local)
1004                         {
1005                                 linVel  = m_softbodyStartTrans.getBasis()*linVel;
1006                         }
1007                         soft->setVelocity(linVel);
1008                 } else
1009                 {
1010                         btTransform xform = m_object->getWorldTransform();
1011                         if (local)
1012                         {
1013                                 linVel  = xform.getBasis()*linVel;
1014                         }
1015                         btRigidBody* body = GetRigidBody();
1016                         if (body)
1017                                 body->setLinearVelocity(linVel);
1018                 }
1019         }
1020 }
1021 void            CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
1022 {
1023         btVector3 impulse(impulseX,impulseY,impulseZ);
1024
1025         if (m_object && impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
1026         {
1027                 m_object->activate();
1028                 if (m_object->isStaticObject())
1029                 {
1030                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
1031                 }
1032                 
1033                 btVector3 pos(attachX,attachY,attachZ);
1034                 btRigidBody* body = GetRigidBody();
1035                 if (body)
1036                         body->applyImpulse(impulse,pos);
1037                         
1038         }
1039
1040 }
1041 void            CcdPhysicsController::SetActive(bool active)
1042 {
1043 }
1044                 // reading out information from physics
1045 void            CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
1046 {
1047         btRigidBody* body = GetRigidBody();
1048         if (body)
1049         {
1050                 const btVector3& linvel = body->getLinearVelocity();
1051                 linvX = linvel.x();
1052                 linvY = linvel.y();
1053                 linvZ = linvel.z();
1054         } else
1055         {
1056                 linvX = 0.f;
1057                 linvY = 0.f;
1058                 linvZ = 0.f;
1059         }
1060
1061 }
1062
1063 void            CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
1064 {
1065         btRigidBody* body = GetRigidBody();
1066         if (body)
1067         {
1068                 const btVector3& angvel= body->getAngularVelocity();
1069                 angVelX = angvel.x();
1070                 angVelY = angvel.y();
1071                 angVelZ = angvel.z();
1072         } else
1073         {
1074                 angVelX = 0.f;
1075                 angVelY = 0.f;
1076                 angVelZ = 0.f;
1077         }
1078 }
1079
1080 void            CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
1081 {
1082         btVector3 pos(posX,posY,posZ);
1083         btRigidBody* body = GetRigidBody();
1084         if (body)
1085         {
1086                 btVector3 rel_pos = pos-body->getCenterOfMassPosition();
1087                 btVector3 linvel = body->getVelocityInLocalPoint(rel_pos);
1088                 linvX = linvel.x();
1089                 linvY = linvel.y();
1090                 linvZ = linvel.z();
1091         } else
1092         {
1093                 linvX = 0.f;
1094                 linvY = 0.f;
1095                 linvZ = 0.f;
1096         }
1097 }
1098 void            CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
1099 {
1100 }
1101
1102                 // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
1103 void            CcdPhysicsController::setRigidBody(bool rigid)
1104 {
1105         if (!rigid)
1106         {
1107                 btRigidBody* body = GetRigidBody();
1108                 if (body)
1109                 {
1110                         //fake it for now
1111                         btVector3 inertia = body->getInvInertiaDiagLocal();
1112                         inertia[1] = 0.f;
1113                         body->setInvInertiaDiagLocal(inertia);
1114                         body->updateInertiaTensor();
1115                 }
1116         }
1117 }
1118
1119                 // clientinfo for raycasts for example
1120 void*           CcdPhysicsController::getNewClientInfo()
1121 {
1122         return m_newClientInfo;
1123 }
1124 void            CcdPhysicsController::setNewClientInfo(void* clientinfo)
1125 {
1126         m_newClientInfo = clientinfo;
1127 }
1128
1129
1130 void    CcdPhysicsController::UpdateDeactivation(float timeStep)
1131 {
1132         btRigidBody* body = GetRigidBody();
1133         if (body)
1134         {
1135                 body->updateDeactivation( timeStep);
1136         }
1137 }
1138
1139 bool CcdPhysicsController::wantsSleeping()
1140 {
1141         btRigidBody* body = GetRigidBody();
1142         if (body)
1143         {
1144                 return body->wantsSleeping();
1145         }
1146         //check it out
1147         return true;
1148 }
1149
1150 PHY_IPhysicsController* CcdPhysicsController::GetReplica()
1151 {
1152         // This is used only to replicate Near and Radar sensor controllers
1153         // The replication of object physics controller is done in KX_BulletPhysicsController::GetReplica()
1154         CcdConstructionInfo cinfo = m_cci;
1155         if (m_shapeInfo)
1156         {
1157                 // This situation does not normally happen
1158                 cinfo.m_collisionShape = m_shapeInfo->CreateBulletShape();
1159         } 
1160         else if (m_collisionShape)
1161         {
1162                 switch (m_collisionShape->getShapeType())
1163                 {
1164                 case SPHERE_SHAPE_PROXYTYPE:
1165                         {
1166                                 btSphereShape* orgShape = (btSphereShape*)m_collisionShape;
1167                                 cinfo.m_collisionShape = new btSphereShape(*orgShape);
1168                                 break;
1169                         }
1170
1171                 case CONE_SHAPE_PROXYTYPE:
1172                         {
1173                                 btConeShape* orgShape = (btConeShape*)m_collisionShape;
1174                                 cinfo.m_collisionShape = new btConeShape(*orgShape);
1175                                 break;
1176                         }
1177
1178                 default:
1179                         {
1180                                 return 0;
1181                         }
1182                 }
1183         }
1184
1185         cinfo.m_MotionState = new DefaultMotionState();
1186         cinfo.m_shapeInfo = m_shapeInfo;
1187
1188         CcdPhysicsController* replica = new CcdPhysicsController(cinfo);
1189         return replica;
1190 }
1191
1192 ///////////////////////////////////////////////////////////
1193 ///A small utility class, DefaultMotionState
1194 ///
1195 ///////////////////////////////////////////////////////////
1196
1197 DefaultMotionState::DefaultMotionState()
1198 {
1199         m_worldTransform.setIdentity();
1200         m_localScaling.setValue(1.f,1.f,1.f);
1201 }
1202
1203
1204 DefaultMotionState::~DefaultMotionState()
1205 {
1206
1207 }
1208
1209 void    DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
1210 {
1211         posX = m_worldTransform.getOrigin().x();
1212         posY = m_worldTransform.getOrigin().y();
1213         posZ = m_worldTransform.getOrigin().z();
1214 }
1215
1216 void    DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
1217 {
1218         scaleX = m_localScaling.getX();
1219         scaleY = m_localScaling.getY();
1220         scaleZ = m_localScaling.getZ();
1221 }
1222
1223 void    DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
1224 {
1225         quatIma0 = m_worldTransform.getRotation().x();
1226         quatIma1 = m_worldTransform.getRotation().y();
1227         quatIma2 = m_worldTransform.getRotation().z();
1228         quatReal = m_worldTransform.getRotation()[3];
1229 }
1230                 
1231 void    DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
1232 {
1233         btPoint3 pos(posX,posY,posZ);
1234         m_worldTransform.setOrigin( pos );
1235 }
1236
1237 void    DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
1238 {
1239         btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
1240         m_worldTransform.setRotation( orn );
1241 }
1242                 
1243 void    DefaultMotionState::calculateWorldTransformations()
1244 {
1245
1246 }
1247
1248 // Shape constructor
1249 std::map<RAS_MeshObject*, CcdShapeConstructionInfo*> CcdShapeConstructionInfo::m_meshShapeMap;
1250
1251 CcdShapeConstructionInfo* CcdShapeConstructionInfo::FindMesh(RAS_MeshObject* mesh, bool polytope)
1252 {
1253         if (polytope)
1254                 // not yet supported
1255                 return NULL;
1256
1257         std::map<RAS_MeshObject*,CcdShapeConstructionInfo*>::const_iterator mit = m_meshShapeMap.find(mesh);
1258         if (mit != m_meshShapeMap.end())
1259                 return mit->second;
1260         return NULL;
1261 }
1262
1263 bool CcdShapeConstructionInfo::SetMesh(RAS_MeshObject* meshobj, bool polytope,bool useGimpact)
1264 {
1265         m_useGimpact = useGimpact;
1266
1267         // assume no shape information
1268         // no support for dynamic change of shape yet
1269         assert(m_meshObject == NULL);
1270         m_shapeType = PHY_SHAPE_NONE;
1271         m_vertexArray.clear();
1272         m_polygonIndexArray.clear();
1273         m_meshObject = NULL;
1274
1275         if (!meshobj)
1276                 return false;
1277
1278         // Mesh has no polygons!
1279         int numpolys = meshobj->NumPolygons();
1280         if (!numpolys)
1281         {
1282                 return false;
1283         }
1284
1285         // check that we have at least one colliding polygon
1286         int numvalidpolys = 0;
1287
1288         for (int p=0; p<numpolys; p++)
1289         {
1290                 RAS_Polygon* poly = meshobj->GetPolygon(p);
1291
1292                 // only add polygons that have the collisionflag set
1293                 if (poly->IsCollider())
1294                 {
1295                         numvalidpolys++;
1296                         break;
1297                 }
1298         }
1299
1300         // No collision polygons
1301         if (numvalidpolys < 1)
1302                 return false;
1303
1304         m_shapeType = (polytope) ? PHY_SHAPE_POLYTOPE : PHY_SHAPE_MESH;
1305
1306         numvalidpolys = 0;
1307
1308         for (int p2=0; p2<numpolys; p2++)
1309         {
1310                 RAS_Polygon* poly = meshobj->GetPolygon(p2);
1311
1312                 // only add polygons that have the collisionflag set
1313                 if (poly->IsCollider())
1314                 {   
1315                         //Bullet can raycast any shape, so
1316                         if (polytope)
1317                         {
1318                                 for (int i=0;i<poly->VertexCount();i++)
1319                                 {
1320                                         const float* vtx = poly->GetVertex(i)->getXYZ();
1321                                         btPoint3 point(vtx[0],vtx[1],vtx[2]);
1322                                         //avoid duplicates (could better directly use vertex offsets, rather than a vertex compare)
1323                                         bool found = false;
1324                                         for (int j=0;j<m_vertexArray.size();j++)
1325                                         {
1326                                                 if (m_vertexArray[j]==point)
1327                                                 {
1328                                                         found = true;
1329                                                         break;
1330                                                 }
1331                                         }
1332                                         if (!found)
1333                                                 m_vertexArray.push_back(point);
1334
1335                                         numvalidpolys++;
1336                                 }
1337                         } else
1338                         {
1339                                 {
1340                                         const float* vtx = poly->GetVertex(2)->getXYZ();
1341                                         btPoint3 vertex0(vtx[0],vtx[1],vtx[2]);
1342
1343                                         vtx = poly->GetVertex(1)->getXYZ();
1344                                         btPoint3 vertex1(vtx[0],vtx[1],vtx[2]);
1345
1346                                         vtx = poly->GetVertex(0)->getXYZ();
1347                                         btPoint3 vertex2(vtx[0],vtx[1],vtx[2]);
1348
1349                                         m_vertexArray.push_back(vertex0);
1350                                         m_vertexArray.push_back(vertex1);
1351                                         m_vertexArray.push_back(vertex2);
1352                                         m_polygonIndexArray.push_back(p2);
1353                                         numvalidpolys++;
1354                                 }
1355                                 if (poly->VertexCount() == 4)
1356                                 {
1357                                         const float* vtx = poly->GetVertex(3)->getXYZ();
1358                                         btPoint3 vertex0(vtx[0],vtx[1],vtx[2]);
1359
1360                                         vtx = poly->GetVertex(2)->getXYZ();
1361                                         btPoint3 vertex1(vtx[0],vtx[1],vtx[2]);
1362
1363                                         vtx = poly->GetVertex(0)->getXYZ();
1364                                         btPoint3 vertex2(vtx[0],vtx[1],vtx[2]);
1365
1366                                         m_vertexArray.push_back(vertex0);
1367                                         m_vertexArray.push_back(vertex1);
1368                                         m_vertexArray.push_back(vertex2);
1369                                         m_polygonIndexArray.push_back(p2);
1370                                         numvalidpolys++;
1371                                 }
1372                         }               
1373                 }
1374         }
1375
1376         if (!numvalidpolys)
1377         {
1378                 // should not happen
1379                 m_shapeType = PHY_SHAPE_NONE;
1380                 return false;
1381         }
1382         m_meshObject = meshobj;
1383         if (!polytope)
1384         {
1385                 // triangle shape can be shared, store the mesh object in the map
1386                 m_meshShapeMap.insert(std::pair<RAS_MeshObject*,CcdShapeConstructionInfo*>(meshobj,this));
1387         }
1388         return true;
1389 }
1390
1391 btCollisionShape* CcdShapeConstructionInfo::CreateBulletShape()
1392 {
1393         btCollisionShape* collisionShape = 0;
1394         btTriangleMeshShape* concaveShape = 0;
1395         btTriangleMesh* collisionMeshData = 0;
1396         btCompoundShape* compoundShape = 0;
1397         CcdShapeConstructionInfo* nextShapeInfo;
1398
1399         switch (m_shapeType) 
1400         {
1401         case PHY_SHAPE_NONE:
1402                 break;
1403
1404         case PHY_SHAPE_BOX:
1405                 collisionShape = new btBoxShape(m_halfExtend);
1406                 break;
1407
1408         case PHY_SHAPE_SPHERE:
1409                 collisionShape = new btSphereShape(m_radius);
1410                 break;
1411
1412         case PHY_SHAPE_CYLINDER:
1413                 collisionShape = new btCylinderShapeZ(m_halfExtend);
1414                 break;
1415
1416         case PHY_SHAPE_CONE:
1417                 collisionShape = new btConeShapeZ(m_radius, m_height);
1418                 break;
1419
1420         case PHY_SHAPE_POLYTOPE:
1421                 collisionShape = new btConvexHullShape(&m_vertexArray.begin()->getX(), m_vertexArray.size());
1422                 break;
1423
1424         case PHY_SHAPE_MESH:
1425                 // Let's use the latest btScaledBvhTriangleMeshShape: it allows true sharing of 
1426                 // triangle mesh information between duplicates => drastic performance increase when 
1427                 // duplicating complex mesh objects. 
1428                 // BUT it causes a small performance decrease when sharing is not required: 
1429                 // 9 multiplications/additions and one function call for each triangle that passes the mid phase filtering
1430                 // One possible optimization is to use directly the btBvhTriangleMeshShape when the scale is 1,1,1
1431                 // and btScaledBvhTriangleMeshShape otherwise.
1432                 if (m_useGimpact)
1433                 {
1434                                 collisionMeshData = new btTriangleMesh();
1435                                 
1436
1437                                 // m_vertexArray is necessarily a multiple of 3
1438                                 for (std::vector<btPoint3>::iterator it=m_vertexArray.begin(); it != m_vertexArray.end(); )
1439                                 {
1440                                         collisionMeshData->addTriangle(*it++,*it++,*it++);
1441                                 }
1442                                 btGImpactMeshShape* gimpactShape =  new btGImpactMeshShape(collisionMeshData);
1443
1444                                 collisionShape = gimpactShape;
1445                                 gimpactShape->updateBound();
1446
1447                 } else
1448                 {
1449                         if (!m_unscaledShape)
1450                         {
1451                                 collisionMeshData = new btTriangleMesh(true,false);
1452                                 collisionMeshData->m_weldingThreshold = m_weldingThreshold;
1453
1454                                 // m_vertexArray is necessarily a multiple of 3
1455                                 for (std::vector<btPoint3>::iterator it=m_vertexArray.begin(); it != m_vertexArray.end(); )
1456                                 {
1457                                         collisionMeshData->addTriangle(*it++,*it++,*it++);
1458                                 }
1459                                 // this shape will be shared and not deleted until shapeInfo is deleted
1460                                 m_unscaledShape = new btBvhTriangleMeshShape( collisionMeshData, true );
1461                                 m_unscaledShape->recalcLocalAabb();
1462                         }
1463                         collisionShape = new btScaledBvhTriangleMeshShape(m_unscaledShape, btVector3(1.0f,1.0f,1.0f));
1464                 }
1465                 break;
1466
1467         case PHY_SHAPE_COMPOUND:
1468                 if (m_shapeArray.size() > 0)
1469                 {
1470                         compoundShape = new btCompoundShape();
1471                         for (std::vector<CcdShapeConstructionInfo*>::iterator sit = m_shapeArray.begin();
1472                                  sit != m_shapeArray.end();
1473                                  sit++)
1474                         {
1475                                 collisionShape = (*sit)->CreateBulletShape();
1476                                 if (collisionShape)
1477                                 {
1478                                         collisionShape->setLocalScaling((*sit)->m_childScale);
1479                                         compoundShape->addChildShape((*sit)->m_childTrans, collisionShape);
1480                                 }
1481                         }
1482                         collisionShape = compoundShape;
1483                 }
1484         }
1485         return collisionShape;
1486 }
1487
1488 void CcdShapeConstructionInfo::AddShape(CcdShapeConstructionInfo* shapeInfo)
1489 {
1490         m_shapeArray.push_back(shapeInfo);
1491 }
1492
1493 CcdShapeConstructionInfo::~CcdShapeConstructionInfo()
1494 {
1495         for (std::vector<CcdShapeConstructionInfo*>::iterator sit = m_shapeArray.begin();
1496                  sit != m_shapeArray.end();
1497                  sit++)
1498         {
1499                 (*sit)->Release();
1500         }
1501         m_shapeArray.clear();
1502         if (m_unscaledShape)
1503         {
1504                 DeleteBulletShape(m_unscaledShape);
1505         }
1506         m_vertexArray.clear();
1507         if (m_shapeType == PHY_SHAPE_MESH && m_meshObject != NULL) 
1508         {
1509                 std::map<RAS_MeshObject*,CcdShapeConstructionInfo*>::iterator mit = m_meshShapeMap.find(m_meshObject);
1510                 if (mit != m_meshShapeMap.end() && mit->second == this)
1511                 {
1512                         m_meshShapeMap.erase(mit);
1513                 }
1514         }
1515 }
1516
1517