Created a KX_SoftBodyDeformer for real-time soft bodies.
[blender.git] / source / gameengine / Physics / Sumo / SumoPhysicsController.cpp
1 /**
2  * @file $Id$
3  *
4  * ***** BEGIN GPL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software Foundation,
18  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
19  *
20  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
21  * All rights reserved.
22  *
23  * The Original Code is: all of this file.
24  *
25  * Contributor(s): none yet.
26  *
27  * ***** END GPL LICENSE BLOCK *****
28  */
29
30 #include "SumoPhysicsController.h"
31 #include "PHY_IMotionState.h"
32 #include "SM_Object.h"
33 #include "MT_Quaternion.h"
34
35
36 SumoPhysicsController::SumoPhysicsController(
37                                 class SM_Scene* sumoScene,
38                                 class SM_Object* sumoObj,
39                                 class PHY_IMotionState* motionstate,
40
41                                 bool dyna) 
42                 : 
43                 m_sumoObj(sumoObj) , 
44                 m_sumoScene(sumoScene),
45                 m_bFirstTime(true),
46                 m_bDyna(dyna),
47                 m_MotionState(motionstate)
48 {
49         if (m_sumoObj)
50         {
51
52                 PHY__Vector3 pos1;
53                 getPosition(pos1);
54                 MT_Point3 pos(pos1);
55                 
56                 //temp debugging check
57                 //assert(pos.length() < 100000.f);
58
59                 //need this to do the upcast after the solid/sumo collision callback
60                 m_sumoObj->setPhysicsClientObject(this);
61                 //if it is a dyna, register for a callback
62                 m_sumoObj->registerCallback(*this);
63         }
64 };
65
66
67
68 SumoPhysicsController::~SumoPhysicsController()
69 {
70         if (m_sumoObj)
71         {
72                 m_sumoScene->remove(*m_sumoObj);
73                 
74                 delete m_sumoObj;
75                 m_sumoObj = NULL;
76         }
77 }
78
79 float SumoPhysicsController::getMass()
80 {
81         if (m_sumoObj)
82         {
83                 const SM_ShapeProps *shapeprops = m_sumoObj->getShapeProps();
84                 if(shapeprops!=NULL) return shapeprops->m_mass;
85         }
86         return 0.f;
87 }
88
89 bool SumoPhysicsController::SynchronizeMotionStates(float)
90 {
91         if (m_bFirstTime)
92         {
93                 setSumoTransform(!m_bFirstTime);
94                 m_bFirstTime = false;
95         }
96         return false;
97 }
98  
99
100
101
102 void SumoPhysicsController::GetWorldOrientation(MT_Matrix3x3& mat)
103 {
104         float orn[4];
105         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
106         MT_Quaternion quat(orn);
107         mat.setRotation(quat);
108
109 }
110
111 void SumoPhysicsController::getPosition(PHY__Vector3&   pos) const
112 {
113         assert(m_sumoObj);
114
115         pos[0] = m_sumoObj->getPosition()[0];
116         pos[1] = m_sumoObj->getPosition()[0];
117         pos[2] = m_sumoObj->getPosition()[0];
118
119         //m_MotionState->getWorldPosition(pos[0],pos[1],pos[2]);
120 }
121
122 void SumoPhysicsController::GetWorldPosition(MT_Point3& pos)
123 {
124 //      assert(m_sumoObj);
125
126 //      pos[0] = m_sumoObj->getPosition()[0];
127 //      pos[1] = m_sumoObj->getPosition()[0];
128 //      pos[2] = m_sumoObj->getPosition()[0];
129
130         float worldpos[3];
131         m_MotionState->getWorldPosition(worldpos[0],worldpos[1],worldpos[2]);
132         pos[0]=worldpos[0];
133         pos[1]=worldpos[1];
134         pos[2]=worldpos[2];
135 }
136
137 void SumoPhysicsController::GetWorldScaling(MT_Vector3& scale)
138 {
139         float worldscale[3];
140         m_MotionState->getWorldScaling(worldscale[0],worldscale[1],worldscale[2]);
141         scale[0]=worldscale[0];
142         scale[1]=worldscale[1];
143         scale[2]=worldscale[2];
144 }
145
146
147                 // kinematic methods
148 void            SumoPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
149 {
150         if (m_sumoObj)
151         {
152                 MT_Matrix3x3 mat;
153                 GetWorldOrientation(mat);
154                 MT_Vector3 dloc(dlocX,dlocY,dlocZ);
155
156                 MT_Point3 newpos = m_sumoObj->getPosition();
157                 
158                 newpos += (local ? mat * dloc : dloc);
159                 m_sumoObj->setPosition(newpos);
160         }
161
162 }
163 void            SumoPhysicsController::RelativeRotate(const float drot[12],bool local)
164 {
165         if (m_sumoObj )
166         {
167                 MT_Matrix3x3 drotmat(drot);
168                 MT_Matrix3x3 currentOrn;
169                 GetWorldOrientation(currentOrn);
170
171                 m_sumoObj->setOrientation(m_sumoObj->getOrientation()*(local ? 
172                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)).getRotation());
173         }
174
175 }
176 void            SumoPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
177 {
178         m_sumoObj->setOrientation(MT_Quaternion(quatImag0,quatImag1,quatImag2,quatReal));
179 }
180
181 void            SumoPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
182 {
183         const MT_Quaternion& q = m_sumoObj->getOrientation();
184         quatImag0 = q[0];
185         quatImag1 = q[1];
186         quatImag2 = q[2];
187         quatReal = q[3];
188 }
189
190 void            SumoPhysicsController::setPosition(float posX,float posY,float posZ)
191 {
192         m_sumoObj->setPosition(MT_Point3(posX,posY,posZ));
193 }
194
195 void            SumoPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
196 {
197         if (!m_bDyna)
198                 m_sumoObj->setScaling(MT_Vector3(scaleX,scaleY,scaleZ));
199 }
200         
201         // physics methods
202 void            SumoPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
203 {
204         if (m_sumoObj)
205         {
206                 MT_Vector3 torque(torqueX,torqueY,torqueZ);
207
208                 MT_Matrix3x3 orn;
209                 GetWorldOrientation(orn);
210                 m_sumoObj->applyTorque(local ?
211                         orn * torque :
212                         torque);
213         }
214 }
215
216 void            SumoPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
217 {
218         if (m_sumoObj)
219         {
220                 MT_Vector3 force(forceX,forceY,forceZ);
221
222                 MT_Matrix3x3 orn;
223                 GetWorldOrientation(orn);
224
225                 m_sumoObj->applyCenterForce(local ?
226                                                 orn * force :
227                                                 force);
228         }
229 }
230
231 void            SumoPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
232 {
233         if (m_sumoObj)
234         {
235                 MT_Vector3 ang_vel(ang_velX,ang_velY,ang_velZ);
236
237                 MT_Matrix3x3 orn;
238                 GetWorldOrientation(orn);
239
240                 m_sumoObj->setExternalAngularVelocity(local ?
241                                                 orn * ang_vel :
242                                                 ang_vel);
243         }
244 }
245         
246 void            SumoPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
247 {
248         if (m_sumoObj )
249         {
250                 MT_Matrix3x3 orn;
251                 GetWorldOrientation(orn);
252
253                 MT_Vector3 lin_vel(lin_velX,lin_velY,lin_velZ);
254                 m_sumoObj->setExternalLinearVelocity(local ?
255                                                            orn * lin_vel :
256                                                            lin_vel);
257         }
258 }
259
260 void            SumoPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
261 {
262         if (m_sumoObj)
263                 m_sumoObj->resolveCombinedVelocities(MT_Vector3(linvelX,linvelY,linvelZ),MT_Vector3(angVelX,angVelY,angVelZ));
264 }
265
266
267
268
269 void            SumoPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
270 {
271         if (m_sumoObj)
272         {
273                 MT_Point3 attach(attachX,attachY,attachZ);
274                 MT_Vector3 impulse(impulseX,impulseY,impulseZ);
275                 m_sumoObj->applyImpulse(attach,impulse);
276         }
277
278 }
279
280 void            SumoPhysicsController::SuspendDynamics()
281 {
282         m_suspendDynamics=true;
283                 
284         if (m_sumoObj)
285         {
286                 m_sumoObj->suspendDynamics();
287                 m_sumoObj->setLinearVelocity(MT_Vector3(0,0,0));
288                 m_sumoObj->setAngularVelocity(MT_Vector3(0,0,0));
289                 m_sumoObj->calcXform();
290         }
291 }
292
293 void            SumoPhysicsController::RestoreDynamics()
294 {
295         m_suspendDynamics=false;
296
297         if (m_sumoObj)
298         {
299                 m_sumoObj->restoreDynamics();
300         }
301 }
302
303
304 /**  
305         reading out information from physics
306 */
307 void            SumoPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
308 {
309         if (m_sumoObj)
310         {
311                 // get velocity from the physics object (m_sumoObj)
312                 const MT_Vector3& vel = m_sumoObj->getLinearVelocity();
313                 linvX = vel[0];
314                 linvY = vel[1];
315                 linvZ = vel[2];
316         } 
317         else
318         {
319                 linvX = 0.f;
320                 linvY = 0.f;
321                 linvZ = 0.f;
322         }
323 }
324
325 /** 
326         GetVelocity parameters are in geometric coordinates (Origin is not center of mass!).
327 */
328 void            SumoPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
329 {
330         if (m_sumoObj)
331         {
332                 MT_Point3 pos(posX,posY,posZ);
333                 // get velocity from the physics object (m_sumoObj)
334                 const MT_Vector3& vel = m_sumoObj->getVelocity(pos);
335                 linvX = vel[0];
336                 linvY = vel[1];
337                 linvZ = vel[2];
338         } 
339         else
340         {
341                 linvX = 0.f;
342                 linvY = 0.f;
343                 linvZ = 0.f;
344                 
345         }
346 }
347
348 void            SumoPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
349 {
350         const MT_Vector3& force = m_sumoObj->getReactionForce();
351         forceX = force[0];
352         forceY = force[1];
353         forceZ = force[2];
354 }
355
356 void            SumoPhysicsController::setRigidBody(bool rigid)
357 {
358         m_sumoObj->setRigidBody(rigid);
359 }
360
361 void            SumoPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
362 {
363         m_MotionState = motionstate;
364
365         SM_Object* dynaparent=0;
366         SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
367         
368         if (sumoparentctrl)
369         {
370                 dynaparent = sumoparentctrl->GetSumoObject();
371         }
372         
373         SM_Object* orgsumoobject = m_sumoObj;
374         
375         
376         m_sumoObj       =       new SM_Object(
377                 orgsumoobject->getShapeHandle(), 
378                 orgsumoobject->getMaterialProps(),                      
379                 orgsumoobject->getShapeProps(),
380                 dynaparent);
381         
382         m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
383         
384         m_sumoObj->setMargin(orgsumoobject->getMargin());
385         m_sumoObj->setPosition(orgsumoobject->getPosition());
386         m_sumoObj->setOrientation(orgsumoobject->getOrientation());
387         //if it is a dyna, register for a callback
388         m_sumoObj->registerCallback(*this);
389         
390         m_sumoScene->add(* (m_sumoObj));
391 }
392
393 void                    SumoPhysicsController::SetSimulatedTime(float)
394 {
395 }
396         
397         
398 void    SumoPhysicsController::WriteMotionStateToDynamics(bool)
399 {
400
401 }
402 // this is the actual callback from sumo, and the position/orientation
403 //is written to the scenegraph, using the motionstate abstraction
404
405 void SumoPhysicsController::do_me()
406 {
407         MT_assert(m_sumoObj);
408         const MT_Point3& pos = m_sumoObj->getPosition();
409         const MT_Quaternion& orn = m_sumoObj->getOrientation();
410
411         MT_assert(m_MotionState);
412         m_MotionState->setWorldPosition(pos[0],pos[1],pos[2]);
413         m_MotionState->setWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
414 }
415
416
417 void    SumoPhysicsController::setSumoTransform(bool nondynaonly)
418 {
419         if (!nondynaonly || !m_bDyna)
420         {
421                 if (m_sumoObj)
422                 {
423                         MT_Point3 pos;
424                         GetWorldPosition(pos);
425
426                         m_sumoObj->setPosition(pos);
427                         if (m_bDyna)
428                         {
429                                 m_sumoObj->setScaling(MT_Vector3(1,1,1));
430                         } else
431                         {
432                                 MT_Vector3 scale;
433                                 GetWorldScaling(scale);
434                                 m_sumoObj->setScaling(scale);
435                         }
436                         MT_Matrix3x3 orn;
437                         GetWorldOrientation(orn);
438                         m_sumoObj->setOrientation(orn.getRotation());
439                         m_sumoObj->calcXform();
440                 }
441         }
442 }
443
444
445         // clientinfo for raycasts for example
446 void*                           SumoPhysicsController::getNewClientInfo()
447 {
448         if (m_sumoObj)
449                 return m_sumoObj->getClientObject();
450         return 0;
451
452 }
453 void                            SumoPhysicsController::setNewClientInfo(void* clientinfo)
454 {
455         if (m_sumoObj)
456         {
457                 SM_ClientObject* clOb = static_cast<SM_ClientObject*> (clientinfo);
458                 m_sumoObj->setClientObject(clOb);
459         }
460
461 }
462
463 void    SumoPhysicsController::calcXform()
464 {
465         if (m_sumoObj)
466                 m_sumoObj->calcXform();
467 }
468
469 void SumoPhysicsController::SetMargin(float margin) 
470 {
471         if (m_sumoObj)
472                 m_sumoObj->setMargin(margin);
473 }
474
475 float SumoPhysicsController::GetMargin() const
476 {
477         if (m_sumoObj)
478                 m_sumoObj->getMargin();
479         return 0.f;
480 }
481
482 float SumoPhysicsController::GetRadius() const 
483 {
484         if (m_sumoObj && m_sumoObj->getShapeProps())
485         {
486                 return m_sumoObj->getShapeProps()->m_radius;
487         }
488         return 0.f;
489
490 }