diff options
author | Michael Turvey <mwturvey@users.noreply.github.com> | 2017-12-28 08:59:07 -0700 |
---|---|---|
committer | GitHub <noreply@github.com> | 2017-12-28 08:59:07 -0700 |
commit | 8c5ee18e12847dd01290025bd138a1a029f1caf8 (patch) | |
tree | 59e237db42481f79acae0eee79d81f345eb7e105 /redist/linmath.c | |
parent | 6ebd39ca34351f6ac1df1d1f1c3a518784dd18e4 (diff) | |
parent | 8eda10ed94b66a8bb388cc5710dc6b45b1901993 (diff) | |
download | libsurvive-8c5ee18e12847dd01290025bd138a1a029f1caf8.tar.gz libsurvive-8c5ee18e12847dd01290025bd138a1a029f1caf8.tar.bz2 |
Merge pull request #86 from mwturvey/MakeTrackingGood
Make tracking good
Diffstat (limited to 'redist/linmath.c')
-rw-r--r-- | redist/linmath.c | 36 |
1 files changed, 36 insertions, 0 deletions
diff --git a/redist/linmath.c b/redist/linmath.c index 5d51708..1d70ee1 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -143,6 +143,42 @@ void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest) } +void axisanglefromquat(FLT *angle, FLT *axis, FLT *q) +{ + // this way might be fine, too. + //FLT dist = FLT_SQRT((q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); + // + //*angle = 2 * FLT_ATAN2(dist, q[0]); + + //axis[0] = q[1] / dist; + //axis[1] = q[2] / dist; + //axis[2] = q[3] / dist; + + + // Good mathematical foundation for this algorithm found here: + // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm + + FLT tmp[4] = { q[0], q[1], q[2], q[3] }; + + quatnormalize(tmp, q); + + if (FLT_FABS(q[0] - 1) < FLT_EPSILON) + { + // we have a degenerate case where we're rotating approx. 0 degrees + *angle = 0; + axis[0] = 1; + axis[1] = 0; + axis[2] = 0; + return; + } + + axis[0] = tmp[1] / sqrt(1 - (tmp[0] * tmp[0])); + axis[1] = tmp[2] / sqrt(1 - (tmp[0] * tmp[0])); + axis[2] = tmp[3] / sqrt(1 - (tmp[0] * tmp[0])); + + *angle = 2 * FLT_ACOS(tmp[0]); +} + /////////////////////////////////////QUATERNIONS////////////////////////////////////////// //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) //Under the mit/X11 license. |