From a2ba45f43ae02b1e39b1816fe9c1c70c54a7f046 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 10 Mar 2018 23:57:58 -0500 Subject: Switch from pos,quat to pose. Also change initialization order. --- Makefile | 17 +- include/libsurvive/survive.h | 2 +- include/libsurvive/survive_types.h | 2 +- redist/CNFG3D.c | 456 +++++++++++++++++++++++++++++++++++++ redist/CNFG3D.h | 66 ++++++ redist/linmath.c | 10 + simple_pose_test.c | 197 ++++++++++++++++ src/poser_turveytori.c | 6 +- src/survive.c | 40 ++-- src/survive_process.c | 3 +- src/survive_vive.c | 5 + test.c | 6 +- 12 files changed, 776 insertions(+), 34 deletions(-) create mode 100644 redist/CNFG3D.c create mode 100644 redist/CNFG3D.h create mode 100644 simple_pose_test.c diff --git a/Makefile b/Makefile index 25d510f..b4e65dd 100644 --- a/Makefile +++ b/Makefile @@ -1,4 +1,4 @@ -all : lib data_recorder test calibrate calibrate_client +all : lib data_recorder test calibrate calibrate_client simple_pose_test CC:=gcc @@ -26,7 +26,7 @@ GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGCocoaNSImageDriver.o else LDFLAGS:=$(LDFLAGS) -lX11 -lusb-1.0 -DRAWFUNCTIONS=redist/CNFGFunctions.c redist/CNFGXDriver.c +DRAWFUNCTIONS=redist/CNFGFunctions.c redist/CNFGXDriver.c redist/CNFG3D.c GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGXDriver.o endif @@ -41,9 +41,9 @@ LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_ #If you want to use HIDAPI on Linux. -CFLAGS:=$(CFLAGS) -DHIDAPI -REDISTS:=$(REDISTS) redist/hid-linux.o -LDFLAGS:=$(LDFLAGS) -ludev +#CFLAGS:=$(CFLAGS) -DHIDAPI +#REDISTS:=$(REDISTS) redist/hid-linux.o +#LDFLAGS:=$(LDFLAGS) -ludev #Useful Preprocessor Directives: # -DUSE_DOUBLE = use double instead of float for most operations. @@ -62,12 +62,15 @@ LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c) # unused: redist/crc32.c -testCocoa : testCocoa.c $(DRAWFUNCTIONS) +testCocoa : testCocoa.c $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS) test : test.c ./lib/libsurvive.so redist/os_generic.o $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS) +simple_pose_test : simple_pose_test.c ./lib/libsurvive.so redist/os_generic.o $(DRAWFUNCTIONS) + $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS) + data_recorder : data_recorder.c ./lib/libsurvive.so redist/os_generic.c $(DRAWFUNCTIONS) $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS) @@ -92,7 +95,7 @@ calibrate_tcc : $(LIBSURVIVE_C) tcc -DRUNTIME_SYMNUM $(CFLAGS) -o $@ $^ $(LDFLAGS) calibrate.c redist/os_generic.c $(DRAWFUNCTIONS) redist/symbol_enumerator.c clean : - rm -rf *.o src/*.o *~ src/*~ test data_recorder calibrate testCocoa lib/libsurvive.so redist/*.o redist/*~ + rm -rf *.o src/*.o *~ src/*~ test simple_pose_test data_recorder calibrate testCocoa lib/libsurvive.so redist/*.o redist/*~ diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index d9b5f08..f441c89 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -201,7 +201,7 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); -void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion); +void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose); ////////////////////// Survive Drivers //////////////////////////// diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index be1115b..ba0c8f1 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -49,7 +49,7 @@ typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); -typedef void(*raw_pose_func)(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion); +typedef void(*raw_pose_func)(SurviveObject * so, uint8_t lighthouse, FLT *pose); typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount); diff --git a/redist/CNFG3D.c b/redist/CNFG3D.c new file mode 100644 index 0000000..b574400 --- /dev/null +++ b/redist/CNFG3D.c @@ -0,0 +1,456 @@ +//Copyright 2012-2017 <>< Charles Lohr +//You may license this file under the MIT/x11, NewBSD, or any GPL license. +//This is a series of tools useful for software rendering. +//Use of this file with OpenGL is untested. + +#include "CNFG3D.h" +#include +#include + +#define m00 0 +#define m01 1 +#define m02 2 +#define m03 3 +#define m10 4 +#define m11 5 +#define m12 6 +#define m13 7 +#define m20 8 +#define m21 9 +#define m22 10 +#define m23 11 +#define m30 12 +#define m31 13 +#define m32 14 +#define m33 15 + + +void tdIdentity( float * f ) +{ + f[m00] = 1; f[m01] = 0; f[m02] = 0; f[m03] = 0; + f[m10] = 0; f[m11] = 1; f[m12] = 0; f[m13] = 0; + f[m20] = 0; f[m21] = 0; f[m22] = 1; f[m23] = 0; + f[m30] = 0; f[m31] = 0; f[m32] = 0; f[m33] = 1; +} + +void tdZero( float * f ) +{ + f[m00] = 0; f[m01] = 0; f[m02] = 0; f[m03] = 0; + f[m10] = 0; f[m11] = 0; f[m12] = 0; f[m13] = 0; + f[m20] = 0; f[m21] = 0; f[m22] = 0; f[m23] = 0; + f[m30] = 0; f[m31] = 0; f[m32] = 0; f[m33] = 0; +} + +void tdTranslate( float * f, float x, float y, float z ) +{ + float ftmp[16]; + tdIdentity(ftmp); + ftmp[m03] += x; + ftmp[m13] += y; + ftmp[m23] += z; + tdMultiply( f, ftmp, f ); +} + +void tdScale( float * f, float x, float y, float z ) +{ + f[m00] *= x; + f[m01] *= x; + f[m02] *= x; + f[m03] *= x; + + f[m10] *= y; + f[m11] *= y; + f[m12] *= y; + f[m13] *= y; + + f[m20] *= z; + f[m21] *= z; + f[m22] *= z; + f[m23] *= z; +} + +void tdRotateAA( float * f, float angle, float ix, float iy, float iz ) +{ + float ftmp[16]; + + float c = tdCOS( angle*tdDEGRAD ); + float s = tdSIN( angle*tdDEGRAD ); + float absin = tdSQRT( ix*ix + iy*iy + iz*iz ); + float x = ix/absin; + float y = iy/absin; + float z = iz/absin; + + ftmp[m00] = x*x*(1-c)+c; + ftmp[m01] = x*y*(1-c)-z*s; + ftmp[m02] = x*z*(1-c)+y*s; + ftmp[m03] = 0; + + ftmp[m10] = y*x*(1-c)+z*s; + ftmp[m11] = y*y*(1-c)+c; + ftmp[m12] = y*z*(1-c)-x*s; + ftmp[m13] = 0; + + ftmp[m20] = x*z*(1-c)-y*s; + ftmp[m21] = y*z*(1-c)+x*s; + ftmp[m22] = z*z*(1-c)+c; + ftmp[m23] = 0; + + ftmp[m30] = 0; + ftmp[m31] = 0; + ftmp[m32] = 0; + ftmp[m33] = 1; + + tdMultiply( f, ftmp, f ); +} + +void tdRotateEA( float * f, float x, float y, float z ) +{ + float ftmp[16]; + + //x,y,z must be negated for some reason + float X = -x*2*tdQ_PI/360; //Reduced calulation for speed + float Y = -y*2*tdQ_PI/360; + float Z = -z*2*tdQ_PI/360; + float cx = tdCOS(X); + float sx = tdSIN(X); + float cy = tdCOS(Y); + float sy = tdSIN(Y); + float cz = tdCOS(Z); + float sz = tdSIN(Z); + + //Row major + //manually transposed + ftmp[m00] = cy*cz; + ftmp[m10] = (sx*sy*cz)-(cx*sz); + ftmp[m20] = (cx*sy*cz)+(sx*sz); + ftmp[m30] = 0; + + ftmp[m01] = cy*sz; + ftmp[m11] = (sx*sy*sz)+(cx*cz); + ftmp[m21] = (cx*sy*sz)-(sx*cz); + ftmp[m31] = 0; + + ftmp[m02] = -sy; + ftmp[m12] = sx*cy; + ftmp[m22] = cx*cy; + ftmp[m32] = 0; + + ftmp[m03] = 0; + ftmp[m13] = 0; + ftmp[m23] = 0; + ftmp[m33] = 1; + + tdMultiply( f, ftmp, f ); +} + +void tdMultiply( float * fin1, float * fin2, float * fout ) +{ + float fotmp[16]; + int i, j, k; +/* + fotmp[m00] = fin1[m00] * fin2[m00] + fin1[m01] * fin2[m10] + fin1[m02] * fin2[m20] + fin1[m03] * fin2[m30]; + fotmp[m01] = fin1[m00] * fin2[m01] + fin1[m01] * fin2[m11] + fin1[m02] * fin2[m21] + fin1[m03] * fin2[m31]; + fotmp[m02] = fin1[m00] * fin2[m02] + fin1[m01] * fin2[m12] + fin1[m02] * fin2[m22] + fin1[m03] * fin2[m32]; + fotmp[m03] = fin1[m00] * fin2[m03] + fin1[m01] * fin2[m13] + fin1[m02] * fin2[m23] + fin1[m03] * fin2[m33]; + + fotmp[m10] = fin1[m10] * fin2[m00] + fin1[m11] * fin2[m10] + fin1[m12] * fin2[m20] + fin1[m13] * fin2[m30]; + fotmp[m11] = fin1[m10] * fin2[m01] + fin1[m11] * fin2[m11] + fin1[m12] * fin2[m21] + fin1[m13] * fin2[m31]; + fotmp[m12] = fin1[m10] * fin2[m02] + fin1[m11] * fin2[m12] + fin1[m12] * fin2[m22] + fin1[m13] * fin2[m32]; + fotmp[m13] = fin1[m10] * fin2[m03] + fin1[m11] * fin2[m13] + fin1[m12] * fin2[m23] + fin1[m13] * fin2[m33]; + + fotmp[m20] = fin1[m20] * fin2[m00] + fin1[m21] * fin2[m10] + fin1[m22] * fin2[m20] + fin1[m23] * fin2[m30]; + fotmp[m21] = fin1[m20] * fin2[m01] + fin1[m21] * fin2[m11] + fin1[m22] * fin2[m21] + fin1[m23] * fin2[m31]; + fotmp[m22] = fin1[m20] * fin2[m02] + fin1[m21] * fin2[m12] + fin1[m22] * fin2[m22] + fin1[m23] * fin2[m32]; + fotmp[m23] = fin1[m20] * fin2[m03] + fin1[m21] * fin2[m13] + fin1[m22] * fin2[m23] + fin1[m23] * fin2[m33]; + + fotmp[m30] = fin1[m30] * fin2[m00] + fin1[m31] * fin2[m10] + fin1[m32] * fin2[m20] + fin1[m33] * fin2[m30]; + fotmp[m31] = fin1[m30] * fin2[m01] + fin1[m31] * fin2[m11] + fin1[m32] * fin2[m21] + fin1[m33] * fin2[m31]; + fotmp[m32] = fin1[m30] * fin2[m02] + fin1[m31] * fin2[m12] + fin1[m32] * fin2[m22] + fin1[m33] * fin2[m32]; + fotmp[m33] = fin1[m30] * fin2[m03] + fin1[m31] * fin2[m13] + fin1[m32] * fin2[m23] + fin1[m33] * fin2[m33]; +*/ + + for( i = 0; i < 16; i++ ) + { + int xp = i & 0x03; + int yp = i & 0x0c; + fotmp[i] = 0; + for( k = 0; k < 4; k++ ) + { + fotmp[i] += fin1[yp+k] * fin2[(k<<2)|xp]; + } + } + + tdMATCOPY( fout, fotmp ); +} + +void tdPrint( float * f ) +{ + int i; + printf( "{\n" ); + for( i = 0; i < 16; i+=4 ) + { + printf( " %f, %f, %f, %f\n", f[0+i], f[1+i], f[2+i], f[3+i] ); + } + printf( "}\n" ); +} + +void tdTransposeSelf( float * f ) +{ + float fout[16]; + fout[m00] = f[m00]; fout[m01] = f[m10]; fout[m02] = f[m20]; fout[m03] = f[m30]; + fout[m10] = f[m01]; fout[m11] = f[m11]; fout[m12] = f[m21]; fout[m13] = f[m31]; + fout[m20] = f[m02]; fout[m21] = f[m12]; fout[m22] = f[m22]; fout[m23] = f[m32]; + fout[m30] = f[m03]; fout[m31] = f[m13]; fout[m32] = f[m23]; fout[m33] = f[m33]; + tdMATCOPY( f, fout ); +} + + +void tdPerspective( float fovy, float aspect, float zNear, float zFar, float * out ) +{ + float f = 1./tdTAN(fovy * tdQ_PI / 360.0); + out[m00] = f/aspect; out[m01] = 0; out[m02] = 0; out[m03] = 0; + out[m10] = 0; out[m11] = f; out[m12] = 0; out[m13] = 0; + out[m20] = 0; out[m21] = 0; + out[m22] = (zFar + zNear)/(zNear - zFar); + out[m23] = 2*zFar*zNear /(zNear - zFar); + out[m30] = 0; out[m31] = 0; out[m32] = -1; out[m33] = 0; +} + +void tdLookAt( float * m, float * eye, float * at, float * up ) +{ + float out[16]; + float otmp[16]; + float F[3] = { at[0] - eye[0], at[1] - eye[1], at[2] - eye[2] }; + float fdiv = 1./tdSQRT( F[0]*F[0] + F[1]*F[1] + F[2]*F[2] ); + float f[3] = { F[0]*fdiv, F[1]*fdiv, F[2]*fdiv }; + float udiv = 1./tdSQRT( up[0]*up[0] + up[1]*up[1] + up[2]*up[2] ); + float UP[3] = { up[0]*udiv, up[1]*udiv, up[2]*udiv }; + float s[3]; + float u[3]; + tdCross( f, UP, s ); + tdCross( s, f, u ); + + out[m00] = s[0]; out[m01] = s[1]; out[m02] = s[2]; out[m03] = 0; + out[m10] = u[0]; out[m11] = u[1]; out[m12] = u[2]; out[m13] = 0; + out[m20] = -f[0];out[m21] =-f[1]; out[m22] =-f[2]; out[m23] = 0; + out[m30] = 0; out[m31] = 0; out[m32] = 0; out[m33] = 1; + + tdMultiply( m, out, m ); + tdTranslate( m, -eye[0], -eye[1], -eye[2] ); +} + + + + + + + + + + + + +void tdPTransform( const float * pin, float * f, float * pout ) +{ + float ptmp[2]; + ptmp[0] = pin[0] * f[m00] + pin[1] * f[m01] + pin[2] * f[m02] + f[m03]; + ptmp[1] = pin[0] * f[m10] + pin[1] * f[m11] + pin[2] * f[m12] + f[m13]; + pout[2] = pin[0] * f[m20] + pin[1] * f[m21] + pin[2] * f[m22] + f[m23]; + pout[0] = ptmp[0]; + pout[1] = ptmp[1]; +} + +void tdVTransform( const float * pin, float * f, float * pout ) +{ + float ptmp[2]; + ptmp[0] = pin[0] * f[m00] + pin[1] * f[m01] + pin[2] * f[m02]; + ptmp[1] = pin[0] * f[m10] + pin[1] * f[m11] + pin[2] * f[m12]; + pout[2] = pin[0] * f[m20] + pin[1] * f[m21] + pin[2] * f[m22]; + pout[0] = ptmp[0]; + pout[1] = ptmp[1]; +} + +void td4Transform( float * pin, float * f, float * pout ) +{ + float ptmp[3]; + ptmp[0] = pin[0] * f[m00] + pin[1] * f[m01] + pin[2] * f[m02] + pin[3] * f[m03]; + ptmp[1] = pin[0] * f[m10] + pin[1] * f[m11] + pin[2] * f[m12] + pin[3] * f[m13]; + ptmp[2] = pin[0] * f[m20] + pin[1] * f[m21] + pin[2] * f[m22] + pin[3] * f[m23]; + pout[3] = pin[0] * f[m30] + pin[1] * f[m31] + pin[2] * f[m32] + pin[3] * f[m33]; + pout[0] = ptmp[0]; + pout[1] = ptmp[1]; + pout[2] = ptmp[2]; +} + +void td4RTransform( float * pin, float * f, float * pout ) +{ + float ptmp[3]; + ptmp[0] = pin[0] * f[m00] + pin[1] * f[m10] + pin[2] * f[m20] + pin[3] * f[m30]; + ptmp[1] = pin[0] * f[m01] + pin[1] * f[m11] + pin[2] * f[m21] + pin[3] * f[m31]; + ptmp[2] = pin[0] * f[m02] + pin[1] * f[m12] + pin[2] * f[m22] + pin[3] * f[m32]; + pout[3] = pin[0] * f[m03] + pin[1] * f[m13] + pin[2] * f[m23] + pin[3] * f[m33]; + pout[0] = ptmp[0]; + pout[1] = ptmp[1]; + pout[2] = ptmp[2]; +} + +void tdNormalizeSelf( float * vin ) +{ + float vsq = 1./tdSQRT(vin[0]*vin[0] + vin[1]*vin[1] + vin[2]*vin[2]); + vin[0] *= vsq; + vin[1] *= vsq; + vin[2] *= vsq; +} + +void tdCross( float * va, float * vb, float * vout ) +{ + float vtmp[2]; + vtmp[0] = va[1] * vb[2] - va[2] * vb[1]; + vtmp[1] = va[2] * vb[0] - va[0] * vb[2]; + vout[2] = va[0] * vb[1] - va[1] * vb[0]; + vout[0] = vtmp[0]; + vout[1] = vtmp[1]; +} + +float tdDistance( float * va, float * vb ) +{ + float dx = va[0]-vb[0]; + float dy = va[1]-vb[1]; + float dz = va[2]-vb[2]; + + return tdSQRT(dx*dx + dy*dy + dz*dz); +} + +float tdDot( float * va, float * vb ) +{ + return va[0]*vb[0] + va[1]*vb[1] + va[2]*vb[2]; +} + +//Stack functionality. +static float gsMatricies[2][tdMATRIXMAXDEPTH]; +float * gSMatrix = &gsMatricies[0][0]; +static int gsMMode; +static int gsMPlace[2]; + +void tdPush() +{ + if( gsMPlace[gsMMode] > tdMATRIXMAXDEPTH - 2 ) + return; + + tdMATCOPY( &gsMatricies[gsMMode][gsMPlace[gsMMode]], &gsMatricies[gsMMode][gsMPlace[gsMMode] + 1] ); + gsMPlace[gsMMode]++; + + gSMatrix = &gsMatricies[gsMMode][gsMPlace[gsMMode]]; +} + +void tdPop() +{ + if( gsMPlace[gsMMode] < 1 ) + return; + + gsMPlace[gsMMode]--; + + gSMatrix = &gsMatricies[gsMMode][gsMPlace[gsMMode]]; + +} + +void tdMode( int mode ) +{ + if( mode < 0 || mode > 1 ) + return; + + gsMMode = mode; + + gSMatrix = &gsMatricies[gsMMode][gsMPlace[gsMMode]]; + +} + +static float translateX; +static float translateY; +static float scaleX; +static float scaleY; + +void tdSetViewport( float leftx, float topy, float rightx, float bottomy, float pixx, float pixy ) +{ + translateX = leftx; + translateY = bottomy; + scaleX = pixx/(rightx-leftx); + scaleY = pixy/(topy-bottomy); + +} + +void tdFinalPoint( float * pin, float * pout ) +{ + float tdin[4] = { pin[0], pin[1], pin[2], 1. }; + float tmp[4]; + td4Transform( tdin, &gsMatricies[0][gsMPlace[0]], tmp ); +// printf( "XFORM1Out: %f %f %f %f\n", tmp[0], tmp[1], tmp[2], tmp[3] ); + td4Transform( tmp, &gsMatricies[1][gsMPlace[1]], tmp ); +// printf( "XFORM2Out: %f %f %f %f\n", tmp[0], tmp[1], tmp[2], tmp[3] ); + pout[0] = (tmp[0]/tmp[3] - translateX) * scaleX; + pout[1] = (tmp[1]/tmp[3] - translateY) * scaleY; + pout[2] = tmp[2]/tmp[3]; +// printf( "XFORMFOut: %f %f %f\n", pout[0], pout[1], pout[2] ); +} + + + + + + + + + + +static inline float tdNoiseAt( int x, int y ) +{ + return ((x*13241 + y * 33455927)%9293) / 9292.; +// srand( x + y * 1314); +// return ((rand()%1000)/500.) - 1.0; +} + +static inline float tdFade( float f ) +{ + float ft3 = f*f*f; + return ft3 * 10 - ft3 * f * 15 + 6 * ft3 * f * f; +} + +float tdFLerp( float a, float b, float t ) +{ + float fr = tdFade( t ); + return a * (1.-fr) + b * fr; +} + +static inline float tdFNoiseAt( float x, float y ) +{ + int ix = x; + int iy = y; + float fx = x - ix; + float fy = y - iy; + + float a = tdNoiseAt( ix, iy ); + float b = tdNoiseAt( ix+1, iy ); + float c = tdNoiseAt( ix, iy+1 ); + float d = tdNoiseAt( ix+1, iy+1 ); + + float top = tdFLerp( a, b, fx ); + float bottom = tdFLerp( c, d, fx ); + + return tdFLerp( top, bottom, fy ); +} + +float tdPerlin2D( float x, float y ) +{ + int ndepth = 5; + + int depth; + float ret = 0; + for( depth = 0; depth < ndepth; depth++ ) + { + float nx = x / (1<<(ndepth-depth-1)); + float ny = y / (1<<(ndepth-depth-1)); + ret += tdFNoiseAt( nx, ny ) / (1<<(depth+1)); + } + return ret; +} + diff --git a/redist/CNFG3D.h b/redist/CNFG3D.h new file mode 100644 index 0000000..7f74d96 --- /dev/null +++ b/redist/CNFG3D.h @@ -0,0 +1,66 @@ +//Copyright 2012-2017 <>< Charles Lohr +//You may license this file under the MIT/x11, NewBSD, or any GPL license. + +//This is a series of tools useful for software rendering. +//Use of this file with OpenGL is untested. + +#ifndef _3D_H +#define _3D_H + +#include +#define tdCOS cosf +#define tdSIN sinf +#define tdTAN tanf +#define tdSQRT sqrtf +#define tdMATCOPY(x,y) memcpy( x, y, 16*sizeof(float)) +#define tdQ_PI 3.141592653589 +#define tdDEGRAD (tdQ_PI/180.) +#define tdRADDEG (180./tdQ_PI) + + +//General Matrix Functions +void tdIdentity( float * f ); +void tdZero( float * f ); +void tdTranslate( float * f, float x, float y, float z ); //Operates ON f +void tdScale( float * f, float x, float y, float z ); //Operates ON f +void tdRotateAA( float * f, float angle, float x, float y, float z ); //Operates ON f +void tdRotateEA( float * f, float x, float y, float z ); //Operates ON f +void tdMultiply( float * fin1, float * fin2, float * fout ); //Operates ON f +void tdPrint( float * f ); +void tdTransposeSelf( float * f ); + +//Specialty Matrix Functions +void tdPerspective( float fovy, float aspect, float zNear, float zFar, float * out ); //Sets, NOT OPERATES. (FOVX=degrees) +void tdLookAt( float * m, float * eye, float * at, float * up ); //Operates ON f + +//General point functions +#define tdPSet( f, x, y, z ) { f[0] = x; f[1] = y; f[2] = z; } +void tdPTransform( const float * pin, float * f, float * pout ); +void tdVTransform( const float * vin, float * f, float * vout ); +void td4Transform( float * kin, float * f, float * kout ); +void td4RTransform( float * kin, float * f, float * kout ); +void tdNormalizeSelf( float * vin ); +void tdCross( float * va, float * vb, float * vout ); +float tdDistance( float * va, float * vb ); +float tdDot( float * va, float * vb ); +#define tdPSub( x, y, z ) { (z)[0] = (x)[0] - (y)[0]; (z)[1] = (x)[1] - (y)[1]; (z)[2] = (x)[2] - (y)[2]; } +#define tdPAdd( x, y, z ) { (z)[0] = (x)[0] + (y)[0]; (z)[1] = (x)[1] + (y)[1]; (z)[2] = (x)[2] + (y)[2]; } + +//Stack Functionality +#define tdMATRIXMAXDEPTH 32 +extern float * gSMatrix; +void tdPush(); +void tdPop(); +void tdMode( int mode ); +#define tdMODELVIEW 0 +#define tdPROJECTION 1 + +//Final stage tools +void tdSetViewport( float leftx, float topy, float rightx, float bottomy, float pixx, float pixy ); +void tdFinalPoint( float * pin, float * pout ); + + +float tdFLerp( float a, float b, float t ); +float tdPerlin2D( float x, float y ); + +#endif diff --git a/redist/linmath.c b/redist/linmath.c index 1d70ee1..4240ed6 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -490,6 +490,8 @@ void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ) vquat[2] = vec3in[1]; vquat[3] = vec3in[2]; + //XXX WARNING: This code is probably SLOW. See this: https://github.com/axlecrusher/hgengine3/blob/master/Mercury3/basic_light1_v.glsl + quatrotateabout( tquat, quat, vquat ); quatgetconjugate( qrecp, quat ); quatrotateabout( vquat, tquat, qrecp ); @@ -637,3 +639,11 @@ void matrix44transpose(FLT * mout, const FLT * minm ) } +void ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose ) +{ + FLT v3o[3]; + quatrotatevector( v3o, &pose[3], pin ); + for(int i = 0; i < 3; i++) + pout[i] = pose[i] + v3o[i]; +} + diff --git a/simple_pose_test.c b/simple_pose_test.c new file mode 100644 index 0000000..4b392b9 --- /dev/null +++ b/simple_pose_test.c @@ -0,0 +1,197 @@ +#ifdef __linux__ +#include +#endif +#include +#include +#include +#include +#include "src/survive_cal.h" +#include +#include +#include +#include +#include +struct SurviveContext * ctx; + +void HandleKey( int keycode, int bDown ) +{ + if( !bDown ) return; + + if( keycode == 'O' || keycode == 'o' ) + { + survive_send_magic(ctx,1,0,0); + } + if( keycode == 'F' || keycode == 'f' ) + { + survive_send_magic(ctx,0,0,0); + } +} + +void HandleButton( int x, int y, int button, int bDown ) +{ +} + +void HandleMotion( int x, int y, int mask ) +{ +} + +void HandleDestroy() +{ +} + +FLT hpos[3]; +FLT hpos2[3]; + +void testprog_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose) +{ + survive_default_raw_pose_process(so, lighthouse, pose ); + + if( lighthouse != 0 || strcmp( so->codename, "HMD" ) != 0 ) + return; + + // print the pose; +/* double qw = quat[0]; + double qx = quat[1]; + double qy = quat[2]; + double qz = quat[3]; + + hra = 2 * acos(qw); + hrx = qx / sqrt(1-qw*qw); + hry = qy / sqrt(1-qw*qw); + hrz = qz / sqrt(1-qw*qw); + hra *= 180/3.14159; + + hx = pos[0]; + hy = pos[1]; + hz = pos[2];*/ + + printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [ang:%08.2f %08.2f %08.2f %08.2f]\n", lighthouse, so->codename, pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6] ); + + hpos[0] = pose[0]; + hpos[1] = pose[1]; + hpos[2] = pose[2]; + FLT hposin[3] = { 0, 0, 1 }; + ApplyPoseToPoint( hpos2, hposin, pose ); + + fflush(stdout); +} + + +void testprog_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ) +{ +// printf( "%d: %d %d\n", sensor_id, acode, timecode ); +} + +void DrawLineSegment( float x, float y, float z, float x2, float y2, float z2 ) +{ + //float pti[6] = { 1, sin(TimeSinceStart), cos(TimeSinceStart ), 1, 0, 0}; + float pa[3] = { x, y, z }; + float pb[3] = { x2, y2, z2 }; + + tdFinalPoint( pa, pa ); + tdFinalPoint( pb, pb ); + + if( pa[2] >= 1.0 ) return; + if( pb[2] >= 1.0 ) return; + if( pa[2] < 0 ) return; + if( pb[2] < 0 ) return; + + int dx, dy; + for( dx = 0; dx < 2; dx++ ) + for( dy = 0; dy < 2; dy++ ) + { + CNFGTackPixel( pa[0]+dx, pa[1]+dy ); + CNFGTackSegment( pa[0]+dx, pa[1]+dy, pb[0]+dx, pb[1]+dy ); + } +} + + +void *GUIThread(void*v) +{ + CNFGSetup( "Orientation test", 640, 480 ); + double fLast = 0; + short screenx, screeny; + double TimeSinceStart = 0; + while(1) + { + double Now = OGGetAbsoluteTime(); + double dt = Now - fLast; + fLast = Now; + TimeSinceStart += dt; + + CNFGClearFrame(); + CNFGHandleInput(); + + CNFGColor( 0xFFFFFF ); + CNFGGetDimensions( &screenx, &screeny ); + + int x, y; + float eye[3] = { 8, 8, 8}; + float at[3] = { 0,0, 0 }; + float up[3] = { 0,0, 1 }; + + tdSetViewport( -1, -1, 1, 1, screenx, screeny ); + + tdMode( tdPROJECTION ); + tdIdentity( gSMatrix ); + tdPerspective( 40, ((float)screenx)/((float)screeny), .1, 200., gSMatrix ); + + tdMode( tdMODELVIEW ); + tdIdentity( gSMatrix ); + tdLookAt( gSMatrix, eye, at, up ); + + CNFGColor( 0x00ffff ); DrawLineSegment( hpos[0], hpos[1], hpos[2], hpos2[0], hpos2[1], hpos2[2] ); + CNFGColor( 0x0000ff ); DrawLineSegment( 0, 0, 0, 1, 0, 0 ); + CNFGColor( 0xff0000 ); DrawLineSegment( 0, 0, 0, 0, 1, 0 ); + CNFGColor( 0x00ff00 ); DrawLineSegment( 0, 0, 0, 0, 0, 1 ); + CNFGColor( 0xffffff ); DrawLineSegment( 0, 0, 0, 0, 0, 0 ); + CNFGSwapBuffers(); + OGUSleep(10000); + } +} + + +int main() +{ + int magicon = 0; + double Start = OGGetAbsoluteTime(); + + ctx = survive_init( 0 ); + + //survive_install_button_fn(ctx, testprog_button_process); + survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); + //survive_install_imu_fn(ctx, testprog_imu_process); + survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); + //survive_install_angle_fn(ctx, testprog_angle_process ); + + ctx->bsd[0].PositionSet = ctx->bsd[1].PositionSet = 1; + int i; + for( i = 0; i < 2; i++ ) + { + SurvivePose * p = &ctx->bsd[i].Pose; + p->Pos[0] = 0; + p->Pos[1] = 0; + p->Pos[2] = 0; + p->Rot[0] = 1; + p->Rot[1] = 0; + p->Rot[2] = 0; + p->Rot[3] = 0; + } + + OGCreateThread( GUIThread, 0 ); + + if( !ctx ) + { + fprintf( stderr, "Fatal. Could not start\n" ); + return 1; + } + + //survive_cal_install(ctx); + + + while(survive_poll(ctx) == 0) + { + //Do Stuff + } +} + diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 94d572e..4398b66 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1616,7 +1616,7 @@ static void QuickPose(SurviveObject *so, int lh) if (sensorCount > 4) { - FLT pos[3], quat[4]; + FLT pose[7]; // TODO: This countdown stuff is a total hack! // it basically ignores all the logic to find the most reliable data points @@ -1637,12 +1637,12 @@ static void QuickPose(SurviveObject *so, int lh) - SolveForLighthouse(pos, quat, to, so, 0, lh, 0); + SolveForLighthouse(&pose[0], &pose[3], to, so, 0, lh, 0); //printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); if (so->ctx->rawposeproc) { - so->ctx->rawposeproc(so, lh, pos, quat); + so->ctx->rawposeproc(so, lh, pose); } if (ttDebug) printf("!\n"); diff --git a/src/survive.c b/src/survive.c index 76bf8e4..21e1f0e 100755 --- a/src/survive.c +++ b/src/survive.c @@ -138,7 +138,6 @@ SurviveContext * survive_init_internal( int headless ) init_config_group(&ctx->lh_config[1],10); config_read(ctx, "config.json"); - ctx->activeLighthouses = config_read_uint32(ctx->global_config_values, "LighthouseCount", 2); config_read_lighthouse(ctx->lh_config, &(ctx->bsd[0]), 0); config_read_lighthouse(ctx->lh_config, &(ctx->bsd[1]), 1); @@ -150,16 +149,19 @@ SurviveContext * survive_init_internal( int headless ) ctx->imuproc = survive_default_imu_process; ctx->angleproc = survive_default_angle_process; - const char * DriverName; - while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) ) - { - DeviceDriver dd = GetDriver( DriverName ); - printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i ); - r = dd( ctx ); - printf( "Driver %s reports status %d\n", DriverName, r ); - } + + // initialize the button queue + memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue)); + ctx->buttonQueue.buttonservicesem = OGCreateSema(); + + // start the thread to process button data + ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); + survive_install_button_fn(ctx, NULL); + survive_install_raw_pose_fn(ctx, NULL); i = 0; + const char * DriverName; + //const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserDummy"); const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserTurveyTori"); PoserCB PreferredPoserCB = 0; @@ -179,6 +181,17 @@ SurviveContext * survive_init_internal( int headless ) SV_ERROR( "Error. Cannot find any valid poser." ); } + i = 0; + while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) ) + { + DeviceDriver dd = GetDriver( DriverName ); + printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i ); + r = dd( ctx ); + printf( "Driver %s reports status %d\n", DriverName, r ); + } +printf( "REGISTERING DRIVERS\n" ); + + //Apply poser to objects. for( i = 0; i < ctx->objs_ct; i++ ) { ctx->objs[i]->PoserFn = PreferredPoserCB; @@ -187,15 +200,6 @@ SurviveContext * survive_init_internal( int headless ) // saving the config extra to make sure that the user has a config file they can change. config_save(ctx, "config.json"); - // initialize the button queue - memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue)); - - ctx->buttonQueue.buttonservicesem = OGCreateSema(); - - // start the thread to process button data - ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); - survive_install_button_fn(ctx, NULL); - survive_install_raw_pose_fn(ctx, NULL); return ctx; } diff --git a/src/survive_process.c b/src/survive_process.c index 0f19007..1ea3061 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -59,6 +59,7 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode .angle = angle, .lh = lh, }; + so->PoserFn( so, (PoserData *)&l ); } } @@ -101,7 +102,7 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8 //} } -void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat) +void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose) { // print the pose; //printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); diff --git a/src/survive_vive.c b/src/survive_vive.c index b3d990a..5001efd 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1655,6 +1655,11 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic ); printf( "Loading config: %d\n", len ); + if( len < 0 ) + { + return len; + } + { char raw_fname[100]; sprintf( raw_fname, "%s_config.json", so->codename ); diff --git a/test.c b/test.c index 5fc62af..ef7ae45 100644 --- a/test.c +++ b/test.c @@ -84,12 +84,12 @@ void testprog_button_process(SurviveObject * so, uint8_t eventType, uint8_t butt } } -void testprog_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat) +void testprog_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pose) { - survive_default_raw_pose_process(so, lighthouse, pos, quat); + survive_default_raw_pose_process(so, lighthouse, pose); // print the pose; - printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6]); } void testprog_imu_process(SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id) -- cgit v1.2.3