added anisotropic friction support for Bullet. Both for static and dynamic objects
[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         if (m_object && m_cci.m_do_anisotropic)
504         {
505                 m_object->setAnisotropicFriction(m_cci.m_anisotropicFriction);
506         }
507                 
508 }
509
510 static void DeleteBulletShape(btCollisionShape* shape)
511 {
512         if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
513         {
514                 // shapes based on meshes use an interface that contains the vertices.
515                 btTriangleMeshShape* meshShape = static_cast<btTriangleMeshShape*>(shape);
516                 btStridingMeshInterface* meshInterface = meshShape->getMeshInterface();
517                 if (meshInterface)
518                         delete meshInterface;
519         }
520         delete shape;
521 }
522
523 CcdPhysicsController::~CcdPhysicsController()
524 {
525         //will be reference counted, due to sharing
526         if (m_cci.m_physicsEnv)
527                 m_cci.m_physicsEnv->removeCcdPhysicsController(this);
528
529         if (m_MotionState)
530                 delete m_MotionState;
531         if (m_bulletMotionState)
532                 delete m_bulletMotionState;
533         delete m_object;
534
535         if (m_collisionShape)
536         {
537                 // collision shape is always unique to the controller, can delete it here
538                 if (m_collisionShape->isCompound())
539                 {
540                         // bullet does not delete the child shape, must do it here
541                         btCompoundShape* compoundShape = (btCompoundShape*)m_collisionShape;
542                         int numChild = compoundShape->getNumChildShapes();
543                         for (int i=numChild-1 ; i >= 0; i--)
544                         {
545                                 btCollisionShape* childShape = compoundShape->getChildShape(i);
546                                 DeleteBulletShape(childShape);
547                         }
548                 }
549                 DeleteBulletShape(m_collisionShape);
550         }
551         if (m_shapeInfo)
552         {
553                 m_shapeInfo->Release();
554         }
555 }
556
557
558                 /**
559                         SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
560                 */
561 bool            CcdPhysicsController::SynchronizeMotionStates(float time)
562 {
563         //sync non-static to motionstate, and static from motionstate (todo: add kinematic etc.)
564
565         btSoftBody* sb = GetSoftBody();
566         if (sb)
567         {
568                 btVector3 aabbMin,aabbMax;
569                 sb->getAabb(aabbMin,aabbMax);
570                 btVector3 worldPos  = (aabbMax+aabbMin)*0.5f;
571                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
572                 m_MotionState->calculateWorldTransformations();
573                 return true;
574         }
575
576         btRigidBody* body = GetRigidBody();
577
578         if (body && !body->isStaticObject())
579         {
580
581                 const btVector3& worldPos = body->getCenterOfMassPosition();
582                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
583                 
584                 const btQuaternion& worldquat = body->getOrientation();
585                 m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
586
587                 m_MotionState->calculateWorldTransformations();
588
589                 float scale[3];
590                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
591                 btVector3 scaling(scale[0],scale[1],scale[2]);
592                 GetCollisionShape()->setLocalScaling(scaling);
593         } else
594         {
595                 btVector3 worldPos;
596                 btQuaternion worldquat;
597
598 /*              m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
599                 m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
600                 btTransform oldTrans = m_body->getCenterOfMassTransform();
601                 btTransform newTrans(worldquat,worldPos);
602                                 
603                 SetCenterOfMassTransform(newTrans);
604                 //need to keep track of previous position for friction effects...
605                 
606                 m_MotionState->calculateWorldTransformations();
607 */
608                 float scale[3];
609                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
610                 btVector3 scaling(scale[0],scale[1],scale[2]);
611                 GetCollisionShape()->setLocalScaling(scaling);
612         }
613         return true;
614
615 }
616
617                 /**
618                         WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
619                 */
620                 
621 void            CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
622 {
623
624 }
625 void            CcdPhysicsController::WriteDynamicsToMotionState()
626 {
627 }
628                 // controller replication
629 void            CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
630 {
631         m_softBodyTransformInitialized=false;
632         m_MotionState = motionstate;
633         m_registerCount = 0;
634         m_collisionShape = NULL;
635
636         // always create a new shape to avoid scaling bug
637         if (m_shapeInfo)
638         {
639                 m_shapeInfo->AddRef();
640                 m_collisionShape = m_shapeInfo->CreateBulletShape();
641
642                 if (m_collisionShape)
643                 {
644                         // new shape has no scaling, apply initial scaling
645                         m_collisionShape->setMargin(m_cci.m_margin);
646                         m_collisionShape->setLocalScaling(m_cci.m_scaling);
647                         
648                         if (m_cci.m_mass)
649                                 m_collisionShape->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
650                 }
651         }
652
653         m_object = 0;
654         CreateRigidbody();
655
656         btRigidBody* body = GetRigidBody();
657
658         if (body)
659         {
660                 if (m_cci.m_mass)
661                 {
662                         body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
663                 }
664         }                       
665         m_cci.m_physicsEnv->addCcdPhysicsController(this);
666
667
668 /*      SM_Object* dynaparent=0;
669         SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
670         
671         if (sumoparentctrl)
672         {
673                 dynaparent = sumoparentctrl->GetSumoObject();
674         }
675         
676         SM_Object* orgsumoobject = m_sumoObj;
677         
678         
679         m_sumoObj       =       new SM_Object(
680                 orgsumoobject->getShapeHandle(), 
681                 orgsumoobject->getMaterialProps(),                      
682                 orgsumoobject->getShapeProps(),
683                 dynaparent);
684         
685         m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
686         
687         m_sumoObj->setMargin(orgsumoobject->getMargin());
688         m_sumoObj->setPosition(orgsumoobject->getPosition());
689         m_sumoObj->setOrientation(orgsumoobject->getOrientation());
690         //if it is a dyna, register for a callback
691         m_sumoObj->registerCallback(*this);
692         
693         m_sumoScene->add(* (m_sumoObj));
694         */
695
696
697
698 }
699
700
701 void    CcdPhysicsController::SetCenterOfMassTransform(btTransform& xform)
702 {
703         btRigidBody* body = GetRigidBody();
704         if (body)
705         {
706                 body->setCenterOfMassTransform(xform);
707         } else
708         {
709                 //either collision object or soft body?
710                 if (GetSoftBody())
711                 {
712
713                 } else
714                 {
715
716                         if (m_object->isStaticOrKinematicObject())
717                         {
718                                 m_object->setInterpolationWorldTransform(m_object->getWorldTransform());
719                         } else
720                         {
721                                 m_object->setInterpolationWorldTransform(xform);
722                         }
723                         if (body)
724                         {
725                                 body->setInterpolationLinearVelocity(body->getLinearVelocity());
726                                 body->setInterpolationAngularVelocity(body->getAngularVelocity());
727                                 body->updateInertiaTensor();
728                         }
729                         m_object->setWorldTransform(xform);
730                 }
731         }
732 }
733
734                 // kinematic methods
735 void            CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
736 {
737         if (m_object)
738         {
739                 m_object->activate(true);
740                 if (m_object->isStaticObject())
741                 {
742                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
743                 }
744
745                 btRigidBody* body = GetRigidBody();
746
747                 btVector3 dloc(dlocX,dlocY,dlocZ);
748                 btTransform xform = m_object->getWorldTransform();
749         
750                 if (local)
751                 {
752                         dloc = xform.getBasis()*dloc;
753                 }
754
755                 xform.setOrigin(xform.getOrigin() + dloc);
756                 SetCenterOfMassTransform(xform);
757         }
758
759 }
760
761 void            CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
762 {
763         if (m_object)
764         {
765                 m_object->activate(true);
766                 if (m_object->isStaticObject())
767                 {
768                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
769                 }
770
771                 btMatrix3x3 drotmat(    rotval[0],rotval[4],rotval[8],
772                                                                 rotval[1],rotval[5],rotval[9],
773                                                                 rotval[2],rotval[6],rotval[10]);
774
775
776                 btMatrix3x3 currentOrn;
777                 GetWorldOrientation(currentOrn);
778
779                 btTransform xform = m_object->getWorldTransform();
780                 
781                 xform.setBasis(xform.getBasis()*(local ? 
782                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
783
784                 SetCenterOfMassTransform(xform);
785         }
786 }
787
788
789 void CcdPhysicsController::GetWorldOrientation(btMatrix3x3& mat)
790 {
791         float orn[4];
792         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
793         btQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
794         mat.setRotation(quat);
795 }
796
797 void            CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
798 {
799         btQuaternion q = m_object->getWorldTransform().getRotation();
800         quatImag0 = q[0];
801         quatImag1 = q[1];
802         quatImag2 = q[2];
803         quatReal = q[3];
804 }
805 void            CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
806 {
807         if (m_object)
808         {
809                 m_object->activate(true);
810                 if (m_object->isStaticObject())
811                 {
812                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
813                 }
814                 // not required
815                 //m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
816                 btTransform xform  = m_object->getWorldTransform();
817                 xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
818                 SetCenterOfMassTransform(xform);
819                 // not required
820                 //m_bulletMotionState->setWorldTransform(xform);
821                 
822                 
823
824         }
825
826 }
827
828 void CcdPhysicsController::setWorldOrientation(const btMatrix3x3& orn)
829 {
830         if (m_object)
831         {
832                 m_object->activate(true);
833                 if (m_object->isStaticObject())
834                 {
835                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
836                 }
837                 // not required
838                 //m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
839                 btTransform xform  = m_object->getWorldTransform();
840                 xform.setBasis(orn);
841                 SetCenterOfMassTransform(xform);
842                 // not required
843                 //m_bulletMotionState->setWorldTransform(xform);
844                 //only once!
845                 if (!m_softBodyTransformInitialized && GetSoftBody())
846                 {
847                         m_softbodyStartTrans.setBasis(orn);
848                         xform.setOrigin(m_softbodyStartTrans.getOrigin());
849                         GetSoftBody()->transform(xform);
850                         m_softBodyTransformInitialized = true;
851                 }
852
853         }
854
855 }
856
857 void            CcdPhysicsController::setPosition(float posX,float posY,float posZ)
858 {
859         if (m_object)
860         {
861                 m_object->activate(true);
862                 if (m_object->isStaticObject())
863                 {
864                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
865                 }
866                 // not required, this function is only used to update the physic controller
867                 //m_MotionState->setWorldPosition(posX,posY,posZ);
868                 btTransform xform  = m_object->getWorldTransform();
869                 xform.setOrigin(btVector3(posX,posY,posZ));
870                 SetCenterOfMassTransform(xform);
871                 if (!m_softBodyTransformInitialized)
872                         m_softbodyStartTrans.setOrigin(xform.getOrigin());
873                 // not required
874                 //m_bulletMotionState->setWorldTransform(xform);
875         }
876
877
878 }
879 void            CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
880 {
881 }
882
883 void            CcdPhysicsController::getPosition(PHY__Vector3& pos) const
884 {
885         const btTransform& xform = m_object->getWorldTransform();
886         pos[0] = xform.getOrigin().x();
887         pos[1] = xform.getOrigin().y();
888         pos[2] = xform.getOrigin().z();
889 }
890
891 void            CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
892 {
893         if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
894                 !btFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
895                 !btFuzzyZero(m_cci.m_scaling.z()-scaleZ))
896         {
897                 m_cci.m_scaling = btVector3(scaleX,scaleY,scaleZ);
898
899                 if (m_object && m_object->getCollisionShape())
900                 {
901                         m_object->getCollisionShape()->setLocalScaling(m_cci.m_scaling);
902                         
903                         //printf("no inertia recalc for fixed objects with mass=0\n");
904                         btRigidBody* body = GetRigidBody();
905                         if (body && m_cci.m_mass)
906                         {
907                                 body->getCollisionShape()->calculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
908                                 body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
909                         } 
910                         
911                 }
912         }
913 }
914                 
915                 // physics methods
916 void            CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
917 {
918         btVector3 torque(torqueX,torqueY,torqueZ);
919         btTransform xform = m_object->getWorldTransform();
920         
921
922         if (m_object && torque.length2() > (SIMD_EPSILON*SIMD_EPSILON))
923         {
924                 btRigidBody* body = GetRigidBody();
925                 m_object->activate();
926                 if (m_object->isStaticObject())
927                 {
928                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
929                 }
930                 if (local)
931                 {
932                         torque  = xform.getBasis()*torque;
933                 }
934                 if (body)
935                         body->applyTorque(torque);
936         }
937 }
938
939 void            CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
940 {
941         btVector3 force(forceX,forceY,forceZ);
942         
943
944         if (m_object && force.length2() > (SIMD_EPSILON*SIMD_EPSILON))
945         {
946                 m_object->activate();
947                 if (m_object->isStaticObject())
948                 {
949                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
950                 }
951
952                 {
953                         btTransform xform = m_object->getWorldTransform();
954                         
955                         if (local)
956                         {       
957                                 force   = xform.getBasis()*force;
958                         }
959                         btRigidBody* body = GetRigidBody();
960                         if (body)
961                                 body->applyCentralForce(force);
962                         btSoftBody* soft = GetSoftBody();
963                         if (soft)
964                                 soft->addForce(force);
965
966                 }
967         }
968 }
969 void            CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
970 {
971         btVector3 angvel(ang_velX,ang_velY,ang_velZ);
972         if (m_object && angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
973         {
974                 m_object->activate(true);
975                 if (m_object->isStaticObject())
976                 {
977                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
978                 }
979                 {
980                         btTransform xform = m_object->getWorldTransform();
981                         if (local)
982                         {
983                                 angvel  = xform.getBasis()*angvel;
984                         }
985                         btRigidBody* body = GetRigidBody();
986                         if (body)
987                                 body->setAngularVelocity(angvel);
988
989                 }
990         }
991
992 }
993 void            CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
994 {
995
996         btVector3 linVel(lin_velX,lin_velY,lin_velZ);
997         if (m_object/* && linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON)*/)
998         {
999                 m_object->activate(true);
1000                 if (m_object->isStaticObject())
1001                 {
1002                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
1003                 }
1004                 
1005                 btSoftBody* soft = GetSoftBody();
1006                 if (soft)
1007                 {
1008                         if (local)
1009                         {
1010                                 linVel  = m_softbodyStartTrans.getBasis()*linVel;
1011                         }
1012                         soft->setVelocity(linVel);
1013                 } else
1014                 {
1015                         btTransform xform = m_object->getWorldTransform();
1016                         if (local)
1017                         {
1018                                 linVel  = xform.getBasis()*linVel;
1019                         }
1020                         btRigidBody* body = GetRigidBody();
1021                         if (body)
1022                                 body->setLinearVelocity(linVel);
1023                 }
1024         }
1025 }
1026 void            CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
1027 {
1028         btVector3 impulse(impulseX,impulseY,impulseZ);
1029
1030         if (m_object && impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
1031         {
1032                 m_object->activate();
1033                 if (m_object->isStaticObject())
1034                 {
1035                         m_object->setCollisionFlags(m_object->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
1036                 }
1037                 
1038                 btVector3 pos(attachX,attachY,attachZ);
1039                 btRigidBody* body = GetRigidBody();
1040                 if (body)
1041                         body->applyImpulse(impulse,pos);
1042                         
1043         }
1044
1045 }
1046 void            CcdPhysicsController::SetActive(bool active)
1047 {
1048 }
1049                 // reading out information from physics
1050 void            CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
1051 {
1052         btRigidBody* body = GetRigidBody();
1053         if (body)
1054         {
1055                 const btVector3& linvel = body->getLinearVelocity();
1056                 linvX = linvel.x();
1057                 linvY = linvel.y();
1058                 linvZ = linvel.z();
1059         } else
1060         {
1061                 linvX = 0.f;
1062                 linvY = 0.f;
1063                 linvZ = 0.f;
1064         }
1065
1066 }
1067
1068 void            CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
1069 {
1070         btRigidBody* body = GetRigidBody();
1071         if (body)
1072         {
1073                 const btVector3& angvel= body->getAngularVelocity();
1074                 angVelX = angvel.x();
1075                 angVelY = angvel.y();
1076                 angVelZ = angvel.z();
1077         } else
1078         {
1079                 angVelX = 0.f;
1080                 angVelY = 0.f;
1081                 angVelZ = 0.f;
1082         }
1083 }
1084
1085 void            CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
1086 {
1087         btVector3 pos(posX,posY,posZ);
1088         btRigidBody* body = GetRigidBody();
1089         if (body)
1090         {
1091                 btVector3 rel_pos = pos-body->getCenterOfMassPosition();
1092                 btVector3 linvel = body->getVelocityInLocalPoint(rel_pos);
1093                 linvX = linvel.x();
1094                 linvY = linvel.y();
1095                 linvZ = linvel.z();
1096         } else
1097         {
1098                 linvX = 0.f;
1099                 linvY = 0.f;
1100                 linvZ = 0.f;
1101         }
1102 }
1103 void            CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
1104 {
1105 }
1106
1107                 // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
1108 void            CcdPhysicsController::setRigidBody(bool rigid)
1109 {
1110         if (!rigid)
1111         {
1112                 btRigidBody* body = GetRigidBody();
1113                 if (body)
1114                 {
1115                         //fake it for now
1116                         btVector3 inertia = body->getInvInertiaDiagLocal();
1117                         inertia[1] = 0.f;
1118                         body->setInvInertiaDiagLocal(inertia);
1119                         body->updateInertiaTensor();
1120                 }
1121         }
1122 }
1123
1124                 // clientinfo for raycasts for example
1125 void*           CcdPhysicsController::getNewClientInfo()
1126 {
1127         return m_newClientInfo;
1128 }
1129 void            CcdPhysicsController::setNewClientInfo(void* clientinfo)
1130 {
1131         m_newClientInfo = clientinfo;
1132 }
1133
1134
1135 void    CcdPhysicsController::UpdateDeactivation(float timeStep)
1136 {
1137         btRigidBody* body = GetRigidBody();
1138         if (body)
1139         {
1140                 body->updateDeactivation( timeStep);
1141         }
1142 }
1143
1144 bool CcdPhysicsController::wantsSleeping()
1145 {
1146         btRigidBody* body = GetRigidBody();
1147         if (body)
1148         {
1149                 return body->wantsSleeping();
1150         }
1151         //check it out
1152         return true;
1153 }
1154
1155 PHY_IPhysicsController* CcdPhysicsController::GetReplica()
1156 {
1157         // This is used only to replicate Near and Radar sensor controllers
1158         // The replication of object physics controller is done in KX_BulletPhysicsController::GetReplica()
1159         CcdConstructionInfo cinfo = m_cci;
1160         if (m_shapeInfo)
1161         {
1162                 // This situation does not normally happen
1163                 cinfo.m_collisionShape = m_shapeInfo->CreateBulletShape();
1164         } 
1165         else if (m_collisionShape)
1166         {
1167                 switch (m_collisionShape->getShapeType())
1168                 {
1169                 case SPHERE_SHAPE_PROXYTYPE:
1170                         {
1171                                 btSphereShape* orgShape = (btSphereShape*)m_collisionShape;
1172                                 cinfo.m_collisionShape = new btSphereShape(*orgShape);
1173                                 break;
1174                         }
1175
1176                 case CONE_SHAPE_PROXYTYPE:
1177                         {
1178                                 btConeShape* orgShape = (btConeShape*)m_collisionShape;
1179                                 cinfo.m_collisionShape = new btConeShape(*orgShape);
1180                                 break;
1181                         }
1182
1183                 default:
1184                         {
1185                                 return 0;
1186                         }
1187                 }
1188         }
1189
1190         cinfo.m_MotionState = new DefaultMotionState();
1191         cinfo.m_shapeInfo = m_shapeInfo;
1192
1193         CcdPhysicsController* replica = new CcdPhysicsController(cinfo);
1194         return replica;
1195 }
1196
1197 ///////////////////////////////////////////////////////////
1198 ///A small utility class, DefaultMotionState
1199 ///
1200 ///////////////////////////////////////////////////////////
1201
1202 DefaultMotionState::DefaultMotionState()
1203 {
1204         m_worldTransform.setIdentity();
1205         m_localScaling.setValue(1.f,1.f,1.f);
1206 }
1207
1208
1209 DefaultMotionState::~DefaultMotionState()
1210 {
1211
1212 }
1213
1214 void    DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
1215 {
1216         posX = m_worldTransform.getOrigin().x();
1217         posY = m_worldTransform.getOrigin().y();
1218         posZ = m_worldTransform.getOrigin().z();
1219 }
1220
1221 void    DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
1222 {
1223         scaleX = m_localScaling.getX();
1224         scaleY = m_localScaling.getY();
1225         scaleZ = m_localScaling.getZ();
1226 }
1227
1228 void    DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
1229 {
1230         quatIma0 = m_worldTransform.getRotation().x();
1231         quatIma1 = m_worldTransform.getRotation().y();
1232         quatIma2 = m_worldTransform.getRotation().z();
1233         quatReal = m_worldTransform.getRotation()[3];
1234 }
1235                 
1236 void    DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
1237 {
1238         btPoint3 pos(posX,posY,posZ);
1239         m_worldTransform.setOrigin( pos );
1240 }
1241
1242 void    DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
1243 {
1244         btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
1245         m_worldTransform.setRotation( orn );
1246 }
1247                 
1248 void    DefaultMotionState::calculateWorldTransformations()
1249 {
1250
1251 }
1252
1253 // Shape constructor
1254 std::map<RAS_MeshObject*, CcdShapeConstructionInfo*> CcdShapeConstructionInfo::m_meshShapeMap;
1255
1256 CcdShapeConstructionInfo* CcdShapeConstructionInfo::FindMesh(RAS_MeshObject* mesh, bool polytope)
1257 {
1258         if (polytope)
1259                 // not yet supported
1260                 return NULL;
1261
1262         std::map<RAS_MeshObject*,CcdShapeConstructionInfo*>::const_iterator mit = m_meshShapeMap.find(mesh);
1263         if (mit != m_meshShapeMap.end())
1264                 return mit->second;
1265         return NULL;
1266 }
1267
1268 bool CcdShapeConstructionInfo::SetMesh(RAS_MeshObject* meshobj, bool polytope,bool useGimpact)
1269 {
1270         m_useGimpact = useGimpact;
1271
1272         // assume no shape information
1273         // no support for dynamic change of shape yet
1274         assert(m_meshObject == NULL);
1275         m_shapeType = PHY_SHAPE_NONE;
1276         m_vertexArray.clear();
1277         m_polygonIndexArray.clear();
1278         m_meshObject = NULL;
1279
1280         if (!meshobj)
1281                 return false;
1282
1283         // Mesh has no polygons!
1284         int numpolys = meshobj->NumPolygons();
1285         if (!numpolys)
1286         {
1287                 return false;
1288         }
1289
1290         // check that we have at least one colliding polygon
1291         int numvalidpolys = 0;
1292
1293         for (int p=0; p<numpolys; p++)
1294         {
1295                 RAS_Polygon* poly = meshobj->GetPolygon(p);
1296
1297                 // only add polygons that have the collisionflag set
1298                 if (poly->IsCollider())
1299                 {
1300                         numvalidpolys++;
1301                         break;
1302                 }
1303         }
1304
1305         // No collision polygons
1306         if (numvalidpolys < 1)
1307                 return false;
1308
1309         m_shapeType = (polytope) ? PHY_SHAPE_POLYTOPE : PHY_SHAPE_MESH;
1310
1311         numvalidpolys = 0;
1312
1313         for (int p2=0; p2<numpolys; p2++)
1314         {
1315                 RAS_Polygon* poly = meshobj->GetPolygon(p2);
1316
1317                 // only add polygons that have the collisionflag set
1318                 if (poly->IsCollider())
1319                 {   
1320                         //Bullet can raycast any shape, so
1321                         if (polytope)
1322                         {
1323                                 for (int i=0;i<poly->VertexCount();i++)
1324                                 {
1325                                         const float* vtx = poly->GetVertex(i)->getXYZ();
1326                                         btPoint3 point(vtx[0],vtx[1],vtx[2]);
1327                                         //avoid duplicates (could better directly use vertex offsets, rather than a vertex compare)
1328                                         bool found = false;
1329                                         for (int j=0;j<m_vertexArray.size();j++)
1330                                         {
1331                                                 if (m_vertexArray[j]==point)
1332                                                 {
1333                                                         found = true;
1334                                                         break;
1335                                                 }
1336                                         }
1337                                         if (!found)
1338                                                 m_vertexArray.push_back(point);
1339
1340                                         numvalidpolys++;
1341                                 }
1342                         } else
1343                         {
1344                                 {
1345                                         const float* vtx = poly->GetVertex(2)->getXYZ();
1346                                         btPoint3 vertex0(vtx[0],vtx[1],vtx[2]);
1347
1348                                         vtx = poly->GetVertex(1)->getXYZ();
1349                                         btPoint3 vertex1(vtx[0],vtx[1],vtx[2]);
1350
1351                                         vtx = poly->GetVertex(0)->getXYZ();
1352                                         btPoint3 vertex2(vtx[0],vtx[1],vtx[2]);
1353
1354                                         m_vertexArray.push_back(vertex0);
1355                                         m_vertexArray.push_back(vertex1);
1356                                         m_vertexArray.push_back(vertex2);
1357                                         m_polygonIndexArray.push_back(p2);
1358                                         numvalidpolys++;
1359                                 }
1360                                 if (poly->VertexCount() == 4)
1361                                 {
1362                                         const float* vtx = poly->GetVertex(3)->getXYZ();
1363                                         btPoint3 vertex0(vtx[0],vtx[1],vtx[2]);
1364
1365                                         vtx = poly->GetVertex(2)->getXYZ();
1366                                         btPoint3 vertex1(vtx[0],vtx[1],vtx[2]);
1367
1368                                         vtx = poly->GetVertex(0)->getXYZ();
1369                                         btPoint3 vertex2(vtx[0],vtx[1],vtx[2]);
1370
1371                                         m_vertexArray.push_back(vertex0);
1372                                         m_vertexArray.push_back(vertex1);
1373                                         m_vertexArray.push_back(vertex2);
1374                                         m_polygonIndexArray.push_back(p2);
1375                                         numvalidpolys++;
1376                                 }
1377                         }               
1378                 }
1379         }
1380
1381         if (!numvalidpolys)
1382         {
1383                 // should not happen
1384                 m_shapeType = PHY_SHAPE_NONE;
1385                 return false;
1386         }
1387         m_meshObject = meshobj;
1388         if (!polytope)
1389         {
1390                 // triangle shape can be shared, store the mesh object in the map
1391                 m_meshShapeMap.insert(std::pair<RAS_MeshObject*,CcdShapeConstructionInfo*>(meshobj,this));
1392         }
1393         return true;
1394 }
1395
1396 btCollisionShape* CcdShapeConstructionInfo::CreateBulletShape()
1397 {
1398         btCollisionShape* collisionShape = 0;
1399         btTriangleMeshShape* concaveShape = 0;
1400         btTriangleMesh* collisionMeshData = 0;
1401         btCompoundShape* compoundShape = 0;
1402         CcdShapeConstructionInfo* nextShapeInfo;
1403
1404         switch (m_shapeType) 
1405         {
1406         case PHY_SHAPE_NONE:
1407                 break;
1408
1409         case PHY_SHAPE_BOX:
1410                 collisionShape = new btBoxShape(m_halfExtend);
1411                 break;
1412
1413         case PHY_SHAPE_SPHERE:
1414                 collisionShape = new btSphereShape(m_radius);
1415                 break;
1416
1417         case PHY_SHAPE_CYLINDER:
1418                 collisionShape = new btCylinderShapeZ(m_halfExtend);
1419                 break;
1420
1421         case PHY_SHAPE_CONE:
1422                 collisionShape = new btConeShapeZ(m_radius, m_height);
1423                 break;
1424
1425         case PHY_SHAPE_POLYTOPE:
1426                 collisionShape = new btConvexHullShape(&m_vertexArray.begin()->getX(), m_vertexArray.size());
1427                 break;
1428
1429         case PHY_SHAPE_MESH:
1430                 // Let's use the latest btScaledBvhTriangleMeshShape: it allows true sharing of 
1431                 // triangle mesh information between duplicates => drastic performance increase when 
1432                 // duplicating complex mesh objects. 
1433                 // BUT it causes a small performance decrease when sharing is not required: 
1434                 // 9 multiplications/additions and one function call for each triangle that passes the mid phase filtering
1435                 // One possible optimization is to use directly the btBvhTriangleMeshShape when the scale is 1,1,1
1436                 // and btScaledBvhTriangleMeshShape otherwise.
1437                 if (m_useGimpact)
1438                 {
1439                                 collisionMeshData = new btTriangleMesh();
1440                                 
1441
1442                                 // m_vertexArray is necessarily a multiple of 3
1443                                 for (std::vector<btPoint3>::iterator it=m_vertexArray.begin(); it != m_vertexArray.end(); )
1444                                 {
1445                                         collisionMeshData->addTriangle(*it++,*it++,*it++);
1446                                 }
1447                                 btGImpactMeshShape* gimpactShape =  new btGImpactMeshShape(collisionMeshData);
1448
1449                                 collisionShape = gimpactShape;
1450                                 gimpactShape->updateBound();
1451
1452                 } else
1453                 {
1454                         if (!m_unscaledShape)
1455                         {
1456                                 collisionMeshData = new btTriangleMesh(true,false);
1457                                 collisionMeshData->m_weldingThreshold = m_weldingThreshold;
1458
1459                                 // m_vertexArray is necessarily a multiple of 3
1460                                 for (std::vector<btPoint3>::iterator it=m_vertexArray.begin(); it != m_vertexArray.end(); )
1461                                 {
1462                                         collisionMeshData->addTriangle(*it++,*it++,*it++);
1463                                 }
1464                                 // this shape will be shared and not deleted until shapeInfo is deleted
1465                                 m_unscaledShape = new btBvhTriangleMeshShape( collisionMeshData, true );
1466                                 m_unscaledShape->recalcLocalAabb();
1467                         }
1468                         collisionShape = new btScaledBvhTriangleMeshShape(m_unscaledShape, btVector3(1.0f,1.0f,1.0f));
1469                 }
1470                 break;
1471
1472         case PHY_SHAPE_COMPOUND:
1473                 if (m_shapeArray.size() > 0)
1474                 {
1475                         compoundShape = new btCompoundShape();
1476                         for (std::vector<CcdShapeConstructionInfo*>::iterator sit = m_shapeArray.begin();
1477                                  sit != m_shapeArray.end();
1478                                  sit++)
1479                         {
1480                                 collisionShape = (*sit)->CreateBulletShape();
1481                                 if (collisionShape)
1482                                 {
1483                                         collisionShape->setLocalScaling((*sit)->m_childScale);
1484                                         compoundShape->addChildShape((*sit)->m_childTrans, collisionShape);
1485                                 }
1486                         }
1487                         collisionShape = compoundShape;
1488                 }
1489         }
1490         return collisionShape;
1491 }
1492
1493 void CcdShapeConstructionInfo::AddShape(CcdShapeConstructionInfo* shapeInfo)
1494 {
1495         m_shapeArray.push_back(shapeInfo);
1496 }
1497
1498 CcdShapeConstructionInfo::~CcdShapeConstructionInfo()
1499 {
1500         for (std::vector<CcdShapeConstructionInfo*>::iterator sit = m_shapeArray.begin();
1501                  sit != m_shapeArray.end();
1502                  sit++)
1503         {
1504                 (*sit)->Release();
1505         }
1506         m_shapeArray.clear();
1507         if (m_unscaledShape)
1508         {
1509                 DeleteBulletShape(m_unscaledShape);
1510         }
1511         m_vertexArray.clear();
1512         if (m_shapeType == PHY_SHAPE_MESH && m_meshObject != NULL) 
1513         {
1514                 std::map<RAS_MeshObject*,CcdShapeConstructionInfo*>::iterator mit = m_meshShapeMap.find(m_meshObject);
1515                 if (mit != m_meshShapeMap.end() && mit->second == this)
1516                 {
1517                         m_meshShapeMap.erase(mit);
1518                 }
1519         }
1520 }
1521
1522