aboutsummaryrefslogtreecommitdiff
path: root/redist
diff options
context:
space:
mode:
authorJustin Berger <j.david.berger@gmail.com>2018-03-21 09:11:54 -0600
committerJustin Berger <j.david.berger@gmail.com>2018-03-21 09:11:54 -0600
commit8a0831f2b9e458d8f1976c27f7865166ad8de6a4 (patch)
tree028773b7a0ae042c6bb9b3cc21933a617c939b65 /redist
parent0bc3b02ff3c4f975004fb19e226c3177fa811b4e (diff)
downloadlibsurvive-8a0831f2b9e458d8f1976c27f7865166ad8de6a4.tar.gz
libsurvive-8a0831f2b9e458d8f1976c27f7865166ad8de6a4.tar.bz2
Moved pose to linmath to support stronger typing
Diffstat (limited to 'redist')
-rw-r--r--redist/Makefile5
-rw-r--r--redist/linmath.c94
-rw-r--r--redist/linmath.h74
-rw-r--r--redist/lintest.c97
4 files changed, 103 insertions, 167 deletions
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
}