From bf89e4cfa7191c3ea9bb3eeee7005db250001d55 Mon Sep 17 00:00:00 2001 From: ultramn Date: Sat, 4 Feb 2017 20:00:07 -0800 Subject: Added plot_lighthouse in dave folder --- dave/plot_lighthouse/.DS_Store | Bin 0 -> 6148 bytes dave/plot_lighthouse/Makefile | 15 ++ dave/plot_lighthouse/README.txt | 20 +++ dave/plot_lighthouse/fileutil.c | 124 ++++++++++++++ dave/plot_lighthouse/fileutil.h | 34 ++++ dave/plot_lighthouse/glutil.c | 75 +++++++++ dave/plot_lighthouse/glutil.h | 34 ++++ dave/plot_lighthouse/main.c | 336 ++++++++++++++++++++++++++++++++++++++ dave/plot_lighthouse/quaternion.h | 138 ++++++++++++++++ 9 files changed, 776 insertions(+) create mode 100644 dave/plot_lighthouse/.DS_Store create mode 100644 dave/plot_lighthouse/Makefile create mode 100644 dave/plot_lighthouse/README.txt create mode 100644 dave/plot_lighthouse/fileutil.c create mode 100644 dave/plot_lighthouse/fileutil.h create mode 100644 dave/plot_lighthouse/glutil.c create mode 100644 dave/plot_lighthouse/glutil.h create mode 100755 dave/plot_lighthouse/main.c create mode 100755 dave/plot_lighthouse/quaternion.h (limited to 'dave') diff --git a/dave/plot_lighthouse/.DS_Store b/dave/plot_lighthouse/.DS_Store new file mode 100644 index 0000000..493ea1e Binary files /dev/null and b/dave/plot_lighthouse/.DS_Store differ diff --git a/dave/plot_lighthouse/Makefile b/dave/plot_lighthouse/Makefile new file mode 100644 index 0000000..d156bdb --- /dev/null +++ b/dave/plot_lighthouse/Makefile @@ -0,0 +1,15 @@ +UNAME := $(shell uname) + +ifeq ($(UNAME), Linux) +CFLAGS:= -lGL -lGLU -lglut +endif + +# Darwin is Mac OSX !! +ifeq ($(UNAME), Darwin) +CFLAGS:= -w -framework OpenGL -framework GLUT +endif + +all: + gcc -O3 -o plot_lighthouse main.c glutil.c fileutil.c ../../redist/os_generic.c $(CFLAGS) +clean: + rm -f plot_lighthouse diff --git a/dave/plot_lighthouse/README.txt b/dave/plot_lighthouse/README.txt new file mode 100644 index 0000000..4e65688 --- /dev/null +++ b/dave/plot_lighthouse/README.txt @@ -0,0 +1,20 @@ +Plots the lighthouse positions, and the hmd bearing angles from stdin using OpenGL + +---- +- Step 1: +---- + +You must run lighthousefind first + +lighthousefind L processed_data.txt > L.txt +lighthousefind R processed_data.txt > R.txt + +you must move L.txt and R.txt into the same directory as this program + +---- +- Step 2 +---- + +Then you need to run processrawcap and pipe the output into plot_lighthouse + +../../tools/process_rawcap/process_to_points [raw data.csv] | ./plot_lighthouse diff --git a/dave/plot_lighthouse/fileutil.c b/dave/plot_lighthouse/fileutil.c new file mode 100644 index 0000000..f892798 --- /dev/null +++ b/dave/plot_lighthouse/fileutil.c @@ -0,0 +1,124 @@ +#include "fileutil.h" +#include +#include + +#define PI 3.14159265358979323846264 + + +og_mutex_t read_mutex; +og_thread_t read_thread; +double read_hmdAngles[NUM_SWEEP][NUM_HMD]; +int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD]; +int read_frameno=0; + +static FILE *fopen_orDie(const char *path, const char *flag) +{ + FILE *f = fopen(path, flag); + if (f == NULL) { + printf("ERROR could not oepn %s for %s\n", path, flag); + exit(1); + } + return f; +} + +static void SeekToken(FILE *f, const char *keyword) +{ + char token[4096]; + do { + fscanf(f, "%s", token); + } while( strcmp(token,keyword)!=0 && !feof(f) ); +} + +void LoadLighthousePos( + const char *path, + float *x, float *y, float *z, + float *qi, float *qj, float *qk, float *qreal) +{ + FILE *f = fopen_orDie(path,"r"); + SeekToken(f, "POS:"); + fscanf(f, "%f %f %f\n", x, y, z); + SeekToken(f, "QUAT:"); + fscanf(f, "%f %f %f %f\n", qreal, qi, qj, qk); + fclose (f); +} + +void LoadHmdProcessedDataAngles( + const char *path, + double angles[NUM_SWEEP][NUM_HMD]) +{ + int i,j; + char type[256]; + char sweep[256]; + int id; + int nSweep; + double ang; + double d1,d2,d3; // revisit these numbers later + + // Initialize all of the angles to -9999 + for (i=0; iNUM_HMD) { continue; } + + OGLockMutex (read_mutex); + read_hmdAngles[sweepId][id] = angle; + OGUnlockMutex(read_mutex); + read_hmdAngleViewed[sweepId][id] = read_frameno; + } +} + + diff --git a/dave/plot_lighthouse/fileutil.h b/dave/plot_lighthouse/fileutil.h new file mode 100644 index 0000000..714fbbd --- /dev/null +++ b/dave/plot_lighthouse/fileutil.h @@ -0,0 +1,34 @@ +#ifndef _fileutil_h_ +#define _fileutil_h_ + +#include +#include "../../redist/os_generic.h" + +void LoadLighthousePos( + const char *path, + float *x, float *y, float *z, + float *qi, float *qj, float *qk, float *qreal); + + +#define NUM_HMD 32 +#define NUM_SWEEP 4 +#define SWEEP_LX 0 +#define SWEEP_LY 1 +#define SWEEP_RX 2 +#define SWEEP_RY 3 +void LoadHmdProcessedDataAngles( + const char *path, + double angle[NUM_SWEEP][NUM_HMD]); + + +extern og_mutex_t read_mutex; +extern og_thread_t read_thread; +extern double read_hmdAngles[NUM_SWEEP][NUM_HMD]; +extern int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD]; +extern int read_frameno; +void *ThreadReadHmtAngles(void *junk); + + +#endif // __fileutil_h_ + + diff --git a/dave/plot_lighthouse/glutil.c b/dave/plot_lighthouse/glutil.c new file mode 100644 index 0000000..dd022a0 --- /dev/null +++ b/dave/plot_lighthouse/glutil.c @@ -0,0 +1,75 @@ +// +// glutil.c +// +// +// Created by user on 2/4/17. +// +// + +#include "glutil.h" + +void DrawGrid( + float minX, float maxX, + float minY, float maxY, + float minZ, float maxZ, + float stepX, float stepY, float stepZ) +{ + float x,y,z; + + glBegin(GL_LINES); + + // X grid stripes + for (y=minY; y +#include "quaternion.h" + +#include +#ifdef __APPLE__ +#include // The GL Header File +#include // The GL Utility Toolkit (Glut) Header +#else +#include +#include +#endif + +void DrawGrid( + float minX, float maxX, + float minY, float maxY, + float minZ, float maxZ, + float stepX, float stepY, float stepZ); + +void DrawCoordinateSystem( + float x, float y, float z, + float qx, float qy, float qz, float qr); + +#endif /* glutil_h */ diff --git a/dave/plot_lighthouse/main.c b/dave/plot_lighthouse/main.c new file mode 100755 index 0000000..c2fd97a --- /dev/null +++ b/dave/plot_lighthouse/main.c @@ -0,0 +1,336 @@ +/* + When creating your project, uncheck OWL, + uncheck Class Library, select Static + instead of Dynamic and change the target + model to Console from GUI. + Also link glut.lib to your project once its done. + */ + +#include // Standard Header For Most Programs +#include +#include +#include +#include "glutil.h" +#include "fileutil.h" + +// Required to set up a window +#define WIDTH 800 +#define HEIGHT 600 +#define FULLSCREEN 0 +int keys[256]; // Regular keyboard keys +int sp_keys[256]; // Special keyboard keycodes (GLUT specific) + +#define PI 3.1415926535897932386264 +#define MOVESPEED 1.0 +#define ROTSPEED 5.0 + +#define GRID_SIZE 6 +#define GRID_MAJOR_STEP 3 +#define GRID_MINOR_STEP 1 + +// Lighthouses +typedef struct Lhouse { + float x,y,z,qi,qj,qk,qreal; // position x,y,z and quaternion +} Lhouse; +Lhouse houseL, houseR; + +// HmdSensor array +double hmdAngles[NUM_SWEEP][NUM_HMD]; + + +static void HmdDir(double LX, double LY, double *pX, double *pY, double *pZ) { + // Normals for the two planes + double a = cos(LX); double b = 0.0; double c = -sin(LX); // X,Z plane + double A = 0.0; double B = cos(LY); double C = -sin(LY); // Y,Z plane +// double A=0.0; double B=1.0; double C=0.0; + printf("rotX %f rotY %f x,z plane normal %f %f %f y,z plane normal %f %f %f\n", + 180/PI*LX,180/PI*LY, a,b,c, A,B,C); + + // calculate the direction + double x = (b*C - B*c) / (B*a - A*b); + double y = (a*C - A*c) / (A*b - B*a); + double z = 1.0; + + // Normalize + double len = sqrt(x*x + y*y + z*z); + x/=len; y/=len; z/=len; + printf(" xyz %f %f %f\n", x, y, z); + + *pX=x; *pY=y; *pZ=z; +} + +// View space +float posx=0.0f; +float posy=0.0f; +float posz=0.0f; +float rotx=0.0f; +float roty=0.0f; +float rotz=0.0f; + +/* + * init() is called at program startup + */ +void init() +{ + int i,j; + + // Load the lighthouses + LoadLighthousePos("L.txt", &houseL.x,&houseL.y,&houseL.z,&houseL.qi,&houseL.qj,&houseL.qk,&houseL.qreal); + LoadLighthousePos("R.txt", &houseR.x,&houseR.y,&houseR.z,&houseR.qi,&houseR.qj,&houseR.qk,&houseR.qreal); + printf("L pos %f %f %f quat %f %f %f %f\n", houseL.x,houseL.y,houseL.z,houseL.qi,houseL.qj,houseL.qk,houseL.qreal); + printf("R pos %f %f %f quat %f %f %f %f\n", houseR.x,houseR.y,houseR.z,houseR.qi,houseR.qj,houseR.qk,houseR.qreal); +/* + // Load the bearing angles + LoadHmdProcessedDataAngles("processed_data.txt", hmdAngles); +*/ + // Spawn the thread to read the hmt angles + memset(read_hmdAngleViewed, 0, NUM_HMD*NUM_SWEEP*sizeof(int)); + read_mutex = OGCreateMutex(); + for (i=0; i<32; i++) { + for (j=0; j<4; j++) { + read_hmdAngles[j][i] = -9999.0; + } + } + read_thread = OGCreateThread(ThreadReadHmtAngles,NULL); + + for (i=0; i<32; i++) { + if (hmdAngles[0][i]!=-9999.0 && hmdAngles[1][i]!=-9999.0 && hmdAngles[2][i]!=-9999.0 && hmdAngles[3][i]!=-9999.0) + { + printf("hmd %d lx %f ly %f rx %f ry %f\n", + i, + (180.0/PI) * hmdAngles[SWEEP_LX][i], + (180.0/PI) * hmdAngles[SWEEP_LY][i], + (180.0/PI) * hmdAngles[SWEEP_RX][i], + (180.0/PI) * hmdAngles[SWEEP_RY][i]); + } + } + + // Initialize OpenGL + glShadeModel(GL_SMOOTH); // Enable Smooth Shading + glClearColor(0.6f, 0.8f, 1.0f, 0.5f); // Black Background + glClearDepth(1.0f); // Depth Buffer Setup + glEnable(GL_DEPTH_TEST); // Enables Depth Testing + glDepthFunc(GL_LEQUAL); // The Type Of Depth Testing To Do + glEnable ( GL_COLOR_MATERIAL ); + glDisable(GL_CULL_FACE); + glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST); +} + +/* + * draw() is called once every frame + */ +void draw() +{ + int i,j; + + //------------------------ + // Check for keyboard input + //------------------------ + if (keys['w'] || keys['W']) { + posz += MOVESPEED; + } + if (keys['s'] || keys['S']) { + posz -= MOVESPEED; + } + if (keys['a'] || keys['A']) { + roty += ROTSPEED; + } + if (keys['d'] || keys['D']) { + roty -= ROTSPEED; + } + if (keys[27]) { + exit(0); + } + + //------------------------ + // Update the scene + //------------------------ + + //------------------------ + // Draw using OpenGL + //------------------------ + + // Clear the screen + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Clear Screen And Depth Buffer + + // Translate and rotate the camera + glLoadIdentity(); // Reset The Current Modelview Matrix + glTranslatef(-posx,-posy,posz); // Move Left 1.5 Units And Into The + + glRotatef(-rotx, 1.0f, 0.0f, 0.0f); + glRotatef(-roty+90.0, 0.0f, 1.0f, 0.0f); + glRotatef(-rotz, 0.0f, 0.0f, 1.0f); + + glScalef(1.0, 1.0, -1.0); +// printf("rot: %f %f %f pos: %f %f %f\n", rotx, roty, rotz, posx, posy, posz); + + //----------------- + // Draw the grid + //----------------- +// glPointSize(1.0); +// glColor3f(0.5, 0.5, 0.5); +// DrawGrid(-GRID_SIZE, GRID_SIZE, -GRID_SIZE, GRID_SIZE, -GRID_SIZE, GRID_SIZE, +// GRID_MINOR_STEP, GRID_MINOR_STEP, GRID_MINOR_STEP); + + + glLineWidth(1.0); + + glPointSize(4.0); + glColor3f(0.0, 0.0, 0.0); + DrawGrid(-GRID_SIZE, GRID_SIZE, -GRID_SIZE, GRID_SIZE, -GRID_SIZE, GRID_SIZE, + GRID_MAJOR_STEP, GRID_MAJOR_STEP, GRID_MAJOR_STEP); + + //----------------- + // Plot the lighthouses + //----------------- + + glLineWidth(2.0); + + // Left lighthouse coordinate system + DrawCoordinateSystem( + houseL.x, houseL.y, houseL.z, + houseL.qi, houseL.qj, houseL.qk, houseL.qreal); + + // Rightt lighthouse coordinate system + DrawCoordinateSystem( + houseR.x, houseR.y, houseR.z, + houseR.qi, houseR.qj, houseR.qk, houseR.qreal); + + glPointSize(15.0); + glBegin(GL_POINTS); + + // Left house in red + glColor3f(1,0,0); + glVertex3f(houseL.x, houseL.z, houseL.y); + + // Green is the origin (hmd) + glColor3f(0,1,0); + glVertex3f(0,0,0); + + // Right house in blue + glColor3f(0,0,1); + glVertex3f(houseR.x, houseR.z, houseR.y); + + glEnd(); + + //------------------- + // Draw the bearing angles + //------------------- + + // Read the hmd angles + OGLockMutex(read_mutex); + for (i=0; i<4; i++) { + for (j=0; j<32; j++) { + hmdAngles[i][j] = read_hmdAngles[i][j]; + } + } + OGUnlockMutex(read_mutex); + + // For every head mount angle + glBegin(GL_LINES); + for (i=0; i<32; i++) { + + // If the bearings exist + if (/*read_hmdAngleViewed[0][i] >= read_frameno-6 && + read_hmdAngleViewed[1][i] >= read_frameno-6 && + read_hmdAngleViewed[2][i] >= read_frameno-6 && + read_hmdAngleViewed[3][i] >= read_frameno-6 &&*/ + hmdAngles[0][i]!=-9999.0 && hmdAngles[1][i]!=-9999.0 && hmdAngles[2][i]!=-9999.0 && hmdAngles[3][i]!=-9999.0) + { + // Get the hmd bearings + double Ldx,Ldy,Ldz,Rdx,Rdy,Rdz; + HmdDir(hmdAngles[SWEEP_LX][i], hmdAngles[SWEEP_LY][i], &Ldx, &Ldy, &Ldz); + HmdDir(hmdAngles[SWEEP_RX][i], hmdAngles[SWEEP_RY][i], &Rdx, &Rdy, &Rdz); + + // Rotate by the lighthouse coordinate systems + Quaternion L0,L,R0,R,Lq,Rq; + QuaternionSet(L0,Ldx,Ldy,Ldz,0.0); + QuaternionSet(R0,Rdx,Rdy,Rdz,0.0); + QuaternionSet(Lq,houseL.qi,houseL.qj,houseL.qk,houseL.qreal); + QuaternionSet(Rq,houseR.qi,houseR.qj,houseR.qk,houseR.qreal); + QuaternionRot(L, Lq, L0); // L is world space bearing from Left lighthouse to hmd + QuaternionRot(R, Rq, R0); // R is world space bearing from Right lighthouse to hmd + + // Plot the lines + const double dist=10.0; + glVertex3f(houseL.x, houseL.z, houseL.y); + glVertex3f(houseL.x+dist*L.i, houseL.z+dist*L.k, houseL.y+dist*L.j); + glVertex3f(houseR.x, houseR.z, houseR.y); + glVertex3f(houseR.x+dist*R.i, houseR.z+dist*R.k, houseR.y+dist*R.j); + } + } + glEnd(); + + read_frameno++; + + // + // Draw the Terrain + // + glutSwapBuffers ( ); // Swap The Buffers To Not Be Left With A Clear Screen +} + +/* + * resize() is called when we change the screen size + */ +void resize(int width, int height) // Resize And Initialize The GL Window +{ + glViewport(0,0,width,height); // Reset The Current Viewport + + glMatrixMode(GL_PROJECTION); // Select The Projection Matrix + glLoadIdentity(); // Reset The Projection Matrix + + // Uncomment For a 3D perspective + gluPerspective(45.0f,(float)width/(float)height,0.1f,1000.0f); + // Uncomment for a 2D perspective + //glOrtho(0.0, WIDTH, 0.0, HEIGHT, -10.0, 10.0); + + glMatrixMode(GL_MODELVIEW); // Select The Modelview Matrix + glLoadIdentity(); // Reset The Modelview Matrix +} + +/* + * These functions are called whenever a key is pressed + */ +void keyboardDown ( unsigned char key, int x, int y ) // Create Keyboard Function +{ + keys[key] = 1; +} +void keyboardUp ( unsigned char key, int x, int y ) +{ + keys[key] = 0; +} + +void specialKeyDown ( int key, int x, int y ) // Create Special Function (required for arrow keys) +{ + if (key<256) { + sp_keys[key] = 1; + } +} +void specialKeyUp (int key, int x, int y) +{ + if (key<256) { + sp_keys[key] = 0; + } +} + +int main ( int argc, char** argv ) // Create Main Function For Bringing It All Together +{ + glutInit ( &argc, argv ); // Erm Just Write It =) + glutInitDisplayMode ( GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE ); // Display Mode + glutInitWindowSize ( WIDTH, HEIGHT ); // If glutFullScreen wasn't called this is the window size + glutCreateWindow ( "OpenGL" ); // Window Title (argv[0] for current directory as title) + if (FULLSCREEN) { + glutFullScreen ( ); // Put Into Full Screen + } + glutDisplayFunc ( draw ); // Matching Earlier Functions To Their Counterparts + glutIdleFunc ( draw ); + glutReshapeFunc ( resize ); + glutKeyboardFunc ( keyboardDown ); + glutKeyboardUpFunc ( keyboardUp ); + glutSpecialFunc ( specialKeyDown ); + glutSpecialUpFunc ( specialKeyUp ); + init (); + glutMainLoop ( ); // Initialize The Main Loop + return 0; +} + diff --git a/dave/plot_lighthouse/quaternion.h b/dave/plot_lighthouse/quaternion.h new file mode 100755 index 0000000..050e5b8 --- /dev/null +++ b/dave/plot_lighthouse/quaternion.h @@ -0,0 +1,138 @@ +// +// quaternion.h +// Game +// +// Created by user on 9/8/16. +// Copyright © 2016 user. All rights reserved. +// + +#ifndef quaternion_h +#define quaternion_h + +typedef struct { + float i,j,k,r; +} Quaternion; +typedef struct { + float x,y,z,theta; +} AxisAngle; + +typedef struct { + Quaternion rot; // orientation + Quaternion pos; // positon +} QuaternionBone; + +#define printq2(name,quat) printf("%s i %f j %f k %f r %f\n", name, quat.i, quat.j, quat.k, quat.r) + +#define QuaternionNormalize(quat) { \ + float inv_len= 1.0f / sqrt( quat.i*quat.i + quat.j*quat.j + quat.k*quat.k + quat.r*quat.r ); \ + quat.i *= inv_len; \ + quat.j *= inv_len; \ + quat.k *= inv_len; } + +#define QuaternionSet(res,I,J,K,R) { res.i=I; res.j=J; res.k=K; res.r=R; } + +#define AxisAngleOfQuaternion(res,q) { \ + float len = sqrt(q.i*q.i + q.j*q.j + q.k*q.k); \ + if (len > 0.0001) { \ + float inv_len = 1.0f / len; \ + res.x = q.i * inv_len; \ + res.y = q.j * inv_len; \ + res.z = q.k * inv_len; \ + } else { \ + res.x=1.0f; res.y=0.0f; res.z=0.0f; \ + } \ + res.theta = 2.0 * acos( q.r ); } + +#define QuaternionOfAxisAngle(res,aa) QuaternionSetAxisAngle(res, (aa).x, (aa).y, (aa).z, (aa).theta) + +#define QuaternionSetAxisAngle(res,x,y,z,theta) { \ + float inv_lenXYZ = 1.0f / sqrt( (x)*(x) + (y)*(y) + (z)*(z) ); \ + float cos_half_theta = cos( 0.5f*(theta) ); \ + float sin_half_theta = sin( 0.5f*(theta) ); \ + res.i = (x) * sin_half_theta * inv_lenXYZ; \ + res.j = (y) * sin_half_theta * inv_lenXYZ; \ + res.k = (z) * sin_half_theta * inv_lenXYZ; \ + res.r = cos_half_theta; } + +#define QuaternionIdentity(q) { q.i=0.0f; q.j=0.0f; q.k=0.0f; q.r=1.0f; } + +#define QuaternionInv(res,a) { \ + res.i = -(a).i; \ + res.j = -(a).j; \ + res.k = -(a).k; \ + res.r = (a).r; } + +#define QuaternionMult(res,a,b) { \ + res.i = (a).i*(b).r + (a).j*(b).k - (a).k*(b).j + (a).r*(b).i; \ + res.j = -(a).i*(b).k + (a).j*(b).r + (a).k*(b).i + (a).r*(b).j; \ + res.k = (a).i*(b).j - (a).j*(b).i + (a).k*(b).r + (a).r*(b).k; \ + res.r = -(a).i*(b).i - (a).j*(b).j - (a).k*(b).k + (a).r*(b).r; } + +#define QuaternionSub(res,a,b) { \ + res.i = (a).i - (b).i; \ + res.j = (a).j - (b).j; \ + res.k = (a).k - (b).k; \ + res.r = (a).r - (b).r; } + +#define QuaternionAdd(res,a,b) { \ + res.i = (a).i + (b).i; \ + res.j = (a).j + (b).j; \ + res.k = (a).k + (b).k; \ + res.r = (a).r + (b).r; } + +#define QuaternionRot(res,q,p) { \ + Quaternion q_inv,left; \ + QuaternionInv(q_inv,q); \ + QuaternionMult(left,q,p); \ + QuaternionMult(res,left,q_inv); } + +#define QuaternionSlerp(res,delta,u,v) { \ + Quaternion rot,u_inv; \ + float theta,inv_s,scale; \ + QuaternionInv(u_inv,u); \ + QuaternionMult(rot,v,u_inv); \ + if (rot.r < 0.998 && rot.r > -0.998) { \ + theta = acos(rot.r); \ + inv_s = 1.0f / sin(theta); \ + theta *= delta; \ + scale = inv_s * sin(theta); \ + rot.i *= scale; \ + rot.j *= scale; \ + rot.k *= scale; \ + rot.r = cos(theta); \ + QuaternionMult(res,rot,u); \ + } else { \ + res = u; \ + } } + +/* +#define QuaternionSlerp(res,delta,u,v) { \ + Quaternion rot,u_inv; \ + float theta,inv_s,scale; \ + QuaternionInv(u_inv,u); \ + QuaternionMult(rot,v,u_inv); \ + if (rot.r < 0.998) { \ + printq2("slerp v",v); \ + theta = acos(rot.r); \ + inv_s = 1.0f / sin(theta); \ + printf("slerp theta %f inv_s %f\n", theta, inv_s); \ + theta *= delta; \ + scale = inv_s * sin(theta); \ + rot.i *= scale; \ + rot.j *= scale; \ + rot.k *= scale; \ + rot.r = cos(theta); \ + QuaternionMult(res,rot,u); \ + } else { \ + res = u; \ + } \ + printq2("slerp res", res); } +*/ + +#define QuaternionBoneTransform(out,in,bone) { \ + Quaternion subtracted,rotated; \ + QuaternionSub(subtracted,in,bone.pos); \ + QuaternionRot(rotated,bone.rot,subtracted); \ + QuaternionAdd(out,rotated,bone.pos); } + +#endif /* quaternion_h */ -- cgit v1.2.3