f1e30b4a1e3093ea1aafdb334e39399d714458dc
[blender.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                 psb->m_cfg.collisions   =       btSoftBody::fCollision::SDF_RS + btSoftBody::fCollision::VF_SS;//CL_SS;
292                 //psb->m_cfg.collisions =       btSoftBody::fCollision::CL_SS + btSoftBody::fCollision::CL_RS;
293                 
294                 //btSoftBody::Material* pm=psb->appendMaterial();
295                 btSoftBody::Material*   pm=psb->m_materials[0];
296                 
297                 pm->m_kLST                              =       m_cci.m_linearStiffness;
298                 pm->m_kAST                              =       m_cci.m_angularStiffness;
299                 pm->m_kVST                              =       m_cci.m_volumePreservation;
300                 
301
302                 
303                 //pm->m_kAST = 0.01f;
304                 //pm->m_kVST = 0.001f;
305                 psb->generateBendingConstraints(2,pm);
306                 psb->m_cfg.piterations = 4;
307                 psb->m_cfg.viterations = 4;
308                 psb->m_cfg.diterations = 4;
309                 psb->m_cfg.citerations = 4;
310                 if (m_cci.m_gamesoftFlag & 2)//OB_SB_GOAL)
311                 {
312                         psb->setPose(false,true);//
313                 } else
314                 {
315                         psb->setPose(true,false);
316                 }
317
318                 psb->m_cfg.kDF                          =       0.5;
319                 //psb->m_cfg.kMT                                =       0.05;
320                 psb->m_cfg.piterations          =       5;
321                 
322                 psb->m_cfg.piterations          =       5;
323                 //psb->m_cfg.kVC                                =       20;
324
325                 psb->randomizeConstraints();
326
327 /*
328                 psb->m_cfg.kDF = 0.1f;//1.f;
329                 psb->m_cfg.kDP          =       0.0001;
330                 //psb->m_cfg.kDP                =       0.005;
331                 psb->m_cfg.kCHR         =       0.1;
332                 //psb->m_cfg.kVCF = 0.1f;
333                 psb->m_cfg.kVCF = 0.0001f;
334                 //psb->m_cfg.kAHR = 0.1f;
335                 psb->m_cfg.kAHR = 0.0001f;
336                 psb->m_cfg.kMT = 0.1f;
337                 //psb->m_cfg.kDF=1;
338                 */
339
340 //              psb->activate();
341 //              psb->setActivationState(1);
342 //              psb->setDeactivationTime(1.f);
343                 
344                 //psb->m_materials[0]->m_kLST   =       0.1+(i/(btScalar)(n-1))*0.9;
345                 psb->setTotalMass(m_cci.m_mass);
346                 psb->generateClusters(64);              
347                 psb->setCollisionFlags(0);
348
349
350
351
352
353                         ///create a mapping between graphics mesh vertices and soft body vertices
354                 {
355                         RAS_MeshObject* rasMesh= GetShapeInfo()->GetMesh();
356
357                         if (rasMesh && !m_softbodyMappingDone)
358                         {
359                                 
360                                 //printf("apply\n");
361                                 RAS_MeshSlot::iterator it;
362                                 RAS_MeshMaterial *mmat;
363                                 RAS_MeshSlot *slot;
364                                 size_t i;
365
366                                 //for each material
367                                 for (int m=0;m<rasMesh->NumMaterials();m++)
368                                 {
369                                         // The vertex cache can only be updated for this deformer:
370                                         // Duplicated objects with more than one ploymaterial (=multiple mesh slot per object)
371                                         // share the same mesh (=the same cache). As the rendering is done per polymaterial
372                                         // cycling through the objects, the entire mesh cache cannot be updated in one shot.
373                                         mmat = rasMesh->GetMeshMaterial(m);
374
375                                         slot = mmat->m_baseslot;
376                                         for(slot->begin(it); !slot->end(it); slot->next(it))
377                                         {
378                                                 int index = 0;
379                                                 for(i=it.startvertex; i<it.endvertex; i++,index++) 
380                                                 {
381                                                         RAS_TexVert* vertex = &it.vertex[i];
382                                                         
383
384                                                         //search closest index, and store it in vertex
385                                                         vertex->setSoftBodyIndex(0);
386                                                         btScalar maxDistSqr = 1e30;
387                                                         btSoftBody::tNodeArray&   nodes(psb->m_nodes);
388                                                         btVector3 xyz = btVector3(vertex->getXYZ()[0],vertex->getXYZ()[1],vertex->getXYZ()[2]);
389                                                         for (int n=0;n<nodes.size();n++)
390                                                         {
391                                                                 btScalar distSqr = (nodes[n].m_x - xyz).length2();
392                                                                 if (distSqr<maxDistSqr)
393                                                                 {
394                                                                         maxDistSqr = distSqr;
395                                                                         
396                                                                         vertex->setSoftBodyIndex(n);
397                                                                 }
398                                                         }
399                                                 }
400                                         }
401                                 }
402                         }
403                 }
404                 
405                 m_softbodyMappingDone = true;
406
407
408
409
410
411
412 //              m_object->setCollisionShape(rbci.m_collisionShape);
413                 btTransform startTrans;
414
415                 if (rbci.m_motionState)
416                 {
417                         rbci.m_motionState->getWorldTransform(startTrans);
418                 } else
419                 {
420                         startTrans = rbci.m_startWorldTransform;
421                 }
422                 //startTrans.setIdentity();
423
424                 //m_object->setWorldTransform(startTrans);
425                 //m_object->setInterpolationWorldTransform(startTrans);
426                 m_MotionState->setWorldPosition(startTrans.getOrigin().getX(),startTrans.getOrigin().getY(),startTrans.getOrigin().getZ());
427                 m_MotionState->setWorldOrientation(0,0,0,1);
428
429                 if (!m_prototypeTransformInitialized)
430                 {
431                         m_prototypeTransformInitialized = true;
432                         m_softBodyTransformInitialized = true;
433                         GetSoftBody()->transform(startTrans);
434                 }
435
436 //              btVector3 wp = m_softBody->getWorldTransform().getOrigin();
437 //              MT_Point3 center(wp.getX(),wp.getY(),wp.getZ());
438 //              m_gameobj->NodeSetWorldPosition(center);
439
440
441         } else
442         {
443                 btRigidBody::btRigidBodyConstructionInfo rbci(m_cci.m_mass,m_bulletMotionState,m_collisionShape,m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
444                 rbci.m_linearDamping = m_cci.m_linearDamping;
445                 rbci.m_angularDamping = m_cci.m_angularDamping;
446                 rbci.m_friction = m_cci.m_friction;
447                 rbci.m_restitution = m_cci.m_restitution;
448                 m_object = new btRigidBody(rbci);
449         }
450         
451         //
452         // init the rigidbody properly
453         //
454         
455         //setMassProps this also sets collisionFlags
456         //convert collision flags!
457         //special case: a near/radar sensor controller should not be defined static or it will
458         //generate loads of static-static collision messages on the console
459         if ((m_cci.m_collisionFilterGroup & CcdConstructionInfo::SensorFilter) != 0)
460         {
461                 // reset the flags that have been set so far
462                 GetCollisionObject()->setCollisionFlags(0);
463         }
464         GetCollisionObject()->setCollisionFlags(m_object->getCollisionFlags() | m_cci.m_collisionFlags);
465         btRigidBody* body = GetRigidBody();
466
467         if (body)
468         {
469                 body->setGravity( m_cci.m_gravity);
470                 body->setDamping(m_cci.m_linearDamping, m_cci.m_angularDamping);
471
472                 if (!m_cci.m_bRigid)
473                 {
474                         body->setAngularFactor(0.f);
475                 }
476         }
477 }
478
479 static void DeleteBulletShape(btCollisionShape* shape)
480 {
481         if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
482         {
483                 // shapes based on meshes use an interface that contains the vertices.
484                 btTriangleMeshShape* meshShape = static_cast<btTriangleMeshShape*>(shape);
485                 btStridingMeshInterface* meshInterface = meshShape->getMeshInterface();
486                 if (meshInterface)
487                         delete meshInterface;
488         }
489         delete shape;
490 }
491
492 CcdPhysicsController::~CcdPhysicsController()
493 {
494         //will be reference counted, due to sharing
495         if (m_cci.m_physicsEnv)
496                 m_cci.m_physicsEnv->removeCcdPhysicsController(this);
497
498         if (m_MotionState)
499                 delete m_MotionState;
500         if (m_bulletMotionState)
501                 delete m_bulletMotionState;
502         delete m_object;
503
504         if (m_collisionShape)
505         {
506                 // collision shape is always unique to the controller, can delete it here
507                 if (m_collisionShape->isCompound())
508                 {
509                         // bullet does not delete the child shape, must do it here
510                         btCompoundShape* compoundShape = (btCompoundShape*)m_collisionShape;
511                         int numChild = compoundShape->getNumChildShapes();
512                         for (int i=numChild-1 ; i >= 0; i--)
513                         {
514                                 btCollisionShape* childShape = compoundShape->getChildShape(i);
515                                 DeleteBulletShape(childShape);
516                         }
517                 }
518                 DeleteBulletShape(m_collisionShape);
519         }
520         if (m_shapeInfo)
521         {
522                 m_shapeInfo->Release();
523         }
524 }
525
526
527                 /**
528                         SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
529                 */
530 bool            CcdPhysicsController::SynchronizeMotionStates(float time)
531 {
532         //sync non-static to motionstate, and static from motionstate (todo: add kinematic etc.)
533
534         btSoftBody* sb = GetSoftBody();
535         if (sb)
536         {
537                 btVector3 aabbMin,aabbMax;
538                 sb->getAabb(aabbMin,aabbMax);
539                 btVector3 worldPos  = (aabbMax+aabbMin)*0.5f;
540                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
541                 m_MotionState->calculateWorldTransformations();
542                 return true;
543         }
544
545         btRigidBody* body = GetRigidBody();
546
547         if (body && !body->isStaticObject())
548         {
549
550                 const btVector3& worldPos = body->getCenterOfMassPosition();
551                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
552                 
553                 const btQuaternion& worldquat = body->getOrientation();
554                 m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
555
556                 m_MotionState->calculateWorldTransformations();
557
558                 float scale[3];
559                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
560                 btVector3 scaling(scale[0],scale[1],scale[2]);
561                 GetCollisionShape()->setLocalScaling(scaling);
562         } else
563         {
564                 btVector3 worldPos;
565                 btQuaternion worldquat;
566
567 /*              m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
568                 m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
569                 btTransform oldTrans = m_body->getCenterOfMassTransform();
570                 btTransform newTrans(worldquat,worldPos);
571                                 
572                 SetCenterOfMassTransform(newTrans);
573                 //need to keep track of previous position for friction effects...
574                 
575                 m_MotionState->calculateWorldTransformations();
576 */
577                 float scale[3];
578                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
579                 btVector3 scaling(scale[0],scale[1],scale[2]);
580                 GetCollisionShape()->setLocalScaling(scaling);
581         }
582         return true;
583
584 }
585
586                 /**
587                         WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
588                 */
589                 
590 void            CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
591 {
592
593 }
594 void            CcdPhysicsController::WriteDynamicsToMotionState()
595 {
596 }
597                 // controller replication
598 void            CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
599 {
600         m_softBodyTransformInitialized=false;
601         m_MotionState = motionstate;
602         m_registerCount = 0;
603         m_collisionShape = NULL;
604
605         // always create a new shape to avoid scaling bug
606         if (m_shapeInfo)
607         {
608                 m_shapeInfo->AddRef();
609                 m_collisionShape = m_shapeInfo->CreateBulletShape();
610
611                 if (m_collisionShape)
612                 {
613                         // new shape has no scaling, apply initial scaling
614                         m_collisionShape->setMargin(m_cci.m_margin);
615                         m_collisionShape->setLocalScaling(m_cci.m_scaling);
616                         
617                         if (m_cci.m_mass)
618                                 m_collisionShape->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
619                 }
620         }
621
622         m_object = 0;
623         CreateRigidbody();
624
625         btRigidBody* body = GetRigidBody();
626
627         if (body)
628         {
629                 if (m_cci.m_mass)
630                 {
631                         body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
632                 }
633         }                       
634         m_cci.m_physicsEnv->addCcdPhysicsController(this);
635
636
637 /*      SM_Object* dynaparent=0;
638         SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
639         
640         if (sumoparentctrl)
641         {
642                 dynaparent = sumoparentctrl->GetSumoObject();
643         }
644         
645         SM_Object* orgsumoobject = m_sumoObj;
646         
647         
648         m_sumoObj       =       new SM_Object(
649                 orgsumoobject->getShapeHandle(), 
650                 orgsumoobject->getMaterialProps(),                      
651                 orgsumoobject->getShapeProps(),
652                 dynaparent);
653         
654         m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
655         
656         m_sumoObj->setMargin(orgsumoobject->getMargin());
657         m_sumoObj->setPosition(orgsumoobject->getPosition());
658         m_sumoObj->setOrientation(orgsumoobject->getOrientation());
659         //if it is a dyna, register for a callback
660         m_sumoObj->registerCallback(*this);
661         
662         m_sumoScene->add(* (m_sumoObj));
663         */
664
665
666
667 }
668
669
670 void    CcdPhysicsController::SetCenterOfMassTransform(btTransform& xform)
671 {
672         btRigidBody* body = GetRigidBody();
673         if (body)
674         {
675                 body->setCenterOfMassTransform(xform);
676         } else
677         {
678                 //either collision object or soft body?
679                 if (GetSoftBody())
680                 {
681
682                 } else
683                 {
684
685                         if (m_object->isStaticOrKinematicObject())
686                         {
687                                 m_object->setInterpolationWorldTransform(m_object->getWorldTransform());
688                         } else
689                         {
690                                 m_object->setInterpolationWorldTransform(xform);
691                         }
692                         if (body)
693                         {
694                                 body->setInterpolationLinearVelocity(body->getLinearVelocity());
695                                 body->setInterpolationAngularVelocity(body->getAngularVelocity());
696                                 body->updateInertiaTensor();
697                         }
698                         m_object->setWorldTransform(xform);
699                 }
700         }
701 }
702
703                 // kinematic methods
704 void            CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
705 {
706         if (m_object)
707         {
708                 m_object->activate(true);
709                 if (m_object->isStaticObject())
710                 {
711                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
712                 }
713
714                 btRigidBody* body = GetRigidBody();
715
716                 btVector3 dloc(dlocX,dlocY,dlocZ);
717                 btTransform xform = m_object->getWorldTransform();
718         
719                 if (local)
720                 {
721                         dloc = xform.getBasis()*dloc;
722                 }
723
724                 xform.setOrigin(xform.getOrigin() + dloc);
725                 SetCenterOfMassTransform(xform);
726         }
727
728 }
729
730 void            CcdPhysicsController::RelativeRotate(const float rotval[9],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                 btMatrix3x3 drotmat(    rotval[0],rotval[4],rotval[8],
741                                                                 rotval[1],rotval[5],rotval[9],
742                                                                 rotval[2],rotval[6],rotval[10]);
743
744
745                 btMatrix3x3 currentOrn;
746                 GetWorldOrientation(currentOrn);
747
748                 btTransform xform = m_object->getWorldTransform();
749                 
750                 xform.setBasis(xform.getBasis()*(local ? 
751                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
752
753                 SetCenterOfMassTransform(xform);
754         }
755 }
756
757
758 void CcdPhysicsController::GetWorldOrientation(btMatrix3x3& mat)
759 {
760         float orn[4];
761         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
762         btQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
763         mat.setRotation(quat);
764 }
765
766 void            CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
767 {
768         btQuaternion q = m_object->getWorldTransform().getRotation();
769         quatImag0 = q[0];
770         quatImag1 = q[1];
771         quatImag2 = q[2];
772         quatReal = q[3];
773 }
774 void            CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
775 {
776         if (m_object)
777         {
778                 m_object->activate(true);
779                 if (m_object->isStaticObject())
780                 {
781                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
782                 }
783                 // not required
784                 //m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
785                 btTransform xform  = m_object->getWorldTransform();
786                 xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
787                 SetCenterOfMassTransform(xform);
788                 // not required
789                 //m_bulletMotionState->setWorldTransform(xform);
790                 
791                 
792
793         }
794
795 }
796
797 void CcdPhysicsController::setWorldOrientation(const btMatrix3x3& orn)
798 {
799         if (m_object)
800         {
801                 m_object->activate(true);
802                 if (m_object->isStaticObject())
803                 {
804                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
805                 }
806                 // not required
807                 //m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
808                 btTransform xform  = m_object->getWorldTransform();
809                 xform.setBasis(orn);
810                 SetCenterOfMassTransform(xform);
811                 // not required
812                 //m_bulletMotionState->setWorldTransform(xform);
813                 //only once!
814                 if (!m_softBodyTransformInitialized && GetSoftBody())
815                 {
816                         m_softbodyStartTrans.setBasis(orn);
817                         xform.setOrigin(m_softbodyStartTrans.getOrigin());
818                         GetSoftBody()->transform(xform);
819                         m_softBodyTransformInitialized = true;
820                 }
821
822         }
823
824 }
825
826 void            CcdPhysicsController::setPosition(float posX,float posY,float posZ)
827 {
828         if (m_object)
829         {
830                 m_object->activate(true);
831                 if (m_object->isStaticObject())
832                 {
833                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
834                 }
835                 // not required, this function is only used to update the physic controller
836                 //m_MotionState->setWorldPosition(posX,posY,posZ);
837                 btTransform xform  = m_object->getWorldTransform();
838                 xform.setOrigin(btVector3(posX,posY,posZ));
839                 SetCenterOfMassTransform(xform);
840                 if (!m_softBodyTransformInitialized)
841                         m_softbodyStartTrans.setOrigin(xform.getOrigin());
842                 // not required
843                 //m_bulletMotionState->setWorldTransform(xform);
844         }
845
846
847 }
848 void            CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
849 {
850 }
851
852 void            CcdPhysicsController::getPosition(PHY__Vector3& pos) const
853 {
854         const btTransform& xform = m_object->getWorldTransform();
855         pos[0] = xform.getOrigin().x();
856         pos[1] = xform.getOrigin().y();
857         pos[2] = xform.getOrigin().z();
858 }
859
860 void            CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
861 {
862         if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
863                 !btFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
864                 !btFuzzyZero(m_cci.m_scaling.z()-scaleZ))
865         {
866                 m_cci.m_scaling = btVector3(scaleX,scaleY,scaleZ);
867
868                 if (m_object && m_object->getCollisionShape())
869                 {
870                         m_object->getCollisionShape()->setLocalScaling(m_cci.m_scaling);
871                         
872                         //printf("no inertia recalc for fixed objects with mass=0\n");
873                         btRigidBody* body = GetRigidBody();
874                         if (body && m_cci.m_mass)
875                         {
876                                 body->getCollisionShape()->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
877                                 body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
878                         } 
879                         
880                 }
881         }
882 }
883                 
884                 // physics methods
885 void            CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
886 {
887         btVector3 torque(torqueX,torqueY,torqueZ);
888         btTransform xform = m_object->getWorldTransform();
889         
890
891         if (m_object && torque.length2() > (SIMD_EPSILON*SIMD_EPSILON))
892         {
893                 btRigidBody* body = GetRigidBody();
894                 m_object->activate();
895                 if (m_object->isStaticObject())
896                 {
897                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
898                 }
899                 if (local)
900                 {
901                         torque  = xform.getBasis()*torque;
902                 }
903                 if (body)
904                         body->applyTorque(torque);
905         }
906 }
907
908 void            CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
909 {
910         btVector3 force(forceX,forceY,forceZ);
911         
912
913         if (m_object && force.length2() > (SIMD_EPSILON*SIMD_EPSILON))
914         {
915                 m_object->activate();
916                 if (m_object->isStaticObject())
917                 {
918                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
919                 }
920
921                 {
922                         btTransform xform = m_object->getWorldTransform();
923                         
924                         if (local)
925                         {       
926                                 force   = xform.getBasis()*force;
927                         }
928                         btRigidBody* body = GetRigidBody();
929                         if (body)
930                                 body->applyCentralForce(force);
931                         btSoftBody* soft = GetSoftBody();
932                         if (soft)
933                                 soft->addForce(force);
934
935                 }
936         }
937 }
938 void            CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
939 {
940         btVector3 angvel(ang_velX,ang_velY,ang_velZ);
941         if (m_object && angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
942         {
943                 m_object->activate(true);
944                 if (m_object->isStaticObject())
945                 {
946                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
947                 }
948                 {
949                         btTransform xform = m_object->getWorldTransform();
950                         if (local)
951                         {
952                                 angvel  = xform.getBasis()*angvel;
953                         }
954                         btRigidBody* body = GetRigidBody();
955                         if (body)
956                                 body->setAngularVelocity(angvel);
957
958                 }
959         }
960
961 }
962 void            CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
963 {
964
965         btVector3 linVel(lin_velX,lin_velY,lin_velZ);
966         if (m_object && linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
967         {
968                 m_object->activate(true);
969                 if (m_object->isStaticObject())
970                 {
971                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
972                 }
973                 
974                 btSoftBody* soft = GetSoftBody();
975                 if (soft)
976                 {
977                         if (local)
978                         {
979                                 linVel  = m_softbodyStartTrans.getBasis()*linVel;
980                         }
981                         soft->setVelocity(linVel);
982                 } else
983                 {
984                         btTransform xform = m_object->getWorldTransform();
985                         if (local)
986                         {
987                                 linVel  = xform.getBasis()*linVel;
988                         }
989                         btRigidBody* body = GetRigidBody();
990                         if (body)
991                                 body->setLinearVelocity(linVel);
992                 }
993         }
994 }
995 void            CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
996 {
997         btVector3 impulse(impulseX,impulseY,impulseZ);
998
999         if (m_object && impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
1000         {
1001                 m_object->activate();
1002                 if (m_object->isStaticObject())
1003                 {
1004                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
1005                 }
1006                 
1007                 btVector3 pos(attachX,attachY,attachZ);
1008                 btRigidBody* body = GetRigidBody();
1009                 if (body)
1010                         body->applyImpulse(impulse,pos);
1011                         
1012         }
1013
1014 }
1015 void            CcdPhysicsController::SetActive(bool active)
1016 {
1017 }
1018                 // reading out information from physics
1019 void            CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
1020 {
1021         btRigidBody* body = GetRigidBody();
1022         if (body)
1023         {
1024                 const btVector3& linvel = body->getLinearVelocity();
1025                 linvX = linvel.x();
1026                 linvY = linvel.y();
1027                 linvZ = linvel.z();
1028         } else
1029         {
1030                 linvX = 0.f;
1031                 linvY = 0.f;
1032                 linvZ = 0.f;
1033         }
1034
1035 }
1036
1037 void            CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
1038 {
1039         btRigidBody* body = GetRigidBody();
1040         if (body)
1041         {
1042                 const btVector3& angvel= body->getAngularVelocity();
1043                 angVelX = angvel.x();
1044                 angVelY = angvel.y();
1045                 angVelZ = angvel.z();
1046         } else
1047         {
1048                 angVelX = 0.f;
1049                 angVelY = 0.f;
1050                 angVelZ = 0.f;
1051         }
1052 }
1053
1054 void            CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
1055 {
1056         btVector3 pos(posX,posY,posZ);
1057         btRigidBody* body = GetRigidBody();
1058         if (body)
1059         {
1060                 btVector3 rel_pos = pos-body->getCenterOfMassPosition();
1061                 btVector3 linvel = body->getVelocityInLocalPoint(rel_pos);
1062                 linvX = linvel.x();
1063                 linvY = linvel.y();
1064                 linvZ = linvel.z();
1065         } else
1066         {
1067                 linvX = 0.f;
1068                 linvY = 0.f;
1069                 linvZ = 0.f;
1070         }
1071 }
1072 void            CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
1073 {
1074 }
1075
1076                 // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
1077 void            CcdPhysicsController::setRigidBody(bool rigid)
1078 {
1079         if (!rigid)
1080         {
1081                 btRigidBody* body = GetRigidBody();
1082                 if (body)
1083                 {
1084                         //fake it for now
1085                         btVector3 inertia = body->getInvInertiaDiagLocal();
1086                         inertia[1] = 0.f;
1087                         body->setInvInertiaDiagLocal(inertia);
1088                         body->updateInertiaTensor();
1089                 }
1090         }
1091 }
1092
1093                 // clientinfo for raycasts for example
1094 void*           CcdPhysicsController::getNewClientInfo()
1095 {
1096         return m_newClientInfo;
1097 }
1098 void            CcdPhysicsController::setNewClientInfo(void* clientinfo)
1099 {
1100         m_newClientInfo = clientinfo;
1101 }
1102
1103
1104 void    CcdPhysicsController::UpdateDeactivation(float timeStep)
1105 {
1106         btRigidBody* body = GetRigidBody();
1107         if (body)
1108         {
1109                 body->updateDeactivation( timeStep);
1110         }
1111 }
1112
1113 bool CcdPhysicsController::wantsSleeping()
1114 {
1115         btRigidBody* body = GetRigidBody();
1116         if (body)
1117         {
1118                 return body->wantsSleeping();
1119         }
1120         //check it out
1121         return true;
1122 }
1123
1124 PHY_IPhysicsController* CcdPhysicsController::GetReplica()
1125 {
1126         // This is used only to replicate Near and Radar sensor controllers
1127         // The replication of object physics controller is done in KX_BulletPhysicsController::GetReplica()
1128         CcdConstructionInfo cinfo = m_cci;
1129         if (m_shapeInfo)
1130         {
1131                 // This situation does not normally happen
1132                 cinfo.m_collisionShape = m_shapeInfo->CreateBulletShape();
1133         } 
1134         else if (m_collisionShape)
1135         {
1136                 switch (m_collisionShape->getShapeType())
1137                 {
1138                 case SPHERE_SHAPE_PROXYTYPE:
1139                         {
1140                                 btSphereShape* orgShape = (btSphereShape*)m_collisionShape;
1141                                 cinfo.m_collisionShape = new btSphereShape(*orgShape);
1142                                 break;
1143                         }
1144
1145                 case CONE_SHAPE_PROXYTYPE:
1146                         {
1147                                 btConeShape* orgShape = (btConeShape*)m_collisionShape;
1148                                 cinfo.m_collisionShape = new btConeShape(*orgShape);
1149                                 break;
1150                         }
1151
1152                 default:
1153                         {
1154                                 return 0;
1155                         }
1156                 }
1157         }
1158
1159         cinfo.m_MotionState = new DefaultMotionState();
1160         cinfo.m_shapeInfo = m_shapeInfo;
1161
1162         CcdPhysicsController* replica = new CcdPhysicsController(cinfo);
1163         return replica;
1164 }
1165
1166 ///////////////////////////////////////////////////////////
1167 ///A small utility class, DefaultMotionState
1168 ///
1169 ///////////////////////////////////////////////////////////
1170
1171 DefaultMotionState::DefaultMotionState()
1172 {
1173         m_worldTransform.setIdentity();
1174         m_localScaling.setValue(1.f,1.f,1.f);
1175 }
1176
1177
1178 DefaultMotionState::~DefaultMotionState()
1179 {
1180
1181 }
1182
1183 void    DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
1184 {
1185         posX = m_worldTransform.getOrigin().x();
1186         posY = m_worldTransform.getOrigin().y();
1187         posZ = m_worldTransform.getOrigin().z();
1188 }
1189
1190 void    DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
1191 {
1192         scaleX = m_localScaling.getX();
1193         scaleY = m_localScaling.getY();
1194         scaleZ = m_localScaling.getZ();
1195 }
1196
1197 void    DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
1198 {
1199         quatIma0 = m_worldTransform.getRotation().x();
1200         quatIma1 = m_worldTransform.getRotation().y();
1201         quatIma2 = m_worldTransform.getRotation().z();
1202         quatReal = m_worldTransform.getRotation()[3];
1203 }
1204                 
1205 void    DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
1206 {
1207         btPoint3 pos(posX,posY,posZ);
1208         m_worldTransform.setOrigin( pos );
1209 }
1210
1211 void    DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
1212 {
1213         btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
1214         m_worldTransform.setRotation( orn );
1215 }
1216                 
1217 void    DefaultMotionState::calculateWorldTransformations()
1218 {
1219
1220 }
1221
1222 // Shape constructor
1223 std::map<RAS_MeshObject*, CcdShapeConstructionInfo*> CcdShapeConstructionInfo::m_meshShapeMap;
1224
1225 CcdShapeConstructionInfo* CcdShapeConstructionInfo::FindMesh(RAS_MeshObject* mesh, bool polytope)
1226 {
1227         if (polytope)
1228                 // not yet supported
1229                 return NULL;
1230
1231         std::map<RAS_MeshObject*,CcdShapeConstructionInfo*>::const_iterator mit = m_meshShapeMap.find(mesh);
1232         if (mit != m_meshShapeMap.end())
1233                 return mit->second;
1234         return NULL;
1235 }
1236
1237 bool CcdShapeConstructionInfo::SetMesh(RAS_MeshObject* meshobj, bool polytope,bool useGimpact)
1238 {
1239         m_useGimpact = useGimpact;
1240
1241         // assume no shape information
1242         // no support for dynamic change of shape yet
1243         assert(m_meshObject == NULL);
1244         m_shapeType = PHY_SHAPE_NONE;
1245         m_vertexArray.clear();
1246         m_polygonIndexArray.clear();
1247         m_meshObject = NULL;
1248
1249         if (!meshobj)
1250                 return false;
1251
1252         // Mesh has no polygons!
1253         int numpolys = meshobj->NumPolygons();
1254         if (!numpolys)
1255         {
1256                 return false;
1257         }
1258
1259         // check that we have at least one colliding polygon
1260         int numvalidpolys = 0;
1261
1262         for (int p=0; p<numpolys; p++)
1263         {
1264                 RAS_Polygon* poly = meshobj->GetPolygon(p);
1265
1266                 // only add polygons that have the collisionflag set
1267                 if (poly->IsCollider())
1268                 {
1269                         numvalidpolys++;
1270                         break;
1271                 }
1272         }
1273
1274         // No collision polygons
1275         if (numvalidpolys < 1)
1276                 return false;
1277
1278         m_shapeType = (polytope) ? PHY_SHAPE_POLYTOPE : PHY_SHAPE_MESH;
1279
1280         numvalidpolys = 0;
1281
1282         for (int p2=0; p2<numpolys; p2++)
1283         {
1284                 RAS_Polygon* poly = meshobj->GetPolygon(p2);
1285
1286                 // only add polygons that have the collisionflag set
1287                 if (poly->IsCollider())
1288                 {   
1289                         //Bullet can raycast any shape, so
1290                         if (polytope)
1291                         {
1292                                 for (int i=0;i<poly->VertexCount();i++)
1293                                 {
1294                                         const float* vtx = poly->GetVertex(i)->getXYZ();
1295                                         btPoint3 point(vtx[0],vtx[1],vtx[2]);
1296                                         //avoid duplicates (could better directly use vertex offsets, rather than a vertex compare)
1297                                         bool found = false;
1298                                         for (int j=0;j<m_vertexArray.size();j++)
1299                                         {
1300                                                 if (m_vertexArray[j]==point)
1301                                                 {
1302                                                         found = true;
1303                                                         break;
1304                                                 }
1305                                         }
1306                                         if (!found)
1307                                                 m_vertexArray.push_back(point);
1308
1309                                         numvalidpolys++;
1310                                 }
1311                         } else
1312                         {
1313                                 {
1314                                         const float* vtx = poly->GetVertex(2)->getXYZ();
1315                                         btPoint3 vertex0(vtx[0],vtx[1],vtx[2]);
1316
1317                                         vtx = poly->GetVertex(1)->getXYZ();
1318                                         btPoint3 vertex1(vtx[0],vtx[1],vtx[2]);
1319
1320                                         vtx = poly->GetVertex(0)->getXYZ();
1321                                         btPoint3 vertex2(vtx[0],vtx[1],vtx[2]);
1322
1323                                         m_vertexArray.push_back(vertex0);
1324                                         m_vertexArray.push_back(vertex1);
1325                                         m_vertexArray.push_back(vertex2);
1326                                         m_polygonIndexArray.push_back(p2);
1327                                         numvalidpolys++;
1328                                 }
1329                                 if (poly->VertexCount() == 4)
1330                                 {
1331                                         const float* vtx = poly->GetVertex(3)->getXYZ();
1332                                         btPoint3 vertex0(vtx[0],vtx[1],vtx[2]);
1333
1334                                         vtx = poly->GetVertex(2)->getXYZ();
1335                                         btPoint3 vertex1(vtx[0],vtx[1],vtx[2]);
1336
1337                                         vtx = poly->GetVertex(0)->getXYZ();
1338                                         btPoint3 vertex2(vtx[0],vtx[1],vtx[2]);
1339
1340                                         m_vertexArray.push_back(vertex0);
1341                                         m_vertexArray.push_back(vertex1);
1342                                         m_vertexArray.push_back(vertex2);
1343                                         m_polygonIndexArray.push_back(p2);
1344                                         numvalidpolys++;
1345                                 }
1346                         }               
1347                 }
1348         }
1349
1350         if (!numvalidpolys)
1351         {
1352                 // should not happen
1353                 m_shapeType = PHY_SHAPE_NONE;
1354                 return false;
1355         }
1356         m_meshObject = meshobj;
1357         if (!polytope)
1358         {
1359                 // triangle shape can be shared, store the mesh object in the map
1360                 m_meshShapeMap.insert(std::pair<RAS_MeshObject*,CcdShapeConstructionInfo*>(meshobj,this));
1361         }
1362         return true;
1363 }
1364
1365 btCollisionShape* CcdShapeConstructionInfo::CreateBulletShape()
1366 {
1367         btCollisionShape* collisionShape = 0;
1368         btTriangleMeshShape* concaveShape = 0;
1369         btTriangleMesh* collisionMeshData = 0;
1370         btCompoundShape* compoundShape = 0;
1371         CcdShapeConstructionInfo* nextShapeInfo;
1372
1373         switch (m_shapeType) 
1374         {
1375         case PHY_SHAPE_NONE:
1376                 break;
1377
1378         case PHY_SHAPE_BOX:
1379                 collisionShape = new btBoxShape(m_halfExtend);
1380                 break;
1381
1382         case PHY_SHAPE_SPHERE:
1383                 collisionShape = new btSphereShape(m_radius);
1384                 break;
1385
1386         case PHY_SHAPE_CYLINDER:
1387                 collisionShape = new btCylinderShapeZ(m_halfExtend);
1388                 break;
1389
1390         case PHY_SHAPE_CONE:
1391                 collisionShape = new btConeShapeZ(m_radius, m_height);
1392                 break;
1393
1394         case PHY_SHAPE_POLYTOPE:
1395                 collisionShape = new btConvexHullShape(&m_vertexArray.begin()->getX(), m_vertexArray.size());
1396                 break;
1397
1398         case PHY_SHAPE_MESH:
1399                 // Let's use the latest btScaledBvhTriangleMeshShape: it allows true sharing of 
1400                 // triangle mesh information between duplicates => drastic performance increase when 
1401                 // duplicating complex mesh objects. 
1402                 // BUT it causes a small performance decrease when sharing is not required: 
1403                 // 9 multiplications/additions and one function call for each triangle that passes the mid phase filtering
1404                 // One possible optimization is to use directly the btBvhTriangleMeshShape when the scale is 1,1,1
1405                 // and btScaledBvhTriangleMeshShape otherwise.
1406                 if (m_useGimpact)
1407                 {
1408                                 collisionMeshData = new btTriangleMesh();
1409                                 
1410
1411                                 // m_vertexArray is necessarily a multiple of 3
1412                                 for (std::vector<btPoint3>::iterator it=m_vertexArray.begin(); it != m_vertexArray.end(); )
1413                                 {
1414                                         collisionMeshData->addTriangle(*it++,*it++,*it++);
1415                                 }
1416                                 btGImpactMeshShape* gimpactShape =  new btGImpactMeshShape(collisionMeshData);
1417
1418                                 collisionShape = gimpactShape;
1419                                 gimpactShape->updateBound();
1420
1421                 } else
1422                 {
1423                         if (!m_unscaledShape)
1424                         {
1425                                 collisionMeshData = new btTriangleMesh(true,false);
1426                                 collisionMeshData->m_weldingThreshold = m_weldingThreshold;
1427
1428                                 // m_vertexArray is necessarily a multiple of 3
1429                                 for (std::vector<btPoint3>::iterator it=m_vertexArray.begin(); it != m_vertexArray.end(); )
1430                                 {
1431                                         collisionMeshData->addTriangle(*it++,*it++,*it++);
1432                                 }
1433                                 // this shape will be shared and not deleted until shapeInfo is deleted
1434                                 m_unscaledShape = new btBvhTriangleMeshShape( collisionMeshData, true );
1435                                 m_unscaledShape->recalcLocalAabb();
1436                         }
1437                         collisionShape = new btScaledBvhTriangleMeshShape(m_unscaledShape, btVector3(1.0f,1.0f,1.0f));
1438                 }
1439                 break;
1440
1441         case PHY_SHAPE_COMPOUND:
1442                 if (m_shapeArray.size() > 0)
1443                 {
1444                         compoundShape = new btCompoundShape();
1445                         for (std::vector<CcdShapeConstructionInfo*>::iterator sit = m_shapeArray.begin();
1446                                  sit != m_shapeArray.end();
1447                                  sit++)
1448                         {
1449                                 collisionShape = (*sit)->CreateBulletShape();
1450                                 if (collisionShape)
1451                                 {
1452                                         collisionShape->setLocalScaling((*sit)->m_childScale);
1453                                         compoundShape->addChildShape((*sit)->m_childTrans, collisionShape);
1454                                 }
1455                         }
1456                         collisionShape = compoundShape;
1457                 }
1458         }
1459         return collisionShape;
1460 }
1461
1462 void CcdShapeConstructionInfo::AddShape(CcdShapeConstructionInfo* shapeInfo)
1463 {
1464         m_shapeArray.push_back(shapeInfo);
1465 }
1466
1467 CcdShapeConstructionInfo::~CcdShapeConstructionInfo()
1468 {
1469         for (std::vector<CcdShapeConstructionInfo*>::iterator sit = m_shapeArray.begin();
1470                  sit != m_shapeArray.end();
1471                  sit++)
1472         {
1473                 (*sit)->Release();
1474         }
1475         m_shapeArray.clear();
1476         if (m_unscaledShape)
1477         {
1478                 DeleteBulletShape(m_unscaledShape);
1479         }
1480         m_vertexArray.clear();
1481         if (m_shapeType == PHY_SHAPE_MESH && m_meshObject != NULL) 
1482         {
1483                 std::map<RAS_MeshObject*,CcdShapeConstructionInfo*>::iterator mit = m_meshShapeMap.find(m_meshObject);
1484                 if (mit != m_meshShapeMap.end() && mit->second == this)
1485                 {
1486                         m_meshShapeMap.erase(mit);
1487                 }
1488         }
1489 }
1490
1491