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