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