From 41b2091f9be605b39ce33f3ce8bb47ab1aeabcc7 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 15 Dec 2016 22:57:51 -0500 Subject: Remove old planetest --- tools/planetest/Makefile | 10 -- tools/planetest/camfind.c | 385 ---------------------------------------------- tools/planetest/linmath.c | 326 --------------------------------------- tools/planetest/linmath.h | 59 ------- 4 files changed, 780 deletions(-) delete mode 100644 tools/planetest/Makefile delete mode 100644 tools/planetest/camfind.c delete mode 100644 tools/planetest/linmath.c delete mode 100644 tools/planetest/linmath.h (limited to 'tools') diff --git a/tools/planetest/Makefile b/tools/planetest/Makefile deleted file mode 100644 index 37f02d7..0000000 --- a/tools/planetest/Makefile +++ /dev/null @@ -1,10 +0,0 @@ -all : camfind - -CFLAGS:=-g -O4 -DFLT=float -ffast-math -LDFLAGS:=$(CFLAGS) -lm - -camfind : camfind.o linmath.o - gcc -o $@ $^ $(LDFLAGS) - -clean : - rm -rf *.o *~ camfind diff --git a/tools/planetest/camfind.c b/tools/planetest/camfind.c deleted file mode 100644 index 1e9b2b9..0000000 --- a/tools/planetest/camfind.c +++ /dev/null @@ -1,385 +0,0 @@ -#include -#include -#include "linmath.h" -#include -#include - -#define PTS 32 -#define MAX_CHECKS 20000 - -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() -FLT LighthousePos[3] = { 0, 0, 0 }; -FLT LighthouseQuat[4] = { 1, 0, 0, 0 }; - -//Actual values -FLT xyzypr[6] = { 0, 2, 2, 0, 0, 0 }; - - -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; - - FLT dx, dy, dz; - - FLT bestxyzypr[6]; - memcpy( bestxyzypr, xyzypr, sizeof( xyzypr ) ); - - FLT fullrange = 5; //Maximum search space for positions. (Relative to HMD) - for( cycle = 0; cycle < 20; cycle ++ ) - { - - //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 ) - { -#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; - FLT ft; - int reopt = 0; - - //Try refining the search for the best orientation several times. - for( reopt = 0; reopt < 4; reopt++ ) - { - //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 ) ); } - } - //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() -{ - FLT xyzyprchange[6]; - memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) ); - quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); - memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - FLT ft = RunTest(1); - printf( "Final RMS: %f\n", ft ); -} - - -FLT RunOpti( int axis ) -{ - FLT xyzyprchange[6]; - memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) ); - int i; - FLT minv = 1e20; - FLT fv = -3.3; - FLT bv = 0; - - - //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 ) ); - FLT ft = RunTest(0); - //printf( " %d / %f = %f\n", ft, fv ); - if( ft < minv ) { minv = ft; bv = fv; } - fv += 0.1; - } - - xyzyprchange[axis] = bv; -#if 1 - //Now, do a more precise binary-ish search. - FLT jumpamount = 0.15; - for( i = 0; i < 20; i++ ) - { - 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 ) ); - FLT ft = RunTest(0); - if( ft < minv ) { minv = ft; bv = fv; } - - fv = xyzyprchange[axis] = orig + jumpamount; - quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); - memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - ft = RunTest(0); - if( ft < minv ) { minv = ft; bv = fv; } - - fv = xyzyprchange[axis] = orig - jumpamount; - quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); - memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - ft = RunTest(0); - if( ft < minv ) { minv = ft; bv = fv; } - - xyzyprchange[axis] = bv; - - //Tighten up search area. Doing by 0.5 can cause unusual instabilities. - jumpamount *= 0.6; - } -#endif - xyzypr[axis] = bv; - return minv; -} - - -FLT RunTest( int print ) -{ - int k; - 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; - FLT angle = hmd_point_angles[k]; - if( print ) printf( "%d %d : angle: %f / ", axis, pt, angle ); - - //XXX TODO: This is critical. We need to properly define the planes. - FLT plane_normal[3] = { 0, 0, 0 }; - if( axis == 0 ) - { - 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. - - //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); -} - - -int LoadData( char Camera ) -{ - int calpts[MAX_CHECKS*4]; //X (0) or Y (1), ID, offset - int calptscount; - - FILE * f = fopen( "correct_hmd_points.csv", "r" ); - int pt = 0; - if( !f ) { fprintf( stderr, "error: can't open hmd points.\n" ); return -5; } - while(!feof(f) && !ferror(f) && pt < PTS) - { - float fa, fb, fc; - int r = fscanf( f,"%g,%g,%g\n", &fa, &fb, &fc ); - hmd_points[pt*3+0] = fa; - hmd_points[pt*3+1] = fb; - hmd_points[pt*3+2] = fc; - pt++; - if( r != 3 ) - { - fprintf( stderr, "Not enough entries on line %d\n", pt ); - return -8; - } - } - if( pt < PTS ) - { - fprintf( stderr, "Not enough points.\n" ); - return -9; - } - fclose( f ); - 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; } - while( !feof(f) && !ferror(f) ) - { - char * lineptr; - size_t n; - lineptr = 0; - n = 0; - ssize_t r = getline( &lineptr, &n, f ); - char lf[10]; - char xory[10]; - char dev[10]; - int timestamp; - int sensorid; - int offset; - int code; - int length; - int rk = sscanf( lineptr, "%9s %9s %9s %d %d %d %d %d\n", lf, xory, dev, ×tamp, &sensorid, &code, &offset, &length ); - if( lf[0] == Camera && rk == 8 ) - { - //calpts - if( xory[0] == 'X' ) - { - calpts[xck*3+0] = 0; - } - else if( xory[0] == 'Y' ) - { - calpts[xck*3+0] = 1; - } - else - { - printf( "Confusing line\n" ); - continue; - } - - calpts[xck*3+1] = sensorid; - calpts[xck*3+2] = offset; - xck++; - } - if( lineptr ) free( lineptr ); - } - printf( "Cal points: %d\n", xck ); - calptscount = xck; - - int i; - for( i = 0; i < calptscount; i++ ) - { - int isy = calpts[i*3+0]; - int pt = calpts[i*3+1]; - int ofs = calpts[i*3+2]; - hmd_point_counts[pt*2+isy]++; - hmd_point_angles[pt*2+isy]+=ofs; - } - for( i = 0; i < 32; i++ ) - { - if( hmd_point_counts[i*2+0] < 100 ) hmd_point_counts[i*2+0] = 0; - if( hmd_point_counts[i*2+1] < 100 ) hmd_point_counts[i*2+1] = 0; - - 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 deleted file mode 100644 index 88643b4..0000000 --- a/tools/planetest/linmath.c +++ /dev/null @@ -1,326 +0,0 @@ -//Copyright 2013 <>< C. N. Lohr. This file licensed under the terms of the MIT license. - -#include "linmath.h" -#include - -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( 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( 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( FLT * out, const FLT * a, FLT scalar ) -{ - out[0] = a[0] * scalar; - out[1] = a[1] * scalar; - out[2] = a[2] * scalar; -} - -void normalize3d( FLT * out, const FLT * in ) -{ - 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; -} - -FLT dot3d( const FLT * a, const FLT * b ) -{ - return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; -} - -int compare3d( const FLT * a, const FLT * b, FLT epsilon ) -{ - if( !a || !b ) return 0; - if( a[2] - b[2] > epsilon ) return 1; - if( b[2] - a[2] > epsilon ) return -1; - if( a[1] - b[1] > epsilon ) return 1; - if( b[1] - a[1] > epsilon ) return -1; - if( a[0] - b[0] > epsilon ) return 1; - if( b[0] - a[0] > epsilon ) return -1; - return 0; -} - -void copy3d( FLT * out, const FLT * in ) -{ - out[0] = in[0]; - out[1] = in[1]; - out[2] = in[2]; -} - - - -/////////////////////////////////////QUATERNIONS////////////////////////////////////////// -//Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) -//Under the mit/X11 license. - - - - -void quatsetnone( FLT * q ) -{ - q[0] = 0; q[1] = 0; q[2] = 0; q[3] = 1; -} - -void quatcopy( FLT * qout, const FLT * qin ) -{ - qout[0] = qin[0]; - qout[1] = qin[1]; - qout[2] = qin[2]; - qout[3] = qin[3]; -} - -void quatfromeuler( FLT * q, const FLT * euler ) -{ - FLT X = euler[0]/2.0f; //roll - 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); - - //Correct according to - //http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles - q[0] = cx*cy*cz+sx*sy*sz;//q1 - q[1] = sx*cy*cz-cx*sy*sz;//q2 - q[2] = cx*sy*cz+sx*cy*sz;//q3 - q[3] = cx*cy*sz-sx*sy*cz;//q4 - quatnormalize( q, 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] ) ); - 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] ) ); -} - -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); - q[1] = sn * v[0]; - q[2] = sn * v[1]; - q[3] = sn * v[2]; - - quatnormalize( q, q ); -} - -FLT quatmagnitude( const FLT * q ) -{ - return 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])); -} - - -void quatnormalize( FLT * qout, const FLT * qin ) -{ - FLT imag = quatinvsqmagnitude( qin ); - quatscale( qout, qin, imag ); -} - -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]; - - //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 ) -{ - qout[0] = qin[0]; - qout[1] = -qin[1]; - qout[2] = -qin[2]; - qout[3] = -qin[3]; -} - -void quatgetreciprocal( FLT * qout, const FLT * qin ) -{ - FLT m = quatinvsqmagnitude(qin); - quatgetconjugate( qout, qin ); - quatscale( qout, qout, m ); -} - -void quatsub( FLT * qout, const FLT * a, const FLT * b ) -{ - qout[0] = a[0] - b[0]; - qout[1] = a[1] - b[1]; - qout[2] = a[2] - b[2]; - qout[3] = a[3] - b[3]; -} - -void quatadd( FLT * qout, const FLT * a, const FLT * b ) -{ - qout[0] = a[0] + b[0]; - qout[1] = a[1] + b[1]; - qout[2] = a[2] + b[2]; - qout[3] = a[3] + b[3]; -} - -void quatrotateabout( FLT * qout, const FLT * a, const FLT * b ) -{ - FLT q1[4]; - FLT q2[4]; - - quatnormalize( q1, a ); - quatnormalize( q2, b ); - - qout[0] = (q1[0]*q2[0])-(q1[1]*q2[1])-(q1[2]*q2[2])-(q1[3]*q2[3]); - qout[1] = (q1[0]*q2[1])+(q1[1]*q2[0])+(q1[2]*q2[3])-(q1[3]*q2[2]); - qout[2] = (q1[0]*q2[2])-(q1[1]*q2[3])+(q1[2]*q2[0])+(q1[3]*q2[1]); - qout[3] = (q1[0]*q2[3])+(q1[1]*q2[2])-(q1[2]*q2[1])+(q1[3]*q2[0]); -} - -void quatscale( FLT * qout, const FLT * qin, FLT s ) -{ - qout[0] = qin[0] * s; - qout[1] = qin[1] * s; - qout[2] = qin[2] * s; - qout[3] = qin[3] * s; -} - - -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( 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( 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]); - q[2] = (qa[0]*qb[2])+(qa[2]*qb[0]); - q[3] = (qa[0]*qb[3])+(qa[3]*qb[0]); -} - -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( FLT * q, const FLT * qa, const FLT * qb, FLT t ) -{ - FLT an[4]; - FLT bn[4]; - quatnormalize( an, qa ); - quatnormalize( bn, qb ); - 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. - if ( 1 - (cosTheta*cosTheta) <= 0 ) - sinTheta = 0; - else - sinTheta = sqrt(1 - (cosTheta*cosTheta)); - - FLT Theta = acos(cosTheta); //Theta is half the angle between the 2 MQuaternions - - if(fabs(Theta) < DEFAULT_EPSILON ) - quatcopy( q, qa ); - else if(fabs(sinTheta) < DEFAULT_EPSILON ) - { - quatadd( q, qa, qb ); - quatscale( q, q, 0.5 ); - } - else - { - FLT aside[4]; - FLT bside[4]; - quatscale( bside, qb, sin( t * Theta ) ); - quatscale( aside, qa, sin((1-t)*Theta) ); - quatadd( q, aside, bside ); - quatscale( q, q, 1./sinTheta ); - } -} - -void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ) -{ - FLT tquat[4]; - FLT vquat[4]; - FLT qrecp[4]; - vquat[0] = 0; - vquat[1] = vec3in[0]; - vquat[2] = vec3in[1]; - vquat[3] = vec3in[2]; - - quatrotateabout( tquat, quat, vquat ); - quatgetreciprocal( qrecp, quat ); - quatrotateabout( vquat, tquat, qrecp ); - - vec3out[0] = vquat[1]; - vec3out[1] = vquat[2]; - vec3out[2] = vquat[3]; -} - - - diff --git a/tools/planetest/linmath.h b/tools/planetest/linmath.h deleted file mode 100644 index 881ca70..0000000 --- a/tools/planetest/linmath.h +++ /dev/null @@ -1,59 +0,0 @@ -//Copyright 2013 <>< C. N. Lohr. This file licensed under the terms of the MIT license. - -#ifndef _LINMATH_H -#define _LINMATH_H - -//Yes, I know it's kind of arbitrary. -#define DEFAULT_EPSILON 0.001 - - -//NOTE: Inputs may never be output with cross product. -void cross3d( FLT * out, const FLT * a, const FLT * b ); - -void sub3d( FLT * out, const FLT * a, const FLT * b ); - -void add3d( FLT * out, const FLT * a, const FLT * b ); - -void scale3d( FLT * out, const FLT * a, FLT scalar ); - -void normalize3d( FLT * out, const FLT * in ); - -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 FLT * a, const FLT * b, FLT epsilon ); - -void copy3d( FLT * out, const FLT * in ); - - - - -//Quaternion things... - -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