From 26f55787820874629519c589f7c7c1ba1334ef54 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Wed, 8 Mar 2017 21:08:30 -0500 Subject: return null if no file is opened --- redist/json_helpers.c | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'redist') diff --git a/redist/json_helpers.c b/redist/json_helpers.c index b7ccb40..4a3ba55 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -76,6 +76,7 @@ uint32_t JSON_STRING_LEN; char* load_file_to_mem(const char* path) { FILE * f = fopen( path, "r" ); + if (f==NULL) return NULL; fseek( f, 0, SEEK_END ); int len = ftell( f ); fseek( f, 0, SEEK_SET ); @@ -116,7 +117,10 @@ static uint16_t json_load_array(const char* JSON_STRING, jsmntok_t* tokens, uint void json_load_file(const char* path) { uint32_t i = 0; + char* JSON_STRING = load_file_to_mem(path); + if (JSON_STRING==NULL) return; + JSON_STRING_LEN = strlen(JSON_STRING); jsmn_parser parser; -- cgit v1.3.1 From 765bf1623299e81f5f95b78a78e7d66997e4ad0c Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 9 Mar 2017 20:00:42 -0500 Subject: Fix wrong quat functions + Add matrix->quat conversion. --- redist/linmath.c | 58 ++++++++++++++++++++++++++++++++++++++++++++------------ redist/linmath.h | 5 +++-- 2 files changed, 49 insertions(+), 14 deletions(-) (limited to 'redist') diff --git a/redist/linmath.c b/redist/linmath.c index 1c5c25b..d76fbba 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -152,7 +152,7 @@ FLT quatmagnitude( const FLT * q ) FLT quatinvsqmagnitude( const FLT * q ) { - return ((FLT)1.)/((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); + return ((FLT)1.)/FLT_SQRT((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); } @@ -168,17 +168,17 @@ void quattomatrix(FLT * matrix44, const FLT * qin) 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 xx = 2 * q[1] * q[1]; + FLT xy = 2 * q[1] * q[2]; + FLT xz = 2 * q[1] * q[3]; + FLT xw = 2 * q[1] * q[0]; - FLT yy = 2 * q[1] * q[1]; - FLT yz = 2 * q[1] * q[2]; - FLT yw = 2 * q[1] * q[3]; + FLT yy = 2 * q[2] * q[2]; + FLT yz = 2 * q[2] * q[3]; + FLT yw = 2 * q[2] * q[0]; - FLT zz = 2 * q[2] * q[2]; - FLT zw = 2 * q[2] * q[3]; + FLT zz = 2 * q[3] * q[3]; + FLT zw = 2 * q[3] * q[0]; //opengl major matrix44[0] = 1 - yy - zz; @@ -202,6 +202,40 @@ void quattomatrix(FLT * matrix44, const FLT * qin) matrix44[15] = 1; } + +void quatfrommatrix( FLT * q, const FLT * matrix44 ) +{ + //Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ + float tr = matrix44[0] + matrix44[5] + matrix44[10]; + + if (tr > 0) { + float S = sqrt(tr+1.0) * 2; // S=4*qw + q[0] = 0.25 * S; + q[1] = (matrix44[9] - matrix44[6]) / S; + q[2] = (matrix44[2] - matrix44[8]) / S; + q[3] = (matrix44[4] - matrix44[1]) / S; + } else if ((matrix44[0] > matrix44[5])&(matrix44[0] > matrix44[10])) { + float S = sqrt(1.0 + matrix44[0] - matrix44[5] - matrix44[10]) * 2; // S=4*qx + q[0] = (matrix44[9] - matrix44[6]) / S; + q[1] = 0.25 * S; + q[2] = (matrix44[1] + matrix44[4]) / S; + q[3] = (matrix44[2] + matrix44[8]) / S; + } else if (matrix44[5] > matrix44[10]) { + float S = sqrt(1.0 + matrix44[5] - matrix44[0] - matrix44[10]) * 2; // S=4*qy + q[0] = (matrix44[2] - matrix44[8]) / S; + q[1] = (matrix44[1] + matrix44[4]) / S; + q[2] = 0.25 * S; + q[3] = (matrix44[6] + matrix44[9]) / S; + } else { + float S = sqrt(1.0 + matrix44[10] - matrix44[0] - matrix44[5]) * 2; // S=4*qz + q[0] = (matrix44[4] - matrix44[1]) / S; + q[1] = (matrix44[2] + matrix44[8]) / S; + q[2] = (matrix44[6] + matrix44[9]) / S; + q[3] = 0.25 * S; + } +} + + void quattomatrix33(FLT * matrix33, const FLT * qin) { FLT q[4]; @@ -270,8 +304,8 @@ void quatrotateabout( FLT * qout, const FLT * a, const FLT * b ) FLT q1[4]; FLT q2[4]; - quatnormalize( q1, a ); - quatnormalize( q2, b ); + //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]); diff --git a/redist/linmath.h b/redist/linmath.h index caec281..ec20534 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -7,8 +7,8 @@ #define DEFAULT_EPSILON 0.001 //For printf -#define PFTHREE(x) x[0], x[1], x[2] -#define PFFOUR(x) x[0], x[1], x[2], x[3] +#define PFTHREE(x) (x)[0], (x)[1], (x)[2] +#define PFFOUR(x) (x)[0], (x)[1], (x)[2], (x)[3] #define LINMATHPI ((FLT)3.141592653589) @@ -76,6 +76,7 @@ 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 quatfrommatrix( FLT * q, const FLT * matrix44 ); void quatgetconjugate( FLT * qout, const FLT * qin ); void quatgetreciprocal( FLT * qout, const FLT * qin ); void quatsub( FLT * qout, const FLT * a, const FLT * b ); -- cgit v1.3.1 From 6541d1e63358324400ab446a4d1457941a0e90cf Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 9 Mar 2017 20:35:55 -0500 Subject: Update linmath to allow for transforming the pose solver for testing purposes. --- dave/AffineSolve.c | 50 ++++++++++++++++++++++++++++++++++++++++++++------ dave/Makefile | 2 +- redist/linmath.c | 17 +++++++++-------- redist/linmath.h | 2 +- 4 files changed, 55 insertions(+), 16 deletions(-) (limited to 'redist') diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c index f29e29e..6e17f26 100644 --- a/dave/AffineSolve.c +++ b/dave/AffineSolve.c @@ -10,8 +10,8 @@ #include #include #include "dclapack.h" - -#define LH_ID 0 +#include +#define LH_ID 1 #define NUM_HMD 32 #define MAX_POINTS 128 @@ -209,7 +209,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLOAT ydist1 = 1.0 / uhat_len; //0.25*PI / uhat_len; FLOAT ydist2 = 1.0 / rhat_len; //0.25*PI / rhat_len; FLOAT ydist = 1.0 / urhat_len; -// printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); + //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); //-------------------- // Rescale the axies to be of the proper length @@ -223,6 +223,9 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLOAT x_y = sqrt(1.0 - x[0][0]*x[0][0] - x[2][0]*x[2][0]); FLOAT y_y = sqrt(1.0 - y[0][0]*y[0][0] - y[2][0]*y[2][0]); FLOAT z_y = sqrt(1.0 - z[0][0]*z[0][0] - z[2][0]*z[2][0]); + if( x_y != x_y ) x_y = 0; + if( y_y != y_y ) y_y = 0; + if( z_y != z_y ) z_y = 0; // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLOAT bestErr = 9999.0; @@ -243,17 +246,18 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLOAT cx,cy,cz; CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]); FLOAT hand = cx*z[0][0] + cy*z_y + cz*z[2][0]; -// printf("err %f hand %f\n", err, hand); + printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far - if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } + //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } + if ( i == 0 && j == 1 && k == 0) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } z_y = -z_y; } y_y = -y_y; } x_y = -x_y; } -// printf("bestErr %f\n", bestErr); + printf("bestErr %f\n", bestErr); /* for (i=0; i #include +#include void cross3d( FLT * out, const FLT * a, const FLT * b ) { @@ -299,14 +300,9 @@ void quatadd( FLT * qout, const FLT * a, const FLT * b ) qout[3] = a[3] + b[3]; } -void quatrotateabout( FLT * qout, const FLT * a, const FLT * b ) +void quatrotateabout( FLT * qout, const FLT * q1, const FLT * q2 ) { - FLT q1[4]; - FLT q2[4]; - - //quatnormalize( q1, a ); - //quatnormalize( q2, b ); - + //NOTE: Does not normalize 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]); @@ -396,7 +392,7 @@ void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ) vquat[3] = vec3in[2]; quatrotateabout( tquat, quat, vquat ); - quatgetreciprocal( qrecp, quat ); + quatgetconjugate( qrecp, quat ); quatrotateabout( vquat, tquat, qrecp ); vec3out[0] = vquat[1]; @@ -515,6 +511,11 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest) } +void matrix44copy(FLT * mout, const FLT * minm ) +{ + memcpy( mout, minm, sizeof( FLT ) * 16 ); +} + ///////////////////////////////////////Matrix Rotations//////////////////////////////////// ////Originally from Stack Overflow ////Under cc by-sa 3.0 diff --git a/redist/linmath.h b/redist/linmath.h index ec20534..676d182 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -104,7 +104,7 @@ void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]) Matrix3x3 inverseM33(const Matrix3x3 mat); - +void matrix44copy(FLT * mout, const FLT * minm ); -- cgit v1.3.1