00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "motionsystem.h"
00025 #include "error.h"
00026
00027 using namespace lux;
00028
00029 MotionSystem::MotionSystem(float st, float et,
00030 const Transform &s, const Transform &e) {
00031
00032 startTime = st;
00033 endTime = et;
00034 start = s;
00035 end = e;
00036 startMat = start.GetMatrix();
00037 endMat = end.GetMatrix();
00038
00039
00040 hasTranslationX = hasTranslationY = hasTranslationZ =
00041 hasScaleX = hasScaleY = hasScaleZ = hasRotation = isActive = false;
00042
00043 if (!DecomposeMatrix(startMat, startT)) {
00044 luxError(LUX_MATH, LUX_WARNING, "Singular start matrix in MotionSystem, interpolation disabled");
00045 return;
00046 }
00047 if (!DecomposeMatrix(endMat, endT)) {
00048 luxError(LUX_MATH, LUX_WARNING, "Singular end matrix in MotionSystem, interpolation disabled");
00049 return;
00050 }
00051
00052 startQ = Quaternion(startT.R);
00053 startQ.Normalize();
00054
00055 endQ = Quaternion(endT.R);
00056 endQ.Normalize();
00057
00058 hasTranslationX = startT.Tx != endT.Tx;
00059 hasTranslationY = startT.Ty != endT.Ty;
00060 hasTranslationZ = startT.Tz != endT.Tz;
00061 hasTranslation = hasTranslationX ||
00062 hasTranslationY || hasTranslationZ;
00063
00064 hasScaleX = startT.Sx != endT.Sx;
00065 hasScaleY = startT.Sy != endT.Sy;
00066 hasScaleZ = startT.Sz != endT.Sz;
00067 hasScale = hasScaleX || hasScaleY || hasScaleZ;
00068
00069 hasRotation = fabsf(Dot(startQ, endQ) - 1) >= 1e-6;
00070
00071 isActive = hasTranslation ||
00072 hasScale || hasRotation;
00073 }
00074
00075 Transform MotionSystem::Sample(float time) const {
00076 if (!isActive)
00077 return start;
00078
00079
00080 if(time <= startTime)
00081 return start;
00082
00083 if(time >= endTime)
00084 return end;
00085
00086 float w = endTime - startTime;
00087 float d = time - startTime;
00088 float le = d / w;
00089
00090 float interMatrix[4][4];
00091
00092
00093 if (hasTranslation && !(hasScale || hasRotation)) {
00094 memcpy(interMatrix, startMat->m, sizeof(float) * 16);
00095 if (hasTranslationX)
00096 interMatrix[0][3] = Lerp(le, startT.Tx, endT.Tx);
00097 if (hasTranslationY)
00098 interMatrix[1][3] = Lerp(le, startT.Ty, endT.Ty);
00099 if (hasTranslationZ)
00100 interMatrix[2][3] = Lerp(le, startT.Tz, endT.Tz);
00101 return Transform(interMatrix);
00102 }
00103
00104 if (hasRotation) {
00105
00106 Quaternion interQ = Quaternion::Slerp(le, startQ, endQ);
00107 interQ.ToMatrix(interMatrix);
00108 } else
00109 memcpy(interMatrix, startT.R->m, sizeof(float) * 16);
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125 if (hasScale) {
00126 float Sx = Lerp(le, startT.Sx, endT.Sx);
00127 float Sy = Lerp(le, startT.Sy, endT.Sy);
00128 float Sz = Lerp(le, startT.Sz, endT.Sz);
00129
00130
00131 for (int j = 0; j < 3; j++) {
00132 interMatrix[0][j] = Sx * interMatrix[0][j];
00133 interMatrix[1][j] = Sy * interMatrix[1][j];
00134 interMatrix[2][j] = Sz * interMatrix[2][j];
00135 }
00136 } else {
00137 for (int j = 0; j < 3; j++) {
00138 interMatrix[0][j] = startT.Sx * interMatrix[0][j];
00139 interMatrix[1][j] = startT.Sy * interMatrix[1][j];
00140 interMatrix[2][j] = startT.Sz * interMatrix[2][j];
00141 }
00142 }
00143
00144 if (hasTranslationX) {
00145 interMatrix[0][3] = Lerp(le, startT.Tx, endT.Tx);
00146 } else {
00147 interMatrix[0][3] = startT.Tx;
00148 }
00149
00150 if (hasTranslationY) {
00151 interMatrix[1][3] = Lerp(le, startT.Ty, endT.Ty);
00152 } else {
00153 interMatrix[1][3] = startT.Ty;
00154 }
00155
00156 if (hasTranslationZ) {
00157 interMatrix[2][3] = Lerp(le, startT.Tz, endT.Tz);
00158 } else {
00159 interMatrix[2][3] = startT.Tz;
00160 }
00161
00162 return Transform(interMatrix);
00163 }
00164
00165 void V4MulByMatrix(const boost::shared_ptr<Matrix4x4> &A, const float x[4], float b[4]) {
00166 b[0] = A->m[0][0]*x[0] + A->m[0][1]*x[1] + A->m[0][2]*x[2] + A->m[0][3]*x[3];
00167 b[1] = A->m[1][0]*x[0] + A->m[1][1]*x[1] + A->m[1][2]*x[2] + A->m[1][3]*x[3];
00168 b[2] = A->m[2][0]*x[0] + A->m[2][1]*x[1] + A->m[2][2]*x[2] + A->m[2][3]*x[3];
00169 b[3] = A->m[3][0]*x[0] + A->m[3][1]*x[1] + A->m[3][2]*x[2] + A->m[3][3]*x[3];
00170 }
00171
00172 bool MotionSystem::DecomposeMatrix(const boost::shared_ptr<Matrix4x4> m, Transforms &trans) const {
00173
00174 boost::shared_ptr<Matrix4x4> locmat(new Matrix4x4(m->m));
00175
00176
00177 if (locmat->m[3][3] == 0)
00178 return false;
00179 for (int i = 0; i < 4; i++)
00180 for (int j = 0; j < 4; j++)
00181 locmat->m[i][j] /= locmat->m[3][3];
00182
00183
00184
00185 boost::shared_ptr<Matrix4x4> pmat(new Matrix4x4(locmat->m));
00186 for (int i = 0; i < 3; i++)
00187 pmat->m[i][3] = 0;
00188 pmat->m[3][3] = 1;
00189
00190
00191 if ( pmat->Determinant() == 0.0 )
00192 return false;
00193
00194
00195 if ( locmat->m[3][0] != 0 || locmat->m[3][1] != 0 ||
00196 locmat->m[3][2] != 0 ) {
00197
00198 float prhs[4];
00199 prhs[0] = locmat->m[3][0];
00200 prhs[1] = locmat->m[3][1];
00201 prhs[2] = locmat->m[3][2];
00202 prhs[3] = locmat->m[3][3];
00203
00204
00205
00206
00207
00208
00209
00210 boost::shared_ptr<Matrix4x4> tinvpmat =
00211 pmat->Inverse()->Transpose();
00212 float psol[4];
00213 V4MulByMatrix(tinvpmat, prhs, psol);
00214
00215
00216 trans.Px = psol[0];
00217 trans.Py = psol[1];
00218 trans.Pz = psol[2];
00219 trans.Pw = psol[3];
00220
00221
00222 locmat->m[3][0] = locmat->m[3][1] =
00223 locmat->m[3][2] = 0;
00224 locmat->m[3][3] = 1;
00225 };
00226
00227
00228
00229
00230
00231 trans.Tx = locmat->m[0][3];
00232 trans.Ty = locmat->m[1][3];
00233 trans.Tz = locmat->m[2][3];
00234 for (int i = 0; i < 3; i++ )
00235 locmat->m[i][3] = 0;
00236
00237 Vector row[3];
00238
00239 for (int i = 0; i < 3; i++ ) {
00240 row[i].x = locmat->m[i][0];
00241 row[i].y = locmat->m[i][1];
00242 row[i].z = locmat->m[i][2];
00243 }
00244
00245
00246 trans.Sx = row[0].Length();
00247 row[0] *= 1 / trans.Sx;
00248
00249
00250 trans.Sxy = Dot(row[0], row[1]);
00251 row[1] -= trans.Sxy * row[0];
00252
00253
00254 trans.Sy = row[1].Length();
00255 row[1] *= 1.0 / trans.Sy;
00256 trans.Sxy /= trans.Sy;
00257
00258
00259 trans.Sxz = Dot(row[0], row[2]);
00260 row[2] -= trans.Sxz * row[0];
00261 trans.Syz = Dot(row[1], row[2]);
00262 row[2] -= trans.Syz * row[1];
00263
00264
00265 trans.Sz = row[2].Length();
00266 row[2] *= 1.0 / trans.Sz;
00267 trans.Sxz /= trans.Sz;
00268 trans.Syz /= trans.Sz;
00269
00270
00271
00272
00273
00274 if (Dot(row[0], Cross(row[1], row[2])) < 0) {
00275 trans.Sx *= -1;
00276 trans.Sy *= -1;
00277 trans.Sz *= -1;
00278 for (int i = 0; i < 3; i++ ) {
00279 row[i].x *= -1;
00280 row[i].y *= -1;
00281 row[i].z *= -1;
00282 }
00283 }
00284
00285
00286 for (int i = 0; i < 3; i++ ) {
00287 locmat->m[i][0] = row[i].x;
00288 locmat->m[i][1] = row[i].y;
00289 locmat->m[i][2] = row[i].z;
00290 }
00291 trans.R = locmat;
00292
00293 return true;
00294 }
00295