From 0586f134e02938e1a9dd86ad92e41c2b2443fee0 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 7 Feb 2017 17:43:12 -0700 Subject: Replacing rotation matrix (wip) --- redist/linmath.c | 185 +++++++++++++++++++++++++++++++++++++++++++------------ redist/linmath.h | 4 +- 2 files changed, 150 insertions(+), 39 deletions(-) (limited to 'redist') diff --git a/redist/linmath.c b/redist/linmath.c index ea44432..f5f6b22 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -64,7 +64,7 @@ void copy3d( FLT * out, const FLT * in ) out[2] = in[2]; } -FLT magnitude3d( FLT * a ) +FLT magnitude3d(const FLT * a ) { return FLT_SQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]); } @@ -88,12 +88,17 @@ FLT anglebetween3d( FLT * a, FLT * b ) -void quatsetnone( FLT * q ) +void quatsetnone(FLT * q) { - q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; + q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } -void quatcopy( FLT * qout, const FLT * qin ) +void quatsetidentity(FLT * q) +{ + q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 1; +} + +void quatcopy(FLT * qout, const FLT * qin) { qout[0] = qin[0]; qout[1] = qin[1]; @@ -162,47 +167,79 @@ void quatnormalize( FLT * qout, const FLT * qin ) quatscale( qout, qin, imag ); } -void quattomatrix( FLT * matrix44, const FLT * qin ) +void quattomatrix(FLT * matrix44, const FLT * qin) { - FLT q[4]; - 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 yy = 2*q[1]*q[1]; - FLT yz = 2*q[1]*q[2]; - FLT yw = 2*q[1]*q[3]; - - FLT zz = 2*q[2]*q[2]; - FLT zw = 2*q[2]*q[3]; + FLT q[4]; + quatnormalize(q, qin); - //opengl major - matrix44[0] = 1-yy-zz; - matrix44[1] = xy-zw; - matrix44[2] = xz+yw; - matrix44[3] = 0; + //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]; - matrix44[4] = xy+zw; - matrix44[5] = 1-xx-zz; - matrix44[6] = yz-xw; - matrix44[7] = 0; + FLT yy = 2 * q[1] * q[1]; + FLT yz = 2 * q[1] * q[2]; + FLT yw = 2 * q[1] * q[3]; - matrix44[8] = xz-yw; - matrix44[9] = yz+xw; - matrix44[10] = 1-xx-yy; - matrix44[11] = 0; + FLT zz = 2 * q[2] * q[2]; + FLT zw = 2 * q[2] * q[3]; - matrix44[12] = 0; - matrix44[13] = 0; - matrix44[14] = 0; - matrix44[15] = 1; + //opengl major + matrix44[0] = 1 - yy - zz; + matrix44[1] = xy - zw; + matrix44[2] = xz + yw; + matrix44[3] = 0; + + matrix44[4] = xy + zw; + matrix44[5] = 1 - xx - zz; + matrix44[6] = yz - xw; + matrix44[7] = 0; + + matrix44[8] = xz - yw; + matrix44[9] = yz + xw; + matrix44[10] = 1 - xx - yy; + matrix44[11] = 0; + + matrix44[12] = 0; + matrix44[13] = 0; + matrix44[14] = 0; + matrix44[15] = 1; } -void quatgetconjugate( FLT * qout, const FLT * qin ) +void quattomatrix33(FLT * matrix33, const FLT * qin) +{ + FLT q[4]; + 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 yy = 2 * q[1] * q[1]; + FLT yz = 2 * q[1] * q[2]; + FLT yw = 2 * q[1] * q[3]; + + FLT zz = 2 * q[2] * q[2]; + FLT zw = 2 * q[2] * q[3]; + + //opengl major + matrix33[0] = 1 - yy - zz; + matrix33[1] = xy - zw; + matrix33[2] = xz + yw; + + matrix33[3] = xy + zw; + matrix33[4] = 1 - xx - zz; + matrix33[5] = yz - xw; + + matrix33[6] = xz - yw; + matrix33[7] = yz + xw; + matrix33[8] = 1 - xx - yy; +} + +void quatgetconjugate(FLT * qout, const FLT * qin) { qout[0] = qin[0]; qout[1] = -qin[1]; @@ -365,6 +402,78 @@ Matrix3x3 inverseM33(const Matrix3x3 mat) return newMat; } +void rotation_between_vecs_to_m3(FLT m[3][3], const FLT v1[3], const FLT v2[3]) +{ + FLT q[4]; + + getRotationTo(q, v1, v2); + + quattomatrix33(&(m[0][0]), q); +} + +// This function based on code from Object-oriented Graphics Rendering Engine +// Copyright(c) 2000 - 2012 Torus Knot Software Ltd +// under MIT license +// http://www.ogre3d.org/docs/api/1.9/_ogre_vector3_8h_source.html + +/** Gets the shortest arc quaternion to rotate this vector to the destination +vector. +@remarks +If you call this with a dest vector that is close to the inverse +of this vector, we will rotate 180 degrees around a generated axis if +since in this case ANY axis of rotation is valid. +*/ +void getRotationTo(FLT *q, const FLT *src, const FLT *dest) +{ + // Based on Stan Melax's article in Game Programming Gems + + // Copy, since cannot modify local + FLT v0[3]; + FLT v1[3]; + normalize3d(v0, src); + normalize3d(v1, dest); + + FLT d = dot3d(v0, v1);// v0.dotProduct(v1); + // If dot == 1, vectors are the same + if (d >= 1.0f) + { + quatsetidentity(q); + return; + } + if (d < (1e-6f - 1.0f)) + { + // Generate an axis + FLT unitX[3] = { 1, 0, 0 }; + FLT unitY[3] = { 0, 1, 0 }; + + FLT axis[3]; + cross3d(axis, unitX, src); // pick an angle + if ((axis[0] < 1.0e-35f) && + (axis[1] < 1.0e-35f) && + (axis[2] < 1.0e-35f)) // pick another if colinear + { + cross3d(axis, unitY, src); + } + normalize3d(axis, axis); + quatfromaxisangle(q, axis, LINMATHPI); + } + else + { + FLT s = FLT_SQRT((1 + d) * 2); + FLT invs = 1 / s; + + FLT c[3]; + cross3d(c, v0, v1); + + q[0] = c[0] * invs; + q[1] = c[1] * invs; + q[2] = c[2] * invs; + q[3] = s * 0.5f; + + quatnormalize(q, q); + } +} + /////////////////////////////////////Matrix Rotations//////////////////////////////////// //Originally from Stack Overflow //Under cc by-sa 3.0 diff --git a/redist/linmath.h b/redist/linmath.h index 530d291..3b11c1b 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -59,7 +59,7 @@ int compare3d( const FLT * a, const FLT * b, FLT epsilon ); void copy3d( FLT * out, const FLT * in ); -FLT magnitude3d( FLT * a ); +FLT magnitude3d(const FLT * a ); FLT anglebetween3d( FLT * a, FLT * b ); @@ -87,6 +87,8 @@ void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb ); void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ); void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ); +void getRotationTo(FLT *q, const FLT *src, const FLT *dest); + // Matrix Stuff typedef struct -- cgit v1.2.3