From 8a0831f2b9e458d8f1976c27f7865166ad8de6a4 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 21 Mar 2018 09:11:54 -0600 Subject: Moved pose to linmath to support stronger typing --- redist/Makefile | 5 +++ redist/linmath.c | 94 +++++++++++++++++++----------------------------------- redist/linmath.h | 74 ++++++++++++++++++++++-------------------- redist/lintest.c | 97 +++++++++++++++----------------------------------------- 4 files changed, 103 insertions(+), 167 deletions(-) (limited to 'redist') diff --git a/redist/Makefile b/redist/Makefile index 1437758..58536c9 100644 --- a/redist/Makefile +++ b/redist/Makefile @@ -9,6 +9,11 @@ lintest : lintest.c linmath.c linmath.h test_dcl : test_dcl.c dclhelpers.c minimal_opencv.c ../src/epnp/epnp.c gcc -o $@ $^ os_generic.c -DFLT=double -lpthread -lcblas -lm -llapacke -O3 -msse2 -ftree-vectorize +.run_tests: clean all + ./lintest + ./test_dcl + ./jsmntest + clean : rm -rf *.o *~ jsmntest lintest diff --git a/redist/linmath.c b/redist/linmath.c index c302f5b..16f771e 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -183,24 +183,18 @@ void axisanglefromquat(FLT *angle, FLT *axis, FLT *q) //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) //Under the mit/X11 license. - - - -void quatsetnone(FLT * q) -{ +void quatsetnone(LinmathQuat q) { q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } -void quatcopy(FLT * qout, const FLT * qin) -{ +void quatcopy(LinmathQuat qout, const LinmathQuat qin) { qout[0] = qin[0]; qout[1] = qin[1]; qout[2] = qin[2]; qout[3] = qin[3]; } -void quatfromeuler( FLT * q, const FLT * euler ) -{ +void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler) { FLT X = euler[0]/2.0f; //roll FLT Y = euler[1]/2.0f; //pitch FLT Z = euler[2]/2.0f; //yaw @@ -221,16 +215,14 @@ void quatfromeuler( FLT * q, const FLT * euler ) quatnormalize( q, q ); } -void quattoeuler( FLT * euler, const FLT * q ) -{ +void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q) { //According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009) euler[0] = FLT_ATAN2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2])); euler[1] = FLT_ASIN(2 * (q[0] * q[2] - q[3] * q[1])); euler[2] = FLT_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 ) -{ +void quatfromaxisangle(LinmathQuat q, const FLT *axis, FLT radians) { FLT v[3]; normalize3d( v, axis ); @@ -243,25 +235,20 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) quatnormalize( q, q ); } -FLT quatmagnitude( const FLT * q ) -{ +FLT quatmagnitude(const LinmathQuat q) { return FLT_SQRT((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); } -FLT quatinvsqmagnitude( const FLT * q ) -{ +FLT quatinvsqmagnitude(const LinmathQuat q) { return ((FLT)1.)/FLT_SQRT((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); } - -void quatnormalize( FLT * qout, const FLT * qin ) -{ +void quatnormalize(LinmathQuat qout, const LinmathQuat qin) { FLT imag = quatinvsqmagnitude( qin ); quatscale( qout, qin, imag ); } -void quattomatrix(FLT * matrix44, const FLT * qin) -{ +void quattomatrix(FLT *matrix44, const LinmathQuat qin) { FLT q[4]; quatnormalize(q, qin); @@ -332,8 +319,7 @@ void quatfrommatrix33(FLT *q, const FLT *m) { } } -void quatfrommatrix( FLT * q, const FLT * matrix44 ) -{ +void quatfrommatrix(LinmathQuat q, const FLT *matrix44) { //Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ FLT tr = matrix44[0] + matrix44[5] + matrix44[10]; @@ -366,8 +352,7 @@ void quatfrommatrix( FLT * q, const FLT * matrix44 ) // Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/ -void quattomatrix33(FLT * matrix33, const FLT * qin) -{ +void quattomatrix33(FLT *matrix33, const LinmathQuat qin) { FLT q[4]; quatnormalize(q, qin); @@ -399,39 +384,34 @@ void quattomatrix33(FLT * matrix33, const FLT * qin) matrix33[8] = 1 - xx - yy; } -void quatgetconjugate(FLT * qout, const FLT * qin) -{ +void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin) { qout[0] = qin[0]; qout[1] = -qin[1]; qout[2] = -qin[2]; qout[3] = -qin[3]; } -void quatgetreciprocal( FLT * qout, const FLT * qin ) -{ +void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin) { FLT m = quatinvsqmagnitude(qin); quatgetconjugate( qout, qin ); quatscale( qout, qout, m ); } -void quatsub( FLT * qout, const FLT * a, const FLT * b ) -{ +void quatsub(LinmathQuat 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 ) -{ +void quatadd(LinmathQuat 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 * q1, const FLT * q2 ) -{ +void quatrotateabout(LinmathQuat qout, const LinmathQuat q1, const LinmathQuat q2) { //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]); @@ -439,44 +419,37 @@ void quatrotateabout( FLT * qout, const FLT * q1, const FLT * q2 ) 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 ) -{ +void quatscale(LinmathQuat qout, const LinmathQuat 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 ) -{ +FLT quatinnerproduct(const LinmathQuat qa, const LinmathQuat 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 ) -{ +void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat 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 ) -{ +void quatevenproduct(LinmathQuat q, LinmathQuat qa, LinmathQuat 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 ) -{ +void quatoddproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat 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 ) -{ +void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t) { FLT an[4]; FLT bn[4]; quatnormalize( an, qa ); @@ -511,8 +484,7 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ) } } -void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ) -{ +void quatrotatevector(FLT *vec3out, const LinmathQuat quat, const FLT *vec3in) { //See: http://www.geeks3d.com/20141201/how-to-rotate-a-vertex-by-a-quaternion-in-glsl/ FLT tmp[3]; @@ -665,19 +637,19 @@ void matrix44transpose(FLT * mout, const FLT * minm ) } -void ApplyPoseToPoint(LINMATH_POINT pout, const LINMATH_POSE pose, const LINMATH_POINT pin) { - quatrotatevector( pout, &pose[3], pin ); - add3d( pout, pout, &pose[0] ); +void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin) { + quatrotatevector(pout, pose->Rot, pin); + add3d(pout, pout, pose->Pos); } -void ApplyPoseToPose(LINMATH_POSE pout, const LINMATH_POSE lhs_pose, const LINMATH_POSE rhs_pose) { - ApplyPoseToPoint(pout, lhs_pose, rhs_pose); - quatrotateabout(&pout[3], &lhs_pose[3], &rhs_pose[3]); +void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_pose, const LinmathPose *rhs_pose) { + ApplyPoseToPoint(pout->Pos, lhs_pose, rhs_pose->Pos); + quatrotateabout(pout->Rot, lhs_pose->Rot, rhs_pose->Rot); } -void InvertPose(LINMATH_POSE poseout, const LINMATH_POSE pose) { - quatgetreciprocal(&poseout[3], &pose[3]); +void InvertPose(LinmathPose *poseout, const LinmathPose *pose) { + quatgetreciprocal(poseout->Rot, pose->Rot); - quatrotatevector(&poseout[0], &poseout[3], &pose[0]); - scale3d(&poseout[0], &poseout[0], -1); + quatrotatevector(poseout->Pos, poseout->Rot, pose->Pos); + scale3d(poseout->Pos, poseout->Pos, -1); } diff --git a/redist/linmath.h b/redist/linmath.h index 57b98d3..782a13d 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -47,6 +47,14 @@ #define FLT_FABS FLT_FABS__ #endif +typedef FLT LinmathQuat[4]; // This is the [wxyz] quaternion, in wxyz format. +typedef FLT LinmathPoint3d[3]; +typedef FLT linmathVec3d[3]; + +typedef struct LinmathPose { + LinmathPoint3d Pos; + LinmathQuat Rot; +} LinmathPose; //NOTE: Inputs may never be output with cross product. void cross3d( FLT * out, const FLT * a, const FLT * b ); @@ -72,52 +80,48 @@ FLT anglebetween3d( FLT * a, FLT * b ); void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle); void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest); -void axisanglefromquat(FLT *angle, FLT *axis, FLT *quat); - +void axisanglefromquat(FLT *angle, FLT *axis, LinmathQuat quat); //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 quatfrommatrix( FLT * q, const FLT * matrix44 ); -void quatfrommatrix33(FLT *q, const FLT *matrix33); -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 ); -void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest); - -//Poses are Position: [x, y, z] Quaternion: [q, x, y, z] -typedef FLT *LINMATH_POSE; - -// Points are [x, y, z] -typedef FLT *LINMATH_POINT; +typedef FLT LinmathEulerAngle[3]; + +void quatsetnone(LinmathQuat q); +void quatcopy(LinmathQuat q, const LinmathQuat qin); +void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler); +void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q); +void quatfromaxisangle(LinmathQuat q, const FLT *axis, FLT radians); +FLT quatmagnitude(const LinmathQuat q); +FLT quatinvsqmagnitude(const LinmathQuat q); +void quatnormalize(LinmathQuat qout, const LinmathQuat qin); // Safe for in to be same as out. +void quattomatrix(FLT *matrix44, const LinmathQuat q); +void quatfrommatrix(LinmathQuat q, const FLT *matrix44); +void quatfrommatrix33(LinmathQuat q, const FLT *matrix33); +void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin); +void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin); +void quatsub(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); +void quatadd(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); +void quatrotateabout(LinmathQuat qout, const LinmathQuat a, + const LinmathQuat b); // same as quat multiply, not piecewise multiply. +void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s); +FLT quatinnerproduct(const LinmathQuat qa, const LinmathQuat qb); +void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb); +void quatevenproduct(LinmathQuat q, LinmathQuat qa, LinmathQuat qb); +void quatoddproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb); +void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t); +void quatrotatevector(FLT *vec3out, const LinmathQuat quat, const FLT *vec3in); +void quatfrom2vectors(LinmathQuat q, const FLT *src, const FLT *dest); // This is the quat equivalent of 'pout = pose * pin' if pose were a 4x4 matrix in homogenous space -void ApplyPoseToPoint(LINMATH_POINT pout, const LINMATH_POSE pose, const LINMATH_POINT pin); +void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); // This is the quat equivalent of 'pout = lhs_pose * rhs_pose' if poses were a 4x4 matrix in homogenous space -void ApplyPoseToPose(LINMATH_POSE pout, const LINMATH_POSE lhs_pose, const LINMATH_POSE rhs_pose); +void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_pose, const LinmathPose *rhs_pose); // This is the quat equivlant of 'pose_in^-1'; so that ApplyPoseToPose(..., InvertPose(..., pose_in), pose_in) == // Identity ( [0, 0, 0], [1, 0, 0, 0] ) // by definition. -void InvertPose(LINMATH_POSE poseout, const LINMATH_POSE pose_in); +void InvertPose(LinmathPose *poseout, const LinmathPose *pose_in); // Matrix Stuff diff --git a/redist/lintest.c b/redist/lintest.c index 4765e68..4c53da7 100644 --- a/redist/lintest.c +++ b/redist/lintest.c @@ -32,72 +32,72 @@ void printFLTA(FLT *a, int length) { void testInvertPose() { - FLT rotAroundYOffset[7] = {1, 1, 1, .5, 0, .5, 0}; - FLT pose_out[7]; + LinmathPose rotAroundYOffset = {1, 1, 1, .5, 0, .5, 0}; + LinmathPose pose_out; { - FLT expected[] = {1, -1, -1, 0.7071068, 0, -0.7071068, 0}; - InvertPose(pose_out, rotAroundYOffset); - ASSERT_FLTA_EQUALS(pose_out, expected, 7); + LinmathPose expected = {1, -1, -1, 0.7071068, 0, -0.7071068, 0}; + InvertPose(&pose_out, &rotAroundYOffset); + ASSERT_FLTA_EQUALS(pose_out.Pos, expected.Pos, 7); FLT identity[] = {0, 0, 0, 1, 0, 0, 0}; - ApplyPoseToPose(pose_out, expected, rotAroundYOffset); - quatnormalize(&pose_out[3], &pose_out[3]); - ASSERT_FLTA_EQUALS(pose_out, identity, 7); + ApplyPoseToPose(&pose_out, &expected, &rotAroundYOffset); + quatnormalize(pose_out.Rot, pose_out.Rot); + ASSERT_FLTA_EQUALS(pose_out.Pos, identity, 7); } } void testApplyPoseToPose() { - FLT rotAroundYOffset[7] = {1, 1, 1, 0, 0, 1, 0}; - FLT pose_out[7]; + LinmathPose rotAroundYOffset = {1, 1, 1, 0, 0, 1, 0}; + LinmathPose pose_out; { - FLT pt[] = {0, 1, 0, 0, 0, 1, 0}; - FLT expected[] = {1, 2, 1, -1, 0, 0, 0}; - ApplyPoseToPose(pose_out, rotAroundYOffset, pt); - quatnormalize(&pose_out[3], &pose_out[3]); - ASSERT_FLTA_EQUALS(pose_out, expected, 7); + LinmathPose pt = {0, 1, 0, 0, 0, 1, 0}; + LinmathPose expected = {1, 2, 1, -1, 0, 0, 0}; + ApplyPoseToPose(&pose_out, &rotAroundYOffset, &pt); + quatnormalize(pose_out.Rot, pose_out.Rot); + ASSERT_FLTA_EQUALS(pose_out.Pos, expected.Pos, 7); } { - FLT pt[] = {0, 1, 0, 0, 1, 0, 0}; - FLT expected[] = {1, 2, 1, 0, 0, 0, -1}; - ApplyPoseToPose(pose_out, rotAroundYOffset, pt); - ASSERT_FLTA_EQUALS(pose_out, expected, 7); + LinmathPose pt = {0, 1, 0, 0, 1, 0, 0}; + LinmathPose expected = {1, 2, 1, 0, 0, 0, -1}; + ApplyPoseToPose(&pose_out, &rotAroundYOffset, &pt); + ASSERT_FLTA_EQUALS(pose_out.Pos, expected.Pos, 7); } } void testApplyPoseToPoint() { - FLT rotAroundY[7] = {0, 0, 0, 0, 0, 1, 0}; + LinmathPose rotAroundY = {0, 0, 0, 0, 0, 1, 0}; FLT pt_out[3]; { FLT pt[3] = {0, 1, 0}; FLT expected[3] = {0, 1, 0}; - ApplyPoseToPoint(pt_out, rotAroundY, pt); + ApplyPoseToPoint(pt_out, &rotAroundY, pt); ASSERT_FLTA_EQUALS(pt_out, expected, 3); } { FLT pt[3] = {1, 1, 0}; FLT expected[3] = {-1, 1, 0}; - ApplyPoseToPoint(pt_out, rotAroundY, pt); + ApplyPoseToPoint(pt_out, &rotAroundY, pt); ASSERT_FLTA_EQUALS(pt_out, expected, 3); } - FLT rotAroundYOffset[7] = {1, 1, 1, 0, 0, 1, 0}; + LinmathPose rotAroundYOffset = {1, 1, 1, 0, 0, 1, 0}; { FLT pt[3] = {0, 1, 0}; FLT expected[3] = {1, 2, 1}; - ApplyPoseToPoint(pt_out, rotAroundYOffset, pt); + ApplyPoseToPoint(pt_out, &rotAroundYOffset, pt); ASSERT_FLTA_EQUALS(pt_out, expected, 3); } { FLT pt[3] = {1, 1, 0}; FLT expected[3] = {0, 2, 1}; - ApplyPoseToPoint(pt_out, rotAroundYOffset, pt); + ApplyPoseToPoint(pt_out, &rotAroundYOffset, pt); ASSERT_FLTA_EQUALS(pt_out, expected, 3); } } @@ -163,53 +163,8 @@ int main() printf( "%f %f %f\n", PFTHREE( pOut1 ) ); printf( "%f %f %f\n", PFTHREE( pOut2 ) ); */ - return -1; - -#endif + return 0; -#if 0 - - FLT e[3] = { 1,1,3.14 }; - FLT q[4]; - FLT m[16]; - FLT pt[3] = { 1, 1, 1 }; - - q[0] = 0; - q[1] = 0; - q[2] = 0; - q[3] = 1; - - quatrotatevector( pt, q, pt ); - printf( "%f %f %f\n", PFTHREE( pt ) ); - printf( "\n" ); - - quatfromeuler( q, e ); - printf( "%f %f %f %f\n\n", PFFOUR( q ) ); - quattomatrix(m,q); - printf( "%f %f %f %f\n", PFFOUR( &m[0] ) ); - printf( "%f %f %f %f\n", PFFOUR( &m[4] ) ); - printf( "%f %f %f %f\n", PFFOUR( &m[8] ) ); - printf( "%f %f %f %f\n\n", PFFOUR( &m[12] ) ); - quatfrommatrix(q,m ); - printf( "%f %f %f %f\n\n", PFFOUR( q ) ); - quattoeuler( e,q ); - printf( "E: %f %f %f\n", e[0], e[1], e[2] ); - - - FLT pfromlh[3] = { 0, 1, 0 }; - FLT p[3] = { 0, 1, 0 }; - quatrotatevector( p, q, p ); - printf( "%f %f %f\n", PFTHREE( p ) ); - printf( "Flipping rotation\n" ); - q[0] *= -1; //Wow that was easy. - quatrotatevector( p, q, p ); - printf( "%f %f %f\n", PFTHREE( p ) ); - - //Try setting up a pose. -// FLT mypose[7] = { 0, 0, 10, q[0], q[1], q[2], q[3] ); -// ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose ); -//void InvertPose( FLT * poseout, const FLT * pose ); - #endif } -- cgit v1.2.3