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