58b873672584470cec806296af6614fac2abf68d
[blender.git] / source / gameengine / Ketsji / KX_TrackToActuator.cpp
1 //
2 // Replace the mesh for this actuator's parent
3 //
4 // $Id$
5 //
6 // ***** BEGIN GPL LICENSE BLOCK *****
7 //
8 // This program is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU General Public License
10 // as published by the Free Software Foundation; either version 2
11 // of the License, or (at your option) any later version.
12 //
13 // This program is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16 // GNU General Public License for more details.
17 //
18 // You should have received a copy of the GNU General Public License
19 // along with this program; if not, write to the Free Software Foundation,
20 // Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
21 //
22 // The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
23 // All rights reserved.
24 //
25 // The Original Code is: all of this file.
26 //
27 // Contributor(s): none yet.
28 //
29 // ***** END GPL LICENSE BLOCK *****
30
31 // todo: not all trackflags / upflags are implemented/tested !
32 // m_trackflag is used to determine the forward tracking direction
33 // m_upflag for the up direction
34 // normal situation is +y for forward, +z for up
35
36 #include "MT_Scalar.h"
37 #include "SCA_IActuator.h"
38 #include "KX_TrackToActuator.h"
39 #include "SCA_IScene.h"
40 #include "SCA_LogicManager.h"
41 #include <math.h>
42 #include <iostream>
43 #include "KX_GameObject.h"
44
45 #ifdef HAVE_CONFIG_H
46 #include <config.h>
47 #endif
48
49 /* ------------------------------------------------------------------------- */
50 /* Native functions                                                          */
51 /* ------------------------------------------------------------------------- */
52
53
54
55 KX_TrackToActuator::KX_TrackToActuator(SCA_IObject *gameobj, 
56                                                                        SCA_IObject *ob,
57                                                                            int time,
58                                                                            bool allow3D,
59                                                                            int trackflag,
60                                                                            int upflag,
61                                                                            PyTypeObject* T)
62                                                                            :
63                                                                                 SCA_IActuator(gameobj, T)
64 {
65     m_time = time;
66     m_allow3D = allow3D;
67     m_object = ob;
68         m_trackflag = trackflag;
69         m_upflag = upflag;
70         m_parentobj = 0;
71         
72         if (m_object){
73                 m_object->RegisterActuator(this);
74                 KX_GameObject* curobj = (KX_GameObject*) GetParent();
75
76                 m_parentobj = curobj->GetParent(); // check if the object is parented 
77                 if (m_parentobj) {  // if so, store the initial local rotation
78                         m_parentlocalmat = m_parentobj->GetSGNode()->GetLocalOrientation();
79                 }
80         }
81
82 } /* End of constructor */
83
84
85
86 /* old function from Blender */
87 MT_Matrix3x3 EulToMat3(float *eul)
88 {
89         MT_Matrix3x3 mat;
90         float ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
91         
92         ci = cos(eul[0]); 
93         cj = cos(eul[1]); 
94         ch = cos(eul[2]);
95         si = sin(eul[0]); 
96         sj = sin(eul[1]); 
97         sh = sin(eul[2]);
98         cc = ci*ch; 
99         cs = ci*sh; 
100         sc = si*ch; 
101         ss = si*sh;
102
103         mat[0][0] = cj*ch; 
104         mat[1][0] = sj*sc-cs; 
105         mat[2][0] = sj*cc+ss;
106         mat[0][1] = cj*sh; 
107         mat[1][1] = sj*ss+cc; 
108         mat[2][1] = sj*cs-sc;
109         mat[0][2] = -sj;         
110         mat[1][2] = cj*si;    
111         mat[2][2] = cj*ci;
112
113         return mat;
114 }
115
116
117
118 /* old function from Blender */
119 void Mat3ToEulOld(MT_Matrix3x3 mat, float *eul)
120 {
121         MT_Scalar cy;
122         
123         cy = sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
124
125         if (cy > 16.0*FLT_EPSILON) {
126                 eul[0] = atan2(mat[1][2], mat[2][2]);
127                 eul[1] = atan2(-mat[0][2], cy);
128                 eul[2] = atan2(mat[0][1], mat[0][0]);
129         } else {
130                 eul[0] = atan2(-mat[2][1], mat[1][1]);
131                 eul[1] = atan2(-mat[0][2], cy);
132                 eul[2] = 0.0;
133         }
134 }
135
136
137
138 /* old function from Blender */
139 void compatible_eulFast(float *eul, float *oldrot)
140 {
141         float dx, dy, dz;
142         
143         /* verschillen van ong 360 graden corrigeren */
144
145         dx= eul[0] - oldrot[0];
146         dy= eul[1] - oldrot[1];
147         dz= eul[2] - oldrot[2];
148
149         if( fabs(dx) > 5.1) {
150                 if(dx > 0.0) eul[0] -= MT_2_PI; else eul[0]+= MT_2_PI;
151         }
152         if( fabs(dy) > 5.1) {
153                 if(dy > 0.0) eul[1] -= MT_2_PI; else eul[1]+= MT_2_PI;
154         }
155         if( fabs(dz) > 5.1 ) {
156                 if(dz > 0.0) eul[2] -= MT_2_PI; else eul[2]+= MT_2_PI;
157         }
158 }
159
160
161
162 MT_Matrix3x3 matrix3x3_interpol(MT_Matrix3x3 oldmat, MT_Matrix3x3 mat, int m_time)
163 {
164         float eul[3], oldeul[3];        
165
166         Mat3ToEulOld(oldmat, oldeul);
167         Mat3ToEulOld(mat, eul);
168         compatible_eulFast(eul, oldeul);
169         
170         eul[0]= (m_time*oldeul[0] + eul[0])/(1.0+m_time);
171         eul[1]= (m_time*oldeul[1] + eul[1])/(1.0+m_time);
172         eul[2]= (m_time*oldeul[2] + eul[2])/(1.0+m_time);
173         
174         return EulToMat3(eul);
175 }
176
177
178
179 KX_TrackToActuator::~KX_TrackToActuator()
180 {
181         if (m_object)
182                 m_object->UnregisterActuator(this);
183 } /* end of destructor */
184
185 void KX_TrackToActuator::ProcessReplica()
186 {
187         // the replica is tracking the same object => register it
188         if (m_object)
189                 m_object->RegisterActuator(this);
190         SCA_IActuator::ProcessReplica();
191 }
192
193
194 bool KX_TrackToActuator::UnlinkObject(SCA_IObject* clientobj)
195 {
196         if (clientobj == m_object)
197         {
198                 // this object is being deleted, we cannot continue to track it.
199                 m_object = NULL;
200                 return true;
201         }
202         return false;
203 }
204
205 bool KX_TrackToActuator::Update(double curtime, bool frame)
206 {
207         bool result = false;    
208         bool bNegativeEvent = IsNegativeEvent();
209         RemoveAllEvents();
210
211         if (bNegativeEvent)
212         {
213                 // do nothing on negative events
214         }
215         else if (m_object)
216         {
217                 KX_GameObject* curobj = (KX_GameObject*) GetParent();
218                 MT_Vector3 dir = ((KX_GameObject*)m_object)->NodeGetWorldPosition() - curobj->NodeGetWorldPosition();
219                 dir.normalize();
220                 MT_Vector3 up(0,0,1);
221                 
222                 
223 #ifdef DSADSA
224                 switch (m_upflag)
225                 {
226                 case 0:
227                         {
228                                 up = MT_Vector3(1.0,0,0);
229                                 break;
230                         } 
231                 case 1:
232                         {
233                                 up = MT_Vector3(0,1.0,0);
234                                 break;
235                         }
236                 case 2:
237                 default:
238                         {
239                                 up = MT_Vector3(0,0,1.0);
240                         }
241                 }
242 #endif 
243                 if (m_allow3D)
244                 {
245                         up = (up - up.dot(dir) * dir).normalized();
246                         
247                 }
248                 else
249                 {
250                         dir = (dir - up.dot(dir)*up).normalized();
251                 }
252                 
253                 MT_Vector3 left;
254                 MT_Matrix3x3 mat;
255                 
256                 switch (m_trackflag)
257                 {
258                 case 0: // TRACK X
259                         {
260                                 // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
261                                 left  = dir.normalized();
262                                 dir = (left.cross(up)).normalized();
263                                 mat.setValue (
264                                         left[0], dir[0],up[0], 
265                                         left[1], dir[1],up[1],
266                                         left[2], dir[2],up[2]
267                                         );
268                                 
269                                 break;
270                         };
271                 case 1: // TRACK Y
272                         {
273                                 // (0.0 , 1.0 , 0.0 ) y direction is forward, z (0.0 , 0.0 , 1.0 ) up
274                                 left  = (dir.cross(up)).normalized();
275                                 mat.setValue (
276                                         left[0], dir[0],up[0], 
277                                         left[1], dir[1],up[1],
278                                         left[2], dir[2],up[2]
279                                         );
280                                 
281                                 break;
282                         }
283                         
284                 case 2: // track Z
285                         {
286                                 left = up.normalized();
287                                 up = dir.normalized();
288                                 dir = left;
289                                 left  = (dir.cross(up)).normalized();
290                                 mat.setValue (
291                                         left[0], dir[0],up[0], 
292                                         left[1], dir[1],up[1],
293                                         left[2], dir[2],up[2]
294                                         );
295                                 break;
296                         }
297                         
298                 case 3: // TRACK -X
299                         {
300                                 // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
301                                 left  = -dir.normalized();
302                                 dir = -(left.cross(up)).normalized();
303                                 mat.setValue (
304                                         left[0], dir[0],up[0], 
305                                         left[1], dir[1],up[1],
306                                         left[2], dir[2],up[2]
307                                         );
308                                 
309                                 break;
310                         };
311                 case 4: // TRACK -Y
312                         {
313                                 // (0.0 , -1.0 , 0.0 ) -y direction is forward, z (0.0 , 0.0 , 1.0 ) up
314                                 left  = (-dir.cross(up)).normalized();
315                                 mat.setValue (
316                                         left[0], -dir[0],up[0], 
317                                         left[1], -dir[1],up[1],
318                                         left[2], -dir[2],up[2]
319                                         );
320                                 break;
321                         }
322                 case 5: // track -Z
323                         {
324                                 left = up.normalized();
325                                 up = -dir.normalized();
326                                 dir = left;
327                                 left  = (dir.cross(up)).normalized();
328                                 mat.setValue (
329                                         left[0], dir[0],up[0], 
330                                         left[1], dir[1],up[1],
331                                         left[2], dir[2],up[2]
332                                         );
333                                 
334                                 break;
335                         }
336                         
337                 default:
338                         {
339                                 // (1.0 , 0.0 , 0.0 ) -x direction is forward, z (0.0 , 0.0 , 1.0 ) up
340                                 left  = -dir.normalized();
341                                 dir = -(left.cross(up)).normalized();
342                                 mat.setValue (
343                                         left[0], dir[0],up[0], 
344                                         left[1], dir[1],up[1],
345                                         left[2], dir[2],up[2]
346                                         );
347                         }
348                 }
349                 
350                 MT_Matrix3x3 oldmat;
351                 oldmat= curobj->NodeGetWorldOrientation();
352                 
353                 /* erwin should rewrite this! */
354                 mat= matrix3x3_interpol(oldmat, mat, m_time);
355                 
356
357                 if(m_parentobj){ // check if the model is parented and calculate the child transform
358                                 
359                         MT_Point3 localpos;
360                         localpos = curobj->GetSGNode()->GetLocalPosition();
361                         // Get the inverse of the parent matrix
362                         MT_Matrix3x3 parentmatinv;
363                         parentmatinv = m_parentobj->NodeGetWorldOrientation ().inverse ();                              
364                         // transform the local coordinate system into the parents system
365                         mat = parentmatinv * mat;
366                         // append the initial parent local rotation matrix
367                         mat = m_parentlocalmat * mat;
368
369                         // set the models tranformation properties
370                         curobj->NodeSetLocalOrientation(mat);
371                         curobj->NodeSetLocalPosition(localpos);
372                         curobj->UpdateTransform();
373                 }
374                 else
375                 {
376                         curobj->NodeSetLocalOrientation(mat);
377                 }
378
379                 result = true;
380         }
381
382         return result;
383 }
384
385
386
387 /* ------------------------------------------------------------------------- */
388 /* Python functions                                                          */
389 /* ------------------------------------------------------------------------- */
390
391
392
393 /* Integration hooks ------------------------------------------------------- */
394 PyTypeObject KX_TrackToActuator::Type = {
395         PyObject_HEAD_INIT(&PyType_Type)
396         0,
397         "KX_TrackToActuator",
398         sizeof(KX_TrackToActuator),
399         0,
400         PyDestructor,
401         0,
402         __getattr,
403         __setattr,
404         0, //&MyPyCompare,
405         __repr,
406         0, //&cvalue_as_number,
407         0,
408         0,
409         0,
410         0
411 };
412
413
414
415 PyParentObject KX_TrackToActuator::Parents[] = {
416         &KX_TrackToActuator::Type,
417                 &SCA_IActuator::Type,
418                 &SCA_ILogicBrick::Type,
419                 &CValue::Type,
420                 NULL
421 };
422
423
424
425 PyMethodDef KX_TrackToActuator::Methods[] = {
426         {"setObject", (PyCFunction) KX_TrackToActuator::sPySetObject, METH_VARARGS, SetObject_doc},
427         {"getObject", (PyCFunction) KX_TrackToActuator::sPyGetObject, METH_VARARGS, GetObject_doc},
428         {"setTime", (PyCFunction) KX_TrackToActuator::sPySetTime, METH_VARARGS, SetTime_doc},
429         {"getTime", (PyCFunction) KX_TrackToActuator::sPyGetTime, METH_VARARGS, GetTime_doc},
430         {"setUse3D", (PyCFunction) KX_TrackToActuator::sPySetUse3D, METH_VARARGS, SetUse3D_doc},
431         {"getUse3D", (PyCFunction) KX_TrackToActuator::sPyGetUse3D, METH_VARARGS, GetUse3D_doc},
432         {NULL,NULL} //Sentinel
433 };
434
435
436
437 PyObject* KX_TrackToActuator::_getattr(const STR_String& attr)
438 {
439         _getattr_up(SCA_IActuator);
440 }
441
442
443
444 /* 1. setObject */
445 char KX_TrackToActuator::SetObject_doc[] = 
446 "setObject(object)\n"
447 "\t- object: string\n"
448 "\tSet the object to track with the parent of this actuator.\n";
449 PyObject* KX_TrackToActuator::PySetObject(PyObject* self, PyObject* args, PyObject* kwds) {
450         PyObject* gameobj;
451         if (PyArg_ParseTuple(args, "O!", &KX_GameObject::Type, &gameobj))
452         {
453                 if (m_object != NULL)
454                         m_object->UnregisterActuator(this);
455                 m_object = (SCA_IObject*)gameobj;
456                 if (m_object)
457                         m_object->RegisterActuator(this);
458                 Py_Return;
459         }
460         PyErr_Clear();
461         
462         char* objectname;
463         if (PyArg_ParseTuple(args, "s", &objectname))
464         {
465                 if (m_object != NULL)
466                         m_object->UnregisterActuator(this);
467                 m_object= static_cast<SCA_IObject*>(SCA_ILogicBrick::m_sCurrentLogicManager->GetGameObjectByName(STR_String(objectname)));
468                 if (m_object)
469                         m_object->RegisterActuator(this);
470                 Py_Return;
471         }
472         
473         return NULL;
474 }
475
476
477
478 /* 2. getObject */
479 char KX_TrackToActuator::GetObject_doc[] = 
480 "getObject()\n"
481 "\tReturns the object to track with the parent of this actuator.\n";
482 PyObject* KX_TrackToActuator::PyGetObject(PyObject* self, PyObject* args, PyObject* kwds)
483 {
484         if (!m_object)
485                 Py_Return;
486
487         return PyString_FromString(m_object->GetName());
488 }
489
490
491
492 /* 3. setTime */
493 char KX_TrackToActuator::SetTime_doc[] = 
494 "setTime(time)\n"
495 "\t- time: integer\n"
496 "\tSet the time in frames with which to delay the tracking motion.\n";
497 PyObject* KX_TrackToActuator::PySetTime(PyObject* self, PyObject* args, PyObject* kwds)
498 {
499         int timeArg;
500         
501         if (!PyArg_ParseTuple(args, "i", &timeArg))
502         {
503                 return NULL;
504         }
505         
506         m_time= timeArg;
507         
508         Py_Return;
509 }
510
511
512
513 /* 4.getTime */
514 char KX_TrackToActuator::GetTime_doc[] = 
515 "getTime()\n"
516 "\t- time: integer\n"
517 "\tReturn the time in frames with which the tracking motion is delayed.\n";
518 PyObject* KX_TrackToActuator::PyGetTime(PyObject* self, PyObject* args, PyObject* kwds)
519 {
520         return PyInt_FromLong(m_time);
521 }
522
523
524
525 /* 5. getUse3D */
526 char KX_TrackToActuator::GetUse3D_doc[] = 
527 "getUse3D()\n"
528 "\tReturns 1 if the motion is allowed to extend in the z-direction.\n";
529 PyObject* KX_TrackToActuator::PyGetUse3D(PyObject* self, PyObject* args, PyObject* kwds)
530 {
531         return PyInt_FromLong(!(m_allow3D == 0));
532 }
533
534
535
536 /* 6. setUse3D */
537 char KX_TrackToActuator::SetUse3D_doc[] = 
538 "setUse3D(value)\n"
539 "\t- value: 0 or 1\n"
540 "\tSet to 1 to allow the tracking motion to extend in the z-direction,\n"
541 "\tset to 0 to lock the tracking motion to the x-y plane.\n";
542 PyObject* KX_TrackToActuator::PySetUse3D(PyObject* self, PyObject* args, PyObject* kwds)
543 {
544         int boolArg;
545         
546         if (!PyArg_ParseTuple(args, "i", &boolArg)) {
547                 return NULL;
548         }
549         
550         m_allow3D = !(boolArg == 0);
551         
552         Py_Return;
553 }
554
555 /* eof */