90b7850946b96afee7fb2d5dad4324ea767bea60
[blender.git] / source / gameengine / Ketsji / KX_TrackToActuator.cpp
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Contributor(s): none yet.
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  */
27
28 /** \file gameengine/Ketsji/KX_TrackToActuator.cpp
29  *  \ingroup ketsji
30  *
31  * Replace the mesh for this actuator's parent
32  */
33
34 /* todo: not all trackflags / upflags are implemented/tested !
35  * m_trackflag is used to determine the forward tracking direction
36  * m_upflag for the up direction
37  * normal situation is +y for forward, +z for up */
38
39 #include "MT_Scalar.h"
40 #include "SCA_IActuator.h"
41 #include "KX_TrackToActuator.h"
42 #include "SCA_IScene.h"
43 #include "SCA_LogicManager.h"
44 #include <math.h>
45 #include <iostream>
46 #include "KX_GameObject.h"
47
48 #include "PyObjectPlus.h"
49
50 /* ------------------------------------------------------------------------- */
51 /* Native functions                                                          */
52 /* ------------------------------------------------------------------------- */
53
54 KX_TrackToActuator::KX_TrackToActuator(SCA_IObject *gameobj, 
55                                        SCA_IObject *ob,
56                                        int time,
57                                        bool allow3D,
58                                        int trackflag,
59                                        int upflag)
60     : SCA_IActuator(gameobj, KX_ACT_TRACKTO)
61 {
62         m_time = time;
63         m_allow3D = allow3D;
64         m_object = ob;
65         m_trackflag = trackflag;
66         m_upflag = upflag;
67         m_parentobj = 0;
68         
69         if (m_object)
70                 m_object->RegisterActuator(this);
71
72         {
73                 // if the object is vertex parented, don't check parent orientation as the link is broken
74                 if (!((KX_GameObject*)gameobj)->IsVertexParent()) {
75                         m_parentobj = ((KX_GameObject*)gameobj)->GetParent(); // check if the object is parented 
76                         if (m_parentobj) {  
77                                 // if so, store the initial local rotation
78                                 // this is needed to revert the effect of the parent inverse node (TBC)
79                                 m_parentlocalmat = m_parentobj->GetSGNode()->GetLocalOrientation();
80                                 // use registration mechanism rather than AddRef, it creates zombie objects
81                                 m_parentobj->RegisterActuator(this);
82                         }
83                 }
84         }
85
86 } /* End of constructor */
87
88
89
90 /* old function from Blender */
91 static MT_Matrix3x3 EulToMat3(float eul[3])
92 {
93         MT_Matrix3x3 mat;
94         float ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
95         
96         ci = cos(eul[0]); 
97         cj = cos(eul[1]); 
98         ch = cos(eul[2]);
99         si = sin(eul[0]); 
100         sj = sin(eul[1]); 
101         sh = sin(eul[2]);
102         cc = ci*ch; 
103         cs = ci*sh; 
104         sc = si*ch; 
105         ss = si*sh;
106
107         mat[0][0] = cj*ch; 
108         mat[1][0] = sj*sc-cs; 
109         mat[2][0] = sj*cc+ss;
110         mat[0][1] = cj*sh; 
111         mat[1][1] = sj*ss+cc; 
112         mat[2][1] = sj*cs-sc;
113         mat[0][2] = -sj;         
114         mat[1][2] = cj*si;    
115         mat[2][2] = cj*ci;
116
117         return mat;
118 }
119
120
121
122 /* old function from Blender */
123 static void Mat3ToEulOld(MT_Matrix3x3 mat, float eul[3])
124 {
125         const float cy = sqrtf(mat[0][0] * mat[0][0] + mat[0][1] * mat[0][1]);
126
127         if (cy > (float)(16.0f * FLT_EPSILON)) {
128                 eul[0] = atan2f( mat[1][2], mat[2][2]);
129                 eul[1] = atan2f(-mat[0][2], cy);
130                 eul[2] = atan2f( mat[0][1], mat[0][0]);
131         }
132         else {
133                 eul[0] = atan2f(-mat[2][1], mat[1][1]);
134                 eul[1] = atan2f(-mat[0][2], cy);
135                 eul[2] = 0.0;
136         }
137 }
138
139
140
141 /* old function from Blender */
142 static void compatible_eulFast(float *eul, float *oldrot)
143 {
144         float dx, dy, dz;
145         
146         /* angular difference of 360 degrees */
147
148         dx = eul[0] - oldrot[0];
149         dy = eul[1] - oldrot[1];
150         dz = eul[2] - oldrot[2];
151
152         if (fabsf(dx) > (float)MT_PI) {
153                 if (dx > 0.0f) eul[0] -= (float)MT_2_PI; else eul[0] += (float)MT_2_PI;
154         }
155         if (fabsf(dy) > (float)MT_PI) {
156                 if (dy > 0.0f) eul[1] -= (float)MT_2_PI; else eul[1] += (float)MT_2_PI;
157         }
158         if (fabsf(dz) > (float)MT_PI) {
159                 if (dz > 0.0f) eul[2] -= (float)MT_2_PI; else eul[2] += (float)MT_2_PI;
160         }
161 }
162
163
164
165 static MT_Matrix3x3 matrix3x3_interpol(MT_Matrix3x3 oldmat, MT_Matrix3x3 mat, int m_time)
166 {
167         float eul[3], oldeul[3];
168
169         Mat3ToEulOld(oldmat, oldeul);
170         Mat3ToEulOld(mat, eul);
171         compatible_eulFast(eul, oldeul);
172         
173         eul[0] = (m_time * oldeul[0] + eul[0]) / (1.0f + m_time);
174         eul[1] = (m_time * oldeul[1] + eul[1]) / (1.0f + m_time);
175         eul[2] = (m_time * oldeul[2] + eul[2]) / (1.0f + m_time);
176         
177         return EulToMat3(eul);
178 }
179
180
181
182 KX_TrackToActuator::~KX_TrackToActuator()
183 {
184         if (m_object)
185                 m_object->UnregisterActuator(this);
186         if (m_parentobj)
187                 m_parentobj->UnregisterActuator(this);
188 } /* end of destructor */
189
190 void KX_TrackToActuator::ProcessReplica()
191 {
192         // the replica is tracking the same object => register it
193         if (m_object)
194                 m_object->RegisterActuator(this);
195         if (m_parentobj)
196                 m_parentobj->RegisterActuator(this);
197         SCA_IActuator::ProcessReplica();
198 }
199
200
201 bool KX_TrackToActuator::UnlinkObject(SCA_IObject* clientobj)
202 {
203         if (clientobj == m_object)
204         {
205                 // this object is being deleted, we cannot continue to track it.
206                 m_object = NULL;
207                 return true;
208         }
209         if (clientobj == m_parentobj)
210         {
211                 m_parentobj = NULL;
212                 return true;
213         }
214         return false;
215 }
216
217 void KX_TrackToActuator::Relink(CTR_Map<CTR_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->UnregisterActuator(this);
231                 m_parentobj= (KX_GameObject*)(*h_parobj);
232                 m_parentobj->RegisterActuator(this);
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 = up.cross(left).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 = up.cross(left).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 = up.cross(left).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 #ifdef WITH_PYTHON
419
420 /* ------------------------------------------------------------------------- */
421 /* Python functions                                                          */
422 /* ------------------------------------------------------------------------- */
423
424 /* Integration hooks ------------------------------------------------------- */
425 PyTypeObject KX_TrackToActuator::Type = {
426         PyVarObject_HEAD_INIT(NULL, 0)
427         "KX_TrackToActuator",
428         sizeof(PyObjectPlus_Proxy),
429         0,
430         py_base_dealloc,
431         0,
432         0,
433         0,
434         0,
435         py_base_repr,
436         0,0,0,0,0,0,0,0,0,
437         Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
438         0,0,0,0,0,0,0,
439         Methods,
440         0,
441         0,
442         &SCA_IActuator::Type,
443         0,0,0,0,0,0,
444         py_base_new
445 };
446
447 PyMethodDef KX_TrackToActuator::Methods[] = {
448         {NULL,NULL} //Sentinel
449 };
450
451 PyAttributeDef KX_TrackToActuator::Attributes[] = {
452         KX_PYATTRIBUTE_INT_RW("time",0,1000,true,KX_TrackToActuator,m_time),
453         KX_PYATTRIBUTE_BOOL_RW("use3D",KX_TrackToActuator,m_allow3D),
454         KX_PYATTRIBUTE_RW_FUNCTION("object", KX_TrackToActuator, pyattr_get_object, pyattr_set_object),
455
456         { NULL }        //Sentinel
457 };
458
459 PyObject *KX_TrackToActuator::pyattr_get_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
460 {
461         KX_TrackToActuator* actuator = static_cast<KX_TrackToActuator*>(self);
462         if (!actuator->m_object)
463                 Py_RETURN_NONE;
464         else
465                 return actuator->m_object->GetProxy();
466 }
467
468 int KX_TrackToActuator::pyattr_set_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
469 {
470         KX_TrackToActuator* actuator = static_cast<KX_TrackToActuator*>(self);
471         KX_GameObject *gameobj;
472                 
473         if (!ConvertPythonToGameObject(value, &gameobj, true, "actuator.object = value: KX_TrackToActuator"))
474                 return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error
475                 
476         if (actuator->m_object != NULL)
477                 actuator->m_object->UnregisterActuator(actuator);
478
479         actuator->m_object = (SCA_IObject*) gameobj;
480                 
481         if (actuator->m_object)
482                 actuator->m_object->RegisterActuator(actuator);
483                 
484         return PY_SET_ATTR_SUCCESS;
485 }
486
487 #endif // WITH_PYTHON
488
489 /* eof */