00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include "MT_Scalar.h"
00012 #include "SCA_IActuator.h"
00013 #include "KX_TrackToActuator.h"
00014 #include "SCA_IScene.h"
00015 #include "SCA_LogicManager.h"
00016 #include <math.h>
00017 #include <iostream>
00018 #include "KX_GameObject.h"
00019
00020
00021
00022
00023 KX_TrackToActuator::KX_TrackToActuator(SCA_IObject *gameobj,
00024 SCA_IObject *ob,
00025 int time,
00026 bool threedee,
00027 int trackflag,
00028 int upflag,
00029 PyTypeObject* T)
00030 : SCA_IActuator(gameobj, T)
00031 {
00032 m_time = time;
00033 m_allow3D = threedee;
00034 m_object = ob;
00035 m_trackflag = trackflag;
00036 m_upflag = upflag;
00037 }
00038
00039
00040
00041
00042 MT_Matrix3x3 EulToMat3(float *eul)
00043 {
00044 MT_Matrix3x3 mat;
00045 float ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
00046
00047 ci = cos(eul[0]);
00048 cj = cos(eul[1]);
00049 ch = cos(eul[2]);
00050 si = sin(eul[0]);
00051 sj = sin(eul[1]);
00052 sh = sin(eul[2]);
00053 cc = ci*ch;
00054 cs = ci*sh;
00055 sc = si*ch;
00056 ss = si*sh;
00057
00058 mat[0][0] = cj*ch;
00059 mat[1][0] = sj*sc-cs;
00060 mat[2][0] = sj*cc+ss;
00061 mat[0][1] = cj*sh;
00062 mat[1][1] = sj*ss+cc;
00063 mat[2][1] = sj*cs-sc;
00064 mat[0][2] = -sj;
00065 mat[1][2] = cj*si;
00066 mat[2][2] = cj*ci;
00067
00068 return mat;
00069 }
00070
00071
00072
00073 void Mat3ToEul(MT_Matrix3x3 mat, float *eul)
00074 {
00075 MT_Scalar cy;
00076
00077 cy = sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]);
00078
00079 if (cy > 16.0*FLT_EPSILON) {
00080 eul[0] = atan2(mat[1][2], mat[2][2]);
00081 eul[1] = atan2(-mat[0][2], cy);
00082 eul[2] = atan2(mat[0][1], mat[0][0]);
00083 } else {
00084 eul[0] = atan2(-mat[2][1], mat[1][1]);
00085 eul[1] = atan2(-mat[0][2], cy);
00086 eul[2] = 0.0;
00087 }
00088 }
00089
00090
00091
00092 void compatible_eulFast(float *eul, float *oldrot)
00093 {
00094 float dx, dy, dz;
00095
00096
00097
00098 dx= eul[0] - oldrot[0];
00099 dy= eul[1] - oldrot[1];
00100 dz= eul[2] - oldrot[2];
00101
00102 if( fabs(dx) > 5.1) {
00103 if(dx > 0.0) eul[0] -= MT_2_PI; else eul[0]+= MT_2_PI;
00104 }
00105 if( fabs(dy) > 5.1) {
00106 if(dy > 0.0) eul[1] -= MT_2_PI; else eul[1]+= MT_2_PI;
00107 }
00108 if( fabs(dz) > 5.1 ) {
00109 if(dz > 0.0) eul[2] -= MT_2_PI; else eul[2]+= MT_2_PI;
00110 }
00111 }
00112
00113
00114
00115 MT_Matrix3x3 matrix3x3_interpol(MT_Matrix3x3 oldmat, MT_Matrix3x3 mat, int m_time)
00116 {
00117 float eul[3], oldeul[3];
00118
00119 Mat3ToEul(oldmat, oldeul);
00120 Mat3ToEul(mat, eul);
00121 compatible_eulFast(eul, oldeul);
00122
00123 eul[0]= (m_time*oldeul[0] + eul[0])/(1.0+m_time);
00124 eul[1]= (m_time*oldeul[1] + eul[1])/(1.0+m_time);
00125 eul[2]= (m_time*oldeul[2] + eul[2])/(1.0+m_time);
00126
00127 return EulToMat3(eul);
00128 }
00129
00130
00131 KX_TrackToActuator::~KX_TrackToActuator()
00132 {
00133
00134 }
00135
00136 bool KX_TrackToActuator::Update(double curtime,double deltatime)
00137 {
00138 bool result = false;
00139 bool bNegativeEvent = IsNegativeEvent();
00140
00141 if (bNegativeEvent)
00142 return false;
00143
00144 KX_GameObject* curobj = (KX_GameObject*) GetParent();
00145
00146 MT_Vector3 dir = ((KX_GameObject*)m_object)->NodeGetWorldPosition() - curobj->NodeGetWorldPosition();
00147 dir.normalize();
00148 MT_Vector3 up(0,0,1);
00149
00150
00151 #ifdef DSADSA
00152 switch (m_upflag)
00153 {
00154 case 0:
00155 {
00156 up = MT_Vector3(1.0,0,0);
00157 break;
00158 }
00159 case 1:
00160 {
00161 up = MT_Vector3(0,1.0,0);
00162 break;
00163 }
00164 case 2:
00165 default:
00166 up = MT_Vector3(0,0,1.0);
00167 }
00168 #endif
00169 if (m_allow3D)
00170 {
00171 up = (up - up.dot(dir) * dir).normalized();
00172
00173 } else
00174 {
00175 dir = (dir - up.dot(dir)*up).normalized();
00176 }
00177
00178 MT_Vector3 left;
00179 MT_Matrix3x3 mat;
00180
00181 switch (m_trackflag)
00182 {
00183 case 0:
00184 {
00185
00186 left = dir.normalized();
00187 dir = (left.cross(up)).normalized();
00188 mat.setValue (
00189 left[0], dir[0],up[0],
00190 left[1], dir[1],up[1],
00191 left[2], dir[2],up[2]
00192 );
00193
00194 break;
00195 };
00196 case 1:
00197 {
00198
00199 left = (dir.cross(up)).normalized();
00200 mat.setValue (
00201 left[0], dir[0],up[0],
00202 left[1], dir[1],up[1],
00203 left[2], dir[2],up[2]
00204 );
00205
00206 break;
00207 }
00208
00209 case 2:
00210 {
00211 left = up.normalized();
00212 up = dir.normalized();
00213 dir = left;
00214 left = (dir.cross(up)).normalized();
00215 mat.setValue (
00216 left[0], dir[0],up[0],
00217 left[1], dir[1],up[1],
00218 left[2], dir[2],up[2]
00219 );
00220 break;
00221 }
00222
00223 case 3:
00224 {
00225
00226 left = -dir.normalized();
00227 dir = -(left.cross(up)).normalized();
00228 mat.setValue (
00229 left[0], dir[0],up[0],
00230 left[1], dir[1],up[1],
00231 left[2], dir[2],up[2]
00232 );
00233
00234 break;
00235 };
00236 case 4:
00237 {
00238
00239 left = (-dir.cross(up)).normalized();
00240 mat.setValue (
00241 left[0], -dir[0],up[0],
00242 left[1], -dir[1],up[1],
00243 left[2], -dir[2],up[2]
00244 );
00245 break;
00246 }
00247 case 5:
00248 {
00249 left = up.normalized();
00250 up = -dir.normalized();
00251 dir = left;
00252 left = (dir.cross(up)).normalized();
00253 mat.setValue (
00254 left[0], dir[0],up[0],
00255 left[1], dir[1],up[1],
00256 left[2], dir[2],up[2]
00257 );
00258
00259 break;
00260 }
00261
00262 default:
00263 {
00264
00265 left = -dir.normalized();
00266 dir = -(left.cross(up)).normalized();
00267 mat.setValue (
00268 left[0], dir[0],up[0],
00269 left[1], dir[1],up[1],
00270 left[2], dir[2],up[2]
00271 );
00272
00273 }
00274
00275 }
00276
00277 MT_Matrix3x3 oldmat;
00278 oldmat= curobj->NodeGetWorldOrientation();
00279
00280
00281 mat= matrix3x3_interpol(oldmat, mat, m_time);
00282
00283 curobj->NodeSetLocalOrientation(mat);
00284
00285
00286 return true;
00287 }
00288
00289
00290
00291
00292
00293
00294 PyTypeObject KX_TrackToActuator::Type = {
00295 PyObject_HEAD_INIT(&PyType_Type)
00296 0,
00297 "KX_TrackToActuator",
00298 sizeof(KX_TrackToActuator),
00299 0,
00300 PyDestructor,
00301 0,
00302 __getattr,
00303 __setattr,
00304 0,
00305 __repr,
00306 0,
00307 0,
00308 0,
00309 0,
00310 0
00311 };
00312
00313 PyParentObject KX_TrackToActuator::Parents[] = {
00314 &KX_TrackToActuator::Type,
00315 &SCA_IActuator::Type,
00316 &SCA_ILogicBrick::Type,
00317 &CValue::Type,
00318 NULL
00319 };
00320 PyMethodDef KX_TrackToActuator::Methods[] = {
00321 {"setObject", (PyCFunction) KX_TrackToActuator::sPySetObject, METH_VARARGS, SetObject_doc},
00322 {"getObject", (PyCFunction) KX_TrackToActuator::sPyGetObject, METH_VARARGS, GetObject_doc},
00323 {"setTime", (PyCFunction) KX_TrackToActuator::sPySetTime, METH_VARARGS, SetTime_doc},
00324 {"getTime", (PyCFunction) KX_TrackToActuator::sPyGetTime, METH_VARARGS, GetTime_doc},
00325 {"setUse3D", (PyCFunction) KX_TrackToActuator::sPySetUse3D, METH_VARARGS, SetUse3D_doc},
00326 {"getUse3D", (PyCFunction) KX_TrackToActuator::sPyGetUse3D, METH_VARARGS, GetUse3D_doc},
00327 {NULL,NULL}
00328 };
00329
00330
00331 PyObject* KX_TrackToActuator::_getattr(char* attr)
00332 {
00333 _getattr_up(SCA_IActuator);
00334 }
00335
00336
00337 char KX_TrackToActuator::SetObject_doc[] =
00338 "setObject(object)\n"
00339 "\t- object: string\n"
00340 "\tSet the object to track with the parent of this actuator.\n";
00341 PyObject* KX_TrackToActuator::PySetObject(PyObject* self, PyObject* args, PyObject* kwds) {
00342 char* nameArg;
00343
00344 if (!PyArg_ParseTuple(args, "s", &nameArg)) {
00345 return NULL;
00346 }
00347 CValue* gameobj = SCA_ILogicBrick::m_sCurrentLogicManager->GetGameObjectByName(CCString(nameArg));
00348
00349 m_object= (SCA_IObject*)gameobj;
00350
00351 Py_Return;
00352 }
00353
00354 char KX_TrackToActuator::GetObject_doc[] =
00355 "getObject()\n"
00356 "\tReturns the object to track with the parent of this actuator.\n";
00357 PyObject* KX_TrackToActuator::PyGetObject(PyObject* self, PyObject* args, PyObject* kwds) {
00358 return PyString_FromString(m_object->GetName());
00359 }
00360
00361
00362 char KX_TrackToActuator::SetTime_doc[] =
00363 "setTime(time)\n"
00364 "\t- time: integer\n"
00365 "\tSet the time in frames with which to delay the tracking motion.\n";
00366 PyObject* KX_TrackToActuator::PySetTime(PyObject* self, PyObject* args, PyObject* kwds) {
00367 int timeArg;
00368
00369 if (!PyArg_ParseTuple(args, "i", &timeArg)) {
00370 return NULL;
00371 }
00372
00373 m_time= timeArg;
00374
00375 Py_Return;
00376 }
00377
00378 char KX_TrackToActuator::GetTime_doc[] =
00379 "getTime()\n"
00380 "\t- time: integer\n"
00381 "\tReturn the time in frames with which the tracking motion is delayed.\n";
00382 PyObject* KX_TrackToActuator::PyGetTime(PyObject* self, PyObject* args, PyObject* kwds) {
00383 return PyInt_FromLong(m_time);
00384 }
00385
00386
00387
00388 char KX_TrackToActuator::GetUse3D_doc[] =
00389 "getUse3D()\n"
00390 "\tReturns 1 if the motion is allowed to extend in the z-direction.\n";
00391 PyObject* KX_TrackToActuator::PyGetUse3D(PyObject* self, PyObject* args, PyObject* kwds) {
00392 return PyInt_FromLong(!(m_allow3D == 0));
00393 }
00394
00395 char KX_TrackToActuator::SetUse3D_doc[] =
00396 "setUse3D(value)\n"
00397 "\t- value: 0 or 1\n"
00398 "\tSet to 1 to allow the tracking motion to extend in the z-direction,\n"
00399 "\tset to 0 to lock the tracking motion to the x-y plane.\n";
00400 PyObject* KX_TrackToActuator::PySetUse3D(PyObject* self, PyObject* args, PyObject* kwds) {
00401 int boolArg;
00402
00403 if (!PyArg_ParseTuple(args, "i", &boolArg)) {
00404 return NULL;
00405 }
00406
00407 m_allow3D = !(boolArg == 0);
00408
00409 Py_Return;
00410 }
00411
00412
