beccdfaad93db038e6fa5e735e1174acd59db087
[blender-staging.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 /* 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 "EXP_PyObjectPlus.h"
48
49 /* ------------------------------------------------------------------------- */
50 /* Native functions                                                          */
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                         }
82                 }
83         }
84
85 } /* End of constructor */
86
87
88
89 /* old function from Blender */
90 static MT_Matrix3x3 EulToMat3(float eul[3])
91 {
92         MT_Matrix3x3 mat;
93         float ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
94         
95         ci = cosf(eul[0]);
96         cj = cosf(eul[1]);
97         ch = cosf(eul[2]);
98         si = sinf(eul[0]);
99         sj = sinf(eul[1]);
100         sh = sinf(eul[2]);
101         cc = ci*ch; 
102         cs = ci*sh; 
103         sc = si*ch; 
104         ss = si*sh;
105
106         mat[0][0] = cj*ch; 
107         mat[1][0] = sj*sc-cs; 
108         mat[2][0] = sj*cc+ss;
109         mat[0][1] = cj*sh; 
110         mat[1][1] = sj*ss+cc; 
111         mat[2][1] = sj*cs-sc;
112         mat[0][2] = -sj;         
113         mat[1][2] = cj*si;    
114         mat[2][2] = cj*ci;
115
116         return mat;
117 }
118
119
120
121 /* old function from Blender */
122 static void Mat3ToEulOld(MT_Matrix3x3 mat, float eul[3])
123 {
124         const float cy = sqrtf(mat[0][0] * mat[0][0] + mat[0][1] * mat[0][1]);
125
126         if (cy > (float)(16.0f * FLT_EPSILON)) {
127                 eul[0] = atan2f( mat[1][2], mat[2][2]);
128                 eul[1] = atan2f(-mat[0][2], cy);
129                 eul[2] = atan2f( mat[0][1], mat[0][0]);
130         }
131         else {
132                 eul[0] = atan2f(-mat[2][1], mat[1][1]);
133                 eul[1] = atan2f(-mat[0][2], cy);
134                 eul[2] = 0.0f;
135         }
136 }
137
138
139
140 /* old function from Blender */
141 static void compatible_eulFast(float *eul, float *oldrot)
142 {
143         float dx, dy, dz;
144         
145         /* angular difference of 360 degrees */
146
147         dx = eul[0] - oldrot[0];
148         dy = eul[1] - oldrot[1];
149         dz = eul[2] - oldrot[2];
150
151         if (fabsf(dx) > (float)MT_PI) {
152                 if (dx > 0.0f) eul[0] -= (float)MT_2_PI; else eul[0] += (float)MT_2_PI;
153         }
154         if (fabsf(dy) > (float)MT_PI) {
155                 if (dy > 0.0f) eul[1] -= (float)MT_2_PI; else eul[1] += (float)MT_2_PI;
156         }
157         if (fabsf(dz) > (float)MT_PI) {
158                 if (dz > 0.0f) eul[2] -= (float)MT_2_PI; else eul[2] += (float)MT_2_PI;
159         }
160 }
161
162
163
164 static MT_Matrix3x3 matrix3x3_interpol(MT_Matrix3x3 oldmat, MT_Matrix3x3 mat, int m_time)
165 {
166         float eul[3], oldeul[3];
167
168         Mat3ToEulOld(oldmat, oldeul);
169         Mat3ToEulOld(mat, eul);
170         compatible_eulFast(eul, oldeul);
171         
172         eul[0] = (m_time * oldeul[0] + eul[0]) / (1.0f + m_time);
173         eul[1] = (m_time * oldeul[1] + eul[1]) / (1.0f + m_time);
174         eul[2] = (m_time * oldeul[2] + eul[2]) / (1.0f + m_time);
175         
176         return EulToMat3(eul);
177 }
178
179 static float basis_cross(int n, int m)
180 {
181         switch (n - m) {
182                 case 1:
183                 case -2:
184                         return 1.0f;
185
186                 case -1:
187                 case 2:
188                         return -1.0f;
189
190                 default:
191                         return 0.0f;
192         }
193 }
194
195 /* vectomat function obtained from constrain.c and modified to work with MOTO library */
196 static MT_Matrix3x3 vectomat(MT_Vector3 vec, short axis, short upflag, short threedimup)
197 {
198         MT_Matrix3x3 mat;
199         MT_Vector3 y(MT_Scalar(0.0f), MT_Scalar(1.0f), MT_Scalar(0.0f));
200         MT_Vector3 z(MT_Scalar(0.0f), MT_Scalar(0.0f), MT_Scalar(1.0f)); /* world Z axis is the global up axis */
201         MT_Vector3 proj;
202         MT_Vector3 right;
203         MT_Scalar mul;
204         int right_index;
205
206         /* Normalized Vec vector*/
207         vec = vec.safe_normalized_vec(z);
208
209         /* if 2D doesn't move the up vector */
210         if (!threedimup){
211                 vec.setValue(MT_Scalar(vec[0]), MT_Scalar(vec[1]), MT_Scalar(0.0f));
212                 vec = (vec - z.dot(vec)*z).safe_normalized_vec(z);
213         }
214
215         if (axis > 2)
216                 axis -= 3;
217         else
218                 vec = -vec;
219
220         /* project the up vector onto the plane specified by vec */
221         /* first z onto vec... */
222         mul = z.dot(vec) / vec.dot(vec);
223         proj = vec * mul;
224         /* then onto the plane */
225         proj = z - proj;
226         /* proj specifies the transformation of the up axis */
227         proj = proj.safe_normalized_vec(y);
228
229         /* Normalized cross product of vec and proj specifies transformation of the right axis */
230         right = proj.cross(vec);
231         right.normalize();
232
233         if (axis != upflag) {
234                 right_index = 3 - axis - upflag;
235
236                 /* account for up direction, track direction */
237                 right = right * basis_cross(axis, upflag);
238                 mat.setRow(right_index, right);
239                 mat.setRow(upflag, proj);
240                 mat.setRow(axis, vec);
241                 mat = mat.inverse();
242         }
243         /* identity matrix - don't do anything if the two axes are the same */
244         else {
245                 mat.setIdentity();
246         }
247
248         return mat;
249 }
250
251 KX_TrackToActuator::~KX_TrackToActuator()
252 {
253         if (m_object)
254                 m_object->UnregisterActuator(this);
255         if (m_parentobj)
256                 m_parentobj->UnregisterActuator(this);
257 } /* end of destructor */
258
259 void KX_TrackToActuator::ProcessReplica()
260 {
261         // the replica is tracking the same object => register it
262         if (m_object)
263                 m_object->RegisterActuator(this);
264         if (m_parentobj)
265                 m_parentobj->RegisterActuator(this);
266         SCA_IActuator::ProcessReplica();
267 }
268
269
270 bool KX_TrackToActuator::UnlinkObject(SCA_IObject* clientobj)
271 {
272         if (clientobj == m_object)
273         {
274                 // this object is being deleted, we cannot continue to track it.
275                 m_object = NULL;
276                 return true;
277         }
278         if (clientobj == m_parentobj)
279         {
280                 m_parentobj = NULL;
281                 return true;
282         }
283         return false;
284 }
285
286 void KX_TrackToActuator::Relink(CTR_Map<CTR_HashedPtr, void*> *obj_map)
287 {
288         void **h_obj = (*obj_map)[m_object];
289         if (h_obj) {
290                 if (m_object)
291                         m_object->UnregisterActuator(this);
292                 m_object = (SCA_IObject*)(*h_obj);
293                 m_object->RegisterActuator(this);
294         }
295
296         void **h_parobj = (*obj_map)[m_parentobj];
297         if (h_parobj) {
298                 if (m_parentobj)
299                         m_parentobj->UnregisterActuator(this);
300                 m_parentobj= (KX_GameObject*)(*h_parobj);
301                 m_parentobj->RegisterActuator(this);
302         }
303 }
304
305
306 bool KX_TrackToActuator::Update(double curtime, bool frame)
307 {
308         bool result = false;
309         bool bNegativeEvent = IsNegativeEvent();
310         RemoveAllEvents();
311
312         if (bNegativeEvent)
313         {
314                 // do nothing on negative events
315         }
316         else if (m_object)
317         {
318                 KX_GameObject* curobj = (KX_GameObject*) GetParent();
319                 MT_Vector3 dir = curobj->NodeGetWorldPosition() - ((KX_GameObject*)m_object)->NodeGetWorldPosition();
320                 MT_Matrix3x3 mat;
321                 MT_Matrix3x3 oldmat;
322
323                 mat = vectomat(dir, m_trackflag, m_upflag, m_allow3D);
324                 oldmat = curobj->NodeGetWorldOrientation();
325                 
326                 /* erwin should rewrite this! */
327                 mat = matrix3x3_interpol(oldmat, mat, m_time);
328                 
329                 /* check if the model is parented and calculate the child transform */
330                 if (m_parentobj) {
331                                 
332                         MT_Point3 localpos;
333                         localpos = curobj->GetSGNode()->GetLocalPosition();
334                         // Get the inverse of the parent matrix
335                         MT_Matrix3x3 parentmatinv;
336                         parentmatinv = m_parentobj->NodeGetWorldOrientation().inverse();
337                         // transform the local coordinate system into the parents system
338                         mat = parentmatinv * mat;
339                         // append the initial parent local rotation matrix
340                         mat = m_parentlocalmat * mat;
341
342                         // set the models tranformation properties
343                         curobj->NodeSetLocalOrientation(mat);
344                         curobj->NodeSetLocalPosition(localpos);
345                         //curobj->UpdateTransform();
346                 }
347                 else {
348                         curobj->NodeSetLocalOrientation(mat);
349                 }
350
351                 result = true;
352         }
353
354         return result;
355 }
356
357 #ifdef WITH_PYTHON
358
359 /* ------------------------------------------------------------------------- */
360 /* Python functions                                                          */
361 /* ------------------------------------------------------------------------- */
362
363 /* Integration hooks ------------------------------------------------------- */
364 PyTypeObject KX_TrackToActuator::Type = {
365         PyVarObject_HEAD_INIT(NULL, 0)
366         "KX_TrackToActuator",
367         sizeof(PyObjectPlus_Proxy),
368         0,
369         py_base_dealloc,
370         0,
371         0,
372         0,
373         0,
374         py_base_repr,
375         0,0,0,0,0,0,0,0,0,
376         Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
377         0,0,0,0,0,0,0,
378         Methods,
379         0,
380         0,
381         &SCA_IActuator::Type,
382         0,0,0,0,0,0,
383         py_base_new
384 };
385
386 PyMethodDef KX_TrackToActuator::Methods[] = {
387         {NULL,NULL} //Sentinel
388 };
389
390 PyAttributeDef KX_TrackToActuator::Attributes[] = {
391         KX_PYATTRIBUTE_INT_RW("time",0,1000,true,KX_TrackToActuator,m_time),
392         KX_PYATTRIBUTE_BOOL_RW("use3D",KX_TrackToActuator,m_allow3D),
393         KX_PYATTRIBUTE_INT_RW("upAxis", 0, 2, true, KX_TrackToActuator,m_upflag),
394         KX_PYATTRIBUTE_INT_RW("trackAxis", 0, 5, true, KX_TrackToActuator,m_trackflag),
395         KX_PYATTRIBUTE_RW_FUNCTION("object", KX_TrackToActuator, pyattr_get_object, pyattr_set_object),
396
397         { NULL }        //Sentinel
398 };
399
400 PyObject *KX_TrackToActuator::pyattr_get_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
401 {
402         KX_TrackToActuator* actuator = static_cast<KX_TrackToActuator*>(self);
403         if (!actuator->m_object)
404                 Py_RETURN_NONE;
405         else
406                 return actuator->m_object->GetProxy();
407 }
408
409 int KX_TrackToActuator::pyattr_set_object(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef, PyObject *value)
410 {
411         KX_TrackToActuator* actuator = static_cast<KX_TrackToActuator*>(self);
412         KX_GameObject *gameobj;
413                 
414         if (!ConvertPythonToGameObject(actuator->GetLogicManager(), value, &gameobj, true, "actuator.object = value: KX_TrackToActuator"))
415                 return PY_SET_ATTR_FAIL; // ConvertPythonToGameObject sets the error
416                 
417         if (actuator->m_object != NULL)
418                 actuator->m_object->UnregisterActuator(actuator);
419
420         actuator->m_object = (SCA_IObject*) gameobj;
421                 
422         if (actuator->m_object)
423                 actuator->m_object->RegisterActuator(actuator);
424                 
425         return PY_SET_ATTR_SUCCESS;
426 }
427
428 #endif // WITH_PYTHON
429
430 /* eof */