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