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. --- redist/linmath.c | 17 +++++++++-------- redist/linmath.h | 2 +- 2 files changed, 10 insertions(+), 9 deletions(-) (limited to 'redist') diff --git a/redist/linmath.c b/redist/linmath.c index d76fbba..72c00a4 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -3,6 +3,7 @@ #include "linmath.h" #include #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.2.3