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