From 071ce2fb0b6155a5b3139fb55a928401b549ab66 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 14 Dec 2016 22:36:47 -0500 Subject: got a little further... need to switch to a more procedural approach. --- tools/planetest/Makefile | 2 +- tools/planetest/camfind.c | 248 +++++++++++++++++++++++++++++++--------------- tools/planetest/linmath.c | 130 ++++++++++++------------ tools/planetest/linmath.h | 58 +++++------ 4 files changed, 264 insertions(+), 174 deletions(-) (limited to 'tools') diff --git a/tools/planetest/Makefile b/tools/planetest/Makefile index 0e55f12..37f02d7 100644 --- a/tools/planetest/Makefile +++ b/tools/planetest/Makefile @@ -1,6 +1,6 @@ all : camfind -CFLAGS:=-g -O4 +CFLAGS:=-g -O4 -DFLT=float -ffast-math LDFLAGS:=$(CFLAGS) -lm camfind : camfind.o linmath.o diff --git a/tools/planetest/camfind.c b/tools/planetest/camfind.c index 6a6650b..1e9b2b9 100644 --- a/tools/planetest/camfind.c +++ b/tools/planetest/camfind.c @@ -7,125 +7,174 @@ #define PTS 32 #define MAX_CHECKS 20000 -double hmd_points[PTS*3]; -double hmd_point_angles[PTS*2]; +FLT hmd_points[PTS*3]; +FLT hmd_norms[PTS*3]; +FLT hmd_point_angles[PTS*2]; int hmd_point_counts[PTS*2]; int LoadData( char Camera ); //Values used for RunTest() -float LighthousePos[3] = { 0, 0, 0 }; -float LighthouseQuat[4] = { 1, 0, 0, 0 }; +FLT LighthousePos[3] = { 0, 0, 0 }; +FLT LighthouseQuat[4] = { 1, 0, 0, 0 }; //Actual values -float xyzypr[6] = { 0, 0, 0, 0, 0, 0 }; +FLT xyzypr[6] = { 0, 2, 2, 0, 0, 0 }; -float RunOpti( int axis ); -float RunTest( int print ); +FLT RunOpti( int axis ); +FLT RunTest( int print ); void PrintOpti(); int main() { int i; + //Load either 'L' (LH1) or 'R' (LH2) data. if( LoadData( 'R' ) ) return 5; int opti = 0; int cycle = 0; int axis = 0; - float dx, dy, dz; + FLT dx, dy, dz; - float bestxyzypr[6]; + FLT bestxyzypr[6]; memcpy( bestxyzypr, xyzypr, sizeof( xyzypr ) ); - float fullrange = 5; - for( cycle = 0; cycle < 4; cycle ++ ) + FLT fullrange = 5; //Maximum search space for positions. (Relative to HMD) + for( cycle = 0; cycle < 20; cycle ++ ) { -// for( axis = 0; axis < 3; axis++ ) -// { - float bestxyzyprrunning[6]; - float beste = 1e20; - for( dz = -fullrange; dz < fullrange; dz += fullrange/5 ) - { - for( dy = -fullrange; dy < fullrange; dy += fullrange/5 ) + + //Adjust position, one axis at a time, over and over until we zero in. +#define FULLSEARCH + +#ifndef FULLSEARCH + for( axis = 0; axis < 3; axis++ ) +#endif + { + FLT bestxyzyprrunning[6]; + FLT beste = 1e20; + + //Try a bunch of points in this axis. +#ifdef FULLSEARCH + for( dz = -fullrange; dz <= fullrange; dz += fullrange/3 ) + for( dy = -fullrange; dy <= fullrange; dy += fullrange/3 ) + for( dx = -fullrange; dx <= fullrange; dx += fullrange/3 ) { - for( dx = -fullrange; dx < fullrange; dx += fullrange/5 ) +#else + for( dx = -fullrange; dx <= fullrange; dx += fullrange/5 ) { +#endif + //Specificially adjust one axis at a time, searching for the best. memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) ); +#ifdef FULLSEARCH xyzypr[0] += dx; xyzypr[1] += dy; xyzypr[2] += dz; +#else + xyzypr[axis] += dx; +#endif + //if( axis == 2 && xyzypr[2] < 0 ) continue; + xyzypr[3] = 0; xyzypr[4] = 0; xyzypr[5] = 0; - float ft; + FLT ft; int reopt = 0; - for( reopt = 0; reopt < 10; reopt++ ) - for( opti = 3; opti < 6; opti++ ) + + //Try refining the search for the best orientation several times. + for( reopt = 0; reopt < 4; reopt++ ) { - ft = RunOpti( opti ); + //XXX This search mechanism is insufficient :( + //Search through each axis every time. + for( opti = 3; opti < 6; opti++ ) + { + ft = RunOpti( opti ); + } + if( ft < beste ) { beste = ft; memcpy( bestxyzyprrunning, xyzypr, sizeof( xyzypr ) ); } } - if( ft < beste ) { beste = ft; memcpy( bestxyzyprrunning, xyzypr, sizeof( xyzypr ) ); } - //printf( "%f %f %f %f\n", xyzypr[0], xyzypr[1], xyzypr[2], ft ); - }}} - memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) ); - printf( "%f %f %f %f %f %f = %f\n", bestxyzypr[0], bestxyzypr[1], bestxyzypr[2], bestxyzypr[3], bestxyzypr[4], bestxyzypr[5], beste ); -// } - - fullrange *= 0.3; + //printf( " %f %f %f %f\n", xyzypr[0], xyzypr[1], xyzypr[2], ft ); +#ifndef FULLSEARCH + memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) ); +#endif + } +#ifdef FULLSEARCH + memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) ); +#endif + + //Print out the quality of the lock this time. + FLT dist = sqrt(bestxyzypr[0]*bestxyzypr[0] + bestxyzypr[1]*bestxyzypr[1] + bestxyzypr[2]*bestxyzypr[2]); + printf( "%f %f %f (%f) %f %f %f = %f\n", bestxyzypr[0], bestxyzypr[1], bestxyzypr[2], dist, bestxyzypr[3], bestxyzypr[4], bestxyzypr[5], beste ); + } +/* + if( bestxyzypr[2] < 0 ) + { + bestxyzypr[0] *= -1; + bestxyzypr[1] *= -1; + bestxyzypr[2] *= -1; + } +*/ + //Every cycle, tighten up the search area. + fullrange *= 0.6; } //Use bestxyzypr memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) ); + + + //Print out plane accuracies with these settings. PrintOpti(); } void PrintOpti() { - float xyzyprchange[6]; + FLT xyzyprchange[6]; memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) ); quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - float ft = RunTest(1); + FLT ft = RunTest(1); + printf( "Final RMS: %f\n", ft ); } -float RunOpti( int axis ) +FLT RunOpti( int axis ) { - float xyzyprchange[6]; + FLT xyzyprchange[6]; memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) ); int i; - float minv = 1e20; - float fv = -1.3; - float bv = 0; + FLT minv = 1e20; + FLT fv = -3.3; + FLT bv = 0; - //Coarse linear search - for( i = 0; i < 26; i++ ) + //Coarse linear search first, try to figure out about where + for( i = 0; i < 33*2; i++ ) { xyzyprchange[axis] = fv; quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - float ft = RunTest(0); + FLT ft = RunTest(0); + //printf( " %d / %f = %f\n", ft, fv ); if( ft < minv ) { minv = ft; bv = fv; } fv += 0.1; } xyzyprchange[axis] = bv; - - //Now, do a more precise binary search. - float jumpamount = 0.1; - for( i = 0; i < 30; i++ ) +#if 1 + //Now, do a more precise binary-ish search. + FLT jumpamount = 0.15; + for( i = 0; i < 20; i++ ) { - float orig = xyzyprchange[axis]; + FLT orig = xyzyprchange[axis]; minv = 1e20; + //When searching, consider 'less' 'this' and 'more' + fv = xyzyprchange[axis] = orig; quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - float ft = RunTest(0); + FLT ft = RunTest(0); if( ft < minv ) { minv = ft; bv = fv; } fv = xyzyprchange[axis] = orig + jumpamount; @@ -142,57 +191,61 @@ float RunOpti( int axis ) xyzyprchange[axis] = bv; - jumpamount *= 0.5; + //Tighten up search area. Doing by 0.5 can cause unusual instabilities. + jumpamount *= 0.6; } - - - +#endif xyzypr[axis] = bv; return minv; } -float RunTest( int print ) +FLT RunTest( int print ) { int k; - float totprob = 0.0; + FLT totprob = 0.0; int ict = 0; for( k = 0; k < 64; k++ ) { if( hmd_point_counts[k] == 0 ) continue; int axis = k%2; int pt = k/2; - float angle = (hmd_point_angles[k] - 200000) / 200000 * 3.1415926535/2; //XXX XXX WRONG??? OR SOMETHING??? WHY DIV2 MAKE GOOD? - if( axis == 1) angle = -angle; //Flip coordinate systems - float thiseuler[3] = { 0, 0, 0 }; - thiseuler[axis] = angle; - + FLT angle = hmd_point_angles[k]; if( print ) printf( "%d %d : angle: %f / ", axis, pt, angle ); - //axis = 0: x changes. +y is rotated plane normal. - //axis = 1: y changes. +x is rotated plane normal. - float planequat[4]; - quatfromeuler( planequat, thiseuler ); - quatrotateabout( planequat, LighthouseQuat, planequat ); //Order of rotations may be wrong. - float plane_normal[3] = { 0, 0, 0 }; - plane_normal[!axis] = 1; - quatrotatevector( plane_normal, planequat, plane_normal ); - - //plane_normal is our normal / LighthousePos is our point. - float w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+1], hmd_points[pt*3+2] }; - //float w0[] = { 0, 0, 0 }; - float d = -(plane_normal[0] * LighthousePos[0] + plane_normal[1] * LighthousePos[1] + plane_normal[2] * LighthousePos[2]); - float D = plane_normal[0] * w0[0] + plane_normal[1] * w0[1] + plane_normal[2] * w0[2] + d; - //Point line distance assuming ||normal|| = 1. - if( print ) printf( " %f %f -\n", d, D ); - totprob += (D*D); - ict++; - if( print ) + //XXX TODO: This is critical. We need to properly define the planes. + FLT plane_normal[3] = { 0, 0, 0 }; + if( axis == 0 ) { - //printf( "%d %d ", pt, axis, hmd_points[ ); ///..x.x.x XXX TODO check line intersection and planequat thing + plane_normal[1] = cos(angle); + plane_normal[2] =-sin(angle); } + else + { + plane_normal[0] =-cos(angle); + plane_normal[2] =-sin(angle); + } + + quatrotatevector( plane_normal, LighthouseQuat, plane_normal ); //Rotate plane normal by concatenated rotation. - //printf( " : %f %f %f %f %f %f\n", plane_normal[0], plane_normal[1], plane_normal[2], 1.0, angle, hmd_point_angles[k] ); + //plane_normal is our normal / LighthousePos is our point. + FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+1], hmd_points[pt*3+2] }; + +//May be able to remove this. +// FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+2],-hmd_points[pt*3+1] }; //XXX WRONG Why does this produce the right answers? + + //FLT w0[] = { 0, 0, 0 }; + FLT d = -(plane_normal[0] * LighthousePos[0] + plane_normal[1] * LighthousePos[1] + plane_normal[2] * LighthousePos[2]); + FLT D = plane_normal[0] * w0[0] + plane_normal[1] * w0[1] + plane_normal[2] * w0[2] + d; + //Point line distance assuming ||normal|| = 1. + + FLT delta[] = { LighthousePos[0]-hmd_points[pt*3+0],LighthousePos[1]-hmd_points[pt*3+1],LighthousePos[2]-hmd_points[pt*3+2] }; + FLT dot = delta[0] * hmd_norms[pt*3+0] + delta[1] * hmd_norms[pt*3+1] + delta[2] * hmd_norms[pt*3+2]; +// if( dot < -0.04 ) totprob+=10000000000; + if( print ) printf( " %f %f N:%f\n", d, D, dot ); + totprob += (D*D); //Calculate RMS distance of incorrectitude. + + ict++; } return sqrt(totprob/ict); } @@ -229,6 +282,37 @@ int LoadData( char Camera ) printf( "Loaded %d points\n", pt ); + f = fopen( "hmd_normals.csv", "r" ); + int nrm = 0; + if( !f ) { fprintf( stderr, "error: can't open hmd points.\n" ); return -5; } + while(!feof(f) && !ferror(f) && nrm < PTS) + { + float fa, fb, fc; + int r = fscanf( f,"%g,%g,%g\n", &fa, &fb, &fc ); + hmd_norms[nrm*3+0] = fa; + hmd_norms[nrm*3+1] = fb; + hmd_norms[nrm*3+2] = fc; + nrm++; + if( r != 3 ) + { + fprintf( stderr, "Not enough entries on line %d\n", nrm ); + return -8; + } + } + if( nrm < PTS ) + { + fprintf( stderr, "Not enough points.\n" ); + return -9; + } + if( nrm != pt ) + { + fprintf( stderr, "point/normal counts disagree.\n" ); + return -9; + } + fclose( f ); + printf( "Loaded %d norms\n", nrm ); + + int xck = 0; f = fopen( "third_test_with_time_lengths.csv", "r" ); if( !f ) { fprintf( stderr, "Error: can't open two lighthouses test data.\n" ); return -11; } @@ -290,6 +374,12 @@ int LoadData( char Camera ) hmd_point_angles[i*2+0]/=hmd_point_counts[i*2+0]; hmd_point_angles[i*2+1]/=hmd_point_counts[i*2+1]; + hmd_point_angles[i*2+0] = (hmd_point_angles[i*2+0] - 200000) / 200000 * 3.1415926535/2; // Is it really 800,000 ticks per revolution? + hmd_point_angles[i*2+1] = (hmd_point_angles[i*2+1] - 200000) / 200000 * 3.1415926535/2; // Is it really 800,000 ticks per revolution? } + + + + return 0; } diff --git a/tools/planetest/linmath.c b/tools/planetest/linmath.c index dec8c70..88643b4 100644 --- a/tools/planetest/linmath.c +++ b/tools/planetest/linmath.c @@ -3,48 +3,48 @@ #include "linmath.h" #include -void cross3d( float * out, const float * a, const float * b ) +void cross3d( FLT * out, const FLT * a, const FLT * b ) { out[0] = a[1]*b[2] - a[2]*b[1]; out[1] = a[2]*b[0] - a[0]*b[2]; out[2] = a[0]*b[1] - a[1]*b[0]; } -void sub3d( float * out, const float * a, const float * b ) +void sub3d( FLT * out, const FLT * a, const FLT * b ) { out[0] = a[0] - b[0]; out[1] = a[1] - b[1]; out[2] = a[2] - b[2]; } -void add3d( float * out, const float * a, const float * b ) +void add3d( FLT * out, const FLT * a, const FLT * b ) { out[0] = a[0] + b[0]; out[1] = a[1] + b[1]; out[2] = a[2] + b[2]; } -void scale3d( float * out, const float * a, float scalar ) +void scale3d( FLT * out, const FLT * a, FLT scalar ) { out[0] = a[0] * scalar; out[1] = a[1] * scalar; out[2] = a[2] * scalar; } -void normalize3d( float * out, const float * in ) +void normalize3d( FLT * out, const FLT * in ) { - float r = 1./sqrtf( in[0] * in[0] + in[1] * in[1] + in[2] * in[2] ); + FLT r = 1./sqrtf( 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; } -float dot3d( const float * a, const float * b ) +FLT dot3d( const FLT * a, const FLT * b ) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; } -int compare3d( const float * a, const float * b, float epsilon ) +int compare3d( const FLT * a, const FLT * b, FLT epsilon ) { if( !a || !b ) return 0; if( a[2] - b[2] > epsilon ) return 1; @@ -56,7 +56,7 @@ int compare3d( const float * a, const float * b, float epsilon ) return 0; } -void copy3d( float * out, const float * in ) +void copy3d( FLT * out, const FLT * in ) { out[0] = in[0]; out[1] = in[1]; @@ -72,12 +72,12 @@ void copy3d( float * out, const float * in ) -void quatsetnone( float * q ) +void quatsetnone( FLT * q ) { q[0] = 0; q[1] = 0; q[2] = 0; q[3] = 1; } -void quatcopy( float * qout, const float * qin ) +void quatcopy( FLT * qout, const FLT * qin ) { qout[0] = qin[0]; qout[1] = qin[1]; @@ -85,18 +85,18 @@ void quatcopy( float * qout, const float * qin ) qout[3] = qin[3]; } -void quatfromeuler( float * q, const float * euler ) +void quatfromeuler( FLT * q, const FLT * euler ) { - float X = euler[0]/2.0f; //roll - float Y = euler[1]/2.0f; //pitch - float Z = euler[2]/2.0f; //yaw + FLT X = euler[0]/2.0f; //roll + FLT Y = euler[1]/2.0f; //pitch + FLT Z = euler[2]/2.0f; //yaw - float cx = cosf(X); - float sx = sinf(X); - float cy = cosf(Y); - float sy = sinf(Y); - float cz = cosf(Z); - float sz = sinf(Z); + FLT cx = cosf(X); + FLT sx = sinf(X); + FLT cy = cosf(Y); + FLT sy = sinf(Y); + FLT cz = cosf(Z); + FLT sz = sinf(Z); //Correct according to //http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles @@ -107,7 +107,7 @@ void quatfromeuler( float * q, const float * euler ) quatnormalize( q, q ); } -void quattoeuler( float * euler, const float * q ) +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] ) ); @@ -115,12 +115,12 @@ void quattoeuler( float * euler, const float * q ) euler[2] = atan2( 2 * (q[0]*q[3] + q[1]*q[2]), 1 - 2 * (q[2]*q[2] + q[3]*q[3] ) ); } -void quatfromaxisangle( float * q, const float * axis, float radians ) +void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) { - float v[3]; + FLT v[3]; normalize3d( v, axis ); - float sn = sin(radians/2.0f); + FLT sn = sin(radians/2.0f); q[0] = cos(radians/2.0f); q[1] = sn * v[0]; q[2] = sn * v[1]; @@ -129,40 +129,40 @@ void quatfromaxisangle( float * q, const float * axis, float radians ) quatnormalize( q, q ); } -float quatmagnitude( const float * q ) +FLT quatmagnitude( const FLT * q ) { return sqrt((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); } -float quatinvsqmagnitude( const float * q ) +FLT quatinvsqmagnitude( const FLT * q ) { return 1./((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); } -void quatnormalize( float * qout, const float * qin ) +void quatnormalize( FLT * qout, const FLT * qin ) { - float imag = quatinvsqmagnitude( qin ); + FLT imag = quatinvsqmagnitude( qin ); quatscale( qout, qin, imag ); } -void quattomatrix( float * matrix44, const float * qin ) +void quattomatrix( FLT * matrix44, const FLT * qin ) { - float q[4]; + FLT q[4]; quatnormalize( q, qin ); //Reduced calulation for speed - float xx = 2*q[0]*q[0]; - float xy = 2*q[0]*q[1]; - float xz = 2*q[0]*q[2]; - float xw = 2*q[0]*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]; - float yy = 2*q[1]*q[1]; - float yz = 2*q[1]*q[2]; - float 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]; - float zz = 2*q[2]*q[2]; - float 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; @@ -186,7 +186,7 @@ void quattomatrix( float * matrix44, const float * qin ) matrix44[15] = 1; } -void quatgetconjugate( float * qout, const float * qin ) +void quatgetconjugate( FLT * qout, const FLT * qin ) { qout[0] = qin[0]; qout[1] = -qin[1]; @@ -194,14 +194,14 @@ void quatgetconjugate( float * qout, const float * qin ) qout[3] = -qin[3]; } -void quatgetreciprocal( float * qout, const float * qin ) +void quatgetreciprocal( FLT * qout, const FLT * qin ) { - float m = quatinvsqmagnitude(qin); + FLT m = quatinvsqmagnitude(qin); quatgetconjugate( qout, qin ); quatscale( qout, qout, m ); } -void quatsub( float * qout, const float * a, const float * b ) +void quatsub( FLT * qout, const FLT * a, const FLT * b ) { qout[0] = a[0] - b[0]; qout[1] = a[1] - b[1]; @@ -209,7 +209,7 @@ void quatsub( float * qout, const float * a, const float * b ) qout[3] = a[3] - b[3]; } -void quatadd( float * qout, const float * a, const float * b ) +void quatadd( FLT * qout, const FLT * a, const FLT * b ) { qout[0] = a[0] + b[0]; qout[1] = a[1] + b[1]; @@ -217,10 +217,10 @@ void quatadd( float * qout, const float * a, const float * b ) qout[3] = a[3] + b[3]; } -void quatrotateabout( float * qout, const float * a, const float * b ) +void quatrotateabout( FLT * qout, const FLT * a, const FLT * b ) { - float q1[4]; - float q2[4]; + FLT q1[4]; + FLT q2[4]; quatnormalize( q1, a ); quatnormalize( q2, b ); @@ -231,7 +231,7 @@ void quatrotateabout( float * qout, const float * a, const float * b ) qout[3] = (q1[0]*q2[3])+(q1[1]*q2[2])-(q1[2]*q2[1])+(q1[3]*q2[0]); } -void quatscale( float * qout, const float * qin, float s ) +void quatscale( FLT * qout, const FLT * qin, FLT s ) { qout[0] = qin[0] * s; qout[1] = qin[1] * s; @@ -240,19 +240,19 @@ void quatscale( float * qout, const float * qin, float s ) } -float quatinnerproduct( const float * qa, const float * qb ) +FLT quatinnerproduct( const FLT * qa, const FLT * qb ) { return (qa[0]*qb[0])+(qa[1]*qb[1])+(qa[2]*qb[2])+(qa[3]*qb[3]); } -void quatouterproduct( float * outvec3, float * qa, float * qb ) +void quatouterproduct( FLT * outvec3, FLT * qa, FLT * qb ) { outvec3[0] = (qa[0]*qb[1])-(qa[1]*qb[0])-(qa[2]*qb[3])+(qa[3]*qb[2]); outvec3[1] = (qa[0]*qb[2])+(qa[1]*qb[3])-(qa[2]*qb[0])-(qa[3]*qb[1]); outvec3[2] = (qa[0]*qb[3])-(qa[1]*qb[2])+(qa[2]*qb[1])-(qa[3]*qb[0]); } -void quatevenproduct( float * q, float * qa, float * qb ) +void quatevenproduct( FLT * q, FLT * qa, FLT * qb ) { q[0] = (qa[0]*qb[0])-(qa[1]*qb[1])-(qa[2]*qb[2])-(qa[3]*qb[3]); q[1] = (qa[0]*qb[1])+(qa[1]*qb[0]); @@ -260,21 +260,21 @@ void quatevenproduct( float * q, float * qa, float * qb ) q[3] = (qa[0]*qb[3])+(qa[3]*qb[0]); } -void quatoddproduct( float * outvec3, float * qa, float * qb ) +void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb ) { outvec3[0] = (qa[2]*qb[3])-(qa[3]*qb[2]); outvec3[1] = (qa[3]*qb[1])-(qa[1]*qb[3]); outvec3[2] = (qa[1]*qb[2])-(qa[2]*qb[1]); } -void quatslerp( float * q, const float * qa, const float * qb, float t ) +void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ) { - float an[4]; - float bn[4]; + FLT an[4]; + FLT bn[4]; quatnormalize( an, qa ); quatnormalize( bn, qb ); - float cosTheta = quatinnerproduct(an,bn); - float sinTheta; + FLT cosTheta = quatinnerproduct(an,bn); + FLT sinTheta; //Careful: If cosTheta is exactly one, or even if it's infinitesimally over, it'll // cause SQRT to produce not a number, and screw everything up. @@ -283,7 +283,7 @@ void quatslerp( float * q, const float * qa, const float * qb, float t ) else sinTheta = sqrt(1 - (cosTheta*cosTheta)); - float Theta = acos(cosTheta); //Theta is half the angle between the 2 MQuaternions + FLT Theta = acos(cosTheta); //Theta is half the angle between the 2 MQuaternions if(fabs(Theta) < DEFAULT_EPSILON ) quatcopy( q, qa ); @@ -294,8 +294,8 @@ void quatslerp( float * q, const float * qa, const float * qb, float t ) } else { - float aside[4]; - float bside[4]; + FLT aside[4]; + FLT bside[4]; quatscale( bside, qb, sin( t * Theta ) ); quatscale( aside, qa, sin((1-t)*Theta) ); quatadd( q, aside, bside ); @@ -303,11 +303,11 @@ void quatslerp( float * q, const float * qa, const float * qb, float t ) } } -void quatrotatevector( float * vec3out, const float * quat, const float * vec3in ) +void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ) { - float tquat[4]; - float vquat[4]; - float qrecp[4]; + FLT tquat[4]; + FLT vquat[4]; + FLT qrecp[4]; vquat[0] = 0; vquat[1] = vec3in[0]; vquat[2] = vec3in[1]; diff --git a/tools/planetest/linmath.h b/tools/planetest/linmath.h index 344a547..881ca70 100644 --- a/tools/planetest/linmath.h +++ b/tools/planetest/linmath.h @@ -8,49 +8,49 @@ //NOTE: Inputs may never be output with cross product. -void cross3d( float * out, const float * a, const float * b ); +void cross3d( FLT * out, const FLT * a, const FLT * b ); -void sub3d( float * out, const float * a, const float * b ); +void sub3d( FLT * out, const FLT * a, const FLT * b ); -void add3d( float * out, const float * a, const float * b ); +void add3d( FLT * out, const FLT * a, const FLT * b ); -void scale3d( float * out, const float * a, float scalar ); +void scale3d( FLT * out, const FLT * a, FLT scalar ); -void normalize3d( float * out, const float * in ); +void normalize3d( FLT * out, const FLT * in ); -float dot3d( const float * a, const float * b ); +FLT dot3d( const FLT * a, const FLT * b ); //Returns 0 if equal. If either argument is null, 0 will ALWAYS be returned. -int compare3d( const float * a, const float * b, float epsilon ); +int compare3d( const FLT * a, const FLT * b, FLT epsilon ); -void copy3d( float * out, const float * in ); +void copy3d( FLT * out, const FLT * in ); //Quaternion things... -void quatsetnone( float * q ); -void quatcopy( float * qout, const float * qin ); -void quatfromeuler( float * q, const float * euler ); -void quattoeuler( float * euler, const float * q ); -void quatfromaxisangle( float * q, const float * axis, float radians ); -float quatmagnitude( const float * q ); -float quatinvsqmagnitude( const float * q ); -void quatnormalize( float * qout, const float * qin ); //Safe for in to be same as out. -void quattomatrix( float * matrix44, const float * q ); -void quatgetconjugate( float * qout, const float * qin ); -void quatgetreciprocal( float * qout, const float * qin ); -void quatsub( float * qout, const float * a, const float * b ); -void quatadd( float * qout, const float * a, const float * b ); -void quatrotateabout( float * qout, const float * a, const float * b ); //same as quat multiply, not piecewise multiply. -void quatscale( float * qout, const float * qin, float s ); -float quatinnerproduct( const float * qa, const float * qb ); -void quatouterproduct( float * outvec3, float * qa, float * qb ); -void quatevenproduct( float * q, float * qa, float * qb ); -void quatoddproduct( float * outvec3, float * qa, float * qb ); -void quatslerp( float * q, const float * qa, const float * qb, float t ); -void quatrotatevector( float * vec3out, const float * quat, const float * vec3in ); +void quatsetnone( FLT * q ); +void quatcopy( FLT * qout, const FLT * qin ); +void quatfromeuler( FLT * q, const FLT * euler ); +void quattoeuler( FLT * euler, const FLT * q ); +void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ); +FLT quatmagnitude( const FLT * q ); +FLT quatinvsqmagnitude( const FLT * q ); +void quatnormalize( FLT * qout, const FLT * qin ); //Safe for in to be same as out. +void quattomatrix( FLT * matrix44, const FLT * q ); +void quatgetconjugate( FLT * qout, const FLT * qin ); +void quatgetreciprocal( FLT * qout, const FLT * qin ); +void quatsub( FLT * qout, const FLT * a, const FLT * b ); +void quatadd( FLT * qout, const FLT * a, const FLT * b ); +void quatrotateabout( FLT * qout, const FLT * a, const FLT * b ); //same as quat multiply, not piecewise multiply. +void quatscale( FLT * qout, const FLT * qin, FLT s ); +FLT quatinnerproduct( const FLT * qa, const FLT * qb ); +void quatouterproduct( FLT * outvec3, FLT * qa, FLT * qb ); +void quatevenproduct( FLT * q, FLT * qa, FLT * qb ); +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 ); #endif -- cgit v1.2.3