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