diff options
author | ultramn <dchapm2@umbc.edu> | 2017-03-09 17:41:36 -0800 |
---|---|---|
committer | ultramn <dchapm2@umbc.edu> | 2017-03-09 17:41:36 -0800 |
commit | 031d53bf45d9c8648f8d4c4a8fcad29c456dd932 (patch) | |
tree | acac6d75dc38d0041a5be52c5e3ca457aaded967 /redist/linmath.c | |
parent | a956d29b35aa9c8b7cfd869683afb382da65b3ea (diff) | |
parent | a8dcfa48d3b5831f0a0470c78d71003349c0e758 (diff) | |
download | libsurvive-031d53bf45d9c8648f8d4c4a8fcad29c456dd932.tar.gz libsurvive-031d53bf45d9c8648f8d4c4a8fcad29c456dd932.tar.bz2 |
Merge branch 'master' of https://github.com/cnlohr/libsurvive
Diffstat (limited to 'redist/linmath.c')
-rw-r--r-- | redist/linmath.c | 58 |
1 files changed, 46 insertions, 12 deletions
diff --git a/redist/linmath.c b/redist/linmath.c index 1c5c25b..d76fbba 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -152,7 +152,7 @@ FLT quatmagnitude( const FLT * q ) FLT quatinvsqmagnitude( const FLT * q ) { - return ((FLT)1.)/((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); + return ((FLT)1.)/FLT_SQRT((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); } @@ -168,17 +168,17 @@ void quattomatrix(FLT * matrix44, const FLT * qin) quatnormalize(q, qin); //Reduced calulation for speed - FLT xx = 2 * q[0] * q[0]; - FLT xy = 2 * q[0] * q[1]; - FLT xz = 2 * q[0] * q[2]; - FLT xw = 2 * q[0] * q[3]; + FLT xx = 2 * q[1] * q[1]; + FLT xy = 2 * q[1] * q[2]; + FLT xz = 2 * q[1] * q[3]; + FLT xw = 2 * q[1] * q[0]; - FLT yy = 2 * q[1] * q[1]; - FLT yz = 2 * q[1] * q[2]; - FLT yw = 2 * q[1] * q[3]; + FLT yy = 2 * q[2] * q[2]; + FLT yz = 2 * q[2] * q[3]; + FLT yw = 2 * q[2] * q[0]; - FLT zz = 2 * q[2] * q[2]; - FLT zw = 2 * q[2] * q[3]; + FLT zz = 2 * q[3] * q[3]; + FLT zw = 2 * q[3] * q[0]; //opengl major matrix44[0] = 1 - yy - zz; @@ -202,6 +202,40 @@ void quattomatrix(FLT * matrix44, const FLT * qin) matrix44[15] = 1; } + +void quatfrommatrix( FLT * q, const FLT * matrix44 ) +{ + //Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ + float tr = matrix44[0] + matrix44[5] + matrix44[10]; + + if (tr > 0) { + float S = sqrt(tr+1.0) * 2; // S=4*qw + q[0] = 0.25 * S; + q[1] = (matrix44[9] - matrix44[6]) / S; + q[2] = (matrix44[2] - matrix44[8]) / S; + q[3] = (matrix44[4] - matrix44[1]) / S; + } else if ((matrix44[0] > matrix44[5])&(matrix44[0] > matrix44[10])) { + float S = sqrt(1.0 + matrix44[0] - matrix44[5] - matrix44[10]) * 2; // S=4*qx + q[0] = (matrix44[9] - matrix44[6]) / S; + q[1] = 0.25 * S; + q[2] = (matrix44[1] + matrix44[4]) / S; + q[3] = (matrix44[2] + matrix44[8]) / S; + } else if (matrix44[5] > matrix44[10]) { + float S = sqrt(1.0 + matrix44[5] - matrix44[0] - matrix44[10]) * 2; // S=4*qy + q[0] = (matrix44[2] - matrix44[8]) / S; + q[1] = (matrix44[1] + matrix44[4]) / S; + q[2] = 0.25 * S; + q[3] = (matrix44[6] + matrix44[9]) / S; + } else { + float S = sqrt(1.0 + matrix44[10] - matrix44[0] - matrix44[5]) * 2; // S=4*qz + q[0] = (matrix44[4] - matrix44[1]) / S; + q[1] = (matrix44[2] + matrix44[8]) / S; + q[2] = (matrix44[6] + matrix44[9]) / S; + q[3] = 0.25 * S; + } +} + + void quattomatrix33(FLT * matrix33, const FLT * qin) { FLT q[4]; @@ -270,8 +304,8 @@ void quatrotateabout( FLT * qout, const FLT * a, const FLT * b ) FLT q1[4]; FLT q2[4]; - quatnormalize( q1, a ); - quatnormalize( q2, b ); + //quatnormalize( q1, a ); + //quatnormalize( q2, b ); qout[0] = (q1[0]*q2[0])-(q1[1]*q2[1])-(q1[2]*q2[2])-(q1[3]*q2[3]); qout[1] = (q1[0]*q2[1])+(q1[1]*q2[0])+(q1[2]*q2[3])-(q1[3]*q2[2]); |