diff options
| author | Mike Turvey <mturvey6@gmail.com> | 2017-12-28 08:03:37 -0700 |
|---|---|---|
| committer | Mike Turvey <mturvey6@gmail.com> | 2017-12-28 08:03:37 -0700 |
| commit | ec564d70daa8c1a66018f9606b02b873ae792c84 (patch) | |
| tree | 74cc2f8ad167fb468abe2dfcbd026d8445762f63 /redist | |
| parent | 417ced84b51bfcc392f06a87a1328f679364fc38 (diff) | |
| download | libsurvive-ec564d70daa8c1a66018f9606b02b873ae792c84.tar.gz libsurvive-ec564d70daa8c1a66018f9606b02b873ae792c84.tar.bz2 | |
Start work on determining rotation using quaternions only
Rotation was previously approximated using axis/angle
This change starts down the path of using quaternions exclusively.
This change appears to give at least as good as answers as the axis/angle
model in basic cases (also only tested with 1 lighthouse), but it is
currently much slower and runs in unpredictable time.
Diffstat (limited to 'redist')
| -rw-r--r-- | redist/linmath.c | 36 | ||||
| -rw-r--r-- | redist/linmath.h | 2 |
2 files changed, 38 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. diff --git a/redist/linmath.h b/redist/linmath.h index 8d6cf05..1876b34 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -72,6 +72,8 @@ FLT anglebetween3d( FLT * a, FLT * b ); void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle); void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest); +void axisanglefromquat(FLT *angle, FLT *axis, FLT *quat); + //Quaternion things... |