enabled ipo in combination with physics (no friction effect from kinematic to dynamic...
[blender.git] / extern / bullet / Extras / PhysicsInterface / CcdPhysics / CcdPhysicsController.cpp
1 #include "CcdPhysicsController.h"
2
3 #include "Dynamics/RigidBody.h"
4 #include "PHY_IMotionState.h"
5 #include "BroadphaseCollision/BroadphaseProxy.h"
6 #include "BroadphaseCollision/BroadphaseInterface.h"
7 #include "CollisionShapes/ConvexShape.h"
8 #include "CcdPhysicsEnvironment.h"
9 #include "SimdTransformUtil.h"
10
11 class BP_Proxy;
12
13 ///todo: fill all the empty CcdPhysicsController methods, hook them up to the RigidBody class
14
15 //'temporarily' global variables
16 float   gDeactivationTime = 2.f;
17 bool    gDisableDeactivation = false;
18
19 float gLinearSleepingTreshold = 0.8f;
20 float gAngularSleepingTreshold = 1.0f;
21
22 #include "Dynamics/MassProps.h"
23
24 SimdVector3 startVel(0,0,0);//-10000);
25 CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
26 :m_cci(ci)
27 {
28         m_collisionDelay = 0;
29         m_newClientInfo = 0;
30         
31         m_MotionState = ci.m_MotionState;
32
33         
34         
35         CreateRigidbody();
36         
37
38         
39         #ifdef WIN32
40         if (m_body->getInvMass())
41                 m_body->setLinearVelocity(startVel);
42         #endif
43
44 }
45
46 SimdTransform   CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
47 {
48         SimdTransform trans;
49         float tmp[3];
50         motionState->getWorldPosition(tmp[0],tmp[1],tmp[2]);
51         trans.setOrigin(SimdVector3(tmp[0],tmp[1],tmp[2]));
52
53         SimdQuaternion orn;
54         motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
55         trans.setRotation(orn);
56         return trans;
57
58 }
59
60 void CcdPhysicsController::CreateRigidbody()
61 {
62
63         SimdTransform trans = GetTransformFromMotionState(m_cci.m_MotionState);
64
65         MassProps mp(m_cci.m_mass, m_cci.m_localInertiaTensor);
66
67         m_body = new RigidBody(mp,0,0,m_cci.m_friction,m_cci.m_restitution);
68         m_body->m_collisionShape = m_cci.m_collisionShape;
69         
70
71         //
72         // init the rigidbody properly
73         //
74         
75         m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
76         m_body->setGravity( m_cci.m_gravity);
77         m_body->setDamping(m_cci.m_linearDamping, m_cci.m_angularDamping);
78         m_body->setCenterOfMassTransform( trans );
79
80
81 }
82
83 CcdPhysicsController::~CcdPhysicsController()
84 {
85         //will be reference counted, due to sharing
86         m_cci.m_physicsEnv->removeCcdPhysicsController(this);
87         delete m_MotionState;
88         delete m_body;
89 }
90
91                 /**
92                         SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
93                 */
94 bool            CcdPhysicsController::SynchronizeMotionStates(float time)
95 {
96         //don't sync non-dynamic...
97
98         if (m_body->getInvMass() != 0.f)
99         {
100                 const SimdVector3& worldPos = m_body->getCenterOfMassPosition();
101                 m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
102                 
103                 const SimdQuaternion& worldquat = m_body->getOrientation();
104                 m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
105
106                 m_MotionState->calculateWorldTransformations();
107
108                 float scale[3];
109                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
110                 SimdVector3 scaling(scale[0],scale[1],scale[2]);
111                 GetCollisionShape()->setLocalScaling(scaling);
112         } else
113         {
114                 SimdVector3 worldPos;
115                 SimdQuaternion worldquat;
116
117                 m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
118                 m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
119                 SimdTransform oldTrans = m_body->getCenterOfMassTransform();
120                 SimdTransform newTrans(worldquat,worldPos);
121                 
122                 m_body->setCenterOfMassTransform(newTrans);
123                 //need to keep track of previous position for friction effects...
124                 
125                 m_MotionState->calculateWorldTransformations();
126
127                 float scale[3];
128                 m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
129                 SimdVector3 scaling(scale[0],scale[1],scale[2]);
130                 GetCollisionShape()->setLocalScaling(scaling);
131         }
132         return true;
133
134 }
135
136                 /**
137                         WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
138                 */
139                 
140 void            CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
141 {
142
143 }
144 void            CcdPhysicsController::WriteDynamicsToMotionState()
145 {
146 }
147                 // controller replication
148 void            CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
149 {
150         m_MotionState = motionstate;
151
152         
153
154         m_body = 0;
155         CreateRigidbody();
156         
157         m_cci.m_physicsEnv->addCcdPhysicsController(this);
158
159
160 /*      SM_Object* dynaparent=0;
161         SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
162         
163         if (sumoparentctrl)
164         {
165                 dynaparent = sumoparentctrl->GetSumoObject();
166         }
167         
168         SM_Object* orgsumoobject = m_sumoObj;
169         
170         
171         m_sumoObj       =       new SM_Object(
172                 orgsumoobject->getShapeHandle(), 
173                 orgsumoobject->getMaterialProps(),                      
174                 orgsumoobject->getShapeProps(),
175                 dynaparent);
176         
177         m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
178         
179         m_sumoObj->setMargin(orgsumoobject->getMargin());
180         m_sumoObj->setPosition(orgsumoobject->getPosition());
181         m_sumoObj->setOrientation(orgsumoobject->getOrientation());
182         //if it is a dyna, register for a callback
183         m_sumoObj->registerCallback(*this);
184         
185         m_sumoScene->add(* (m_sumoObj));
186         */
187
188
189
190 }
191
192                 // kinematic methods
193 void            CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
194 {
195         SimdVector3 dloc(dlocX,dlocY,dlocZ);
196         SimdTransform xform = m_body->getCenterOfMassTransform();
197
198         if (local)
199         {
200                 dloc = xform.getBasis()*dloc;
201         }
202
203         xform.setOrigin(xform.getOrigin() + dloc);
204         this->m_body->setCenterOfMassTransform(xform);
205
206 }
207
208 void            CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
209 {
210         if (m_body )
211         {
212                 SimdMatrix3x3 drotmat(  rotval[0],rotval[1],rotval[2],
213                                                                 rotval[4],rotval[5],rotval[6],
214                                                                 rotval[8],rotval[9],rotval[10]);
215
216
217                 SimdMatrix3x3 currentOrn;
218                 GetWorldOrientation(currentOrn);
219
220                 SimdTransform xform = m_body->getCenterOfMassTransform();
221
222                 xform.setBasis(xform.getBasis()*(local ? 
223                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
224
225                 m_body->setCenterOfMassTransform(xform);
226         }
227
228 }
229
230 void CcdPhysicsController::GetWorldOrientation(SimdMatrix3x3& mat)
231 {
232         float orn[4];
233         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
234         SimdQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
235         mat.setRotation(quat);
236 }
237
238 void            CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
239 {
240         SimdQuaternion q = m_body->getCenterOfMassTransform().getRotation();
241         quatImag0 = q[0];
242         quatImag1 = q[1];
243         quatImag2 = q[2];
244         quatReal = q[3];
245 }
246 void            CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
247 {
248         m_body->activate();
249
250         SimdTransform xform  = m_body->getCenterOfMassTransform();
251         xform.setRotation(SimdQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
252         m_body->setCenterOfMassTransform(xform);
253
254 }
255
256 void            CcdPhysicsController::setPosition(float posX,float posY,float posZ)
257 {
258         m_body->activate();
259
260         SimdTransform xform  = m_body->getCenterOfMassTransform();
261         xform.setOrigin(SimdVector3(posX,posY,posZ));
262         m_body->setCenterOfMassTransform(xform);
263
264 }
265 void            CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
266 {
267 }
268
269 void            CcdPhysicsController::getPosition(PHY__Vector3& pos) const
270 {
271         const SimdTransform& xform = m_body->getCenterOfMassTransform();
272         pos[0] = xform.getOrigin().x();
273         pos[1] = xform.getOrigin().y();
274         pos[2] = xform.getOrigin().z();
275 }
276
277 void            CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
278 {
279         if (!SimdFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
280                 !SimdFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
281                 !SimdFuzzyZero(m_cci.m_scaling.z()-scaleZ))
282         {
283                 m_cci.m_scaling = SimdVector3(scaleX,scaleY,scaleZ);
284
285                 if (m_body && m_body->GetCollisionShape())
286                 {
287                         m_body->GetCollisionShape()->setLocalScaling(m_cci.m_scaling);
288                         m_body->GetCollisionShape()->CalculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
289                         m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
290                 }
291         }
292 }
293                 
294                 // physics methods
295 void            CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
296 {
297         SimdVector3 torque(torqueX,torqueY,torqueZ);
298         SimdTransform xform = m_body->getCenterOfMassTransform();
299         if (local)
300         {
301                 torque  = xform.getBasis()*torque;
302         }
303         m_body->applyTorque(torque);
304 }
305
306 void            CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
307 {
308         SimdVector3 force(forceX,forceY,forceZ);
309         SimdTransform xform = m_body->getCenterOfMassTransform();
310         if (local)
311         {
312                 force   = xform.getBasis()*force;
313         }
314         m_body->applyCentralForce(force);
315 }
316 void            CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
317 {
318         SimdVector3 angvel(ang_velX,ang_velY,ang_velZ);
319         SimdTransform xform = m_body->getCenterOfMassTransform();
320         if (local)
321         {
322                 angvel  = xform.getBasis()*angvel;
323         }
324
325         m_body->setAngularVelocity(angvel);
326
327 }
328 void            CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
329 {
330         SimdVector3 linVel(lin_velX,lin_velY,lin_velZ);
331         SimdTransform xform = m_body->getCenterOfMassTransform();
332         if (local)
333         {
334                 linVel  = xform.getBasis()*linVel;
335         }
336         m_body->setLinearVelocity(linVel);
337 }
338 void            CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
339 {
340         
341         SimdVector3 impulse(impulseX,impulseY,impulseZ);
342         SimdVector3 pos(attachX,attachY,attachZ);
343
344         m_body->activate();
345
346         m_body->applyImpulse(impulse,pos);
347
348 }
349 void            CcdPhysicsController::SetActive(bool active)
350 {
351 }
352                 // reading out information from physics
353 void            CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
354 {
355         const SimdVector3& linvel = this->m_body->getLinearVelocity();
356         linvX = linvel.x();
357         linvY = linvel.y();
358         linvZ = linvel.z();
359
360 }
361
362 void            CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
363 {
364         const SimdVector3& angvel= m_body->getAngularVelocity();
365         angVelX = angvel.x();
366         angVelY = angvel.y();
367         angVelZ = angvel.z();
368 }
369
370 void            CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
371 {
372         SimdVector3 pos(posX,posY,posZ);
373         SimdVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
374         SimdVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
375         linvX = linvel.x();
376         linvY = linvel.y();
377         linvZ = linvel.z();
378 }
379 void            CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
380 {
381 }
382
383                 // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
384 void            CcdPhysicsController::setRigidBody(bool rigid)
385 {
386 }
387
388                 // clientinfo for raycasts for example
389 void*           CcdPhysicsController::getNewClientInfo()
390 {
391         return m_newClientInfo;
392 }
393 void            CcdPhysicsController::setNewClientInfo(void* clientinfo)
394 {
395         m_newClientInfo = clientinfo;
396 }
397
398
399 void    CcdPhysicsController::UpdateDeactivation(float timeStep)
400 {
401         if ( (m_body->GetActivationState() == ISLAND_SLEEPING) || (m_body->GetActivationState() == DISABLE_DEACTIVATION))
402                 return;
403
404         if ((m_body->getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) &&
405                 (m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold))
406         {
407                 m_body->m_deactivationTime += timeStep;
408         } else
409         {
410                 m_body->m_deactivationTime=0.f;
411                 m_body->SetActivationState(0);
412         }
413
414 }
415
416 bool CcdPhysicsController::wantsSleeping()
417 {
418
419         if (m_body->GetActivationState() == DISABLE_DEACTIVATION)
420                 return false;
421
422         //disable deactivation
423         if (gDisableDeactivation || (gDeactivationTime == 0.f))
424                 return false;
425
426         if ( (m_body->GetActivationState() == ISLAND_SLEEPING) || (m_body->GetActivationState() == WANTS_DEACTIVATION))
427                 return true;
428
429         if (m_body->m_deactivationTime> gDeactivationTime)
430         {
431                 return true;
432         }
433         return false;
434 }
435