rename and negate DISABLE_PYTHON --> WITH_PYTHON
[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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, 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 /* ------------------------------------------------------------------------- */
48 /* Native functions                                                          */
49 /* ------------------------------------------------------------------------- */
50
51
52
53 KX_TrackToActuator::KX_TrackToActuator(SCA_IObject *gameobj, 
54                                                                        SCA_IObject *ob,
55                                                                            int time,
56                                                                            bool allow3D,
57                                                                            int trackflag,
58                                                                            int upflag)
59                                                                            : SCA_IActuator(gameobj, KX_ACT_TRACKTO)
60 {
61     m_time = time;
62     m_allow3D = allow3D;
63     m_object = ob;
64         m_trackflag = trackflag;
65         m_upflag = upflag;
66         m_parentobj = 0;
67         
68         if (m_object)
69                 m_object->RegisterActuator(this);
70
71         {
72                 // if the object is vertex parented, don't check parent orientation as the link is broken
73                 if (!((KX_GameObject*)gameobj)->IsVertexParent()){
74                         m_parentobj = ((KX_GameObject*)gameobj)->GetParent(); // check if the object is parented 
75                         if (m_parentobj) {  
76                                 // if so, store the initial local rotation
77                                 // this is needed to revert the effect of the parent inverse node (TBC)
78                                 m_parentlocalmat = m_parentobj->GetSGNode()->GetLocalOrientation();
79                                 // use registration mechanism rather than AddRef, it creates zombie objects
80                                 m_parentobj->RegisterActuator(this);
81                                 // GetParent did AddRef, undo here
82                                 m_parentobj->Release();
83                         }
84                 }
85         }
86
87 } /* End of constructor */
88
89
90
91 /* old function from Blender */
92 MT_Matrix3x3 EulToMat3(float *eul)
93 {
94         MT_Matrix3x3 mat;
95         float ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
96         
97         ci = cos(eul[0]); 
98         cj = cos(eul[1]); 
99         ch = cos(eul[2]);
100         si = sin(eul[0]); 
101         sj = sin(eul[1]); 
102         sh = sin(eul[2]);
103         cc = ci*ch; 
104         cs = ci*sh; 
105         sc = si*ch; 
106         ss = si*sh;
107
108         mat[0][0] = cj*ch; 
109         mat[1][0] = sj*sc-cs; 
110         mat[2][0] = sj*cc+ss;
111         mat[0][1] = cj*sh; 
112         mat[1][1] = sj*ss+cc; 
113         mat[2][1] = sj*cs-sc;
114         mat[0][2] = -sj;         
115         mat[1][2] = cj*si;    
116         mat[2][2] = cj*ci;
117
118         return mat;
119 }
120
121
122
123 /* old function from Blender */
124 void Mat3ToEulOld(MT_Matrix3x3 mat, float *eul)
125 {
126         MT_Scalar cy;
127         
128         cy = sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
129
130         if (cy > 16.0*FLT_EPSILON) {
131                 eul[0] = atan2(mat[1][2], mat[2][2]);
132                 eul[1] = atan2(-mat[0][2], cy);
133                 eul[2] = atan2(mat[0][1], mat[0][0]);
134         } else {
135                 eul[0] = atan2(-mat[2][1], mat[1][1]);
136                 eul[1] = atan2(-mat[0][2], cy);
137                 eul[2] = 0.0;
138         }
139 }
140
141
142
143 /* old function from Blender */
144 void compatible_eulFast(float *eul, float *oldrot)
145 {
146         float dx, dy, dz;
147         
148         /* angular difference of 360 degrees */
149
150         dx= eul[0] - oldrot[0];
151         dy= eul[1] - oldrot[1];
152         dz= eul[2] - oldrot[2];
153
154         if( fabs(dx) > MT_PI) {
155                 if(dx > 0.0) eul[0] -= MT_2_PI; else eul[0]+= MT_2_PI;
156         }
157         if( fabs(dy) > MT_PI) {
158                 if(dy > 0.0) eul[1] -= MT_2_PI; else eul[1]+= MT_2_PI;
159         }
160         if( fabs(dz) > MT_PI ) {
161                 if(dz > 0.0) eul[2] -= MT_2_PI; else eul[2]+= MT_2_PI;
162         }
163 }
164
165
166
167 MT_Matrix3x3 matrix3x3_interpol(MT_Matrix3x3 oldmat, MT_Matrix3x3 mat, int m_time)
168 {
169         float eul[3], oldeul[3];        
170
171         Mat3ToEulOld(oldmat, oldeul);
172         Mat3ToEulOld(mat, eul);
173         compatible_eulFast(eul, oldeul);
174         
175         eul[0]= (m_time*oldeul[0] + eul[0])/(1.0+m_time);
176         eul[1]= (m_time*oldeul[1] + eul[1])/(1.0+m_time);
177         eul[2]= (m_time*oldeul[2] + eul[2])/(1.0+m_time);
178         
179         return EulToMat3(eul);
180 }
181
182
183
184 KX_TrackToActuator::~KX_TrackToActuator()
185 {
186         if (m_object)
187                 m_object->UnregisterActuator(this);
188         if (m_parentobj)
189                 m_parentobj->UnregisterActuator(this);
190 } /* end of destructor */
191
192 void KX_TrackToActuator::ProcessReplica()
193 {
194         // the replica is tracking the same object => register it
195         if (m_object)
196                 m_object->RegisterActuator(this);
197         if (m_parentobj)
198                 m_parentobj->RegisterActuator(this);
199         SCA_IActuator::ProcessReplica();
200 }
201
202
203 bool KX_TrackToActuator::UnlinkObject(SCA_IObject* clientobj)
204 {
205         if (clientobj == m_object)
206         {
207                 // this object is being deleted, we cannot continue to track it.
208                 m_object = NULL;
209                 return true;
210         }
211         if (clientobj == m_parentobj)
212         {
213                 m_parentobj = NULL;
214                 return true;
215         }
216         return false;
217 }
218
219 void KX_TrackToActuator::Relink(GEN_Map<GEN_HashedPtr, void*> *obj_map)
220 {
221         void **h_obj = (*obj_map)[m_object];
222         if (h_obj) {
223                 if (m_object)
224                         m_object->UnregisterActuator(this);
225                 m_object = (SCA_IObject*)(*h_obj);
226                 m_object->RegisterActuator(this);
227         }
228
229         void **h_parobj = (*obj_map)[m_parentobj];
230         if (h_parobj) {
231                 if (m_parentobj)
232                         m_parentobj->UnregisterActuator(this);
233                 m_parentobj= (KX_GameObject*)(*h_parobj);
234                 m_parentobj->RegisterActuator(this);
235         }
236 }
237
238
239 bool KX_TrackToActuator::Update(double curtime, bool frame)
240 {
241         bool result = false;    
242         bool bNegativeEvent = IsNegativeEvent();
243         RemoveAllEvents();
244
245         if (bNegativeEvent)
246         {
247                 // do nothing on negative events
248         }
249         else if (m_object)
250         {
251                 KX_GameObject* curobj = (KX_GameObject*) GetParent();
252                 MT_Vector3 dir = ((KX_GameObject*)m_object)->NodeGetWorldPosition() - curobj->NodeGetWorldPosition();
253                 if (dir.length2())
254                         dir.normalize();
255                 MT_Vector3 up(0,0,1);
256                 
257                 
258 #ifdef DSADSA
259                 switch (m_upflag)
260                 {
261                 case 0:
262                         {
263                                 up.setValue(1.0,0,0);
264                                 break;
265                         } 
266                 case 1:
267                         {
268                                 up.setValue(0,1.0,0);
269                                 break;
270                         }
271                 case 2:
272                 default:
273                         {
274                                 up.setValue(0,0,1.0);
275                         }
276                 }
277 #endif 
278                 if (m_allow3D)
279                 {
280                         up = (up - up.dot(dir) * dir).safe_normalized();
281                         
282                 }
283                 else
284                 {
285                         dir = (dir - up.dot(dir)*up).safe_normalized();
286                 }
287                 
288                 MT_Vector3 left;
289                 MT_Matrix3x3 mat;
290                 
291                 switch (m_trackflag)
292                 {
293                 case 0: // TRACK X
294                         {
295                                 // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
296                                 left  = dir.safe_normalized();
297                                 dir = (left.cross(up)).safe_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                                 
304                                 break;
305                         };
306                 case 1: // TRACK Y
307                         {
308                                 // (0.0 , 1.0 , 0.0 ) y direction is forward, z (0.0 , 0.0 , 1.0 ) up
309                                 left  = (dir.cross(up)).safe_normalized();
310                                 mat.setValue (
311                                         left[0], dir[0],up[0], 
312                                         left[1], dir[1],up[1],
313                                         left[2], dir[2],up[2]
314                                         );
315                                 
316                                 break;
317                         }
318                         
319                 case 2: // track Z
320                         {
321                                 left = up.safe_normalized();
322                                 up = dir.safe_normalized();
323                                 dir = left;
324                                 left  = (dir.cross(up)).safe_normalized();
325                                 mat.setValue (
326                                         left[0], dir[0],up[0], 
327                                         left[1], dir[1],up[1],
328                                         left[2], dir[2],up[2]
329                                         );
330                                 break;
331                         }
332                         
333                 case 3: // TRACK -X
334                         {
335                                 // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
336                                 left  = -dir.safe_normalized();
337                                 dir = -(left.cross(up)).safe_normalized();
338                                 mat.setValue (
339                                         left[0], dir[0],up[0], 
340                                         left[1], dir[1],up[1],
341                                         left[2], dir[2],up[2]
342                                         );
343                                 
344                                 break;
345                         };
346                 case 4: // TRACK -Y
347                         {
348                                 // (0.0 , -1.0 , 0.0 ) -y direction is forward, z (0.0 , 0.0 , 1.0 ) up
349                                 left  = (-dir.cross(up)).safe_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                                 break;
356                         }
357                 case 5: // track -Z
358                         {
359                                 left = up.safe_normalized();
360                                 up = -dir.safe_normalized();
361                                 dir = left;
362                                 left  = (dir.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                                 break;
370                         }
371                         
372                 default:
373                         {
374                                 // (1.0 , 0.0 , 0.0 ) -x direction is forward, z (0.0 , 0.0 , 1.0 ) up
375                                 left  = -dir.safe_normalized();
376                                 dir = -(left.cross(up)).safe_normalized();
377                                 mat.setValue (
378                                         left[0], dir[0],up[0], 
379                                         left[1], dir[1],up[1],
380                                         left[2], dir[2],up[2]
381                                         );
382                         }
383                 }
384                 
385                 MT_Matrix3x3 oldmat;
386                 oldmat= curobj->NodeGetWorldOrientation();
387                 
388                 /* erwin should rewrite this! */
389                 mat= matrix3x3_interpol(oldmat, mat, m_time);
390                 
391
392                 if(m_parentobj){ // check if the model is parented and calculate the child transform
393                                 
394                         MT_Point3 localpos;
395                         localpos = curobj->GetSGNode()->GetLocalPosition();
396                         // Get the inverse of the parent matrix
397                         MT_Matrix3x3 parentmatinv;
398                         parentmatinv = m_parentobj->NodeGetWorldOrientation ().inverse ();                              
399                         // transform the local coordinate system into the parents system
400                         mat = parentmatinv * mat;
401                         // append the initial parent local rotation matrix
402                         mat = m_parentlocalmat * mat;
403
404                         // set the models tranformation properties
405                         curobj->NodeSetLocalOrientation(mat);
406                         curobj->NodeSetLocalPosition(localpos);
407                         //curobj->UpdateTransform();
408                 }
409                 else
410                 {
411                         curobj->NodeSetLocalOrientation(mat);
412                 }
413
414                 result = true;
415         }
416
417         return result;
418 }
419
420 #ifdef WITH_PYTHON
421
422 /* ------------------------------------------------------------------------- */
423 /* Python functions                                                          */
424 /* ------------------------------------------------------------------------- */
425
426 /* Integration hooks ------------------------------------------------------- */
427 PyTypeObject KX_TrackToActuator::Type = {
428         PyVarObject_HEAD_INIT(NULL, 0)
429         "KX_TrackToActuator",
430         sizeof(PyObjectPlus_Proxy),
431         0,
432         py_base_dealloc,
433         0,
434         0,
435         0,
436         0,
437         py_base_repr,
438         0,0,0,0,0,0,0,0,0,
439         Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
440         0,0,0,0,0,0,0,
441         Methods,
442         0,
443         0,
444         &SCA_IActuator::Type,
445         0,0,0,0,0,0,
446         py_base_new
447 };
448
449 PyMethodDef KX_TrackToActuator::Methods[] = {
450         {NULL,NULL} //Sentinel
451 };
452
453 PyAttributeDef KX_TrackToActuator::Attributes[] = {
454         KX_PYATTRIBUTE_INT_RW("time",0,1000,true,KX_TrackToActuator,m_time),
455         KX_PYATTRIBUTE_BOOL_RW("use3D",KX_TrackToActuator,m_allow3D),
456         KX_PYATTRIBUTE_RW_FUNCTION("object", KX_TrackToActuator, pyattr_get_object, pyattr_set_object),
457
458         { NULL }        //Sentinel
459 };
460
461 PyObject* KX_TrackToActuator::pyattr_get_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
462 {
463         KX_TrackToActuator* actuator = static_cast<KX_TrackToActuator*>(self);
464         if (!actuator->m_object)        
465                 Py_RETURN_NONE;
466         else
467                 return actuator->m_object->GetProxy();
468 }
469
470 int KX_TrackToActuator::pyattr_set_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
471 {
472         KX_TrackToActuator* actuator = static_cast<KX_TrackToActuator*>(self);
473         KX_GameObject *gameobj;
474                 
475         if (!ConvertPythonToGameObject(value, &gameobj, true, "actuator.object = value: KX_TrackToActuator"))
476                 return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error
477                 
478         if (actuator->m_object != NULL)
479                 actuator->m_object->UnregisterActuator(actuator);       
480
481         actuator->m_object = (SCA_IObject*) gameobj;
482                 
483         if (actuator->m_object)
484                 actuator->m_object->RegisterActuator(actuator);
485                 
486         return PY_SET_ATTR_SUCCESS;
487 }
488
489 #endif // WITH_PYTHON
490
491 /* eof */