upgraded Bullet rigidbody physics to latest version 1.9
[blender-staging.git] / source / gameengine / Physics / Bullet / CcdPhysicsController.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "CcdPhysicsController.h"
17
18 #include "Dynamics/RigidBody.h"
19 #include "PHY_IMotionState.h"
20 #include "BroadphaseCollision/BroadphaseProxy.h"
21 #include "BroadphaseCollision/BroadphaseInterface.h"
22 #include "CollisionShapes/ConvexShape.h"
23 #include "CcdPhysicsEnvironment.h"
24 #include "SimdTransformUtil.h"
25
26 #include "CollisionShapes/SphereShape.h"
27 #include "CollisionShapes/ConeShape.h"
28
29 class BP_Proxy;
30
31 ///todo: fill all the empty CcdPhysicsController methods, hook them up to the RigidBody class
32
33 //'temporarily' global variables
34 float   gDeactivationTime = 2.f;
35 bool    gDisableDeactivation = false;
36
37 float gLinearSleepingTreshold = 0.8f;
38 float gAngularSleepingTreshold = 1.0f;
39
40 #include "Dynamics/MassProps.h"
41
42 SimdVector3 startVel(0,0,0);//-10000);
43 CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
44 :m_cci(ci)
45 {
46         m_collisionDelay = 0;
47         m_newClientInfo = 0;
48         
49         m_MotionState = ci.m_MotionState;
50
51         
52         
53         CreateRigidbody();
54         
55
56         
57         #ifdef WIN32
58         if (m_body->getInvMass())
59                 m_body->setLinearVelocity(startVel);
60         #endif
61
62 }
63
64 SimdTransform   CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
65 {
66         SimdTransform trans;
67         float tmp[3];
68         motionState->getWorldPosition(tmp[0],tmp[1],tmp[2]);
69         trans.setOrigin(SimdVector3(tmp[0],tmp[1],tmp[2]));
70
71         SimdQuaternion orn;
72         motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
73         trans.setRotation(orn);
74         return trans;
75
76 }
77
78 void CcdPhysicsController::CreateRigidbody()
79 {
80
81         SimdTransform trans = GetTransformFromMotionState(m_cci.m_MotionState);
82
83         MassProps mp(m_cci.m_mass, m_cci.m_localInertiaTensor);
84
85         m_body = new RigidBody(mp,0,0,m_cci.m_friction,m_cci.m_restitution);
86         m_body->m_collisionShape = m_cci.m_collisionShape;
87         
88
89         //
90         // init the rigidbody properly
91         //
92         
93         m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
94         //setMassProps this also sets collisionFlags
95         m_body->m_collisionFlags = m_cci.m_collisionFlags;
96         
97         m_body->setGravity( m_cci.m_gravity);
98         m_body->setDamping(m_cci.m_linearDamping, m_cci.m_angularDamping);
99         m_body->setCenterOfMassTransform( trans );
100
101
102 }
103
104 CcdPhysicsController::~CcdPhysicsController()
105 {
106         //will be reference counted, due to sharing
107         m_cci.m_physicsEnv->removeCcdPhysicsController(this);
108         delete m_MotionState;
109         delete m_body;
110 }
111
112                 /**
113                         SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
114                 */
115 bool            CcdPhysicsController::SynchronizeMotionStates(float time)
116 {
117         //sync non-static to motionstate, and static from motionstate (todo: add kinematic etc.)
118
119         if (!m_body->IsStatic())
120         {
121                 const SimdVector3& worldPos = m_body->getCenterOfMassPosition();
122                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
123                 
124                 const SimdQuaternion& worldquat = m_body->getOrientation();
125                 m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
126
127                 m_MotionState->calculateWorldTransformations();
128
129                 float scale[3];
130                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
131                 SimdVector3 scaling(scale[0],scale[1],scale[2]);
132                 GetCollisionShape()->setLocalScaling(scaling);
133         } else
134         {
135                 SimdVector3 worldPos;
136                 SimdQuaternion worldquat;
137
138                 m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
139                 m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
140                 SimdTransform oldTrans = m_body->getCenterOfMassTransform();
141                 SimdTransform newTrans(worldquat,worldPos);
142                                 
143                 m_body->setCenterOfMassTransform(newTrans);
144                 //need to keep track of previous position for friction effects...
145                 
146                 m_MotionState->calculateWorldTransformations();
147
148                 float scale[3];
149                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
150                 SimdVector3 scaling(scale[0],scale[1],scale[2]);
151                 GetCollisionShape()->setLocalScaling(scaling);
152         }
153         return true;
154
155 }
156
157                 /**
158                         WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
159                 */
160                 
161 void            CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
162 {
163
164 }
165 void            CcdPhysicsController::WriteDynamicsToMotionState()
166 {
167 }
168                 // controller replication
169 void            CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
170 {
171         m_MotionState = motionstate;
172
173         
174
175         m_body = 0;
176         CreateRigidbody();
177         
178         m_cci.m_physicsEnv->addCcdPhysicsController(this);
179
180
181 /*      SM_Object* dynaparent=0;
182         SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
183         
184         if (sumoparentctrl)
185         {
186                 dynaparent = sumoparentctrl->GetSumoObject();
187         }
188         
189         SM_Object* orgsumoobject = m_sumoObj;
190         
191         
192         m_sumoObj       =       new SM_Object(
193                 orgsumoobject->getShapeHandle(), 
194                 orgsumoobject->getMaterialProps(),                      
195                 orgsumoobject->getShapeProps(),
196                 dynaparent);
197         
198         m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
199         
200         m_sumoObj->setMargin(orgsumoobject->getMargin());
201         m_sumoObj->setPosition(orgsumoobject->getPosition());
202         m_sumoObj->setOrientation(orgsumoobject->getOrientation());
203         //if it is a dyna, register for a callback
204         m_sumoObj->registerCallback(*this);
205         
206         m_sumoScene->add(* (m_sumoObj));
207         */
208
209
210
211 }
212
213                 // kinematic methods
214 void            CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
215 {
216         if (m_body)
217         {
218                 m_body->activate();
219
220                 SimdVector3 dloc(dlocX,dlocY,dlocZ);
221                 SimdTransform xform = m_body->getCenterOfMassTransform();
222
223                 if (local)
224                 {
225                         dloc = xform.getBasis()*dloc;
226                 }
227
228                 xform.setOrigin(xform.getOrigin() + dloc);
229                 m_body->setCenterOfMassTransform(xform);
230         }
231
232 }
233
234 void            CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
235 {
236         if (m_body )
237         {
238                 m_body->activate();
239
240                 SimdMatrix3x3 drotmat(  rotval[0],rotval[1],rotval[2],
241                                                                 rotval[4],rotval[5],rotval[6],
242                                                                 rotval[8],rotval[9],rotval[10]);
243
244
245                 SimdMatrix3x3 currentOrn;
246                 GetWorldOrientation(currentOrn);
247
248                 SimdTransform xform = m_body->getCenterOfMassTransform();
249
250                 xform.setBasis(xform.getBasis()*(local ? 
251                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
252
253                 m_body->setCenterOfMassTransform(xform);
254         }
255
256 }
257
258 void CcdPhysicsController::GetWorldOrientation(SimdMatrix3x3& mat)
259 {
260         float orn[4];
261         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
262         SimdQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
263         mat.setRotation(quat);
264 }
265
266 void            CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
267 {
268         SimdQuaternion q = m_body->getCenterOfMassTransform().getRotation();
269         quatImag0 = q[0];
270         quatImag1 = q[1];
271         quatImag2 = q[2];
272         quatReal = q[3];
273 }
274 void            CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
275 {
276         m_body->activate();
277
278         SimdTransform xform  = m_body->getCenterOfMassTransform();
279         xform.setRotation(SimdQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
280         m_body->setCenterOfMassTransform(xform);
281
282 }
283
284 void            CcdPhysicsController::setPosition(float posX,float posY,float posZ)
285 {
286         m_body->activate();
287
288         SimdTransform xform  = m_body->getCenterOfMassTransform();
289         xform.setOrigin(SimdVector3(posX,posY,posZ));
290         m_body->setCenterOfMassTransform(xform);
291
292 }
293 void            CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
294 {
295 }
296
297 void            CcdPhysicsController::getPosition(PHY__Vector3& pos) const
298 {
299         const SimdTransform& xform = m_body->getCenterOfMassTransform();
300         pos[0] = xform.getOrigin().x();
301         pos[1] = xform.getOrigin().y();
302         pos[2] = xform.getOrigin().z();
303 }
304
305 void            CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
306 {
307         if (!SimdFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
308                 !SimdFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
309                 !SimdFuzzyZero(m_cci.m_scaling.z()-scaleZ))
310         {
311                 m_cci.m_scaling = SimdVector3(scaleX,scaleY,scaleZ);
312
313                 if (m_body && m_body->GetCollisionShape())
314                 {
315                         m_body->GetCollisionShape()->setLocalScaling(m_cci.m_scaling);
316                         m_body->GetCollisionShape()->CalculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
317                         m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
318                 }
319         }
320 }
321                 
322                 // physics methods
323 void            CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
324 {
325         SimdVector3 torque(torqueX,torqueY,torqueZ);
326         SimdTransform xform = m_body->getCenterOfMassTransform();
327         if (local)
328         {
329                 torque  = xform.getBasis()*torque;
330         }
331         m_body->applyTorque(torque);
332 }
333
334 void            CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
335 {
336         SimdVector3 force(forceX,forceY,forceZ);
337         SimdTransform xform = m_body->getCenterOfMassTransform();
338         if (local)
339         {
340                 force   = xform.getBasis()*force;
341         }
342         m_body->applyCentralForce(force);
343 }
344 void            CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
345 {
346         SimdVector3 angvel(ang_velX,ang_velY,ang_velZ);
347         if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
348         {
349                 m_body->activate();
350         }
351
352         {
353                 SimdTransform xform = m_body->getCenterOfMassTransform();
354                 if (local)
355                 {
356                         angvel  = xform.getBasis()*angvel;
357                 }
358
359                 m_body->setAngularVelocity(angvel);
360         }
361
362 }
363 void            CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
364 {
365
366         SimdVector3 linVel(lin_velX,lin_velY,lin_velZ);
367         if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
368         {
369                 m_body->activate();
370         }
371         
372         {
373                 SimdTransform xform = m_body->getCenterOfMassTransform();
374                 if (local)
375                 {
376                         linVel  = xform.getBasis()*linVel;
377                 }
378                 m_body->setLinearVelocity(linVel);
379         }
380 }
381 void            CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
382 {
383         SimdVector3 impulse(impulseX,impulseY,impulseZ);
384
385         if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
386         {
387                 m_body->activate();
388                 
389                 SimdVector3 pos(attachX,attachY,attachZ);
390
391                 m_body->activate();
392
393                 m_body->applyImpulse(impulse,pos);
394         }
395
396 }
397 void            CcdPhysicsController::SetActive(bool active)
398 {
399 }
400                 // reading out information from physics
401 void            CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
402 {
403         const SimdVector3& linvel = this->m_body->getLinearVelocity();
404         linvX = linvel.x();
405         linvY = linvel.y();
406         linvZ = linvel.z();
407
408 }
409
410 void            CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
411 {
412         const SimdVector3& angvel= m_body->getAngularVelocity();
413         angVelX = angvel.x();
414         angVelY = angvel.y();
415         angVelZ = angvel.z();
416 }
417
418 void            CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
419 {
420         SimdVector3 pos(posX,posY,posZ);
421         SimdVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
422         SimdVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
423         linvX = linvel.x();
424         linvY = linvel.y();
425         linvZ = linvel.z();
426 }
427 void            CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
428 {
429 }
430
431                 // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
432 void            CcdPhysicsController::setRigidBody(bool rigid)
433 {
434         if (!rigid)
435         {
436                 //fake it for now
437                 SimdVector3 inertia = m_body->getInvInertiaDiagLocal();
438                 inertia[1] = 0.f;
439                 m_body->setInvInertiaDiagLocal(inertia);
440                 m_body->updateInertiaTensor();
441         }
442 }
443
444                 // clientinfo for raycasts for example
445 void*           CcdPhysicsController::getNewClientInfo()
446 {
447         return m_newClientInfo;
448 }
449 void            CcdPhysicsController::setNewClientInfo(void* clientinfo)
450 {
451         m_newClientInfo = clientinfo;
452 }
453
454
455 void    CcdPhysicsController::UpdateDeactivation(float timeStep)
456 {
457         if ( (m_body->GetActivationState() == ISLAND_SLEEPING) || (m_body->GetActivationState() == DISABLE_DEACTIVATION))
458                 return;
459
460         if ((m_body->getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) &&
461                 (m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold))
462         {
463                 m_body->m_deactivationTime += timeStep;
464         } else
465         {
466                 m_body->m_deactivationTime=0.f;
467                 m_body->SetActivationState(0);
468         }
469
470 }
471
472 bool CcdPhysicsController::wantsSleeping()
473 {
474
475         if (m_body->GetActivationState() == DISABLE_DEACTIVATION)
476                 return false;
477
478         //disable deactivation
479         if (gDisableDeactivation || (gDeactivationTime == 0.f))
480                 return false;
481
482         if ( (m_body->GetActivationState() == ISLAND_SLEEPING) || (m_body->GetActivationState() == WANTS_DEACTIVATION))
483                 return true;
484
485         if (m_body->m_deactivationTime> gDeactivationTime)
486         {
487                 return true;
488         }
489         return false;
490 }
491
492 PHY_IPhysicsController* CcdPhysicsController::GetReplica()
493 {
494         //very experimental, shape sharing is not implemented yet.
495         //just support SphereShape/ConeShape for now
496
497         CcdConstructionInfo cinfo = m_cci;
498         if (cinfo.m_collisionShape)
499         {
500                 switch (cinfo.m_collisionShape->GetShapeType())
501                 {
502                 case SPHERE_SHAPE_PROXYTYPE:
503                         {
504                                 SphereShape* orgShape = (SphereShape*)cinfo.m_collisionShape;
505                                 cinfo.m_collisionShape = new SphereShape(*orgShape);
506                                 break;
507                         }
508
509                         case CONE_SHAPE_PROXYTYPE:
510                         {
511                                 ConeShape* orgShape = (ConeShape*)cinfo.m_collisionShape;
512                                 cinfo.m_collisionShape = new ConeShape(*orgShape);
513                                 break;
514                         }
515
516
517                 default:
518                         {
519                                 return 0;
520                         }
521                 }
522         }
523
524         cinfo.m_MotionState = new DefaultMotionState();
525
526         CcdPhysicsController* replica = new CcdPhysicsController(cinfo);
527         return replica;
528 }
529
530 ///////////////////////////////////////////////////////////
531 ///A small utility class, DefaultMotionState
532 ///
533 ///////////////////////////////////////////////////////////
534
535 DefaultMotionState::DefaultMotionState()
536 {
537         m_worldTransform.setIdentity();
538         m_localScaling.setValue(1.f,1.f,1.f);
539 }
540
541
542 DefaultMotionState::~DefaultMotionState()
543 {
544
545 }
546
547 void    DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
548 {
549         posX = m_worldTransform.getOrigin().x();
550         posY = m_worldTransform.getOrigin().y();
551         posZ = m_worldTransform.getOrigin().z();
552 }
553
554 void    DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
555 {
556         scaleX = m_localScaling.getX();
557         scaleY = m_localScaling.getY();
558         scaleZ = m_localScaling.getZ();
559 }
560
561 void    DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
562 {
563         quatIma0 = m_worldTransform.getRotation().x();
564         quatIma1 = m_worldTransform.getRotation().y();
565         quatIma2 = m_worldTransform.getRotation().z();
566         quatReal = m_worldTransform.getRotation()[3];
567 }
568                 
569 void    DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
570 {
571         SimdPoint3 pos(posX,posY,posZ);
572         m_worldTransform.setOrigin( pos );
573 }
574
575 void    DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
576 {
577         SimdQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
578         m_worldTransform.setRotation( orn );
579 }
580                 
581 void    DefaultMotionState::calculateWorldTransformations()
582 {
583
584 }
585