diff options
author | cnlohr <lohr85@gmail.com> | 2017-03-12 03:29:24 -0400 |
---|---|---|
committer | cnlohr <lohr85@gmail.com> | 2017-03-12 03:29:24 -0400 |
commit | b754712675b5d644ae950006afe0d1ceba0e1899 (patch) | |
tree | 29de3de4f0265451f6bb88b1979052333320b5c7 /redist/lintest.c | |
parent | 2231abd24eabe35b37a7a293ecdd48e370373a5a (diff) | |
download | libsurvive-b754712675b5d644ae950006afe0d1ceba0e1899.tar.gz libsurvive-b754712675b5d644ae950006afe0d1ceba0e1899.tar.bz2 |
update with another test.
Diffstat (limited to 'redist/lintest.c')
-rw-r--r-- | redist/lintest.c | 48 |
1 files changed, 46 insertions, 2 deletions
diff --git a/redist/lintest.c b/redist/lintest.c index a767670..933d2e5 100644 --- a/redist/lintest.c +++ b/redist/lintest.c @@ -3,7 +3,51 @@ int main() { - + +#ifdef NONTRANSPOSED_DAVE + FLT pLH1[3] = {-0.275796, 3.105430, -0.868643}; + FLT qLH1[4] = {0.803743, -0.347009, 0.475276, 0.087718}; + FLT pNLH1[3] = { 0.122180, 2.772487, -1.511608 }; //1M +x + FLT qNLH1[4] = { 0.796702, -0.389329, 0.457024, -0.069414 }; + + FLT pLH2[3] = {0.120546, 3.252532, -0.098940}; + FLT qLH2[4] = {-0.220026, 0.460836, 0.328534, 0.794534 }; + FLT pNLH2[3] = {-0.183505, 3.356293, 0.695688, }; + FLT qNLH2[4] = {-0.237438, 0.405213, 0.270438, 0.840410 }; +#else + + FLT pLH1[3] = {-0.321299, 3.130532, -0.786460}; + FLT qLH1[4] = {0.794180, 0.336117, -0.485668, -0.142934}; + FLT pNLH1[3] = { 0.113572, 2.791495, -1.495652 }; //1M +x + FLT qNLH1[4] = { 0.807419, 0.372818, -0.451339, 0.073308 }; + + FLT pLH2[3] = {0.153580, 3.251673, -0.190491}; + FLT qLH2[4] = {0.217017, 0.482214, 0.306568, 0.791448 }; + FLT pNLH2[3] = {-0.175330, 3.351943, 0.669623 }; + FLT qNLH2[4] = {0.257241, 0.394159, 0.292555, 0.832392 }; +#endif + + FLT pOut1[3]; + FLT pOut2[3]; + +// qLH1[1]*=-1; +// qLH2[0]*=-1; + + sub3d( pOut1, pLH1, pNLH1 ); + sub3d( pOut2, pLH2, pNLH2 ); + + printf( "%f %f %f\n", PFTHREE( pOut1 ) ); + printf( "%f %f %f\n", PFTHREE( pOut2 ) ); + + quatrotatevector( pOut1, qLH1, pOut1 ); + quatrotatevector( pOut2, qLH2, pOut2 ); + + printf( "%f %f %f\n", PFTHREE( pOut1 ) ); + printf( "%f %f %f\n", PFTHREE( pOut2 ) ); + + return -1; + +/* FLT e[3] = { 1,1,3.14 }; FLT q[4]; FLT m[16]; @@ -39,7 +83,7 @@ int main() 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 ); |