76f5f6e891dbca73ad99a85e9cff592c1d95630a
[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[9],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(orn);
190         m_sumoObj->setOrientation(orn);
191 }
192
193 void            SumoPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
194 {
195         const MT_Quaternion& q = m_sumoObj->getOrientation();
196         quatImag0 = q[0];
197         quatImag1 = q[1];
198         quatImag2 = q[2];
199         quatReal = q[3];
200 }
201
202 void            SumoPhysicsController::setPosition(float posX,float posY,float posZ)
203 {
204         m_sumoObj->setPosition(MT_Point3(posX,posY,posZ));
205 }
206 void            SumoPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
207 {
208         m_sumoObj->setScaling(MT_Vector3(scaleX,scaleY,scaleZ));
209 }
210         
211         // physics methods
212 void            SumoPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
213 {
214         if (m_sumoObj)
215         {
216                 MT_Vector3 torque(torqueX,torqueY,torqueZ);
217
218                 MT_Matrix3x3 orn;
219                 GetWorldOrientation(orn);
220                 m_sumoObj->applyTorque(local ?
221                                                            orn * torque :
222                                                            torque);
223         }
224
225 }
226
227 void            SumoPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
228 {
229         if (m_sumoObj)
230         {
231                 MT_Vector3 force(forceX,forceY,forceZ);
232
233                 MT_Matrix3x3 orn;
234                 GetWorldOrientation(orn);
235
236                 m_sumoObj->applyCenterForce(local ?
237                                                            orn * force :
238                                                            force);
239                 
240         }
241
242 }
243
244 void            SumoPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
245 {
246         if (m_sumoObj)
247         {
248                 MT_Vector3 ang_vel(ang_velX,ang_velY,ang_velZ);
249
250                 MT_Matrix3x3 orn;
251                 GetWorldOrientation(orn);
252
253                 m_sumoObj->setAngularVelocity(local ?
254                                                            orn * ang_vel :
255                                                            ang_vel);
256                 
257         }
258 }
259         
260 void            SumoPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
261 {
262         if (m_sumoObj )
263         {
264                 MT_Matrix3x3 orn;
265                 GetWorldOrientation(orn);
266
267                 MT_Vector3 lin_vel(lin_velX,lin_velY,lin_velZ);
268                 m_sumoObj->setLinearVelocity(local ?
269                                                            orn * lin_vel :
270                                                            lin_vel);
271         }
272 }
273
274 void            SumoPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
275 {
276         if (m_sumoObj)
277         {
278                 MT_Point3 attach(attachX,attachY,attachZ);
279                 MT_Vector3 impulse(impulseX,impulseY,impulseZ);
280                 m_sumoObj->applyImpulse(attach,impulse);
281         }
282
283 }
284
285 void            SumoPhysicsController::SuspendDynamics()
286 {
287         m_suspendDynamics=true;
288                 
289         if (m_sumoObj)
290                 {
291                         m_sumoObj->suspendDynamics();
292                         m_sumoObj->setLinearVelocity(MT_Vector3(0,0,0));
293                         m_sumoObj->setAngularVelocity(MT_Vector3(0,0,0));
294                         m_sumoObj->calcXform();
295                 }
296 }
297
298 void            SumoPhysicsController::RestoreDynamics()
299 {
300         m_suspendDynamics=false;
301
302         if (m_sumoObj)
303         {
304                 m_sumoObj->restoreDynamics();
305         }
306
307
308 }
309
310
311         /**  
312                 reading out information from physics
313         */
314 void            SumoPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
315 {
316         if (m_sumoObj)
317         {
318                 // get velocity from the physics object (m_sumoObj)
319                 const MT_Vector3& vel = m_sumoObj->getLinearVelocity();
320                 linvX = vel[0];
321                 linvY = vel[1];
322                 linvZ = vel[2];
323         } 
324         else
325         {
326                 linvX = 0.f;
327                 linvY = 0.f;
328                 linvZ = 0.f;
329         }
330 }
331         /** 
332                 GetVelocity parameters are in geometric coordinates (Origin is not center of mass!).
333         */
334 void            SumoPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
335 {
336         if (m_sumoObj)
337         {
338                 MT_Point3 pos(posX,posY,posZ);
339                 // get velocity from the physics object (m_sumoObj)
340                 const MT_Vector3& vel = m_sumoObj->getVelocity(pos);
341                 linvX = vel[0];
342                 linvY = vel[1];
343                 linvZ = vel[2];
344         } 
345         else
346         {
347                 linvX = 0.f;
348                 linvY = 0.f;
349                 linvZ = 0.f;
350                 
351         }
352 }
353
354
355 void            SumoPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
356 {
357         const MT_Vector3& force = m_sumoObj->getReactionForce();
358         forceX = force[0];
359         forceY = force[1];
360         forceZ = force[2];
361
362
363 }
364 void            SumoPhysicsController::setRigidBody(bool rigid)
365 {
366         m_sumoObj->setRigidBody(rigid);
367 }
368                 
369
370 void            SumoPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
371 {
372         m_MotionState = motionstate;
373
374         SM_Object* dynaparent=0;
375         SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
376         
377         if (sumoparentctrl)
378         {
379                 dynaparent = sumoparentctrl->GetSumoObject();
380         }
381         
382         SM_Object* orgsumoobject = m_sumoObj;
383         
384         
385         m_sumoObj       =       new SM_Object(
386                 orgsumoobject->getShapeHandle(), 
387                 orgsumoobject->getMaterialProps(),                      
388                 orgsumoobject->getShapeProps(),
389                 dynaparent);
390         
391         m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
392         
393         m_sumoObj->setMargin(orgsumoobject->getMargin());
394         m_sumoObj->setPosition(orgsumoobject->getPosition());
395         m_sumoObj->setOrientation(orgsumoobject->getOrientation());
396         //if it is a dyna, register for a callback
397         m_sumoObj->registerCallback(*this);
398         
399         m_sumoScene->add(* (m_sumoObj));
400 }
401
402 void                    SumoPhysicsController::SetSimulatedTime(float time)
403 {
404 }
405         
406         
407 void    SumoPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
408 {
409
410 }
411 // this is the actual callback from sumo, and the position/orientation
412 //is written to the scenegraph, using the motionstate abstraction
413
414 void SumoPhysicsController::do_me()
415 {
416
417         const MT_Point3& pos = m_sumoObj->getPosition();
418         const MT_Quaternion& orn = m_sumoObj->getOrientation();
419
420         m_MotionState->setWorldPosition(pos[0],pos[1],pos[2]);
421         m_MotionState->setWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
422
423 }
424
425
426 void    SumoPhysicsController::setSumoTransform(bool nondynaonly)
427 {
428         if (!nondynaonly || !m_bDyna)
429         {
430                 if (m_sumoObj)
431                 {
432                         MT_Point3 pos;
433                         GetWorldPosition(pos);
434
435                         m_sumoObj->setPosition(pos);
436                         if (m_bDyna)
437                         {
438                                 m_sumoObj->setScaling(MT_Vector3(1,1,1));
439                         } else
440                         {
441                                 MT_Vector3 scale;
442                                 GetWorldScaling(scale);
443                                 m_sumoObj->setScaling(scale);
444                         }
445                         MT_Matrix3x3 orn;
446                         GetWorldOrientation(orn);
447                         m_sumoObj->setOrientation(orn.getRotation());
448                         m_sumoObj->calcXform();
449                 }
450         }
451 }