Fix BGE bug #7532: TrackTo Actuator does not work correctly if it has a Vertex Parent...
[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
75         if (gameobj->isA(&KX_GameObject::Type))
76         {
77                 // if the object is vertex parented, don't check parent orientation as the link is broken
78                 if (!((KX_GameObject*)gameobj)->IsVertexParent()){
79                         m_parentobj = ((KX_GameObject*)gameobj)->GetParent(); // check if the object is parented 
80                         if (m_parentobj) {  
81                                 // if so, store the initial local rotation
82                                 // this is needed to revert the effect of the parent inverse node (TBC)
83                                 m_parentlocalmat = m_parentobj->GetSGNode()->GetLocalOrientation();
84                         }
85                 }
86         }
87
88 } /* End of constructor */
89
90
91
92 /* old function from Blender */
93 MT_Matrix3x3 EulToMat3(float *eul)
94 {
95         MT_Matrix3x3 mat;
96         float ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
97         
98         ci = cos(eul[0]); 
99         cj = cos(eul[1]); 
100         ch = cos(eul[2]);
101         si = sin(eul[0]); 
102         sj = sin(eul[1]); 
103         sh = sin(eul[2]);
104         cc = ci*ch; 
105         cs = ci*sh; 
106         sc = si*ch; 
107         ss = si*sh;
108
109         mat[0][0] = cj*ch; 
110         mat[1][0] = sj*sc-cs; 
111         mat[2][0] = sj*cc+ss;
112         mat[0][1] = cj*sh; 
113         mat[1][1] = sj*ss+cc; 
114         mat[2][1] = sj*cs-sc;
115         mat[0][2] = -sj;         
116         mat[1][2] = cj*si;    
117         mat[2][2] = cj*ci;
118
119         return mat;
120 }
121
122
123
124 /* old function from Blender */
125 void Mat3ToEulOld(MT_Matrix3x3 mat, float *eul)
126 {
127         MT_Scalar cy;
128         
129         cy = sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
130
131         if (cy > 16.0*FLT_EPSILON) {
132                 eul[0] = atan2(mat[1][2], mat[2][2]);
133                 eul[1] = atan2(-mat[0][2], cy);
134                 eul[2] = atan2(mat[0][1], mat[0][0]);
135         } else {
136                 eul[0] = atan2(-mat[2][1], mat[1][1]);
137                 eul[1] = atan2(-mat[0][2], cy);
138                 eul[2] = 0.0;
139         }
140 }
141
142
143
144 /* old function from Blender */
145 void compatible_eulFast(float *eul, float *oldrot)
146 {
147         float dx, dy, dz;
148         
149         /* verschillen van ong 360 graden corrigeren */
150
151         dx= eul[0] - oldrot[0];
152         dy= eul[1] - oldrot[1];
153         dz= eul[2] - oldrot[2];
154
155         if( fabs(dx) > 5.1) {
156                 if(dx > 0.0) eul[0] -= MT_2_PI; else eul[0]+= MT_2_PI;
157         }
158         if( fabs(dy) > 5.1) {
159                 if(dy > 0.0) eul[1] -= MT_2_PI; else eul[1]+= MT_2_PI;
160         }
161         if( fabs(dz) > 5.1 ) {
162                 if(dz > 0.0) eul[2] -= MT_2_PI; else eul[2]+= MT_2_PI;
163         }
164 }
165
166
167
168 MT_Matrix3x3 matrix3x3_interpol(MT_Matrix3x3 oldmat, MT_Matrix3x3 mat, int m_time)
169 {
170         float eul[3], oldeul[3];        
171
172         Mat3ToEulOld(oldmat, oldeul);
173         Mat3ToEulOld(mat, eul);
174         compatible_eulFast(eul, oldeul);
175         
176         eul[0]= (m_time*oldeul[0] + eul[0])/(1.0+m_time);
177         eul[1]= (m_time*oldeul[1] + eul[1])/(1.0+m_time);
178         eul[2]= (m_time*oldeul[2] + eul[2])/(1.0+m_time);
179         
180         return EulToMat3(eul);
181 }
182
183
184
185 KX_TrackToActuator::~KX_TrackToActuator()
186 {
187         if (m_object)
188                 m_object->UnregisterActuator(this);
189         if (m_parentobj)
190                 m_parentobj->Release();
191 } /* end of destructor */
192
193 void KX_TrackToActuator::ProcessReplica()
194 {
195         // the replica is tracking the same object => register it
196         if (m_object)
197                 m_object->RegisterActuator(this);
198         SCA_IActuator::ProcessReplica();
199 }
200
201
202 bool KX_TrackToActuator::UnlinkObject(SCA_IObject* clientobj)
203 {
204         if (clientobj == m_object)
205         {
206                 // this object is being deleted, we cannot continue to track it.
207                 m_object = NULL;
208                 return true;
209         }
210         return false;
211 }
212
213 bool KX_TrackToActuator::Update(double curtime, bool frame)
214 {
215         bool result = false;    
216         bool bNegativeEvent = IsNegativeEvent();
217         RemoveAllEvents();
218
219         if (bNegativeEvent)
220         {
221                 // do nothing on negative events
222         }
223         else if (m_object)
224         {
225                 KX_GameObject* curobj = (KX_GameObject*) GetParent();
226                 MT_Vector3 dir = ((KX_GameObject*)m_object)->NodeGetWorldPosition() - curobj->NodeGetWorldPosition();
227                 dir.normalize();
228                 MT_Vector3 up(0,0,1);
229                 
230                 
231 #ifdef DSADSA
232                 switch (m_upflag)
233                 {
234                 case 0:
235                         {
236                                 up = MT_Vector3(1.0,0,0);
237                                 break;
238                         } 
239                 case 1:
240                         {
241                                 up = MT_Vector3(0,1.0,0);
242                                 break;
243                         }
244                 case 2:
245                 default:
246                         {
247                                 up = MT_Vector3(0,0,1.0);
248                         }
249                 }
250 #endif 
251                 if (m_allow3D)
252                 {
253                         up = (up - up.dot(dir) * dir).normalized();
254                         
255                 }
256                 else
257                 {
258                         dir = (dir - up.dot(dir)*up).normalized();
259                 }
260                 
261                 MT_Vector3 left;
262                 MT_Matrix3x3 mat;
263                 
264                 switch (m_trackflag)
265                 {
266                 case 0: // TRACK X
267                         {
268                                 // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
269                                 left  = dir.normalized();
270                                 dir = (left.cross(up)).normalized();
271                                 mat.setValue (
272                                         left[0], dir[0],up[0], 
273                                         left[1], dir[1],up[1],
274                                         left[2], dir[2],up[2]
275                                         );
276                                 
277                                 break;
278                         };
279                 case 1: // TRACK Y
280                         {
281                                 // (0.0 , 1.0 , 0.0 ) y direction is forward, z (0.0 , 0.0 , 1.0 ) up
282                                 left  = (dir.cross(up)).normalized();
283                                 mat.setValue (
284                                         left[0], dir[0],up[0], 
285                                         left[1], dir[1],up[1],
286                                         left[2], dir[2],up[2]
287                                         );
288                                 
289                                 break;
290                         }
291                         
292                 case 2: // track Z
293                         {
294                                 left = up.normalized();
295                                 up = dir.normalized();
296                                 dir = left;
297                                 left  = (dir.cross(up)).normalized();
298                                 mat.setValue (
299                                         left[0], dir[0],up[0], 
300                                         left[1], dir[1],up[1],
301                                         left[2], dir[2],up[2]
302                                         );
303                                 break;
304                         }
305                         
306                 case 3: // TRACK -X
307                         {
308                                 // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
309                                 left  = -dir.normalized();
310                                 dir = -(left.cross(up)).normalized();
311                                 mat.setValue (
312                                         left[0], dir[0],up[0], 
313                                         left[1], dir[1],up[1],
314                                         left[2], dir[2],up[2]
315                                         );
316                                 
317                                 break;
318                         };
319                 case 4: // TRACK -Y
320                         {
321                                 // (0.0 , -1.0 , 0.0 ) -y direction is forward, z (0.0 , 0.0 , 1.0 ) up
322                                 left  = (-dir.cross(up)).normalized();
323                                 mat.setValue (
324                                         left[0], -dir[0],up[0], 
325                                         left[1], -dir[1],up[1],
326                                         left[2], -dir[2],up[2]
327                                         );
328                                 break;
329                         }
330                 case 5: // track -Z
331                         {
332                                 left = up.normalized();
333                                 up = -dir.normalized();
334                                 dir = left;
335                                 left  = (dir.cross(up)).normalized();
336                                 mat.setValue (
337                                         left[0], dir[0],up[0], 
338                                         left[1], dir[1],up[1],
339                                         left[2], dir[2],up[2]
340                                         );
341                                 
342                                 break;
343                         }
344                         
345                 default:
346                         {
347                                 // (1.0 , 0.0 , 0.0 ) -x direction is forward, z (0.0 , 0.0 , 1.0 ) up
348                                 left  = -dir.normalized();
349                                 dir = -(left.cross(up)).normalized();
350                                 mat.setValue (
351                                         left[0], dir[0],up[0], 
352                                         left[1], dir[1],up[1],
353                                         left[2], dir[2],up[2]
354                                         );
355                         }
356                 }
357                 
358                 MT_Matrix3x3 oldmat;
359                 oldmat= curobj->NodeGetWorldOrientation();
360                 
361                 /* erwin should rewrite this! */
362                 mat= matrix3x3_interpol(oldmat, mat, m_time);
363                 
364
365                 if(m_parentobj){ // check if the model is parented and calculate the child transform
366                                 
367                         MT_Point3 localpos;
368                         localpos = curobj->GetSGNode()->GetLocalPosition();
369                         // Get the inverse of the parent matrix
370                         MT_Matrix3x3 parentmatinv;
371                         parentmatinv = m_parentobj->NodeGetWorldOrientation ().inverse ();                              
372                         // transform the local coordinate system into the parents system
373                         mat = parentmatinv * mat;
374                         // append the initial parent local rotation matrix
375                         mat = m_parentlocalmat * mat;
376
377                         // set the models tranformation properties
378                         curobj->NodeSetLocalOrientation(mat);
379                         curobj->NodeSetLocalPosition(localpos);
380                         curobj->UpdateTransform();
381                 }
382                 else
383                 {
384                         curobj->NodeSetLocalOrientation(mat);
385                 }
386
387                 result = true;
388         }
389
390         return result;
391 }
392
393
394
395 /* ------------------------------------------------------------------------- */
396 /* Python functions                                                          */
397 /* ------------------------------------------------------------------------- */
398
399
400
401 /* Integration hooks ------------------------------------------------------- */
402 PyTypeObject KX_TrackToActuator::Type = {
403         PyObject_HEAD_INIT(&PyType_Type)
404         0,
405         "KX_TrackToActuator",
406         sizeof(KX_TrackToActuator),
407         0,
408         PyDestructor,
409         0,
410         __getattr,
411         __setattr,
412         0, //&MyPyCompare,
413         __repr,
414         0, //&cvalue_as_number,
415         0,
416         0,
417         0,
418         0
419 };
420
421
422
423 PyParentObject KX_TrackToActuator::Parents[] = {
424         &KX_TrackToActuator::Type,
425                 &SCA_IActuator::Type,
426                 &SCA_ILogicBrick::Type,
427                 &CValue::Type,
428                 NULL
429 };
430
431
432
433 PyMethodDef KX_TrackToActuator::Methods[] = {
434         {"setObject", (PyCFunction) KX_TrackToActuator::sPySetObject, METH_VARARGS, SetObject_doc},
435         {"getObject", (PyCFunction) KX_TrackToActuator::sPyGetObject, METH_VARARGS, GetObject_doc},
436         {"setTime", (PyCFunction) KX_TrackToActuator::sPySetTime, METH_VARARGS, SetTime_doc},
437         {"getTime", (PyCFunction) KX_TrackToActuator::sPyGetTime, METH_VARARGS, GetTime_doc},
438         {"setUse3D", (PyCFunction) KX_TrackToActuator::sPySetUse3D, METH_VARARGS, SetUse3D_doc},
439         {"getUse3D", (PyCFunction) KX_TrackToActuator::sPyGetUse3D, METH_VARARGS, GetUse3D_doc},
440         {NULL,NULL} //Sentinel
441 };
442
443
444
445 PyObject* KX_TrackToActuator::_getattr(const STR_String& attr)
446 {
447         _getattr_up(SCA_IActuator);
448 }
449
450
451
452 /* 1. setObject */
453 char KX_TrackToActuator::SetObject_doc[] = 
454 "setObject(object)\n"
455 "\t- object: string\n"
456 "\tSet the object to track with the parent of this actuator.\n";
457 PyObject* KX_TrackToActuator::PySetObject(PyObject* self, PyObject* args, PyObject* kwds) {
458         PyObject* gameobj;
459         if (PyArg_ParseTuple(args, "O!", &KX_GameObject::Type, &gameobj))
460         {
461                 if (m_object != NULL)
462                         m_object->UnregisterActuator(this);
463                 m_object = (SCA_IObject*)gameobj;
464                 if (m_object)
465                         m_object->RegisterActuator(this);
466                 Py_Return;
467         }
468         PyErr_Clear();
469         
470         char* objectname;
471         if (PyArg_ParseTuple(args, "s", &objectname))
472         {
473                 if (m_object != NULL)
474                         m_object->UnregisterActuator(this);
475                 m_object= static_cast<SCA_IObject*>(SCA_ILogicBrick::m_sCurrentLogicManager->GetGameObjectByName(STR_String(objectname)));
476                 if (m_object)
477                         m_object->RegisterActuator(this);
478                 Py_Return;
479         }
480         
481         return NULL;
482 }
483
484
485
486 /* 2. getObject */
487 char KX_TrackToActuator::GetObject_doc[] = 
488 "getObject()\n"
489 "\tReturns the object to track with the parent of this actuator.\n";
490 PyObject* KX_TrackToActuator::PyGetObject(PyObject* self, PyObject* args, PyObject* kwds)
491 {
492         if (!m_object)
493                 Py_Return;
494
495         return PyString_FromString(m_object->GetName());
496 }
497
498
499
500 /* 3. setTime */
501 char KX_TrackToActuator::SetTime_doc[] = 
502 "setTime(time)\n"
503 "\t- time: integer\n"
504 "\tSet the time in frames with which to delay the tracking motion.\n";
505 PyObject* KX_TrackToActuator::PySetTime(PyObject* self, PyObject* args, PyObject* kwds)
506 {
507         int timeArg;
508         
509         if (!PyArg_ParseTuple(args, "i", &timeArg))
510         {
511                 return NULL;
512         }
513         
514         m_time= timeArg;
515         
516         Py_Return;
517 }
518
519
520
521 /* 4.getTime */
522 char KX_TrackToActuator::GetTime_doc[] = 
523 "getTime()\n"
524 "\t- time: integer\n"
525 "\tReturn the time in frames with which the tracking motion is delayed.\n";
526 PyObject* KX_TrackToActuator::PyGetTime(PyObject* self, PyObject* args, PyObject* kwds)
527 {
528         return PyInt_FromLong(m_time);
529 }
530
531
532
533 /* 5. getUse3D */
534 char KX_TrackToActuator::GetUse3D_doc[] = 
535 "getUse3D()\n"
536 "\tReturns 1 if the motion is allowed to extend in the z-direction.\n";
537 PyObject* KX_TrackToActuator::PyGetUse3D(PyObject* self, PyObject* args, PyObject* kwds)
538 {
539         return PyInt_FromLong(!(m_allow3D == 0));
540 }
541
542
543
544 /* 6. setUse3D */
545 char KX_TrackToActuator::SetUse3D_doc[] = 
546 "setUse3D(value)\n"
547 "\t- value: 0 or 1\n"
548 "\tSet to 1 to allow the tracking motion to extend in the z-direction,\n"
549 "\tset to 0 to lock the tracking motion to the x-y plane.\n";
550 PyObject* KX_TrackToActuator::PySetUse3D(PyObject* self, PyObject* args, PyObject* kwds)
551 {
552         int boolArg;
553         
554         if (!PyArg_ParseTuple(args, "i", &boolArg)) {
555                 return NULL;
556         }
557         
558         m_allow3D = !(boolArg == 0);
559         
560         Py_Return;
561 }
562
563 /* eof */