af93121b50edb516ba70beae3503cd80335a29d2
[blender.git] / source / gameengine / Ketsji / KX_TrackToActuator.cpp
1 /** \file gameengine/Ketsji/KX_TrackToActuator.cpp
2  *  \ingroup ketsji
3  */
4 //
5 // Replace the mesh for this actuator's parent
6 //
7 //
8 // ***** BEGIN GPL LICENSE BLOCK *****
9 //
10 // This program is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU General Public License
12 // as published by the Free Software Foundation; either version 2
13 // of the License, or (at your option) any later version.
14 //
15 // This program is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
18 // GNU General Public License for more details.
19 //
20 // You should have received a copy of the GNU General Public License
21 // along with this program; if not, write to the Free Software Foundation,
22 // Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
23 //
24 // The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
25 // All rights reserved.
26 //
27 // The Original Code is: all of this file.
28 //
29 // Contributor(s): none yet.
30 //
31 // ***** END GPL LICENSE BLOCK *****
32
33 // todo: not all trackflags / upflags are implemented/tested !
34 // m_trackflag is used to determine the forward tracking direction
35 // m_upflag for the up direction
36 // normal situation is +y for forward, +z for up
37
38 #include "MT_Scalar.h"
39 #include "SCA_IActuator.h"
40 #include "KX_TrackToActuator.h"
41 #include "SCA_IScene.h"
42 #include "SCA_LogicManager.h"
43 #include <math.h>
44 #include <iostream>
45 #include "KX_GameObject.h"
46
47 #include "PyObjectPlus.h"
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     : SCA_IActuator(gameobj, KX_ACT_TRACKTO)
62 {
63         m_time = time;
64         m_allow3D = allow3D;
65         m_object = ob;
66         m_trackflag = trackflag;
67         m_upflag = upflag;
68         m_parentobj = 0;
69         
70         if (m_object)
71                 m_object->RegisterActuator(this);
72
73         {
74                 // if the object is vertex parented, don't check parent orientation as the link is broken
75                 if (!((KX_GameObject*)gameobj)->IsVertexParent()) {
76                         m_parentobj = ((KX_GameObject*)gameobj)->GetParent(); // check if the object is parented 
77                         if (m_parentobj) {  
78                                 // if so, store the initial local rotation
79                                 // this is needed to revert the effect of the parent inverse node (TBC)
80                                 m_parentlocalmat = m_parentobj->GetSGNode()->GetLocalOrientation();
81                                 // use registration mechanism rather than AddRef, it creates zombie objects
82                                 m_parentobj->RegisterActuator(this);
83                                 // GetParent did AddRef, undo here
84                                 m_parentobj->Release();
85                         }
86                 }
87         }
88
89 } /* End of constructor */
90
91
92
93 /* old function from Blender */
94 MT_Matrix3x3 EulToMat3(float *eul)
95 {
96         MT_Matrix3x3 mat;
97         float ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
98         
99         ci = cos(eul[0]); 
100         cj = cos(eul[1]); 
101         ch = cos(eul[2]);
102         si = sin(eul[0]); 
103         sj = sin(eul[1]); 
104         sh = sin(eul[2]);
105         cc = ci*ch; 
106         cs = ci*sh; 
107         sc = si*ch; 
108         ss = si*sh;
109
110         mat[0][0] = cj*ch; 
111         mat[1][0] = sj*sc-cs; 
112         mat[2][0] = sj*cc+ss;
113         mat[0][1] = cj*sh; 
114         mat[1][1] = sj*ss+cc; 
115         mat[2][1] = sj*cs-sc;
116         mat[0][2] = -sj;         
117         mat[1][2] = cj*si;    
118         mat[2][2] = cj*ci;
119
120         return mat;
121 }
122
123
124
125 /* old function from Blender */
126 void Mat3ToEulOld(MT_Matrix3x3 mat, float *eul)
127 {
128         MT_Scalar cy;
129         
130         cy = sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
131
132         if (cy > 16.0*FLT_EPSILON) {
133                 eul[0] = atan2(mat[1][2], mat[2][2]);
134                 eul[1] = atan2(-mat[0][2], cy);
135                 eul[2] = atan2(mat[0][1], mat[0][0]);
136         } else {
137                 eul[0] = atan2(-mat[2][1], mat[1][1]);
138                 eul[1] = atan2(-mat[0][2], cy);
139                 eul[2] = 0.0;
140         }
141 }
142
143
144
145 /* old function from Blender */
146 void compatible_eulFast(float *eul, float *oldrot)
147 {
148         float dx, dy, dz;
149         
150         /* angular difference of 360 degrees */
151
152         dx= eul[0] - oldrot[0];
153         dy= eul[1] - oldrot[1];
154         dz= eul[2] - oldrot[2];
155
156         if ( fabs(dx) > MT_PI) {
157                 if (dx > 0.0) eul[0] -= MT_2_PI; else eul[0]+= MT_2_PI;
158         }
159         if ( fabs(dy) > MT_PI) {
160                 if (dy > 0.0) eul[1] -= MT_2_PI; else eul[1]+= MT_2_PI;
161         }
162         if ( fabs(dz) > MT_PI ) {
163                 if (dz > 0.0) eul[2] -= MT_2_PI; else eul[2]+= MT_2_PI;
164         }
165 }
166
167
168
169 MT_Matrix3x3 matrix3x3_interpol(MT_Matrix3x3 oldmat, MT_Matrix3x3 mat, int m_time)
170 {
171         float eul[3], oldeul[3];        
172
173         Mat3ToEulOld(oldmat, oldeul);
174         Mat3ToEulOld(mat, eul);
175         compatible_eulFast(eul, oldeul);
176         
177         eul[0]= (m_time*oldeul[0] + eul[0])/(1.0+m_time);
178         eul[1]= (m_time*oldeul[1] + eul[1])/(1.0+m_time);
179         eul[2]= (m_time*oldeul[2] + eul[2])/(1.0+m_time);
180         
181         return EulToMat3(eul);
182 }
183
184
185
186 KX_TrackToActuator::~KX_TrackToActuator()
187 {
188         if (m_object)
189                 m_object->UnregisterActuator(this);
190         if (m_parentobj)
191                 m_parentobj->UnregisterActuator(this);
192 } /* end of destructor */
193
194 void KX_TrackToActuator::ProcessReplica()
195 {
196         // the replica is tracking the same object => register it
197         if (m_object)
198                 m_object->RegisterActuator(this);
199         if (m_parentobj)
200                 m_parentobj->RegisterActuator(this);
201         SCA_IActuator::ProcessReplica();
202 }
203
204
205 bool KX_TrackToActuator::UnlinkObject(SCA_IObject* clientobj)
206 {
207         if (clientobj == m_object)
208         {
209                 // this object is being deleted, we cannot continue to track it.
210                 m_object = NULL;
211                 return true;
212         }
213         if (clientobj == m_parentobj)
214         {
215                 m_parentobj = NULL;
216                 return true;
217         }
218         return false;
219 }
220
221 void KX_TrackToActuator::Relink(CTR_Map<CTR_HashedPtr, void*> *obj_map)
222 {
223         void **h_obj = (*obj_map)[m_object];
224         if (h_obj) {
225                 if (m_object)
226                         m_object->UnregisterActuator(this);
227                 m_object = (SCA_IObject*)(*h_obj);
228                 m_object->RegisterActuator(this);
229         }
230
231         void **h_parobj = (*obj_map)[m_parentobj];
232         if (h_parobj) {
233                 if (m_parentobj)
234                         m_parentobj->UnregisterActuator(this);
235                 m_parentobj= (KX_GameObject*)(*h_parobj);
236                 m_parentobj->RegisterActuator(this);
237         }
238 }
239
240
241 bool KX_TrackToActuator::Update(double curtime, bool frame)
242 {
243         bool result = false;    
244         bool bNegativeEvent = IsNegativeEvent();
245         RemoveAllEvents();
246
247         if (bNegativeEvent)
248         {
249                 // do nothing on negative events
250         }
251         else if (m_object)
252         {
253                 KX_GameObject* curobj = (KX_GameObject*) GetParent();
254                 MT_Vector3 dir = ((KX_GameObject*)m_object)->NodeGetWorldPosition() - curobj->NodeGetWorldPosition();
255                 if (dir.length2())
256                         dir.normalize();
257                 MT_Vector3 up(0,0,1);
258                 
259                 
260 #ifdef DSADSA
261                 switch (m_upflag)
262                 {
263                 case 0:
264                         {
265                                 up.setValue(1.0,0,0);
266                                 break;
267                         } 
268                 case 1:
269                         {
270                                 up.setValue(0,1.0,0);
271                                 break;
272                         }
273                 case 2:
274                 default:
275                         {
276                                 up.setValue(0,0,1.0);
277                         }
278                 }
279 #endif 
280                 if (m_allow3D)
281                 {
282                         up = (up - up.dot(dir) * dir).safe_normalized();
283                         
284                 }
285                 else
286                 {
287                         dir = (dir - up.dot(dir)*up).safe_normalized();
288                 }
289                 
290                 MT_Vector3 left;
291                 MT_Matrix3x3 mat;
292                 
293                 switch (m_trackflag)
294                 {
295                 case 0: // TRACK X
296                         {
297                                 // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
298                                 left  = dir.safe_normalized();
299                                 dir = (left.cross(up)).safe_normalized();
300                                 mat.setValue (
301                                         left[0], dir[0],up[0], 
302                                         left[1], dir[1],up[1],
303                                         left[2], dir[2],up[2]
304                                         );
305                                 
306                                 break;
307                         };
308                 case 1: // TRACK Y
309                         {
310                                 // (0.0 , 1.0 , 0.0 ) y direction is forward, z (0.0 , 0.0 , 1.0 ) up
311                                 left  = (dir.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                         
321                 case 2: // track Z
322                         {
323                                 left = up.safe_normalized();
324                                 up = dir.safe_normalized();
325                                 dir = left;
326                                 left  = (dir.cross(up)).safe_normalized();
327                                 mat.setValue (
328                                         left[0], dir[0],up[0], 
329                                         left[1], dir[1],up[1],
330                                         left[2], dir[2],up[2]
331                                         );
332                                 break;
333                         }
334                         
335                 case 3: // TRACK -X
336                         {
337                                 // (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
338                                 left  = -dir.safe_normalized();
339                                 dir = -(left.cross(up)).safe_normalized();
340                                 mat.setValue (
341                                         left[0], dir[0],up[0], 
342                                         left[1], dir[1],up[1],
343                                         left[2], dir[2],up[2]
344                                         );
345                                 
346                                 break;
347                         };
348                 case 4: // TRACK -Y
349                         {
350                                 // (0.0 , -1.0 , 0.0 ) -y direction is forward, z (0.0 , 0.0 , 1.0 ) up
351                                 left  = (-dir.cross(up)).safe_normalized();
352                                 mat.setValue (
353                                         left[0], -dir[0],up[0], 
354                                         left[1], -dir[1],up[1],
355                                         left[2], -dir[2],up[2]
356                                         );
357                                 break;
358                         }
359                 case 5: // track -Z
360                         {
361                                 left = up.safe_normalized();
362                                 up = -dir.safe_normalized();
363                                 dir = left;
364                                 left  = (dir.cross(up)).safe_normalized();
365                                 mat.setValue (
366                                         left[0], dir[0],up[0], 
367                                         left[1], dir[1],up[1],
368                                         left[2], dir[2],up[2]
369                                         );
370                                 
371                                 break;
372                         }
373                         
374                 default:
375                         {
376                                 // (1.0 , 0.0 , 0.0 ) -x direction is forward, z (0.0 , 0.0 , 1.0 ) up
377                                 left  = -dir.safe_normalized();
378                                 dir = -(left.cross(up)).safe_normalized();
379                                 mat.setValue (
380                                         left[0], dir[0],up[0], 
381                                         left[1], dir[1],up[1],
382                                         left[2], dir[2],up[2]
383                                         );
384                         }
385                 }
386                 
387                 MT_Matrix3x3 oldmat;
388                 oldmat= curobj->NodeGetWorldOrientation();
389                 
390                 /* erwin should rewrite this! */
391                 mat= matrix3x3_interpol(oldmat, mat, m_time);
392                 
393
394                 if (m_parentobj) { // check if the model is parented and calculate the child transform
395                                 
396                         MT_Point3 localpos;
397                         localpos = curobj->GetSGNode()->GetLocalPosition();
398                         // Get the inverse of the parent matrix
399                         MT_Matrix3x3 parentmatinv;
400                         parentmatinv = m_parentobj->NodeGetWorldOrientation ().inverse ();                              
401                         // transform the local coordinate system into the parents system
402                         mat = parentmatinv * mat;
403                         // append the initial parent local rotation matrix
404                         mat = m_parentlocalmat * mat;
405
406                         // set the models tranformation properties
407                         curobj->NodeSetLocalOrientation(mat);
408                         curobj->NodeSetLocalPosition(localpos);
409                         //curobj->UpdateTransform();
410                 }
411                 else
412                 {
413                         curobj->NodeSetLocalOrientation(mat);
414                 }
415
416                 result = true;
417         }
418
419         return result;
420 }
421
422 #ifdef WITH_PYTHON
423
424 /* ------------------------------------------------------------------------- */
425 /* Python functions                                                          */
426 /* ------------------------------------------------------------------------- */
427
428 /* Integration hooks ------------------------------------------------------- */
429 PyTypeObject KX_TrackToActuator::Type = {
430         PyVarObject_HEAD_INIT(NULL, 0)
431         "KX_TrackToActuator",
432         sizeof(PyObjectPlus_Proxy),
433         0,
434         py_base_dealloc,
435         0,
436         0,
437         0,
438         0,
439         py_base_repr,
440         0,0,0,0,0,0,0,0,0,
441         Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
442         0,0,0,0,0,0,0,
443         Methods,
444         0,
445         0,
446         &SCA_IActuator::Type,
447         0,0,0,0,0,0,
448         py_base_new
449 };
450
451 PyMethodDef KX_TrackToActuator::Methods[] = {
452         {NULL,NULL} //Sentinel
453 };
454
455 PyAttributeDef KX_TrackToActuator::Attributes[] = {
456         KX_PYATTRIBUTE_INT_RW("time",0,1000,true,KX_TrackToActuator,m_time),
457         KX_PYATTRIBUTE_BOOL_RW("use3D",KX_TrackToActuator,m_allow3D),
458         KX_PYATTRIBUTE_RW_FUNCTION("object", KX_TrackToActuator, pyattr_get_object, pyattr_set_object),
459
460         { NULL }        //Sentinel
461 };
462
463 PyObject* KX_TrackToActuator::pyattr_get_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
464 {
465         KX_TrackToActuator* actuator = static_cast<KX_TrackToActuator*>(self);
466         if (!actuator->m_object)        
467                 Py_RETURN_NONE;
468         else
469                 return actuator->m_object->GetProxy();
470 }
471
472 int KX_TrackToActuator::pyattr_set_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
473 {
474         KX_TrackToActuator* actuator = static_cast<KX_TrackToActuator*>(self);
475         KX_GameObject *gameobj;
476                 
477         if (!ConvertPythonToGameObject(value, &gameobj, true, "actuator.object = value: KX_TrackToActuator"))
478                 return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error
479                 
480         if (actuator->m_object != NULL)
481                 actuator->m_object->UnregisterActuator(actuator);       
482
483         actuator->m_object = (SCA_IObject*) gameobj;
484                 
485         if (actuator->m_object)
486                 actuator->m_object->RegisterActuator(actuator);
487                 
488         return PY_SET_ATTR_SUCCESS;
489 }
490
491 #endif // WITH_PYTHON
492
493 /* eof */