From a4cf0b14abb17c313243d0fb84555aec2cef61a0 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Tue, 7 Feb 2017 00:11:39 -0700 Subject: Merging math libraries --- redist/linmath.c | 219 +++++++++++++++++++++++++++++++++++++++++++++++++------ redist/linmath.h | 48 +++++++++++- 2 files changed, 241 insertions(+), 26 deletions(-) (limited to 'redist') diff --git a/redist/linmath.c b/redist/linmath.c index 60fbc21..ea44432 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -2,6 +2,7 @@ #include "linmath.h" #include +#include void cross3d( FLT * out, const FLT * a, const FLT * b ) { @@ -33,7 +34,7 @@ void scale3d( FLT * out, const FLT * a, FLT scalar ) void normalize3d( FLT * out, const FLT * in ) { - FLT r = 1./sqrtf( in[0] * in[0] + in[1] * in[1] + in[2] * in[2] ); + FLT r = ((FLT)1.) / FLT_SQRT(in[0] * in[0] + in[1] * in[1] + in[2] * in[2]); out[0] = in[0] * r; out[1] = in[1] * r; out[2] = in[2] * r; @@ -65,7 +66,7 @@ void copy3d( FLT * out, const FLT * in ) FLT magnitude3d( FLT * a ) { - return sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] ); + return FLT_SQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]); } FLT anglebetween3d( FLT * a, FLT * b ) @@ -77,7 +78,7 @@ FLT anglebetween3d( FLT * a, FLT * b ) FLT dot = dot3d( a, b ); if( dot < -0.9999999 ) return LINMATHPI; if( dot > 0.9999999 ) return 0; - return acos( dot ); + return FLT_ACOS(dot); } /////////////////////////////////////QUATERNIONS////////////////////////////////////////// @@ -106,12 +107,12 @@ void quatfromeuler( FLT * q, const FLT * euler ) FLT Y = euler[1]/2.0f; //pitch FLT Z = euler[2]/2.0f; //yaw - FLT cx = cosf(X); - FLT sx = sinf(X); - FLT cy = cosf(Y); - FLT sy = sinf(Y); - FLT cz = cosf(Z); - FLT sz = sinf(Z); + FLT cx = FLT_COS(X); + FLT sx = FLT_SIN(X); + FLT cy = FLT_COS(Y); + FLT sy = FLT_SIN(Y); + FLT cz = FLT_COS(Z); + FLT sz = FLT_SIN(Z); //Correct according to //http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles @@ -125,9 +126,9 @@ void quatfromeuler( FLT * q, const FLT * euler ) void quattoeuler( FLT * euler, const FLT * q ) { //According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009) - euler[0] = atan2( 2 * (q[0]*q[1] + q[2]*q[3]), 1 - 2 * (q[1]*q[1] + q[2]*q[2] ) ); - euler[1] = asin( 2 * (q[0] *q[2] - q[3]*q[1] ) ); - euler[2] = atan2( 2 * (q[0]*q[3] + q[1]*q[2]), 1 - 2 * (q[2]*q[2] + q[3]*q[3] ) ); + euler[0] = FLT_ATAN2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2])); + euler[1] = FLT_ASIN(2 * (q[0] * q[2] - q[3] * q[1])); + euler[2] = FLT_ATAN2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3])); } void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) @@ -135,8 +136,8 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) FLT v[3]; normalize3d( v, axis ); - FLT sn = sin(radians/2.0f); - q[0] = cos(radians/2.0f); + FLT sn = FLT_SIN(radians / 2.0f); + q[0] = FLT_COS(radians / 2.0f); q[1] = sn * v[0]; q[2] = sn * v[1]; q[3] = sn * v[2]; @@ -146,12 +147,12 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) FLT quatmagnitude( const FLT * q ) { - return sqrt((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); + return FLT_SQRT((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); } FLT quatinvsqmagnitude( const FLT * q ) { - return 1./((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); + return ((FLT)1.)/((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); } @@ -296,13 +297,13 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ) if ( 1 - (cosTheta*cosTheta) <= 0 ) sinTheta = 0; else - sinTheta = sqrt(1 - (cosTheta*cosTheta)); + sinTheta = FLT_SQRT(1 - (cosTheta*cosTheta)); - FLT Theta = acos(cosTheta); //Theta is half the angle between the 2 MQuaternions + FLT Theta = FLT_ACOS(cosTheta); //Theta is half the angle between the 2 MQuaternions - if(fabs(Theta) < DEFAULT_EPSILON ) + if (FLT_FABS(Theta) < DEFAULT_EPSILON) quatcopy( q, qa ); - else if(fabs(sinTheta) < DEFAULT_EPSILON ) + else if (FLT_FABS(sinTheta) < DEFAULT_EPSILON) { quatadd( q, qa, qb ); quatscale( q, q, 0.5 ); @@ -311,10 +312,10 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ) { FLT aside[4]; FLT bside[4]; - quatscale( bside, qb, sin( t * Theta ) ); - quatscale( aside, qa, sin((1-t)*Theta) ); + quatscale( bside, qb, FLT_SIN(t * Theta)); + quatscale( aside, qa, FLT_SIN((1 - t)*Theta)); quatadd( q, aside, bside ); - quatscale( q, q, 1./sinTheta ); + quatscale( q, q, ((FLT)1.)/sinTheta ); } } @@ -338,4 +339,176 @@ void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ) } +// Matrix Stuff + +Matrix3x3 inverseM33(const Matrix3x3 mat) +{ + Matrix3x3 newMat; + for (int a = 0; a < 3; a++) + { + for (int b = 0; b < 3; b++) + { + newMat.val[a][b] = mat.val[a][b]; + } + } + + for (int i = 0; i < 3; i++) + { + for (int j = i + 1; j < 3; j++) + { + FLT tmp = newMat.val[i][j]; + newMat.val[i][j] = newMat.val[j][i]; + newMat.val[j][i] = tmp; + } + } + + return newMat; +} + +/////////////////////////////////////Matrix Rotations//////////////////////////////////// +//Originally from Stack Overflow +//Under cc by-sa 3.0 +// http://stackoverflow.com/questions/23166898/efficient-way-to-calculate-a-3x3-rotation-matrix-from-the-rotation-defined-by-tw +// Copyright 2014 by Campbell Barton +// Copyright 2017 by Michael Turvey + +/** +* Calculate a rotation matrix from 2 normalized vectors. +* +* v1 and v2 must be unit length. +*/ +void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3]) +{ + FLT axis[3]; + /* avoid calculating the angle */ + FLT angle_sin; + FLT angle_cos; + + cross3d(axis, v1, v2); + + angle_sin = normalize_v3(axis); + angle_cos = dot3d(v1, v2); + + if (angle_sin > FLT_EPSILON) { + axis_calc: + axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos); + } + else { + /* Degenerate (co-linear) vectors */ + if (angle_cos > 0.0f) { + /* Same vectors, zero rotation... */ + unit_m3(m); + } + else { + /* Colinear but opposed vectors, 180 rotation... */ + get_orthogonal_vector(axis, v1); + normalize_v3(axis); + angle_sin = 0.0f; /* sin(M_PI) */ + angle_cos = -1.0f; /* cos(M_PI) */ + goto axis_calc; + } + } +} + +void get_orthogonal_vector(FLT out[3], const FLT in[3]) +{ +#ifdef USE_DOUBLE + const FLT x = fabs(in[0]); + const FLT y = fabs(in[1]); + const FLT z = fabs(in[2]); +#else + const FLT x = fabsf(in[0]); + const FLT y = fabsf(in[1]); + const FLT z = fabsf(in[2]); +#endif + + if (x > y && x > z) + { + // x is dominant + out[0] = -in[1] - in[2]; + out[1] = in[0]; + out[2] = in[0]; + } + else if (y > z) + { + // y is dominant + out[0] = in[1]; + out[1] = -in[0] - in[2]; + out[2] = in[1]; + } + else + { + // z is dominant + out[0] = in[2]; + out[1] = in[2]; + out[2] = -in[0] - in[1]; + } +} + +void unit_m3(FLT mat[3][3]) +{ + mat[0][0] = 1; + mat[0][1] = 0; + mat[0][2] = 0; + mat[1][0] = 0; + mat[1][1] = 1; + mat[1][2] = 0; + mat[2][0] = 0; + mat[2][1] = 0; + mat[2][2] = 1; +} + + +FLT normalize_v3(FLT vect[3]) +{ + FLT distance = dot3d(vect, vect); + + if (distance < 1.0e-35f) + { + // distance is too short, just go to zero. + vect[0] = 0; + vect[1] = 0; + vect[2] = 0; + distance = 0; + } + else + { + distance = FLT_SQRT((FLT)distance); + scale3d(vect, vect, 1.0f / distance); + } + + return distance; +} + +/* axis must be unit length */ +void axis_angle_normalized_to_mat3_ex( + FLT mat[3][3], const FLT axis[3], + const FLT angle_sin, const FLT angle_cos) +{ + FLT nsi[3], ico; + FLT n_00, n_01, n_11, n_02, n_12, n_22; + + ico = (1.0f - angle_cos); + nsi[0] = axis[0] * angle_sin; + nsi[1] = axis[1] * angle_sin; + nsi[2] = axis[2] * angle_sin; + + n_00 = (axis[0] * axis[0]) * ico; + n_01 = (axis[0] * axis[1]) * ico; + n_11 = (axis[1] * axis[1]) * ico; + n_02 = (axis[0] * axis[2]) * ico; + n_12 = (axis[1] * axis[2]) * ico; + n_22 = (axis[2] * axis[2]) * ico; + + mat[0][0] = n_00 + angle_cos; + mat[0][1] = n_01 + nsi[2]; + mat[0][2] = n_02 - nsi[1]; + mat[1][0] = n_01 - nsi[2]; + mat[1][1] = n_11 + angle_cos; + mat[1][2] = n_12 + nsi[0]; + mat[2][0] = n_02 + nsi[1]; + mat[2][1] = n_12 - nsi[0]; + mat[2][2] = n_22 + angle_cos; +} + diff --git a/redist/linmath.h b/redist/linmath.h index 5cc7b7d..530d291 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -10,14 +10,37 @@ #define PFTHREE(x) x[0], x[1], x[2] #define PFFOUR(x) x[0], x[1], x[2], x[3] -#define LINMATHPI 3.141592653589 +#define LINMATHPI ((FLT)3.141592653589) + +//uncomment the following line to use double precision instead of single precision. +//#define USE_DOUBLE + +#ifdef USE_DOUBLE + +#define FLT double +#define FLT_SQRT sqrt +#define FLT_SIN sin +#define FLT_COS cos +#define FLT_ACOS acos +#define FLT_ASIN asin +#define FLT_ATAN2 atan2 +#define FLT_FABS fabs + +#else -//If you want, you can define FLT to be double for double precision. -#ifndef FLT #define FLT float +#define FLT_SQRT sqrtf +#define FLT_SIN sinf +#define FLT_COS cosf +#define FLT_ACOS acosf +#define FLT_ASIN asinf +#define FLT_ATAN2 atan2f +#define FLT_FABS fabsf + #endif + //NOTE: Inputs may never be output with cross product. void cross3d( FLT * out, const FLT * a, const FLT * b ); @@ -64,6 +87,25 @@ 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 ); +// Matrix Stuff + +typedef struct +{ + // row, column, (0,0) in upper left + FLT val[3][3]; +} Matrix3x3; + +Matrix3x3 inverseM33(const Matrix3x3 mat); +void get_orthogonal_vector(FLT out[3], const FLT in[3]); +void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3]); +void unit_m3(FLT m[3][3]); +FLT normalize_v3(FLT n[3]); +void axis_angle_normalized_to_mat3_ex( + FLT mat[3][3], + const FLT axis[3], + const FLT angle_sin, + const FLT angle_cos); + #endif -- cgit v1.2.3 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 From ae522f8a06848d467c835d87772580fa7cceb5cd Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 8 Feb 2017 11:42:46 -0700 Subject: Replaced rotation algorithm & cleanup --- redist/linmath.c | 594 ++++++++++++++++++++++++++++--------------------------- redist/linmath.h | 20 +- 2 files changed, 311 insertions(+), 303 deletions(-) (limited to 'redist') diff --git a/redist/linmath.c b/redist/linmath.c index f5f6b22..d5d54e3 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -34,7 +34,7 @@ void scale3d( FLT * out, const FLT * a, FLT scalar ) void normalize3d( FLT * out, const FLT * in ) { - FLT r = ((FLT)1.) / FLT_SQRT(in[0] * in[0] + in[1] * in[1] + in[2] * in[2]); + FLT r = ((FLT)1.) / FLT_SQRT(in[0] * in[0] + in[1] * in[1] + in[2] * in[2]); out[0] = in[0] * r; out[1] = in[1] * r; out[2] = in[2] * r; @@ -66,7 +66,7 @@ void copy3d( FLT * out, const FLT * in ) FLT magnitude3d(const FLT * a ) { - return FLT_SQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]); + return FLT_SQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]); } FLT anglebetween3d( FLT * a, FLT * b ) @@ -78,7 +78,7 @@ FLT anglebetween3d( FLT * a, FLT * b ) FLT dot = dot3d( a, b ); if( dot < -0.9999999 ) return LINMATHPI; if( dot > 0.9999999 ) return 0; - return FLT_ACOS(dot); + return FLT_ACOS(dot); } /////////////////////////////////////QUATERNIONS////////////////////////////////////////// @@ -90,12 +90,12 @@ FLT anglebetween3d( FLT * a, FLT * b ) 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 quatsetidentity(FLT * q) { - q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 1; + q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 1; } void quatcopy(FLT * qout, const FLT * qin) @@ -112,12 +112,12 @@ void quatfromeuler( FLT * q, const FLT * euler ) FLT Y = euler[1]/2.0f; //pitch FLT Z = euler[2]/2.0f; //yaw - FLT cx = FLT_COS(X); - FLT sx = FLT_SIN(X); - FLT cy = FLT_COS(Y); - FLT sy = FLT_SIN(Y); - FLT cz = FLT_COS(Z); - FLT sz = FLT_SIN(Z); + FLT cx = FLT_COS(X); + FLT sx = FLT_SIN(X); + FLT cy = FLT_COS(Y); + FLT sy = FLT_SIN(Y); + FLT cz = FLT_COS(Z); + FLT sz = FLT_SIN(Z); //Correct according to //http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles @@ -131,9 +131,9 @@ void quatfromeuler( FLT * q, const FLT * euler ) void quattoeuler( FLT * euler, const FLT * q ) { //According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009) - euler[0] = FLT_ATAN2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2])); - euler[1] = FLT_ASIN(2 * (q[0] * q[2] - q[3] * q[1])); - euler[2] = FLT_ATAN2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3])); + euler[0] = FLT_ATAN2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2])); + euler[1] = FLT_ASIN(2 * (q[0] * q[2] - q[3] * q[1])); + euler[2] = FLT_ATAN2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3])); } void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) @@ -141,8 +141,8 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) FLT v[3]; normalize3d( v, axis ); - FLT sn = FLT_SIN(radians / 2.0f); - q[0] = FLT_COS(radians / 2.0f); + FLT sn = FLT_SIN(radians / 2.0f); + q[0] = FLT_COS(radians / 2.0f); q[1] = sn * v[0]; q[2] = sn * v[1]; q[3] = sn * v[2]; @@ -152,7 +152,7 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) FLT quatmagnitude( const FLT * q ) { - return FLT_SQRT((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); + return FLT_SQRT((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); } FLT quatinvsqmagnitude( const FLT * q ) @@ -169,74 +169,74 @@ void quatnormalize( FLT * qout, const FLT * qin ) void quattomatrix(FLT * matrix44, const FLT * qin) { - FLT q[4]; - quatnormalize(q, 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]; + //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 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 zz = 2 * q[2] * q[2]; + FLT zw = 2 * q[2] * q[3]; - //opengl major - matrix44[0] = 1 - yy - zz; - matrix44[1] = xy - zw; - matrix44[2] = xz + yw; - matrix44[3] = 0; + //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[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[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; + matrix44[12] = 0; + matrix44[13] = 0; + matrix44[14] = 0; + matrix44[15] = 1; } void quattomatrix33(FLT * matrix33, const FLT * qin) { - FLT q[4]; - quatnormalize(q, 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]; + //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 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 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; + //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[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; + matrix33[6] = xz - yw; + matrix33[7] = yz + xw; + matrix33[8] = 1 - xx - yy; } void quatgetconjugate(FLT * qout, const FLT * qin) @@ -334,13 +334,13 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ) if ( 1 - (cosTheta*cosTheta) <= 0 ) sinTheta = 0; else - sinTheta = FLT_SQRT(1 - (cosTheta*cosTheta)); + sinTheta = FLT_SQRT(1 - (cosTheta*cosTheta)); - FLT Theta = FLT_ACOS(cosTheta); //Theta is half the angle between the 2 MQuaternions + FLT Theta = FLT_ACOS(cosTheta); //Theta is half the angle between the 2 MQuaternions - if (FLT_FABS(Theta) < DEFAULT_EPSILON) + if (FLT_FABS(Theta) < DEFAULT_EPSILON) quatcopy( q, qa ); - else if (FLT_FABS(sinTheta) < DEFAULT_EPSILON) + else if (FLT_FABS(sinTheta) < DEFAULT_EPSILON) { quatadd( q, qa, qb ); quatscale( q, q, 0.5 ); @@ -349,8 +349,8 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ) { FLT aside[4]; FLT bside[4]; - quatscale( bside, qb, FLT_SIN(t * Theta)); - quatscale( aside, qa, FLT_SIN((1 - t)*Theta)); + quatscale( bside, qb, FLT_SIN(t * Theta)); + quatscale( aside, qa, FLT_SIN((1 - t)*Theta)); quatadd( q, aside, bside ); quatscale( q, q, ((FLT)1.)/sinTheta ); } @@ -380,37 +380,47 @@ void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ) Matrix3x3 inverseM33(const Matrix3x3 mat) { - Matrix3x3 newMat; - for (int a = 0; a < 3; a++) - { - for (int b = 0; b < 3; b++) - { - newMat.val[a][b] = mat.val[a][b]; - } - } + Matrix3x3 newMat; + for (int a = 0; a < 3; a++) + { + for (int b = 0; b < 3; b++) + { + newMat.val[a][b] = mat.val[a][b]; + } + } - for (int i = 0; i < 3; i++) - { - for (int j = i + 1; j < 3; j++) - { - FLT tmp = newMat.val[i][j]; - newMat.val[i][j] = newMat.val[j][i]; - newMat.val[j][i] = tmp; - } - } + for (int i = 0; i < 3; i++) + { + for (int j = i + 1; j < 3; j++) + { + FLT tmp = newMat.val[i][j]; + newMat.val[i][j] = newMat.val[j][i]; + newMat.val[j][i] = tmp; + } + } - return newMat; + return newMat; } -void rotation_between_vecs_to_m3(FLT m[3][3], const FLT v1[3], const FLT v2[3]) +void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]) { - FLT q[4]; + FLT q[4]; - getRotationTo(q, v1, v2); + quatfrom2vectors(q, v1, v2); - quattomatrix33(&(m[0][0]), q); + quattomatrix33(&(m->val[0][0]), q); } +void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot) +{ + out[0] = rot.val[0][0] * in[0] + rot.val[1][0] * in[1] + rot.val[2][0] * in[2]; + out[1] = rot.val[0][1] * in[0] + rot.val[1][1] * in[1] + rot.val[2][1] * in[2]; + out[2] = rot.val[0][2] * in[0] + rot.val[1][2] * in[1] + rot.val[2][2] * in[2]; + + return; +} + + // This function based on code from Object-oriented Graphics Rendering Engine // Copyright(c) 2000 - 2012 Torus Knot Software Ltd // under MIT license @@ -423,201 +433,203 @@ 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 -// http://stackoverflow.com/questions/23166898/efficient-way-to-calculate-a-3x3-rotation-matrix-from-the-rotation-defined-by-tw -// Copyright 2014 by Campbell Barton -// Copyright 2017 by Michael Turvey - -/** -* Calculate a rotation matrix from 2 normalized vectors. -* -* v1 and v2 must be unit length. -*/ -void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3]) -{ - FLT axis[3]; - /* avoid calculating the angle */ - FLT angle_sin; - FLT angle_cos; - - cross3d(axis, v1, v2); - - angle_sin = normalize_v3(axis); - angle_cos = dot3d(v1, v2); - - if (angle_sin > FLT_EPSILON) { - axis_calc: - axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos); - } - else { - /* Degenerate (co-linear) vectors */ - if (angle_cos > 0.0f) { - /* Same vectors, zero rotation... */ - unit_m3(m); - } - else { - /* Colinear but opposed vectors, 180 rotation... */ - get_orthogonal_vector(axis, v1); - normalize_v3(axis); - angle_sin = 0.0f; /* sin(M_PI) */ - angle_cos = -1.0f; /* cos(M_PI) */ - goto axis_calc; - } - } -} - -void get_orthogonal_vector(FLT out[3], const FLT in[3]) -{ -#ifdef USE_DOUBLE - const FLT x = fabs(in[0]); - const FLT y = fabs(in[1]); - const FLT z = fabs(in[2]); -#else - const FLT x = fabsf(in[0]); - const FLT y = fabsf(in[1]); - const FLT z = fabsf(in[2]); -#endif - - if (x > y && x > z) - { - // x is dominant - out[0] = -in[1] - in[2]; - out[1] = in[0]; - out[2] = in[0]; - } - else if (y > z) - { - // y is dominant - out[0] = in[1]; - out[1] = -in[0] - in[2]; - out[2] = in[1]; - } - else - { - // z is dominant - out[0] = in[2]; - out[1] = in[2]; - out[2] = -in[0] - in[1]; - } -} - -void unit_m3(FLT mat[3][3]) -{ - mat[0][0] = 1; - mat[0][1] = 0; - mat[0][2] = 0; - mat[1][0] = 0; - mat[1][1] = 1; - mat[1][2] = 0; - mat[2][0] = 0; - mat[2][1] = 0; - mat[2][2] = 1; -} - - -FLT normalize_v3(FLT vect[3]) -{ - FLT distance = dot3d(vect, vect); - - if (distance < 1.0e-35f) - { - // distance is too short, just go to zero. - vect[0] = 0; - vect[1] = 0; - vect[2] = 0; - distance = 0; - } - else - { - distance = FLT_SQRT((FLT)distance); - scale3d(vect, vect, 1.0f / distance); - } - - return distance; -} - -/* axis must be unit length */ -void axis_angle_normalized_to_mat3_ex( - FLT mat[3][3], const FLT axis[3], - const FLT angle_sin, const FLT angle_cos) -{ - FLT nsi[3], ico; - FLT n_00, n_01, n_11, n_02, n_12, n_22; - - ico = (1.0f - angle_cos); - nsi[0] = axis[0] * angle_sin; - nsi[1] = axis[1] * angle_sin; - nsi[2] = axis[2] * angle_sin; - - n_00 = (axis[0] * axis[0]) * ico; - n_01 = (axis[0] * axis[1]) * ico; - n_11 = (axis[1] * axis[1]) * ico; - n_02 = (axis[0] * axis[2]) * ico; - n_12 = (axis[1] * axis[2]) * ico; - n_22 = (axis[2] * axis[2]) * ico; - - mat[0][0] = n_00 + angle_cos; - mat[0][1] = n_01 + nsi[2]; - mat[0][2] = n_02 - nsi[1]; - mat[1][0] = n_01 - nsi[2]; - mat[1][1] = n_11 + angle_cos; - mat[1][2] = n_12 + nsi[0]; - mat[2][0] = n_02 + nsi[1]; - mat[2][1] = n_12 - nsi[0]; - mat[2][2] = n_22 + angle_cos; +void quatfrom2vectors(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); + cross3d(c, v1, v0); + + 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 +//// http://stackoverflow.com/questions/23166898/efficient-way-to-calculate-a-3x3-rotation-matrix-from-the-rotation-defined-by-tw +//// Copyright 2014 by Campbell Barton +//// Copyright 2017 by Michael Turvey +// +///** +//* Calculate a rotation matrix from 2 normalized vectors. +//* +//* v1 and v2 must be unit length. +//*/ +//void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3]) +//{ +// FLT axis[3]; +// /* avoid calculating the angle */ +// FLT angle_sin; +// FLT angle_cos; +// +// cross3d(axis, v1, v2); +// +// angle_sin = normalize_v3(axis); +// angle_cos = dot3d(v1, v2); +// +// if (angle_sin > FLT_EPSILON) { +// axis_calc: +// axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos); +// } +// else { +// /* Degenerate (co-linear) vectors */ +// if (angle_cos > 0.0f) { +// /* Same vectors, zero rotation... */ +// unit_m3(m); +// } +// else { +// /* Colinear but opposed vectors, 180 rotation... */ +// get_orthogonal_vector(axis, v1); +// normalize_v3(axis); +// angle_sin = 0.0f; /* sin(M_PI) */ +// angle_cos = -1.0f; /* cos(M_PI) */ +// goto axis_calc; +// } +// } +//} + +//void get_orthogonal_vector(FLT out[3], const FLT in[3]) +//{ +//#ifdef USE_DOUBLE +// const FLT x = fabs(in[0]); +// const FLT y = fabs(in[1]); +// const FLT z = fabs(in[2]); +//#else +// const FLT x = fabsf(in[0]); +// const FLT y = fabsf(in[1]); +// const FLT z = fabsf(in[2]); +//#endif +// +// if (x > y && x > z) +// { +// // x is dominant +// out[0] = -in[1] - in[2]; +// out[1] = in[0]; +// out[2] = in[0]; +// } +// else if (y > z) +// { +// // y is dominant +// out[0] = in[1]; +// out[1] = -in[0] - in[2]; +// out[2] = in[1]; +// } +// else +// { +// // z is dominant +// out[0] = in[2]; +// out[1] = in[2]; +// out[2] = -in[0] - in[1]; +// } +//} +// +//void unit_m3(FLT mat[3][3]) +//{ +// mat[0][0] = 1; +// mat[0][1] = 0; +// mat[0][2] = 0; +// mat[1][0] = 0; +// mat[1][1] = 1; +// mat[1][2] = 0; +// mat[2][0] = 0; +// mat[2][1] = 0; +// mat[2][2] = 1; +//} + + +//FLT normalize_v3(FLT vect[3]) +//{ +// FLT distance = dot3d(vect, vect); +// +// if (distance < 1.0e-35f) +// { +// // distance is too short, just go to zero. +// vect[0] = 0; +// vect[1] = 0; +// vect[2] = 0; +// distance = 0; +// } +// else +// { +// distance = FLT_SQRT((FLT)distance); +// scale3d(vect, vect, 1.0f / distance); +// } +// +// return distance; +//} + +///* axis must be unit length */ +//void axis_angle_normalized_to_mat3_ex( +// FLT mat[3][3], const FLT axis[3], +// const FLT angle_sin, const FLT angle_cos) +//{ +// FLT nsi[3], ico; +// FLT n_00, n_01, n_11, n_02, n_12, n_22; +// +// ico = (1.0f - angle_cos); +// nsi[0] = axis[0] * angle_sin; +// nsi[1] = axis[1] * angle_sin; +// nsi[2] = axis[2] * angle_sin; +// +// n_00 = (axis[0] * axis[0]) * ico; +// n_01 = (axis[0] * axis[1]) * ico; +// n_11 = (axis[1] * axis[1]) * ico; +// n_02 = (axis[0] * axis[2]) * ico; +// n_12 = (axis[1] * axis[2]) * ico; +// n_22 = (axis[2] * axis[2]) * ico; +// +// mat[0][0] = n_00 + angle_cos; +// mat[0][1] = n_01 + nsi[2]; +// mat[0][2] = n_02 - nsi[1]; +// mat[1][0] = n_01 - nsi[2]; +// mat[1][1] = n_11 + angle_cos; +// mat[1][2] = n_12 + nsi[0]; +// mat[2][0] = n_02 + nsi[1]; +// mat[2][1] = n_12 - nsi[0]; +// mat[2][2] = n_22 + angle_cos; +//} + diff --git a/redist/linmath.h b/redist/linmath.h index 3b11c1b..20a7848 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -87,26 +87,22 @@ 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); +void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest); // Matrix Stuff typedef struct { - // row, column, (0,0) in upper left - FLT val[3][3]; + FLT val[3][3]; // row, column } Matrix3x3; +void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot); +void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]); Matrix3x3 inverseM33(const Matrix3x3 mat); -void get_orthogonal_vector(FLT out[3], const FLT in[3]); -void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3]); -void unit_m3(FLT m[3][3]); -FLT normalize_v3(FLT n[3]); -void axis_angle_normalized_to_mat3_ex( - FLT mat[3][3], - const FLT axis[3], - const FLT angle_sin, - const FLT angle_cos); + + + + #endif -- cgit v1.2.3