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