Merged 15170:15635 from trunk (no conflicts or even merges)
[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                 if (dir.length2())
228                         dir.normalize();
229                 MT_Vector3 up(0,0,1);
230                 
231                 
232 #ifdef DSADSA
233                 switch (m_upflag)
234                 {
235                 case 0:
236                         {
237                                 up = MT_Vector3(1.0,0,0);
238                                 break;
239                         } 
240                 case 1:
241                         {
242                                 up = MT_Vector3(0,1.0,0);
243                                 break;
244                         }
245                 case 2:
246                 default:
247                         {
248                                 up = MT_Vector3(0,0,1.0);
249                         }
250                 }
251 #endif 
252                 if (m_allow3D)
253                 {
254                         up = (up - up.dot(dir) * dir).safe_normalized();
255                         
256                 }
257                 else
258                 {
259                         dir = (dir - up.dot(dir)*up).safe_normalized();
260                 }
261                 
262                 MT_Vector3 left;
263                 MT_Matrix3x3 mat;
264                 
265                 switch (m_trackflag)
266                 {
267                 case 0: // TRACK X
268                         {
269                                 // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
270                                 left  = dir.safe_normalized();
271                                 dir = (left.cross(up)).safe_normalized();
272                                 mat.setValue (
273                                         left[0], dir[0],up[0], 
274                                         left[1], dir[1],up[1],
275                                         left[2], dir[2],up[2]
276                                         );
277                                 
278                                 break;
279                         };
280                 case 1: // TRACK Y
281                         {
282                                 // (0.0 , 1.0 , 0.0 ) y direction is forward, z (0.0 , 0.0 , 1.0 ) up
283                                 left  = (dir.cross(up)).safe_normalized();
284                                 mat.setValue (
285                                         left[0], dir[0],up[0], 
286                                         left[1], dir[1],up[1],
287                                         left[2], dir[2],up[2]
288                                         );
289                                 
290                                 break;
291                         }
292                         
293                 case 2: // track Z
294                         {
295                                 left = up.safe_normalized();
296                                 up = dir.safe_normalized();
297                                 dir = left;
298                                 left  = (dir.cross(up)).safe_normalized();
299                                 mat.setValue (
300                                         left[0], dir[0],up[0], 
301                                         left[1], dir[1],up[1],
302                                         left[2], dir[2],up[2]
303                                         );
304                                 break;
305                         }
306                         
307                 case 3: // TRACK -X
308                         {
309                                 // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
310                                 left  = -dir.safe_normalized();
311                                 dir = -(left.cross(up)).safe_normalized();
312                                 mat.setValue (
313                                         left[0], dir[0],up[0], 
314                                         left[1], dir[1],up[1],
315                                         left[2], dir[2],up[2]
316                                         );
317                                 
318                                 break;
319                         };
320                 case 4: // TRACK -Y
321                         {
322                                 // (0.0 , -1.0 , 0.0 ) -y direction is forward, z (0.0 , 0.0 , 1.0 ) up
323                                 left  = (-dir.cross(up)).safe_normalized();
324                                 mat.setValue (
325                                         left[0], -dir[0],up[0], 
326                                         left[1], -dir[1],up[1],
327                                         left[2], -dir[2],up[2]
328                                         );
329                                 break;
330                         }
331                 case 5: // track -Z
332                         {
333                                 left = up.safe_normalized();
334                                 up = -dir.safe_normalized();
335                                 dir = left;
336                                 left  = (dir.cross(up)).safe_normalized();
337                                 mat.setValue (
338                                         left[0], dir[0],up[0], 
339                                         left[1], dir[1],up[1],
340                                         left[2], dir[2],up[2]
341                                         );
342                                 
343                                 break;
344                         }
345                         
346                 default:
347                         {
348                                 // (1.0 , 0.0 , 0.0 ) -x direction is forward, z (0.0 , 0.0 , 1.0 ) up
349                                 left  = -dir.safe_normalized();
350                                 dir = -(left.cross(up)).safe_normalized();
351                                 mat.setValue (
352                                         left[0], dir[0],up[0], 
353                                         left[1], dir[1],up[1],
354                                         left[2], dir[2],up[2]
355                                         );
356                         }
357                 }
358                 
359                 MT_Matrix3x3 oldmat;
360                 oldmat= curobj->NodeGetWorldOrientation();
361                 
362                 /* erwin should rewrite this! */
363                 mat= matrix3x3_interpol(oldmat, mat, m_time);
364                 
365
366                 if(m_parentobj){ // check if the model is parented and calculate the child transform
367                                 
368                         MT_Point3 localpos;
369                         localpos = curobj->GetSGNode()->GetLocalPosition();
370                         // Get the inverse of the parent matrix
371                         MT_Matrix3x3 parentmatinv;
372                         parentmatinv = m_parentobj->NodeGetWorldOrientation ().inverse ();                              
373                         // transform the local coordinate system into the parents system
374                         mat = parentmatinv * mat;
375                         // append the initial parent local rotation matrix
376                         mat = m_parentlocalmat * mat;
377
378                         // set the models tranformation properties
379                         curobj->NodeSetLocalOrientation(mat);
380                         curobj->NodeSetLocalPosition(localpos);
381                         curobj->UpdateTransform();
382                 }
383                 else
384                 {
385                         curobj->NodeSetLocalOrientation(mat);
386                 }
387
388                 result = true;
389         }
390
391         return result;
392 }
393
394
395
396 /* ------------------------------------------------------------------------- */
397 /* Python functions                                                          */
398 /* ------------------------------------------------------------------------- */
399
400
401
402 /* Integration hooks ------------------------------------------------------- */
403 PyTypeObject KX_TrackToActuator::Type = {
404         PyObject_HEAD_INIT(&PyType_Type)
405         0,
406         "KX_TrackToActuator",
407         sizeof(KX_TrackToActuator),
408         0,
409         PyDestructor,
410         0,
411         __getattr,
412         __setattr,
413         0, //&MyPyCompare,
414         __repr,
415         0, //&cvalue_as_number,
416         0,
417         0,
418         0,
419         0
420 };
421
422
423
424 PyParentObject KX_TrackToActuator::Parents[] = {
425         &KX_TrackToActuator::Type,
426                 &SCA_IActuator::Type,
427                 &SCA_ILogicBrick::Type,
428                 &CValue::Type,
429                 NULL
430 };
431
432
433
434 PyMethodDef KX_TrackToActuator::Methods[] = {
435         {"setObject", (PyCFunction) KX_TrackToActuator::sPySetObject, METH_VARARGS, SetObject_doc},
436         {"getObject", (PyCFunction) KX_TrackToActuator::sPyGetObject, METH_VARARGS, GetObject_doc},
437         {"setTime", (PyCFunction) KX_TrackToActuator::sPySetTime, METH_VARARGS, SetTime_doc},
438         {"getTime", (PyCFunction) KX_TrackToActuator::sPyGetTime, METH_VARARGS, GetTime_doc},
439         {"setUse3D", (PyCFunction) KX_TrackToActuator::sPySetUse3D, METH_VARARGS, SetUse3D_doc},
440         {"getUse3D", (PyCFunction) KX_TrackToActuator::sPyGetUse3D, METH_VARARGS, GetUse3D_doc},
441         {NULL,NULL} //Sentinel
442 };
443
444
445
446 PyObject* KX_TrackToActuator::_getattr(const STR_String& attr)
447 {
448         _getattr_up(SCA_IActuator);
449 }
450
451
452
453 /* 1. setObject */
454 char KX_TrackToActuator::SetObject_doc[] = 
455 "setObject(object)\n"
456 "\t- object: string\n"
457 "\tSet the object to track with the parent of this actuator.\n";
458 PyObject* KX_TrackToActuator::PySetObject(PyObject* self, PyObject* args, PyObject* kwds) {
459         PyObject* gameobj;
460         if (PyArg_ParseTuple(args, "O!", &KX_GameObject::Type, &gameobj))
461         {
462                 if (m_object != NULL)
463                         m_object->UnregisterActuator(this);
464                 m_object = (SCA_IObject*)gameobj;
465                 if (m_object)
466                         m_object->RegisterActuator(this);
467                 Py_Return;
468         }
469         PyErr_Clear();
470         
471         char* objectname;
472         if (PyArg_ParseTuple(args, "s", &objectname))
473         {
474                 if (m_object != NULL)
475                         m_object->UnregisterActuator(this);
476                 m_object= static_cast<SCA_IObject*>(SCA_ILogicBrick::m_sCurrentLogicManager->GetGameObjectByName(STR_String(objectname)));
477                 if (m_object)
478                         m_object->RegisterActuator(this);
479                 Py_Return;
480         }
481         
482         return NULL;
483 }
484
485
486
487 /* 2. getObject */
488 char KX_TrackToActuator::GetObject_doc[] = 
489 "getObject()\n"
490 "\tReturns the object to track with the parent of this actuator.\n";
491 PyObject* KX_TrackToActuator::PyGetObject(PyObject* self, PyObject* args, PyObject* kwds)
492 {
493         if (!m_object)
494                 Py_Return;
495
496         return PyString_FromString(m_object->GetName());
497 }
498
499
500
501 /* 3. setTime */
502 char KX_TrackToActuator::SetTime_doc[] = 
503 "setTime(time)\n"
504 "\t- time: integer\n"
505 "\tSet the time in frames with which to delay the tracking motion.\n";
506 PyObject* KX_TrackToActuator::PySetTime(PyObject* self, PyObject* args, PyObject* kwds)
507 {
508         int timeArg;
509         
510         if (!PyArg_ParseTuple(args, "i", &timeArg))
511         {
512                 return NULL;
513         }
514         
515         m_time= timeArg;
516         
517         Py_Return;
518 }
519
520
521
522 /* 4.getTime */
523 char KX_TrackToActuator::GetTime_doc[] = 
524 "getTime()\n"
525 "\t- time: integer\n"
526 "\tReturn the time in frames with which the tracking motion is delayed.\n";
527 PyObject* KX_TrackToActuator::PyGetTime(PyObject* self, PyObject* args, PyObject* kwds)
528 {
529         return PyInt_FromLong(m_time);
530 }
531
532
533
534 /* 5. getUse3D */
535 char KX_TrackToActuator::GetUse3D_doc[] = 
536 "getUse3D()\n"
537 "\tReturns 1 if the motion is allowed to extend in the z-direction.\n";
538 PyObject* KX_TrackToActuator::PyGetUse3D(PyObject* self, PyObject* args, PyObject* kwds)
539 {
540         return PyInt_FromLong(!(m_allow3D == 0));
541 }
542
543
544
545 /* 6. setUse3D */
546 char KX_TrackToActuator::SetUse3D_doc[] = 
547 "setUse3D(value)\n"
548 "\t- value: 0 or 1\n"
549 "\tSet to 1 to allow the tracking motion to extend in the z-direction,\n"
550 "\tset to 0 to lock the tracking motion to the x-y plane.\n";
551 PyObject* KX_TrackToActuator::PySetUse3D(PyObject* self, PyObject* args, PyObject* kwds)
552 {
553         int boolArg;
554         
555         if (!PyArg_ParseTuple(args, "i", &boolArg)) {
556                 return NULL;
557         }
558         
559         m_allow3D = !(boolArg == 0);
560         
561         Py_Return;
562 }
563
564 /* eof */