diff options
author | cnlohr <lohr85@gmail.com> | 2017-02-11 01:08:26 -0500 |
---|---|---|
committer | cnlohr <lohr85@gmail.com> | 2017-02-11 01:08:26 -0500 |
commit | dffa6477a0ee5672a5b1763a8b99ab4e21b6c94a (patch) | |
tree | 4a38d8739c2c0d295c2154472fa760810053134d /redist | |
parent | 29400a2063fc740c3c33d35649d6f6a4504b6745 (diff) | |
parent | ae522f8a06848d467c835d87772580fa7cceb5cd (diff) | |
download | libsurvive-dffa6477a0ee5672a5b1763a8b99ab4e21b6c94a.tar.gz libsurvive-dffa6477a0ee5672a5b1763a8b99ab4e21b6c94a.tar.bz2 |
Merge branch 'addTorusFind6' of https://github.com/mwturvey/libsurvive into mwturvey-addTorusFind6
Diffstat (limited to 'redist')
-rw-r--r-- | redist/linmath.c | 394 | ||||
-rw-r--r-- | redist/linmath.h | 48 |
2 files changed, 388 insertions, 54 deletions
diff --git a/redist/linmath.c b/redist/linmath.c index 60fbc21..d5d54e3 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -2,6 +2,7 @@ #include "linmath.h" #include <math.h> +#include <float.h> 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; @@ -63,9 +64,9 @@ void copy3d( FLT * out, const FLT * in ) out[2] = in[2]; } -FLT magnitude3d( FLT * a ) +FLT magnitude3d(const 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////////////////////////////////////////// @@ -87,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; } -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]; @@ -106,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 = 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 +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] = 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 +141,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 +152,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])); } @@ -161,38 +167,38 @@ 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 ); - + 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 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 - matrix44[0] = 1-yy-zz; - matrix44[1] = xy-zw; - matrix44[2] = xz+yw; + 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[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[8] = xz - yw; + matrix44[9] = yz + xw; + matrix44[10] = 1 - xx - yy; matrix44[11] = 0; matrix44[12] = 0; @@ -201,7 +207,39 @@ void quattomatrix( FLT * matrix44, const FLT * qin ) 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]; @@ -296,13 +334,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 +349,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 +376,260 @@ 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; +} + +void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]) +{ + FLT q[4]; + + quatfrom2vectors(q, v1, v2); + + 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 +// 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 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 5cc7b7d..20a7848 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 ); @@ -36,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 ); @@ -64,6 +87,23 @@ 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 quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest); + +// Matrix Stuff + +typedef struct +{ + 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); + + + + + #endif |