From 2afbb0313dafe3f52a38ee2061cb1de8043d16e6 Mon Sep 17 00:00:00 2001 From: ultramn Date: Sat, 4 Feb 2017 20:03:12 -0800 Subject: Moved plot_lighthouse to the tools folder --- dave/plot_lighthouse/.DS_Store | Bin 6148 -> 0 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 deletions(-) delete mode 100644 dave/plot_lighthouse/.DS_Store delete mode 100644 dave/plot_lighthouse/Makefile delete mode 100644 dave/plot_lighthouse/README.txt delete mode 100644 dave/plot_lighthouse/fileutil.c delete mode 100644 dave/plot_lighthouse/fileutil.h delete mode 100644 dave/plot_lighthouse/glutil.c delete mode 100644 dave/plot_lighthouse/glutil.h delete mode 100755 dave/plot_lighthouse/main.c delete mode 100755 dave/plot_lighthouse/quaternion.h (limited to 'dave') diff --git a/dave/plot_lighthouse/.DS_Store b/dave/plot_lighthouse/.DS_Store deleted file mode 100644 index 493ea1e..0000000 Binary files a/dave/plot_lighthouse/.DS_Store and /dev/null differ diff --git a/dave/plot_lighthouse/Makefile b/dave/plot_lighthouse/Makefile deleted file mode 100644 index d156bdb..0000000 --- a/dave/plot_lighthouse/Makefile +++ /dev/null @@ -1,15 +0,0 @@ -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 deleted file mode 100644 index 4e65688..0000000 --- a/dave/plot_lighthouse/README.txt +++ /dev/null @@ -1,20 +0,0 @@ -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 deleted file mode 100644 index f892798..0000000 --- a/dave/plot_lighthouse/fileutil.c +++ /dev/null @@ -1,124 +0,0 @@ -#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 deleted file mode 100644 index 714fbbd..0000000 --- a/dave/plot_lighthouse/fileutil.h +++ /dev/null @@ -1,34 +0,0 @@ -#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 deleted file mode 100644 index dd022a0..0000000 --- a/dave/plot_lighthouse/glutil.c +++ /dev/null @@ -1,75 +0,0 @@ -// -// 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 deleted file mode 100755 index c2fd97a..0000000 --- a/dave/plot_lighthouse/main.c +++ /dev/null @@ -1,336 +0,0 @@ -/* - 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 deleted file mode 100755 index 050e5b8..0000000 --- a/dave/plot_lighthouse/quaternion.h +++ /dev/null @@ -1,138 +0,0 @@ -// -// 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