5ea08f855a375a4eb54c60d3eb1cb7f433ed1d78
[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 #include "PyObjectPlus.h"
46
47 #ifdef HAVE_CONFIG_H
48 #include <config.h>
49 #endif
50
51 /* ------------------------------------------------------------------------- */
52 /* Native functions                                                          */
53 /* ------------------------------------------------------------------------- */
54
55
56
57 KX_TrackToActuator::KX_TrackToActuator(SCA_IObject *gameobj, 
58                                                                        SCA_IObject *ob,
59                                                                            int time,
60                                                                            bool allow3D,
61                                                                            int trackflag,
62                                                                            int upflag,
63                                                                            PyTypeObject* T)
64                                                                            :
65                                                                                 SCA_IActuator(gameobj, T)
66 {
67     m_time = time;
68     m_allow3D = allow3D;
69     m_object = ob;
70         m_trackflag = trackflag;
71         m_upflag = upflag;
72         m_parentobj = 0;
73         
74         if (m_object)
75                 m_object->RegisterActuator(this);
76
77         if (gameobj->isA(&KX_GameObject::Type))
78         {
79                 // if the object is vertex parented, don't check parent orientation as the link is broken
80                 if (!((KX_GameObject*)gameobj)->IsVertexParent()){
81                         m_parentobj = ((KX_GameObject*)gameobj)->GetParent(); // check if the object is parented 
82                         if (m_parentobj) {  
83                                 // if so, store the initial local rotation
84                                 // this is needed to revert the effect of the parent inverse node (TBC)
85                                 m_parentlocalmat = m_parentobj->GetSGNode()->GetLocalOrientation();
86                         }
87                 }
88         }
89
90 } /* End of constructor */
91
92
93
94 /* old function from Blender */
95 MT_Matrix3x3 EulToMat3(float *eul)
96 {
97         MT_Matrix3x3 mat;
98         float ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
99         
100         ci = cos(eul[0]); 
101         cj = cos(eul[1]); 
102         ch = cos(eul[2]);
103         si = sin(eul[0]); 
104         sj = sin(eul[1]); 
105         sh = sin(eul[2]);
106         cc = ci*ch; 
107         cs = ci*sh; 
108         sc = si*ch; 
109         ss = si*sh;
110
111         mat[0][0] = cj*ch; 
112         mat[1][0] = sj*sc-cs; 
113         mat[2][0] = sj*cc+ss;
114         mat[0][1] = cj*sh; 
115         mat[1][1] = sj*ss+cc; 
116         mat[2][1] = sj*cs-sc;
117         mat[0][2] = -sj;         
118         mat[1][2] = cj*si;    
119         mat[2][2] = cj*ci;
120
121         return mat;
122 }
123
124
125
126 /* old function from Blender */
127 void Mat3ToEulOld(MT_Matrix3x3 mat, float *eul)
128 {
129         MT_Scalar cy;
130         
131         cy = sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
132
133         if (cy > 16.0*FLT_EPSILON) {
134                 eul[0] = atan2(mat[1][2], mat[2][2]);
135                 eul[1] = atan2(-mat[0][2], cy);
136                 eul[2] = atan2(mat[0][1], mat[0][0]);
137         } else {
138                 eul[0] = atan2(-mat[2][1], mat[1][1]);
139                 eul[1] = atan2(-mat[0][2], cy);
140                 eul[2] = 0.0;
141         }
142 }
143
144
145
146 /* old function from Blender */
147 void compatible_eulFast(float *eul, float *oldrot)
148 {
149         float dx, dy, dz;
150         
151         /* angular difference of 360 degrees */
152
153         dx= eul[0] - oldrot[0];
154         dy= eul[1] - oldrot[1];
155         dz= eul[2] - oldrot[2];
156
157         if( fabs(dx) > MT_PI) {
158                 if(dx > 0.0) eul[0] -= MT_2_PI; else eul[0]+= MT_2_PI;
159         }
160         if( fabs(dy) > MT_PI) {
161                 if(dy > 0.0) eul[1] -= MT_2_PI; else eul[1]+= MT_2_PI;
162         }
163         if( fabs(dz) > MT_PI ) {
164                 if(dz > 0.0) eul[2] -= MT_2_PI; else eul[2]+= MT_2_PI;
165         }
166 }
167
168
169
170 MT_Matrix3x3 matrix3x3_interpol(MT_Matrix3x3 oldmat, MT_Matrix3x3 mat, int m_time)
171 {
172         float eul[3], oldeul[3];        
173
174         Mat3ToEulOld(oldmat, oldeul);
175         Mat3ToEulOld(mat, eul);
176         compatible_eulFast(eul, oldeul);
177         
178         eul[0]= (m_time*oldeul[0] + eul[0])/(1.0+m_time);
179         eul[1]= (m_time*oldeul[1] + eul[1])/(1.0+m_time);
180         eul[2]= (m_time*oldeul[2] + eul[2])/(1.0+m_time);
181         
182         return EulToMat3(eul);
183 }
184
185
186
187 KX_TrackToActuator::~KX_TrackToActuator()
188 {
189         if (m_object)
190                 m_object->UnregisterActuator(this);
191         if (m_parentobj)
192                 m_parentobj->Release();
193 } /* end of destructor */
194
195 void KX_TrackToActuator::ProcessReplica()
196 {
197         // the replica is tracking the same object => register it
198         if (m_object)
199                 m_object->RegisterActuator(this);
200         if (m_parentobj)
201                 m_parentobj->AddRef();
202         SCA_IActuator::ProcessReplica();
203 }
204
205
206 bool KX_TrackToActuator::UnlinkObject(SCA_IObject* clientobj)
207 {
208         if (clientobj == m_object)
209         {
210                 // this object is being deleted, we cannot continue to track it.
211                 m_object = NULL;
212                 return true;
213         }
214         return false;
215 }
216
217 void KX_TrackToActuator::Relink(GEN_Map<GEN_HashedPtr, void*> *obj_map)
218 {
219         void **h_obj = (*obj_map)[m_object];
220         if (h_obj) {
221                 if (m_object)
222                         m_object->UnregisterActuator(this);
223                 m_object = (SCA_IObject*)(*h_obj);
224                 m_object->RegisterActuator(this);
225         }
226
227         void **h_parobj = (*obj_map)[m_parentobj];
228         if (h_parobj) {
229                 if (m_parentobj)
230                         m_parentobj->Release();
231                 m_parentobj= (KX_GameObject*)(*h_parobj);
232                 m_parentobj->AddRef();
233         }
234 }
235
236
237 bool KX_TrackToActuator::Update(double curtime, bool frame)
238 {
239         bool result = false;    
240         bool bNegativeEvent = IsNegativeEvent();
241         RemoveAllEvents();
242
243         if (bNegativeEvent)
244         {
245                 // do nothing on negative events
246         }
247         else if (m_object)
248         {
249                 KX_GameObject* curobj = (KX_GameObject*) GetParent();
250                 MT_Vector3 dir = ((KX_GameObject*)m_object)->NodeGetWorldPosition() - curobj->NodeGetWorldPosition();
251                 if (dir.length2())
252                         dir.normalize();
253                 MT_Vector3 up(0,0,1);
254                 
255                 
256 #ifdef DSADSA
257                 switch (m_upflag)
258                 {
259                 case 0:
260                         {
261                                 up.setValue(1.0,0,0);
262                                 break;
263                         } 
264                 case 1:
265                         {
266                                 up.setValue(0,1.0,0);
267                                 break;
268                         }
269                 case 2:
270                 default:
271                         {
272                                 up.setValue(0,0,1.0);
273                         }
274                 }
275 #endif 
276                 if (m_allow3D)
277                 {
278                         up = (up - up.dot(dir) * dir).safe_normalized();
279                         
280                 }
281                 else
282                 {
283                         dir = (dir - up.dot(dir)*up).safe_normalized();
284                 }
285                 
286                 MT_Vector3 left;
287                 MT_Matrix3x3 mat;
288                 
289                 switch (m_trackflag)
290                 {
291                 case 0: // TRACK X
292                         {
293                                 // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
294                                 left  = dir.safe_normalized();
295                                 dir = (left.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                 case 1: // TRACK Y
305                         {
306                                 // (0.0 , 1.0 , 0.0 ) y direction is forward, z (0.0 , 0.0 , 1.0 ) up
307                                 left  = (dir.cross(up)).safe_normalized();
308                                 mat.setValue (
309                                         left[0], dir[0],up[0], 
310                                         left[1], dir[1],up[1],
311                                         left[2], dir[2],up[2]
312                                         );
313                                 
314                                 break;
315                         }
316                         
317                 case 2: // track Z
318                         {
319                                 left = up.safe_normalized();
320                                 up = dir.safe_normalized();
321                                 dir = left;
322                                 left  = (dir.cross(up)).safe_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                         
331                 case 3: // TRACK -X
332                         {
333                                 // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
334                                 left  = -dir.safe_normalized();
335                                 dir = -(left.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                                 
342                                 break;
343                         };
344                 case 4: // TRACK -Y
345                         {
346                                 // (0.0 , -1.0 , 0.0 ) -y direction is forward, z (0.0 , 0.0 , 1.0 ) up
347                                 left  = (-dir.cross(up)).safe_normalized();
348                                 mat.setValue (
349                                         left[0], -dir[0],up[0], 
350                                         left[1], -dir[1],up[1],
351                                         left[2], -dir[2],up[2]
352                                         );
353                                 break;
354                         }
355                 case 5: // track -Z
356                         {
357                                 left = up.safe_normalized();
358                                 up = -dir.safe_normalized();
359                                 dir = left;
360                                 left  = (dir.cross(up)).safe_normalized();
361                                 mat.setValue (
362                                         left[0], dir[0],up[0], 
363                                         left[1], dir[1],up[1],
364                                         left[2], dir[2],up[2]
365                                         );
366                                 
367                                 break;
368                         }
369                         
370                 default:
371                         {
372                                 // (1.0 , 0.0 , 0.0 ) -x direction is forward, z (0.0 , 0.0 , 1.0 ) up
373                                 left  = -dir.safe_normalized();
374                                 dir = -(left.cross(up)).safe_normalized();
375                                 mat.setValue (
376                                         left[0], dir[0],up[0], 
377                                         left[1], dir[1],up[1],
378                                         left[2], dir[2],up[2]
379                                         );
380                         }
381                 }
382                 
383                 MT_Matrix3x3 oldmat;
384                 oldmat= curobj->NodeGetWorldOrientation();
385                 
386                 /* erwin should rewrite this! */
387                 mat= matrix3x3_interpol(oldmat, mat, m_time);
388                 
389
390                 if(m_parentobj){ // check if the model is parented and calculate the child transform
391                                 
392                         MT_Point3 localpos;
393                         localpos = curobj->GetSGNode()->GetLocalPosition();
394                         // Get the inverse of the parent matrix
395                         MT_Matrix3x3 parentmatinv;
396                         parentmatinv = m_parentobj->NodeGetWorldOrientation ().inverse ();                              
397                         // transform the local coordinate system into the parents system
398                         mat = parentmatinv * mat;
399                         // append the initial parent local rotation matrix
400                         mat = m_parentlocalmat * mat;
401
402                         // set the models tranformation properties
403                         curobj->NodeSetLocalOrientation(mat);
404                         curobj->NodeSetLocalPosition(localpos);
405                         //curobj->UpdateTransform();
406                 }
407                 else
408                 {
409                         curobj->NodeSetLocalOrientation(mat);
410                 }
411
412                 result = true;
413         }
414
415         return result;
416 }
417
418
419
420 /* ------------------------------------------------------------------------- */
421 /* Python functions                                                          */
422 /* ------------------------------------------------------------------------- */
423
424
425
426 /* Integration hooks ------------------------------------------------------- */
427 PyTypeObject KX_TrackToActuator::Type = {
428         PyObject_HEAD_INIT(NULL)
429         0,
430         "KX_TrackToActuator",
431         sizeof(KX_TrackToActuator),
432         0,
433         PyDestructor,
434         0,
435         0,
436         0,
437         0,
438         py_base_repr,
439         0,0,0,0,0,0,
440         py_base_getattro,
441         py_base_setattro,
442         0,0,0,0,0,0,0,0,0,
443         Methods
444 };
445
446
447
448 PyParentObject KX_TrackToActuator::Parents[] = {
449         &KX_TrackToActuator::Type,
450                 &SCA_IActuator::Type,
451                 &SCA_ILogicBrick::Type,
452                 &CValue::Type,
453                 NULL
454 };
455
456
457
458 PyMethodDef KX_TrackToActuator::Methods[] = {
459         // ---> deprecated
460         {"setTime", (PyCFunction) KX_TrackToActuator::sPySetTime, METH_VARARGS, (PY_METHODCHAR)SetTime_doc},
461         {"getTime", (PyCFunction) KX_TrackToActuator::sPyGetTime, METH_VARARGS, (PY_METHODCHAR)GetTime_doc},
462         {"setUse3D", (PyCFunction) KX_TrackToActuator::sPySetUse3D, METH_VARARGS, (PY_METHODCHAR)SetUse3D_doc},
463         {"getUse3D", (PyCFunction) KX_TrackToActuator::sPyGetUse3D, METH_VARARGS, (PY_METHODCHAR)GetUse3D_doc},
464         {"setObject", (PyCFunction) KX_TrackToActuator::sPySetObject, METH_O, (PY_METHODCHAR)SetObject_doc},
465         {"getObject", (PyCFunction) KX_TrackToActuator::sPyGetObject, METH_VARARGS, (PY_METHODCHAR)GetObject_doc},
466         
467         {NULL,NULL} //Sentinel
468 };
469
470 PyAttributeDef KX_TrackToActuator::Attributes[] = {
471         KX_PYATTRIBUTE_INT_RW("time",0,1000,true,KX_TrackToActuator,m_time),
472         KX_PYATTRIBUTE_BOOL_RW("use3D",KX_TrackToActuator,m_allow3D),
473         KX_PYATTRIBUTE_RW_FUNCTION("object", KX_TrackToActuator, pyattr_get_object, pyattr_set_object),
474
475         { NULL }        //Sentinel
476 };
477
478 PyObject* KX_TrackToActuator::pyattr_get_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
479 {
480         KX_TrackToActuator* actuator = static_cast<KX_TrackToActuator*>(self);
481         if (!actuator->m_object)        
482                 Py_RETURN_NONE;
483         else
484                 return actuator->m_object->GetProxy();
485 }
486
487 int KX_TrackToActuator::pyattr_set_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
488 {
489         KX_TrackToActuator* actuator = static_cast<KX_TrackToActuator*>(self);
490         KX_GameObject *gameobj;
491                 
492         if (!ConvertPythonToGameObject(value, &gameobj, true))
493                 return 1; // ConvertPythonToGameObject sets the error
494                 
495         if (actuator->m_object != NULL)
496                 actuator->m_object->UnregisterActuator(actuator);       
497
498         actuator->m_object = (SCA_IObject*) gameobj;
499                 
500         if (actuator->m_object)
501                 actuator->m_object->RegisterActuator(actuator);
502                 
503         return 0;
504 }
505
506
507 PyObject* KX_TrackToActuator::py_getattro(PyObject *attr)
508 {
509         py_getattro_up(SCA_IActuator);
510 }
511
512 int KX_TrackToActuator::py_setattro(PyObject *attr, PyObject* value)
513 {
514         py_setattro_up(SCA_IActuator);
515 }
516
517 /* 1. setObject */
518 const char KX_TrackToActuator::SetObject_doc[] = 
519 "setObject(object)\n"
520 "\t- object: KX_GameObject, string or None\n"
521 "\tSet the object to track with the parent of this actuator.\n";
522 PyObject* KX_TrackToActuator::PySetObject(PyObject* self, PyObject* value)
523 {
524         KX_GameObject *gameobj;
525         
526         ShowDeprecationWarning("setObject()", "the object property");
527         
528         if (!ConvertPythonToGameObject(value, &gameobj, true))
529                 return NULL; // ConvertPythonToGameObject sets the error
530         
531         if (m_object != NULL)
532                 m_object->UnregisterActuator(this);     
533
534         m_object = (SCA_IObject*)gameobj;
535         if (m_object)
536                 m_object->RegisterActuator(this);
537         
538         Py_RETURN_NONE;
539 }
540
541
542
543 /* 2. getObject */
544 const char KX_TrackToActuator::GetObject_doc[] = 
545 "getObject(name_only = 1)\n"
546 "name_only - optional arg, when true will return the KX_GameObject rather then its name\n"
547 "\tReturns the object to track with the parent of this actuator\n";
548 PyObject* KX_TrackToActuator::PyGetObject(PyObject* self, PyObject* args)
549 {
550         int ret_name_only = 1;
551         
552         ShowDeprecationWarning("getObject()", "the object property");
553         
554         if (!PyArg_ParseTuple(args, "|i:getObject", &ret_name_only))
555                 return NULL;
556         
557         if (!m_object)
558                 Py_RETURN_NONE;
559         
560         if (ret_name_only)
561                 return PyString_FromString(m_object->GetName());
562         else
563                 return m_object->GetProxy();
564 }
565
566
567
568 /* 3. setTime */
569 const char KX_TrackToActuator::SetTime_doc[] = 
570 "setTime(time)\n"
571 "\t- time: integer\n"
572 "\tSet the time in frames with which to delay the tracking motion.\n";
573 PyObject* KX_TrackToActuator::PySetTime(PyObject* self, PyObject* args, PyObject* kwds)
574 {
575         ShowDeprecationWarning("setTime()", "the timer property");
576         int timeArg;
577         
578         if (!PyArg_ParseTuple(args, "i:setTime", &timeArg))
579         {
580                 return NULL;
581         }
582         
583         m_time= timeArg;
584         
585         Py_RETURN_NONE;
586 }
587
588
589
590 /* 4.getTime */
591 const char KX_TrackToActuator::GetTime_doc[] = 
592 "getTime()\n"
593 "\t- time: integer\n"
594 "\tReturn the time in frames with which the tracking motion is delayed.\n";
595 PyObject* KX_TrackToActuator::PyGetTime(PyObject* self, PyObject* args, PyObject* kwds)
596 {
597         ShowDeprecationWarning("getTime()", "the timer property");
598         return PyInt_FromLong(m_time);
599 }
600
601
602
603 /* 5. getUse3D */
604 const char KX_TrackToActuator::GetUse3D_doc[] = 
605 "getUse3D()\n"
606 "\tReturns 1 if the motion is allowed to extend in the z-direction.\n";
607 PyObject* KX_TrackToActuator::PyGetUse3D(PyObject* self, PyObject* args, PyObject* kwds)
608 {
609         ShowDeprecationWarning("setTime()", "the use3D property");
610         return PyInt_FromLong(!(m_allow3D == 0));
611 }
612
613
614
615 /* 6. setUse3D */
616 const char KX_TrackToActuator::SetUse3D_doc[] = 
617 "setUse3D(value)\n"
618 "\t- value: 0 or 1\n"
619 "\tSet to 1 to allow the tracking motion to extend in the z-direction,\n"
620 "\tset to 0 to lock the tracking motion to the x-y plane.\n";
621 PyObject* KX_TrackToActuator::PySetUse3D(PyObject* self, PyObject* args, PyObject* kwds)
622 {
623         ShowDeprecationWarning("setTime()", "the use3D property");
624         int boolArg;
625         
626         if (!PyArg_ParseTuple(args, "i:setUse3D", &boolArg)) {
627                 return NULL;
628         }
629         
630         m_allow3D = !(boolArg == 0);
631         
632         Py_RETURN_NONE;
633 }
634
635 /* eof */