pax_global_header00006660000000000000000000000064132714637000014515gustar00rootroot0000000000000052 comment=9db9a9cba80000f1647483730d1a0d83307773c9 cqrlib-CQRlib-1.1.4/000077500000000000000000000000001327146370000141065ustar00rootroot00000000000000cqrlib-CQRlib-1.1.4/CPPQRTest.cpp000066400000000000000000000437631327146370000163540ustar00rootroot00000000000000#include #include #include #include #define USE_LOCAL_HEADERS #ifndef USE_LOCAL_HEADERS #include #else #include "cqrlib.h" #endif int errorcount = 0; int main ( ) { CPPQR q1, q4, qx, qy, qz; double normsq; double PI; double vx[3] = {1.,0.,0.}; double vy[3] = {0.,1.,0.}; double vz[3] = {0.,0.,1.}; double EXX,EXY,EXZ; double EYX,EYY,EYZ; double EZX,EZY,EZZ; PI = 4.*atan2(1.,1.); errorcount=0; if (q1.GetW() !=0. || q1.GetX( ) !=0. || q1.GetY( ) !=0. || q1.GetZ( ) !=0.) { errorcount++; fprintf(stdout," CPPQRCreateEmptyQuaternion for q1 non-zero [ %g, %g, %g, %g ]\n",q1.GetW(),q1.GetX(),q1.GetY( ),q1.GetZ()); } CPPQR q2(1.,2.,3.,4.); if (q2.GetW() !=1. || q2.GetX() !=2. || q2.GetY() !=3. || q2.GetZ() !=4.) { errorcount++; fprintf(stdout," CPPQRCreateQuaternion for q2 wrong value [ %g, %g, %g, %g ] != [1.,2,3.,4.]\n",q2.GetW(),q2.GetX(),q2.GetY(),q2.GetZ()); } CPPQR q3; q3 = q1 + q2; if (q3.GetW() !=1. || q3.GetX() !=2. || q3.GetY() !=3. || q3.GetZ() !=4.) { fprintf(stdout," CPPQRAdd(q3,q1,q2) q3 wrong value [ %g, %g, %g, %g ] != [1.,2.,3.,4.]\n",q3.GetW(),q3.GetX(),q3.GetY(),q3.GetZ()); } q1.Set( -9999.,-9998.,-9997.,-9996.); if (q1.GetW() !=-9999. || q1.GetX() !=-9998. || q1.GetY() !=-9997. || q1.GetZ() !=-9996.) { errorcount++; fprintf(stdout," CPPQRSetQuaternion q1 wrong value [ %g, %g, %g, %g ] != [-9999.,-9998.,-9997.,-9996.]\n",q1.GetW(),q1.GetX(),q1.GetY(),q1.GetZ()); } q1 = q3 - q2; if (q1.GetW() !=0. || q1.GetX() !=0. || q1.GetY() !=0. || q1.GetZ() !=0.) { errorcount++; fprintf(stdout," CPPQR Subtract(q1,q3,q2) for q1 non-zero [ %g, %g, %g, %g ]\n",q1.GetW(),q1.GetX(),q1.GetY(),q1.GetZ()); } q4.Set(1.,-2.,-3.,-4.); if (q4.GetW() != 1. || q4.GetX() !=-2. || q4.GetY() !=-3. || q4.GetZ() !=-4.) { errorcount++; fprintf(stdout," CPPQRSetQuaternion &q4 wrong value [ %g, %g, %g, %g ] != [1.,-2.,-3.,-4.]\n",q4.GetW(),q4.GetX(),q4.GetY(),q4.GetZ()); } q1 = q4 * q2; if (fabs(q1.GetW()-30.)>300.*DBL_EPSILON || fabs(q1.GetX())>300.*DBL_EPSILON || fabs(q1.GetY())>300.*DBL_EPSILON ||fabs(q1.GetZ())>300.*DBL_EPSILON ) { errorcount++; fprintf(stdout," CPPQRMultiply(q1,&q4,q2) q1 wrong value [ %g, %g, %g, %g ] != [30.,0.,0.,0.]\n",q1.GetW(),q1.GetX(),q1.GetY(),q1.GetZ()); } q3 = q1 / q2; if (fabs(q3.GetW()-1.)>60.*DBL_EPSILON || fabs(q3.GetX()+2.)>60.*DBL_EPSILON || fabs(q3.GetY()+3.)>60.*DBL_EPSILON ||fabs(q3.GetZ()+4.)>60.*DBL_EPSILON ) { errorcount++; fprintf(stdout," CPPQRDivide(q3,q1,q2) q3 wrong value [ %g, %g, %g, %g ] != [1.,-2.,-3.,-4.]\n",q3.GetW(),q3.GetX(),q3.GetY(),q3.GetZ()); } q3 = q2 * 3.0; if (fabs(q3.GetW()-3.)>180.*DBL_EPSILON || fabs(q3.GetX()-6.)>180.*DBL_EPSILON || fabs(q3.GetY()-9.)>180.*DBL_EPSILON ||fabs(q3.GetZ()-12.)>180.*DBL_EPSILON ) { errorcount++; fprintf(stdout," CPPQRScalarMultiply(q3,q2,3.) q3 wrong value [ %g, %g, %g, %g ] != [3.,6.,9.,12.]\n",q3.GetW(),q3.GetX(),q3.GetY(),q3.GetZ()); } q3 = q2.Conjugate( ); if (q3.GetW() != 1. || q3.GetX() !=-2. || q3.GetY() !=-3. || q3.GetZ() !=-4.) { errorcount++; fprintf(stdout," CPPQRConjugate(q3,q2) q3 wrong value [ %g, %g, %g, %g ] != [1.,-2.,-3.,-4.]\n",q3.GetW(),q3.GetX(),q3.GetY(),q3.GetZ()); } if ( q4 != q3) { errorcount++; fprintf(stdout," CPPQREqual(&q4,q2) failed\n"); } normsq = q4.Normsq(); if ( fabs(normsq-30.) > 300.*DBL_EPSILON) { errorcount++; fprintf(stdout," CPPQRNormsq(&normsq,&q4) failed normsq=%g != 30\n", normsq); } q3 = q4.Inverse( ); if (fabs(q3.GetW() - 1./30.) > 2.*DBL_EPSILON || fabs(q3.GetX() - 2./30.) > 2.*DBL_EPSILON || fabs(q3.GetY() - 3./30.) > 2.*DBL_EPSILON || fabs(q3.GetZ() - 4./30.) > 2.*DBL_EPSILON) { errorcount++; fprintf(stdout," CPPQRInverse(q3,&q4) q3 wrong value [ %g, %g, %g, %g ] != [1./30.,2./30.,3./30,4./30.]\n",q3.GetW(),q3.GetX(),q3.GetY(),q3.GetZ()); } /* Create quaternions to rotate about the x,y and z-axes by 90 degrees */ qx = CPPQR::Axis2Quaternion( vx, PI/2.0 ); qy = CPPQR::Axis2Quaternion( vy, PI/2.0 ); qz = CPPQR::Axis2Quaternion( vz, PI/2.0 ); if (qx.GetW()<0.||fabs(qx.GetW()*qx.GetW()-.5)>10.*DBL_EPSILON||fabs(qx.GetX()*qx.GetX()-.5)>10.*DBL_EPSILON||fabs(qx.GetY())>10.*DBL_EPSILON||fabs(qx.GetZ())>10.*DBL_EPSILON) { errorcount++; fprintf(stdout,"Axis2Quaternion qx wrong value [ %g, %g, %g, %g ] != [sqrt(1./2.),sqrt(1./2.),0,0]\n",qx.GetW(),qx.GetX(),qx.GetY(),qx.GetZ()); } if (qy.GetW()<0.||fabs(qy.GetW()*qy.GetW()-.5)>10.*DBL_EPSILON||fabs(qy.GetY()*qy.GetY()-.5)>10.*DBL_EPSILON||fabs(qy.GetX())>10.*DBL_EPSILON||fabs(qy.GetZ())>10.*DBL_EPSILON) { errorcount++; fprintf(stdout,"Axis2Quaternion qy wrong value [ %g, %g, %g, %g ] != [sqrt(1./2.),sqrt(1./2.),0,0]\n",qy.GetW(),qy.GetX(),qy.GetY(),qy.GetZ()); } if (qz.GetW()<0.||fabs(qz.GetW()*qz.GetW()-.5)>10.*DBL_EPSILON||fabs(qz.GetZ()*qz.GetZ()-.5)>10.*DBL_EPSILON||fabs(qz.GetX())>10.*DBL_EPSILON||fabs(qz.GetY())>10.*DBL_EPSILON) { errorcount++; fprintf(stdout,"CPPAxis2Quaternion qz wrong value [ %g, %g, %g, %g ] != [sqrt(1./2.),sqrt(1./2.),0,0]\n",qz.GetW(),qz.GetX(),qz.GetY(),qz.GetZ()); } double Matx[9]; double Maty[9]; double Matz[9]; CPPQR::Quaternion2Matrix(Matx,qx); CPPQR::Quaternion2Matrix(Maty,qy); CPPQR::Quaternion2Matrix(Matz,qz); if ( fabs(Matx[0]-1.)>10.*DBL_EPSILON ||fabs(Matx[5]+1.)>10.*DBL_EPSILON ||fabs(Matx[7]-1.)>10.*DBL_EPSILON ||fabs(Matx[1])>10.*DBL_EPSILON ||fabs(Matx[2])>10.*DBL_EPSILON ||fabs(Matx[3])>10.*DBL_EPSILON ||fabs(Matx[4])>10.*DBL_EPSILON ||fabs(Matx[6])>10.*DBL_EPSILON ||fabs(Matx[8])>10.*DBL_EPSILON) { errorcount++; fprintf(stdout," Quaternion2Matrix Matx wrong value \n [ %g, %g, %g ]\n [ %g, %g, %g ]\n [ %g, %g, %g ]\n" "!= [1, 0, 0]\n [0, 0, -1]\n [0, 1, 0]\n", Matx[0]-1.0,Matx[1],Matx[2], Matx[3],Matx[4],Matx[5]+1.0, Matx[6],Matx[7]-1.0,Matx[8]); } if (fabs(Maty[2]-1.)>10.*DBL_EPSILON ||fabs(Maty[4]-1.)>10.*DBL_EPSILON||fabs(Maty[6]+1.)>10.*DBL_EPSILON ||fabs(Maty[0])>10.*DBL_EPSILON ||fabs(Maty[1])>10.*DBL_EPSILON ||fabs(Maty[3])>10.*DBL_EPSILON ||fabs(Maty[5])>10.*DBL_EPSILON ||fabs(Maty[7])>10.*DBL_EPSILON ||fabs(Maty[8])>10.*DBL_EPSILON) { errorcount++; fprintf(stdout," Quaternion2Matrix Maty wrong value \n [ %g, %g, %g]\n [ %g, %g, %g]\n [ %g, %g, %g ]\n" "!= [0, 0, 1]\n [0, 1, 0]\n [-1, 0, 0]\n", Maty[0],Maty[1],Maty[2]-1.0, Maty[3],Maty[4],Maty[5]-1.0, Maty[6]+1.0,Maty[7],Maty[8]); } if (fabs(Matz[1]+1.)>10.*DBL_EPSILON ||fabs(Matz[3]-1.)>10.*DBL_EPSILON||fabs(Matz[8]-1.)>10.*DBL_EPSILON ||fabs(Matz[0])>10.*DBL_EPSILON ||fabs(Matz[2])>10.*DBL_EPSILON ||fabs(Matz[4])>10.*DBL_EPSILON ||fabs(Matz[5])>10.*DBL_EPSILON ||fabs(Matz[6])>10.*DBL_EPSILON ||fabs(Matz[7])>10.*DBL_EPSILON) { errorcount++; fprintf(stdout," Quaternion2Matrix Matz wrong value \n [ %g, %g, %g]\n [ %g, %g, %g]\n [ %g, %g, %g ]\n" "!= [0, -1, 0]\n [1, 0, 0]\n [0, 0, 1]\n", Matz[0],Matz[1],Matz[2], Matz[3],Matz[4],Matz[5], Matz[6],Matz[7],Matz[8]); } EXX = EXY = EXZ = 0.; EYX = EYY = EYZ = 0.; EZX = EZY = EZZ = 0.; qx.Quaternion2Angles(EXX,EXY,EXZ); qy.Quaternion2Angles(EYX,EYY,EYZ); qz.Quaternion2Angles(EZX,EZY,EZZ); q1 = CPPQR::Angles2Quaternion(EXX,EXY,EXZ); q2 = CPPQR::Angles2Quaternion(EYX,EYY,EYZ); q3 = CPPQR::Angles2Quaternion(EZX,EZY,EZZ); q4 = qx / q1; normsq = q4.Normsq( ); if ( fabs(normsq-1.) > 10.*DBL_EPSILON || fabs(q4.GetW()*q4.GetW()-1.) > 10.*DBL_EPSILON) { errorcount++; fprintf(stdout," Angles2Quaternion q1 wrong value [%g, %g, %g, %g] != +/-[%g, %g, %g, %g]\n", qx.GetW(), qx.GetX(), qx.GetY(), qx.GetZ(), q1.GetW(), q1.GetX(), q1.GetY(), q1.GetZ() ); } q4 = qy / q2; normsq = q4.Normsq( ); if (fabs(normsq-1.) > 10.*DBL_EPSILON || fabs(q4.GetW()*q4.GetW()-1.) > 10.*DBL_EPSILON) { fprintf(stdout," Angles2Quaternion q2 wrong value [%g, %g, %g, %g] != +/-[%g, %g, %g, %g]\n", qy.GetW(), qy.GetX(), qy.GetY(), qy.GetZ(), q2.GetW(), q2.GetX(), q2.GetY(), q2.GetZ() ); } q4 = qz / q3; normsq = q4.Normsq( ); if (fabs(normsq-1.) > 10.*DBL_EPSILON || fabs(q4.GetW()*q4.GetW()-1.) > 10.*DBL_EPSILON) { fprintf(stdout," Angles2Quaternion q3 wrong value [%g, %g, %g, %g] != +/-[%g, %g, %g, %g]\n", qz.GetW(), qz.GetX(), qz.GetY(), qz.GetZ(), q3.GetW(), q3.GetX(), q3.GetY(), q3.GetZ() ); } { /* lca */ double m[9] = { 0.0,0.0,1.0, 1.0,0.0,0.0, 0.0,1.0,0.0 }; double mx[9]; double sum; CPPQR qq1, qq2, qq3; CPPQR qq4, qqx, qqy, qqz; CPPQR::Matrix2Quaternion(qq4, m ); CPPQR::Quaternion2Matrix(mx,qq4 ); sum = 0.0; for ( int i=0; i<9; ++i ) sum += fabs(m[i]-mx[i]); if ( sum > 1.0E-8 ) { errorcount++; fprintf( stdout, " Matrix2Quaternion difference\n" ); fprintf( stdout, " qq4 = {%g,%g,%g,%g}\n",qq4.GetW(),qq4.GetX(),qq4.GetY(),qq4.GetZ()); fprintf( stdout, " m = {{%g,%g,%g},{%g,%g,%g},{%g,%g,%g))\n", m[0],m[1],m[2],m[3],m[4],m[5],m[6],m[7],m[8]); fprintf( stdout, " mx = {{%g,%g,%g},{%g,%g,%g},{%g,%g,%g))\n", mx[0],mx[1],mx[2],mx[3],mx[4],mx[5],mx[6],mx[7],mx[8]); } } { CPPQR q1( 3,5,7,11 ); CPPQR q2( q1.GetW(), q1.GetX(), q1.GetY(), q1.GetZ() ); if ( q1 != q2 || !(q1==q2) ) { errorcount++; fprintf( stdout, " CPPQR test constructors, gets, ==, != failed\n" ); } } { CPPQR q; if( ::fabs( q.GetW()) + ::fabs( q.GetX()) + ::fabs( q.GetY()) + ::fabs( q.GetZ()) != 0.0 ) { errorcount++; fprintf( stdout, " CPPQR default constructor not zero\n" ); } } { CPPQR q1( 3,5,7,11 ); CPPQR q2( q1 ); CPPQR q3 = q2; if ( q1 != q2 || q1 != q3 ) { errorcount++; fprintf( stdout, " CPPQR copy constructor or assignment operator failed\n" ); } } { CPPQR q1( 3,5,7,11 ); CPPQR q2; q2.Set( q1.GetW(), q1.GetX(), q1.GetY(), q1.GetZ() ); if( q1 != q2 ) { errorcount++; fprintf( stdout, " CPPQR Set failed\n" ); } } { if( CPPQR(1,3,5,7)+CPPQR(-1,-3,-5,-7) !=CPPQR(0,0,0,0) ) { errorcount++; fprintf( stdout, " CPPQR add failed\n" ); } if( CPPQR(1,3,5,7)-CPPQR(1,3,5,7) !=CPPQR(0,0,0,0) ) { errorcount++; fprintf( stdout, " CPPQR subtract failed\n" ); } } { if ( CPPQR(1,3,5,7)*2.0 != CPPQR(4,12,20,28)/2.0 ) { errorcount++; fprintf( stdout, " CPPQR multiply or divide by scalar failed\n" ); } } { const CPPQR q1( CPPQR( 3,5,7,9 ) ); const double normsq = q1.Normsq( ); if ( normsq != 164.0 ) { errorcount++; fprintf ( stdout, " CPPQR Normsq failed \n" ); } const CPPQR q2 = q1.UnitQ( ); if( q1/sqrt(normsq) != q2 ) { errorcount++; fprintf( stdout, "UnitQ failed\n" ); } } { const CPPQR q1( CPPQR( 3,5,7,9 ) ); if ( q1.GetW() != q1[0] || q1.GetX() != q1[1] || q1.GetY() != q1[2] || q1.GetZ() != q1[3] ) { errorcount++; fprintf( stdout, "component fetches failed\n" ); } } /* Tests on [-sqrt(7),2,3,4] = 6*[-sqrt(7)/6,1/3,1/2,2/3] = 6*[-cos(1.11412994158827),sin(1.11412994158827)*[.3713906763541037, .5570860145311556, .7427813527082074]] = 6*[cos(2.027462712001523),sin(2.027462712001523)*[.3713906763541037, .5570860145311556, .7427813527082074]] = 6*exp([0,.3713906763541037, .5570860145311556, .7427813527082074]*2.027462712001523) so the log should be [log(6),0,0,0] +[0,.3713906763541037, .5570860145311556, .7427813527082074]*2.027462712001523] =[1.791759469228055, 0.752980747892971, 1.129471121839456, 1.505961495785942] Note that the log is multivalued */ { const CPPQR q1( CPPQR( -sqrt(7.),2,3,4 )); if ( q1.GetIm() != CPPQR( 0,2,3,4 ) ) { errorcount++; fprintf( stdout, "GetIm failed\n" ); } if ( q1.GetAxis().Dist(CPPQR( 0,2./sqrt(4.+9.+16.),3./sqrt(4.+9.+16.),4./sqrt(4.+9.+16.) ))>100.*DBL_EPSILON ) { errorcount++; fprintf( stdout, "GetAxis failed\n" ); } if (fabs(q1.GetAngle()-2.027462712001523)>40.*DBL_EPSILON*2.027462712001523) { errorcount++; fprintf( stdout, "GetAngle failed, got %g, expected %g\n",q1.GetAngle(),2.027462712001523 ); } if ((q1.log() - CPPQR(log(6.), 0.752980747892971, 1.129471121839457, 1.505961495785942)).Norm() > 40.*DBL_EPSILON*q1.log().Norm()) { errorcount++; fprintf( stdout, "quaternion log failed log([%g,%g,%g,%g]) = [%g,%g,%g,%g] instead of [%g,%g,%g,%g], normdiff = %g\n", q1.GetW(), q1.GetX(), q1.GetY(), q1.GetZ(), q1.log().GetW(), q1.log().GetX(), q1.log().GetY(), q1.log().GetZ(), log(6.), 0.752980747892971, 1.129471121839457, 1.505961495785942, (q1.log() - CPPQR(log(6.), 0.752980747892971, 1.129471121839457, 1.505961495785942)).Norm() ); } if (((q1.log()).exp()-q1).Norm()>10.*DBL_EPSILON*q1.Norm() || ((q1.exp()).log().exp()-q1.exp()).Norm()>10.*DBL_EPSILON*q1.exp().Norm()) { errorcount++; fprintf( stdout, "log(exp) or exp(log) failed\n," " q = [%g,%g,%g,%g]," " log = [%g,%g,%g,%g], exp(log) = [%g,%g,%g,%g]," " exp = [%g,%g,%g,%g], log(exp) = [%g,%g,%g,%g]\n", q1.GetW(),q1.GetX(),q1.GetY(),q1.GetZ(), q1.log().GetW(),q1.log().GetX(),q1.log().GetY(),q1.log().GetZ(), q1.log().exp().GetW(),q1.log().exp().GetX(),q1.log().exp().GetY(),q1.log().exp().GetZ(), q1.exp().GetW(),q1.exp().GetX(),q1.exp().GetY(),q1.exp().GetZ(), q1.exp().log().exp().GetW(),q1.exp().log().exp().GetX(),q1.exp().log().exp().GetY(),q1.exp().log().exp().GetZ() ); } for (int i = -5; i < 6; i++) { if ((q1.pow(i) - q1.pow(double(i))).Norm() > 10.*DBL_EPSILON*(q1.pow(i)).Norm()) { errorcount++; fprintf( stdout, "integer power double power comparison failed\n,"); } } } { const CPPQR q1( CPPQR( -4.,0.,0.,0. )); const CPPQR q2( CPPQR( -4.,1.,1.,1. )); const CPPQR q3( CPPQR( 4.,0.,0.,0. )); CPPQR qout1, qout2, qout3, qtest1, qtest2, qtest3; for (int i = 1; i < 9; i++) { for (int j = 0; j < i; j++ ) { qout1 = q1.root(i,j); qout2 = q2.root(i,j); qout3 = q3.root(i,j); qtest1 = qout1.pow(i); qtest2 = qout2.pow(i); qtest3 = qout3.pow(i); if (q1.Dist(qtest1) > 100.*DBL_EPSILON*q1.Norm() || q2.Dist(qtest2) > 100.*DBL_EPSILON*q2.Norm() || q3.Dist(qtest3) > 100.*DBL_EPSILON*q3.Norm()) { errorcount++; fprintf(stdout," %d'th root of [%g,%g,%g,%g] = [%g,%g,%g,%g], power = [%g,%g,%g,%g], delta %g\n", i, q1.GetW(), q1.GetX(), q1.GetY(), q1.GetZ(), qout1.GetW(), qout1.GetX(), qout1.GetY(), qout1.GetZ(), qtest1.GetW(), qtest1.GetX(), qtest1.GetY(), qtest1.GetZ(), (q1-qtest1).Norm()); fprintf(stdout," %d'th root of [%g,%g,%g,%g] = [%g,%g,%g,%g], power = [%g,%g,%g,%g], delta %g\n", i, q2.GetW(), q2.GetX(), q2.GetY(), q2.GetZ(), qout2.GetW(), qout2.GetX(), qout2.GetY(), qout2.GetZ(), qtest2.GetW(), qtest2.GetX(), qtest2.GetY(), qtest2.GetZ(), (q2-qtest2).Norm()); fprintf(stdout," %d'th root of [%g,%g,%g,%g] = [%g,%g,%g,%g], power = [%g,%g,%g,%g], delta %g\n", i, q3.GetW(), q3.GetX(), q3.GetY(), q3.GetZ(), qout3.GetW(), qout3.GetX(), qout3.GetY(), qout3.GetZ(), qtest3.GetW(), qtest3.GetX(), qtest3.GetY(), qtest3.GetZ(), (q3-qtest3).Norm()); } } } } return errorcount; } cqrlib-CQRlib-1.1.4/CPPQRTest_orig.lst000066400000000000000000000000001327146370000173650ustar00rootroot00000000000000cqrlib-CQRlib-1.1.4/CQRlibTest.c000066400000000000000000000466411327146370000162410ustar00rootroot00000000000000/* * CQRMTest.c * * * Created by Herbert J. Bernstein on 2/20/09. * Copyright 2009 Herbert J. Bernstein. All rights reserved. * */ /* Work supported in part by NIH NIGMS under grant 1R15GM078077-01 and DOE under grant ER63601-1021466-0009501. Any opinions, findings, and conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the funding agencies. */ /********************************************************************** * * * YOU MAY REDISTRIBUTE THE CQRlib API UNDER THE TERMS OF THE LGPL * * * **********************************************************************/ /************************* LGPL NOTICES ******************************* * * * This library is free software; you can redistribute it and/or * * modify it under the terms of the GNU Lesser General Public * * License as published by the Free Software Foundation; either * * version 2.1 of the License, or (at your option) any later version. * * * * This library is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * * Lesser General Public License for more details. * * * * You should have received a copy of the GNU Lesser General Public * * License along with this library; if not, write to the Free * * Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, * * MA 02110-1301 USA * * * **********************************************************************/ #include #include #include #include #ifndef USE_LOCAL_HEADERS #include #else #include "cqrlib.h" #endif #ifdef USE_MINGW_RAND #define rand(x) random(x) #define srand(x) srandom(x) #endif int main(int argc, char ** argv) { CQRQuaternionHandle q1, q2, q3; CQRQuaternion q4, qx, qy, qz; double normsq; double Matx[3][3], Maty[3][3], Matz[3][3]; double PI; double vx[3] = {1.,0.,0.}; double vy[3] = {0.,1.,0.}; double vz[3] = {0.,0.,1.}; double EXX,EXY,EXZ; double EYX,EYY,EYZ; double EZX,EZY,EZZ; int errorcount; int i, j; PI = 4.*atan2(1.,1.); errorcount = 0; if (CQRCreateEmptyQuaternion(&q1)) { errorcount++; fprintf(stderr," CQRCreatEmptyQuaternion for q1 failed\n"); } if (q1->w !=0. || q1->x !=0. || q1->y !=0. || q1->z !=0.) { errorcount++; fprintf(stderr," CQRCreateEmptyQuaternion for q1 non-zero [ %g, %g, %g, %g ]\n",q1->w,q1->x,q1->y,q1->z); } if (CQRCreateQuaternion(&q2, 1.,2.,3.,4.)) { errorcount++; fprintf(stderr," CQRCreateQuaternion for q2 failed\n"); } if (q2->w !=1. || q2->x !=2. || q2->y !=3. || q2->z !=4.) { errorcount++; fprintf(stderr," CQRCreateQuaternion for q2 wrong value [ %g, %g, %g, %g ] != [1.,2,3.,4.]\n",q2->w,q2->x,q2->y,q2->z); } if (CQRCreateEmptyQuaternion(&q3)) { errorcount++; fprintf(stderr," CQRCreateEmptyQuaternion for q3 failed\n"); } if (CQRAdd(q3,q1,q2)) { errorcount++; fprintf(stderr," CQRAdd(q3,q1,q2) failed\n"); } if (q3->w !=1. || q3->x !=2. || q3->y !=3. || q3->z !=4.) { errorcount++; fprintf(stderr," CQRAdd(q3,q1,q2) q3 wrong value [ %g, %g, %g, %g ] != [1.,2.,3.,4.]\n",q3->w,q3->x,q3->y,q3->z); } if (CQRSetQuaternion(q1,-9999.,-9998.,-9997.,-9996.)) { errorcount++; fprintf(stderr," CQRSetQuaternion(q1,-9999.,-9998.,-9997.,-9996.)\n"); } if (q1->w !=-9999. || q1->x !=-9998. || q1->y !=-9997. || q1->z !=-9996.) { errorcount++; fprintf(stderr," CQRSetQuaternion q1 wrong value [ %g, %g, %g, %g ] != [-9999.,-9998.,-9997.,-9996.]\n",q1->w,q1->x,q1->y,q1->z); } if (CQRSubtract(q1,q3,q2)) { errorcount++; fprintf(stderr," CQRSubtract(q1,q3,q2) failed\n"); } if (q1->w !=0. || q1->x !=0. || q1->y !=0. || q1->z !=0.) { errorcount++; fprintf(stderr," CQR Subtract(q1,q3,q2) for q1 non-zero [ %g, %g, %g, %g ]\n",q1->w,q1->x,q1->y,q1->z); } if (CQRSetQuaternion(&q4,1.,-2.,-3.,-4.)) { errorcount++; fprintf(stderr," CQRSetQuaternion(&q4,1.,-2.,-3.,-4.)\n"); } if (q4.w != 1. || q4.x !=-2. || q4.y !=-3. || q4.z !=-4.) { errorcount++; fprintf(stderr," CQRSetQuaternion &q4 wrong value [ %g, %g, %g, %g ] != [1.,-2.,-3.,-4.]\n",q4.w,q4.x,q4.y,q4.z); } if (CQRMultiply(q1,&q4,q2)) { errorcount++; fprintf(stderr," CQRMultiply(q1,&q4,q2) failed\n"); } if (fabs(q1->w-30.)>300.*DBL_EPSILON || fabs(q1->x)>300.*DBL_EPSILON || fabs(q1->y)>300.*DBL_EPSILON ||fabs(q1->z)>300.*DBL_EPSILON ) { errorcount++; fprintf(stderr," CQRMultiply(q1,&q4,q2) q1 wrong value [ %g, %g, %g, %g ] != [30.,0.,0.,0.]\n",q1->w,q1->x,q1->y,q1->z); } if (CQRDivide(q3,q1,q2)) { errorcount++; fprintf(stderr," CQRDivide(q3,q1,q2) failed\n"); } if (fabs(q3->w-1.)>60.*DBL_EPSILON || fabs(q3->x+2.)>60.*DBL_EPSILON || fabs(q3->y+3.)>60.*DBL_EPSILON ||fabs(q3->z+4.)>60.*DBL_EPSILON ) { errorcount++; fprintf(stderr," CQRDivide(q3,q1,q2) q3 wrong value [ %g, %g, %g, %g ] != [1.,-2.,-3.,-4.]\n",q3->w,q3->x,q3->y,q3->z); } if (CQRScalarMultiply(q3,q2,3.)) { errorcount++; fprintf(stderr," CQRScalarMultiply(q3,q1,3.) failed\n"); } if (fabs(q3->w-3.)>180.*DBL_EPSILON || fabs(q3->x-6.)>180.*DBL_EPSILON || fabs(q3->y-9.)>180.*DBL_EPSILON ||fabs(q3->z-12.)>180.*DBL_EPSILON ) { errorcount++; fprintf(stderr," CQRScalarMultiply(q3,q2,3.) q3 wrong value [ %g, %g, %g, %g ] != [3.,6.,9.,12.]\n",q3->w,q3->x,q3->y,q3->z); } if (CQRConjugate(q3,q2)) { errorcount++; fprintf(stderr," CQRConjugate(q3,q2) failed\n"); } if (q3->w != 1. || q3->x !=-2. || q3->y !=-3. || q3->z !=-4.) { errorcount++; fprintf(stderr," CQRConjugate(q3,q2) q3 wrong value [ %g, %g, %g, %g ] != [1.,-2.,-3.,-4.]\n",q3->w,q3->x,q3->y,q3->z); } if (CQREqual(&q4,q3)) { errorcount++; fprintf(stderr," CQREqual(&q4,q2) failed\n"); } if (CQRNormsq(&normsq,&q4) || fabs(normsq-30.) > 300.*DBL_EPSILON) { errorcount++; fprintf(stderr," CQRNormsq(&normsq,&q4) failed\n"); } if (CQRInverse(q3,&q4)) { errorcount++; fprintf(stderr,"CQRInverse(q3,&q4) failed\n"); } if (fabs(q3->w - 1./30.) > 2.*DBL_EPSILON || fabs(q3->x - 2./30.) > 2.*DBL_EPSILON || fabs(q3->y - 3./30.) > 2.*DBL_EPSILON || fabs(q3->z - 4./30.) > 2.*DBL_EPSILON) { errorcount++; fprintf(stderr," CQRInverse(q3,&q4) q3 wrong value [ %g, %g, %g, %g ] != [1./30.,2./30.,3./30,4./30.]\n",q3->w,q3->x,q3->y,q3->z); } /* Create quaternions to rotate about the x,y and z-axes by 90 degrees */ if (CQRAxis2Quaternion(&qx,vx,PI/2.)||CQRAxis2Quaternion(&qy,vy,PI/2.)||CQRAxis2Quaternion(&qz,vz,PI/2.)){ errorcount++; fprintf(stderr,"Axis2Quaternion failed\n"); } if (qx.w<0.||fabs(qx.w*qx.w-.5)>10.*DBL_EPSILON||fabs(qx.x*qx.x-.5)>10.*DBL_EPSILON||fabs(qx.y)>10.*DBL_EPSILON||fabs(qx.z)>10.*DBL_EPSILON) { errorcount++; fprintf(stderr,"Axis2Quaternion qx wrong value [ %g, %g, %g, %g ] != [sqrt(1./2.),sqrt(1./2.),0,0]\n",qx.w,qx.x,qx.y,qx.z); } if (qy.w<0.||fabs(qy.w*qy.w-.5)>10.*DBL_EPSILON||fabs(qy.y*qy.y-.5)>10.*DBL_EPSILON||fabs(qy.x)>10.*DBL_EPSILON||fabs(qy.z)>10.*DBL_EPSILON) { errorcount++; fprintf(stderr,"Axis2Quaternion qy wrong value [ %g, %g, %g, %g ] != [sqrt(1./2.),sqrt(1./2.),0,0]\n",qy.w,qy.x,qy.y,qy.z); } if (qz.w<0.||fabs(qz.w*qz.w-.5)>10.*DBL_EPSILON||fabs(qz.z*qz.z-.5)>10.*DBL_EPSILON||fabs(qz.x)>10.*DBL_EPSILON||fabs(qz.y)>10.*DBL_EPSILON) { errorcount++; fprintf(stderr,"Axis2Quaternion qz wrong value [ %g, %g, %g, %g ] != [sqrt(1./2.),sqrt(1./2.),0,0]\n",qz.w,qz.x,qz.y,qz.z); } if (CQRQuaternion2Matrix(Matx,&qx)||CQRQuaternion2Matrix(Maty,&qy)||CQRQuaternion2Matrix(Matz,&qz)){ errorcount++; fprintf(stderr," CQRQuaternion2Matrix failed\n"); } if (fabs(Matx[0][0]-1.)>10.*DBL_EPSILON ||fabs(Matx[1][2]+1.)>10.*DBL_EPSILON||fabs(Matx[2][1]-1.)>10.*DBL_EPSILON ||fabs(Matx[0][1])>10.*DBL_EPSILON ||fabs(Matx[0][2])>10.*DBL_EPSILON ||fabs(Matx[1][0])>10.*DBL_EPSILON ||fabs(Matx[1][1])>10.*DBL_EPSILON ||fabs(Matx[2][0])>10.*DBL_EPSILON ||fabs(Matx[2][2])>10.*DBL_EPSILON) { errorcount++; fprintf(stderr," CQRQuaternion2Matrix Matx wrong value \n [ %g, %g, %g ]\n [ %g, %g, %g ]\n [ %g, %g, %g ]\n" "!= [1, 0, 0]\n [0, 0, -1]\n [0, 1, 0]\n", Matx[0][0],Matx[0][1],Matx[0][2], Matx[1][0],Matx[1][1],Matx[1][2], Matx[2][0],Matx[2][1],Matx[2][2]); } if (fabs(Maty[0][2]-1.)>DBL_EPSILON ||fabs(Maty[1][1]-1.)>DBL_EPSILON||fabs(Maty[2][0]+1.)>DBL_EPSILON ||fabs(Maty[0][0])>DBL_EPSILON ||fabs(Maty[0][1])>DBL_EPSILON ||fabs(Maty[1][0])>DBL_EPSILON ||fabs(Maty[1][2])>DBL_EPSILON ||fabs(Maty[2][1])>DBL_EPSILON ||fabs(Maty[2][2])>DBL_EPSILON) { errorcount++; fprintf(stderr," CQRQuaternion2Matrix Maty wrong value \n [ %g, %g, %g]\n [ %g, %g, %g]\n [ %g, %g, %g ]\n" "!= [0, 0, 1]\n [0, 1, 0]\n [-1, 0, 0]\n", Maty[0][0],Maty[0][1],Maty[0][2], Maty[1][0],Maty[1][1],Maty[1][2], Maty[2][0],Maty[2][1],Maty[2][2]); } if (fabs(Matz[0][1]+1.)>DBL_EPSILON ||fabs(Matz[1][0]-1.)>DBL_EPSILON||fabs(Matz[2][2]-1.)>DBL_EPSILON ||fabs(Matz[0][0])>DBL_EPSILON ||fabs(Matz[0][2])>DBL_EPSILON ||fabs(Matz[1][1])>DBL_EPSILON ||fabs(Matz[1][2])>DBL_EPSILON ||fabs(Matz[2][0])>DBL_EPSILON ||fabs(Matz[2][1])>DBL_EPSILON) { errorcount++; fprintf(stderr," CQRQuaternion2Matrix Matz wrong value \n [ %g, %g, %g]\n [ %g, %g, %g]\n [ %g, %g, %g ]\n" "!= [0, -1, 0]\n [1, 0, 0]\n [0, 0, 1]\n", Matz[0][0],Matz[0][1],Matz[0][2], Matz[1][0],Matz[1][1],Matz[1][2], Matz[2][0],Matz[2][1],Matz[2][2]); } EXX = EXY = EXZ = 0.; EYX = EYY = EYZ = 0.; EZX = EZY = EZZ = 0.; if (CQRQuaternion2Angles(&EXX,&EXY,&EXZ,&qx) ||CQRQuaternion2Angles(&EYX,&EYY,&EYZ,&qy) ||CQRQuaternion2Angles(&EZX,&EZY,&EZZ,&qz) ){ errorcount++; fprintf(stderr," CQRQuaternion2Angles failed\n"); } if (CQRAngles2Quaternion(q1,EXX,EXY,EXZ)||CQRAngles2Quaternion(q2,EYX,EYY,EYZ)||CQRAngles2Quaternion(q3,EZX,EZY,EZZ)){ errorcount++; fprintf(stderr," CQRAngles2Quaternion failed\n"); } if (CQRDivide(&q4,&qx,q1) || CQRNormsq(&normsq,&q4) || fabs(normsq-1.) > 10.*DBL_EPSILON || fabs(q4.w*q4.w-1.) > 10.*DBL_EPSILON) { errorcount++; fprintf(stderr," CQRAngles2Quaternion q1 wrong value [%g, %g, %g, %g] != +/-[%g, %g, %g, %g]\n", qx.w, qx.x, qx.y, qx.z, q1->w, q1->x, q1->y, q1->z ); } if (CQRDivide(&q4,&qy,q2) || CQRNormsq(&normsq,&q4) || fabs(normsq-1.) > 10.*DBL_EPSILON || fabs(q4.w*q4.w-1.) > 10.*DBL_EPSILON) { errorcount++; fprintf(stderr," CQRAngles2Quaternion q2 wrong value [%g, %g, %g, %g] != +/-[%g, %g, %g, %g]\n", qy.w, qy.x, qy.y, qy.z, q2->w, q2->x, q2->y, q2->z ); } if (CQRDivide(&q4,&qz,q3) || CQRNormsq(&normsq,&q4) || fabs(normsq-1.) > 10.*DBL_EPSILON || fabs(q4.w*q4.w-1.) > 10.*DBL_EPSILON) { errorcount++; fprintf(stderr," CQRAngles2Quaternion q3 wrong value [%g, %g, %g, %g] != +/-[%g, %g, %g, %g]\n", qz.w, qz.x, qz.y, qz.z, q3->w, q3->x, q3->y, q3->z ); } if(CQRFreeQuaternion(&q1)) { errorcount++; fprintf(stderr," CQRFreeQuaternion(&q1) failed\n"); } if(CQRFreeQuaternion(&q2)) { errorcount++; fprintf(stderr," CQRFreeQuaternion(&q2) failed\n"); } if(CQRFreeQuaternion(&q3)) { errorcount++; fprintf(stderr," CQRFreeQuaternion(&q3) failed\n"); } { CQRQuaternion q1 = {3.,5.,7.,9.}; double qw,qx,qy,qz; CQRGetQuaternionW(&qw,&q1); CQRGetQuaternionX(&qx,&q1); CQRGetQuaternionY(&qy,&q1); CQRGetQuaternionZ(&qz,&q1); if ( qw != q1.w || qx != q1.x || qy != q1.y || qz != q1.z ) { errorcount++; fprintf( stdout, "CQRGetQuaternionW/X/Y/Z failed\n" ); } } /* Tests on [-sqrt(7),2,3,4] = 6*[-sqrt(7)/6,1/3,1/2,2/3] = 6*[-cos(1.11412994158827),sin(1.11412994158827)*[.3713906763541037, .5570860145311556, .7427813527082074]] = 6*[cos(2.027462712001523),sin(2.027462712001523)*[.3713906763541037, .5570860145311556, .7427813527082074]] = 6*exp([0,.3713906763541037, .5570860145311556, .7427813527082074]*2.027462712001523) so the log should be [log(6),0,0,0] +[0,.3713906763541037, .5570860145311556, .7427813527082074]*2.027462712001523] =[1.791759469228055, 0.752980747892971, 1.129471121839456, 1.505961495785942] Note that the log is multivalued */ { CQRQuaternion q1, q1Im, q1axis, q1log, qtemp, q1logexp, q1exp, q1explog, q1explogexp, q1powi, q1powd; double q1angle, norm, q1lognorm; double norm1, norm2, norm3, norm4; CQRMSet(q1,-sqrt(7.),2.,3.,4.); CQRGetQuaternionIm(&q1Im,&q1); if (q1Im.w != 0. || q1Im.x != 2. || q1Im.y != 3. || q1Im.z != 4. ) { errorcount++; fprintf( stdout, "CQRGetQuaternionIm failed\n" ); } CQRGetQuaternionAxis(&q1axis,&q1); if (q1axis.w != 0. || fabs(q1axis.x - 2./sqrt(4.+9.+16.)) > 100.*DBL_EPSILON || fabs(q1axis.y - 3./sqrt(4.+9.+16.)) > 100.*DBL_EPSILON || fabs(q1axis.z - 4./sqrt(4.+9.+16.)) > 100.*DBL_EPSILON ) { errorcount++; fprintf( stdout, "CQRGetQuaternionAxis failed\n" ); } CQRGetQuaternionAngle(&q1angle,&q1); if (fabs(q1angle-2.027462712001523)>10.*DBL_EPSILON*2.027462712001523) { errorcount++; fprintf( stdout, "CQRGetQuaternionAngle failed, got %g, expected %g\n",q1angle,2.027462712001523 ); } CQRLog(&q1log,&q1); CQRMSet(qtemp,log(6.), 0.752980747892971, 1.129471121839457, 1.505961495785942) CQRMSubtract(qtemp,qtemp,q1log); CQRMNorm(norm,qtemp); CQRMNorm(q1lognorm,q1log) if (norm > 10.*DBL_EPSILON*q1lognorm) { errorcount++; fprintf( stdout, "quaternion log failed log([%g,%g,%g,%g]) = [%g,%g,%g,%g] instead of [%g,%g,%g,%g], normdiff = %g\n", q1.w, q1.x, q1.y, q1.z, q1log.w, q1log.x, q1log.y, q1log.z, log(6.), 0.752980747892971, 1.129471121839457, 1.505961495785942, norm); } CQRLog(&q1log,&q1); CQRExp(&q1logexp,&q1log); CQRExp(&q1exp,&q1); CQRLog(&q1explog,&q1exp); CQRExp(&q1explogexp,&q1explog); CQRMSubtract(qtemp,q1logexp,q1); CQRMNorm(norm1,qtemp); CQRMSubtract(qtemp,q1explogexp,q1exp); CQRMNorm(norm2,qtemp); CQRMNorm(norm3,q1); CQRMNorm(norm4,q1exp) if (norm1>40.*DBL_EPSILON*norm3 || norm2>40.*DBL_EPSILON*norm4) { errorcount++; fprintf( stdout, "log(exp) or exp(log) failed\n," " q = [%g,%g,%g,%g]," " log = [%g,%g,%g,%g], exp(log) = [%g,%g,%g,%g]," " exp = [%g,%g,%g,%g], log(exp) = [%g,%g,%g,%g]\n", q1.w,q1.x,q1.y,q1.z, q1log.w,q1log.x,q1log.y,q1log.z, q1logexp.w,q1logexp.x,q1logexp.y,q1logexp.z, q1exp.w,q1exp.x,q1exp.y,q1exp.z, q1explogexp.w,q1explogexp.x,q1explogexp.y,q1explogexp.z ); } for (i = -5; i < 6; i++) { CQRDoublePower(&q1powd,&q1,(double)i); CQRIntegerPower(&q1powi,&q1,i); CQRMSubtract(qtemp,q1powd,q1powi); CQRMNorm(norm,qtemp); CQRMNorm(norm1,q1powi); if (norm > 40.*DBL_EPSILON*norm1) { errorcount++; fprintf( stdout, "integer power double power comparison failed\n,"); } } } { CQRQuaternion q1, q2, q3, qout1, qout2, qout3, qtest1, qtest2, qtest3; double norm1, norm2, norm3; double normq1, normq2, normq3; CQRMSet (q1, -4.,0.,0.,0. ); CQRMSet (q2, -4.,1.,1.,1. ); CQRMSet (q3, 4.,0.,0.,0. ); for (i = 1; i < 9; i++) { for (j = 0; j < i; j++ ) { CQRIntegerRoot(&qout1,&q1,i,j); CQRIntegerRoot(&qout2,&q2,i,j); CQRIntegerRoot(&qout3,&q3,i,j); CQRIntegerPower(&qtest1,&qout1,i); CQRIntegerPower(&qtest2,&qout2,i); CQRIntegerPower(&qtest3,&qout3,i); CQRMNorm(normq1,q1); CQRMNorm(normq2,q2); CQRMNorm(normq3,q3); CQRMDist(norm1,q1,qtest1); CQRMDist(norm2,q2,qtest2); CQRMDist(norm3,q3,qtest3); if (norm1 > 100.*DBL_EPSILON*normq1 || norm2 > 100.*DBL_EPSILON*normq2 || norm3 > 100.*DBL_EPSILON*normq3) { errorcount++; fprintf(stdout," %d'th root of [%g,%g,%g,%g] = [%g,%g,%g,%g], power = [%g,%g,%g,%g], delta %g\n", i, q1.w, q1.x, q1.y, q1.z, qout1.w, qout1.x, qout1.y, qout1.z, qtest1.w, qtest1.x, qtest1.y, qtest1.z, norm1); fprintf(stdout," %d'th root of [%g,%g,%g,%g] = [%g,%g,%g,%g], power = [%g,%g,%g,%g], delta %g\n", i, q2.w, q2.x, q2.y, q2.z, qout2.w, qout2.x, qout2.y, qout2.z, qtest2.w, qtest2.x, qtest2.y, qtest2.z, norm2); fprintf(stdout," %d'th root of [%g,%g,%g,%g] = [%g,%g,%g,%g], power = [%g,%g,%g,%g], delta %g\n", i, q3.w, q3.x, q3.y, q3.z, qout3.w, qout3.x, qout3.y, qout3.z, qtest3.w, qtest3.x, qtest3.y, qtest3.z, norm3); } } } } return errorcount; } cqrlib-CQRlib-1.1.4/CQRlibTest_orig.lst000066400000000000000000000000001327146370000176140ustar00rootroot00000000000000cqrlib-CQRlib-1.1.4/Makefile000066400000000000000000000231031327146370000155450ustar00rootroot00000000000000# # Makefile # CQRlib # # Created by Herbert J. Bernstein on 02/22/09. # Copyright 2009 Herbert J. Bernstein. All rights reserved. # # # Work supported in part by NIH NIGMS under grant 1R15GM078077-01 and DOE # under grant ER63601-1021466-0009501. Any opinions, findings, and # conclusions or recommendations expressed in this material are those of the # author(s) and do not necessarily reflect the views of the funding agencies. #********************************************************************** #* * #* YOU MAY REDISTRIBUTE THE CQRlib API UNDER THE TERMS OF THE LGPL * #* * #**********************************************************************/ #************************* LGPL NOTICES ******************************* #* * #* This library is free software; you can redistribute it and/or * #* modify it under the terms of the GNU Lesser General Public * #* License as published by the Free Software Foundation; either * #* version 2.1 of the License, or (at your option) any later version. * #* * #* This library is distributed in the hope that it will be useful, * #* but WITHOUT ANY WARRANTY; without even the implied warranty of * #* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * #* Lesser General Public License for more details. * #* * #* You should have received a copy of the GNU Lesser General Public * #* License along with this library; if not, write to the Free * #* Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, * #* MA 02110-1301 USA * #* * #**********************************************************************/ # Version string VERSION = 3:0:1 RELEASE = 1.1.0 # # Compiler and compilation flags # CC = gcc CXX = g++ CFLAGS = -g -O2 -Wall -ansi -pedantic CPPFLAGS = $(CFLAGS) -DCQR_NOCCODE=1 # # libtool path if system default is not suitable # #LIBTOOL = $(HOME)/bin/libtool ifndef LIBTOOL LIBTOOL = libtool endif # # If local headers must be quoted uncomment the next line # #USE_LOCAL_HEADERS = 1 # # Directories # ROOT = . LIB = $(ROOT)/lib BIN = $(ROOT)/bin SRC = $(ROOT) INC = $(ROOT) EXAMPLES = $(ROOT) TESTDATA = $(ROOT) #INSTALLDIR = /usr/local INSTALLDIR = $(HOME) # # Include directories # ifdef USE_LOCAL_HEADERS INCLUDES = -DUSE_LOCAL_HEADERS else INCLUDES = -I$(INC) endif COMPILE_COMMAND = $(LIBTOOL) --mode=compile $(CC) $(CFLAGS) $(INCLUDES) $(WARNINGS) -c LIBRARY_LINK_COMMAND = $(LIBTOOL) --mode=link $(CC) -version-info $(VERSION) -rpath $(INSTALLDIR)/lib BUILD_COMMAND_LOCAL = $(LIBTOOL) --mode=link $(CC) $(CFLAGS) $(INCLUDES) BUILD_COMMAND_DYNAMIC = $(LIBTOOL) --mode=link $(CC) $(CFLAGS) -dynamic -I $(INSTALLDIR)/include -L$(INSTALLDIR)/lib BUILD_COMMAND_STATIC = $(LIBTOOL) --mode=link $(CC) $(CFLAGS) -static -I $(INSTALLDIR)/include -L$(INSTALLDIR)/lib CPPCOMPILE_COMMAND = $(LIBTOOL) --mode=compile $(CXX) $(CPPFLAGS) $(INCLUDES) $(WARNINGS) -c CPPLIBRARY_LINK_COMMAND = $(LIBTOOL) --mode=link $(CXX) -version-info $(VERSION) -rpath $(INSTALLDIR)/lib CPPBUILD_COMMAND_LOCAL = $(LIBTOOL) --mode=link $(CXX) $(CPPFLAGS) $(INCLUDES) CPPBUILD_COMMAND_DYNAMIC= $(LIBTOOL) --mode=link $(CXX) $(CPPFLAGS) -dynamic -I $(INSTALLDIR)/include -L$(INSTALLDIR)/lib CPPBUILD_COMMAND_STATIC = $(LIBTOOL) --mode=link $(CXX) $(CPPFLAGS) -static -I $(INSTALLDIR)/include -L$(INSTALLDIR)/lib INSTALL_COMMAND = $(LIBTOOL) --mode=install cp INSTALL_FINISH_COMMAND = $(LIBTOOL) --mode=finish OBJ_EXT = lo LIB_EXT = la ###################################################################### # You should not need to make modifications below this line # ###################################################################### # # Suffixes of files to be used or built # .SUFFIXES: .c .$(OBJ_EXT) .$(LIB_EXT) # # Common dependencies # COMMONDEP = Makefile # # Source files # SOURCE = $(SRC)/cqrlib.c # # Header files # HEADERS = $(INC)/cqrlib.h # Default: instructions # default: @echo ' ' @echo '***************************************************************' @echo ' ' @echo ' PLEASE READ README_CQRlib.txt and lgpl.txt' @echo ' ' @echo ' Before making the CQRlib library and example programs, check' @echo ' that the chosen settings are correct' @echo ' ' @echo ' The current C and C++ compile commands are:' @echo ' ' @echo ' $(COMPILE_COMMAND)' @echo ' $(CPPCOMPILE_COMMAND)' @echo ' ' @echo ' The current library C and C++ link commands are:' @echo ' ' @echo ' $(LIBRARY_LINK_COMMAND)' @echo ' $(CPPLIBRARY_LINK_COMMAND)' @echo ' ' @echo ' The current C library local, dynamic and static build commands are:' @echo ' ' @echo ' $(BUILD_COMMAND_LOCAL)' @echo ' $(BUILD_COMMAND_DYNAMIC)' @echo ' $(BUILD_COMMAND_STATIC)' @echo ' ' @echo ' The current C++ template local, dynamic and static build commands are:' @echo ' ' @echo ' $(CPPBUILD_COMMAND_LOCAL)' @echo ' $(CPPBUILD_COMMAND_DYNAMIC)' @echo ' $(CPPBUILD_COMMAND_STATIC)' @echo ' ' @echo ' Before installing the CQRlib library and example programs, check' @echo ' that the install directory and install commands are correct:' @echo ' ' @echo ' The current values are :' @echo ' ' @echo ' $(INSTALLDIR) ' @echo ' $(INSTALL_COMMAND) ' @echo ' $(INSTALL_FINISH) ' @echo ' ' @echo ' To compile the CQRlib library and example programs type:' @echo ' ' @echo ' make clean' @echo ' make all' @echo ' ' @echo ' To run a set of tests type:' @echo ' ' @echo ' make tests' @echo ' ' @echo ' To clean up the directories type:' @echo ' ' @echo ' make clean' @echo ' ' @echo ' To install the library and binaries type:' @echo ' ' @echo ' make install' @echo ' ' @echo '***************************************************************' @echo ' ' # # Compile the library and examples # all: $(LIB) $(BIN) $(SOURCE) $(HEADERS) \ $(LIB)/libCQRlib.$(LIB_EXT) \ $(BIN)/CQRlibTest $(BIN)/CPPQRTest install: all $(INSTALLDIR) $(INSTALLDIR)/lib $(INSTALLDIR)/include \ $(INC) $(LIB)/libCQRlib.$(LIB_EXT) $(INC)/cqrlib.h $(INSTALL_COMMAND) $(LIB)/libCQRlib.$(LIB_EXT) $(INSTALLDIR)/lib/libCQRlib.$(LIB_EXT) $(INSTALL_FINISH_COMMAND) $(INSTALLDIR)/lib/libCQRlib.$(LIB_EXT) -cp $(INSTALLDIR)/include/cqrlib.h $(INSTALLDIR)/include/CQRlib_old.h cp $(INC)/cqrlib.h $(INSTALLDIR)/include/cqrlib.h chmod 644 $(INSTALLDIR)/include/cqrlib.h echo "Testing final install dynamic" $(BUILD_COMMAND_DYNAMIC) $(EXAMPLES)/CQRlibTest.c \ -lCQRlib -lm -o $(BIN)/CQRlibTest_dynamic $(BIN)/CQRlibTest_dynamic > $(TESTDATA)/CQRlibTest_dynamic.lst diff -b -c $(TESTDATA)/CQRlibTest_orig.lst \ $(TESTDATA)/CQRlibTest_dynamic.lst echo "Testing final install static" $(BUILD_COMMAND_STATIC) $(EXAMPLES)/CQRlibTest.c \ -lCQRlib -lm -o $(BIN)/CQRlibTest_static $(BIN)/CQRlibTest_static > $(TESTDATA)/CQRlibTest_static.lst diff -b -c $(TESTDATA)/CQRlibTest_orig.lst \ $(TESTDATA)/CQRlibTest_static.lst $(CPPBUILD_COMMAND_DYNAMIC) $(EXAMPLES)/CPPQRTest.cpp \ -lm -o $(BIN)/CPPQRTest_dynamic $(BIN)/CPPQRTest_dynamic > $(TESTDATA)/CPPQRTest_dynamic.lst diff -b -c $(TESTDATA)/CPPQRTest_orig.lst \ $(TESTDATA)/CPPQRTest_dynamic.lst $(CPPBUILD_COMMAND_STATIC) $(EXAMPLES)/CPPQRTest.cpp \ -lm -o $(BIN)/CPPQRTest_static $(BIN)/CPPQRTest_static > $(TESTDATA)/CPPQRTest_static.lst diff -b -c $(TESTDATA)/CPPQRTest_orig.lst \ $(TESTDATA)/CPPQRTest_static.lst # # Directories # $(INSTALLDIR): mkdir -p $(INSTALLDIR) $(INSTALLDIR)/lib: $(INSTALLDIR) mkdir -p $(INSTALLDIR)/lib $(INSTALLDIR)/bin: $(INSTALLDIR) mkdir -p $(INSTALLDIR)/bin $(INSTALLDIR)/include: $(INSTALLDIR) mkdir -p $(INSTALLDIR)/include $(LIB): mkdir $(LIB) $(BIN): mkdir $(BIN) # # CQRlib library # $(LIB)/libCQRlib.$(LIB_EXT): $(SOURCE) $(HEADERS) $(COMMONDEP) $(COMPILE_COMMAND) -c $(SOURCE) $(LIBRARY_LINK_COMMAND) -o $(LIB)/libCQRlib.$(LIB_EXT) *.$(OBJ_EXT) # # CQRlibTest example program # $(BIN)/CQRlibTest: $(LIB)/libCQRlib.$(LIB_EXT) $(EXAMPLES)/CQRlibTest.c $(BUILD_COMMAND_LOCAL) $(EXAMPLES)/CQRlibTest.c $(LIB)/libCQRlib.$(LIB_EXT) -lm \ -o $@ # # CPPQRTest example program # $(BIN)/CPPQRTest: $(EXAMPLES)/CPPQRTest.cpp $(CPPBUILD_COMMAND_LOCAL) $(EXAMPLES)/CPPQRTest.cpp -lm \ -o $@ # # Tests # tests: $(LIB) $(BIN) $(BIN)/CQRlibTest \ all $(TESTDATA)/CQRlibTest_orig.lst $(TESTDATA)/CPPQRTest_orig.lst $(BIN)/CQRlibTest > $(TESTDATA)/CQRlibTest.lst diff -b -c $(TESTDATA)/CQRlibTest_orig.lst \ $(TESTDATA)/CQRlibTest.lst $(BIN)/CPPQRTest > $(TESTDATA)/CPPQRTest.lst diff -b -c $(TESTDATA)/CPPQRTest_orig.lst \ $(TESTDATA)/CPPQRTest.lst # # Remove all non-source files # empty: @-rm -rf $(LIB) @-rm -rf $(BIN) @-rm -f $(TESTDATA)/CQRlibTest.lst @-rm -f $(TESTDATA)/CQRlibTest_static.lst @-rm -f $(TESTDATA)/CQRlibTest_dynamic.lst @-rm -f $(TESTDATA)/CPPQRTest.lst @-rm -f $(TESTDATA)/CPPQRTest_static.lst @-rm -f $(TESTDATA)/CPPQRTest_dynamic.lst # # Remove temporary files # clean: @-rm -f core @-rm -f *.o @-rm -f *.$(OBJ_EXT) @-rm -f *.c.* # # Restore to distribution state # distclean: clean empty cqrlib-CQRlib-1.1.4/README000077700000000000000000000000001327146370000176532README_CQRlib.txtustar00rootroot00000000000000cqrlib-CQRlib-1.1.4/README_CQRlib.html000066400000000000000000000652341327146370000171370ustar00rootroot00000000000000 README CQRlib -- API for Quaternion Rotations Get CQRlib at SourceForge.net. Fast, secure and Free Open Source software downloads

CQRlib -- ANSI C API for Quaternion Rotations

Release 1.1.4
29 Apr 2018
© 2008, 2009, 2010, 2014, 2018 Herbert J. Bernstein

You may distribute the CQRlib API under the LGPL

The 1.1.4 release is a documentation change to reflect a move of the source to github. The 1.1.3 release parenthesized uses of *this that caused errors from OSX clang. Thanks to Zack Settel for reporting the problem. The 1.1.2 release improved the portability of the code for Visual Studio. The 1.1.1 release relaxed some of the test constraints and parametrized the tests against DBL_EPSILON and added the Dist and Distsq functions. The 1.1 release added functions for log, exp, power and root, added a macro form of the norm and fixed the macro for inverse. The 1.0.6 release fixed an error in the CQRHLERPDist definition and comments. The 1.0.5 release added SLERP/HLERP support in C++ and C, moved from the vector project. The 1.0.4 release added a version of L. Andrews adaptation to a C++ template. The 1.0.3 release changed from use of a FAR macro to use of a CQR_FAR macro to avoid name conflicts. the macros for malloc, free, memmove and memset were also changed. The 1.0.2 release of 14 June 2009 corrected the Makefile for case-sensitive file systems and to include -lm in loading. Release 1.0.1 of 23 February 2009 was a minor documentation update to the original 1.0 release of 22 February 2009.

CQRlib is an ANSI C implementation of a utility library for quaternion arithmetic and quaternion rotation math. See

Work supported in part by NIH NIGMS under grant 1R15GM078077-01 and DOE under grant ER63601-1021466-0009501. Any opinions, findings, and conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the funding agencies.

Installation

The CQRlib package is available at https://github.com/yayahjb/cqrlib.git. A source zip file is available at https://github.com/yayahjb/cqrlib/archive/master.zip

When the source is downloaded and unpacked, you should have a directory cqrlib or master. To build you may need to install the libtool-bin package. To see the current settings for a build execute

make

which should give the following information: 


PLEASE READ README_CQRlib.txt and lgpl.txt
 
 Before making the CQRlib library and example programs, check
 that the chosen settings are correct
 
 The current C and C++ compile commands are:
 
   libtool --mode=compile gcc -g -O2  -Wall -ansi -pedantic -I.  -c
   libtool --mode=compile g++ -g -O2  -Wall -ansi -pedantic -DCQR_NOCCODE=1 -I.  -c
 
 The current library C and C++ link commands are:
 
   libtool --mode=link  gcc -version-info 3:0:1 -rpath /home/yaya/lib
   libtool --mode=link g++ -version-info 3:0:1 -rpath /home/yaya/lib
 
 The current C library local, dynamic and static build commands are:
 
   libtool --mode=link gcc -g -O2  -Wall -ansi -pedantic -I.
   libtool --mode=link gcc -g -O2  -Wall -ansi -pedantic -dynamic -I /home/yaya/include -L/home/yaya/lib
   libtool --mode=link gcc -g -O2  -Wall -ansi -pedantic -static -I /home/yaya/include -L/home/yaya/lib
 
 The current C++ template local, dynamic and static build commands are:
 
   libtool --mode=link g++ -g -O2  -Wall -ansi -pedantic -DCQR_NOCCODE=1 -I.
   libtool --mode=link g++ -g -O2  -Wall -ansi -pedantic -DCQR_NOCCODE=1 -dynamic -I /home/yaya/include -L/home/yaya/lib
   libtool --mode=link g++ -g -O2  -Wall -ansi -pedantic -DCQR_NOCCODE=1 -static -I /home/yaya/include -L/home/yaya/lib
 
 Before installing the CQRlib library and example programs, check
 that the install directory and install commands are correct:
 
 The current values are :
 
   /home/yaya 
   libtool --mode=install cp 
    
 
 To compile the CQRlib library and example programs type:
 
   make clean
   make all
 
 To run a set of tests type:
 
   make tests
 
 To clean up the directories type:
 
   make clean
 
 To install the library and binaries type:
 
   make install

If these settings need to be changed, edit Makefile. On some systems, e.g. Mac OS X, the default libtool is not appropriate. In that case you should install a recent version of libtool. The CQRlib kit has been tested with libtool versions 1.3.5, 1.5.4 and 2.4.6. If the system libtool is not to be used, define the variable LIBTOOL to be the path to the libtool executable, e.g. in bash

export LIBTOOL=$HOME/bin/libtool

of in the Makefie

LIBTOOL = $(HOME)/bin/libtool

If you need to include local header files using #include "..." instead of #include <...>, define the variable USE_LOCAL_HEADERS. This definition is forced if _MSC_VER is defined, meaning that local headers will automatically be used for Visual Studio.

Synopsis

#include <cqrlib.h> 


    /* CQRCreateQuaternion -- create a quaternion = w +ix+jy+kz */
    
    int CQRCreateQuaternion(CQRQuaternionHandle * quaternion, double w, double x, double y, double z); 
    
    /* CQRCreateEmptyQuaternion -- create a quaternion = 0 +i0+j0+k0 */
    
    int CQRCreateEmptyQuaternion(CQRQuaternionHandle * quaternion) ;
    
    /* CQRFreeQuaternion -- free a quaternion  */
    
    int CQRFreeQuaternion(CQRQuaternionHandle * quaternion);        
    
    /* CQRSetQuaternion -- create an existing quaternion = w +ix+jy+kz */
    
    int CQRSetQuaternion( CQRQuaternionHandle quaternion, double w, double x, double y, double z);

    /* CQRGetQuaternionW -- get the w component of a quaternion */
    
    int CQRGetQuaternionW( double CQR_FAR * qw, CQRQuaternionHandle q );
    
    /* CQRGetQuaternionX -- get the x component of a quaternion */
    
    int CQRGetQuaternionX( double CQR_FAR * qx, CQRQuaternionHandle q );
    
    /* CQRGetQuaternionY -- get the y component of a quaternion */
    
    int CQRGetQuaternionY( double CQR_FAR * qy, CQRQuaternionHandle q );
    
    /* CQRGetQuaternionZ -- get the z component of a quaternion */
    
    int CQRGetQuaternionZ( double CQR_FAR * qz, CQRQuaternionHandle q );
    
    /* CQRGetQuaternionIm -- get the imaginary component of a quaternion */
    
    int CQRGetQuaternionIm( CQRQuaternionHandle quaternion, CQRQuaternionHandle q );
    
    /* CQRGetQuaternionAxis -- get the axis for the polar representation of a quaternion */
    
    int CQRGetQuaternionAxis( CQRQuaternionHandle quaternion, CQRQuaternionHandle q );
    
    /* CQRGetQuaternionAngle -- get the angular component of the polar representation
     of aquaternion */
    
    int CQRGetQuaternionAngle( double CQR_FAR * angle, CQRQuaternionHandle q );
    
    /* CQRLog -- get the natural logarithm of a quaternion */
    
    int CQRLog( CQRQuaternionHandle quaternion, CQRQuaternionHandle q );
    
    /* CQRExp -- get the exponential (exp) of a quaternion */
    
    int CQRExp( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ); 
    
    /* CQRQuaternionPower -- take a quarernion to a quaternion power */
    
    int CQRQuaternionPower( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, CQRQuaternionHandle p);
    
    /* CQRDoublePower -- take a quarernion to a double power */
    
    int CQRDoublePower( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, double p);
    
    /* CQRIntegerPower -- take a quaternion to an integer power */
    
    int CQRIntegerPower( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, int p);
    
    /* CQRIntegerRoot -- take the given integer root  of a quaternion, returning
     the indicated mth choice from among multiple roots.
     For reals the cycle runs through first the i-based
     roots, then the j-based roots and then the k-based roots,
     out of the infinite number of possible roots of reals. */
    
    int CQRIntegerRoot( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, int r, int m);

    /*  CQRAdd -- add a quaternion (q1) to a quaternion (q2) */
    
    int CQRAdd (CQRQuaternionHandle quaternion,  CQRQuaternionHandle q1, CQRQuaternionHandle q2 );
    
    /*  CQRSubtract -- subtract a quaternion (q2) from a quaternion (q1)  */
    
    int CQRSubtract (CQRQuaternionHandle quaternion,  CQRQuaternionHandle q1, CQRQuaternionHandle q2 );
    
    /*  CQRMultiply -- multiply a quaternion (q1) by quaternion (q2)  */
    
    int CQRMultiply (CQRQuaternionHandle quaternion,  CQRQuaternionHandle q1, CQRQuaternionHandle q2 );
    
    /*  CQRDot -- dot product of quaternion (q1) by quaternion (q2) as 4-vectors  */
    
    int CQRDot (double CQR_FAR * dotprod,  CQRQuaternionHandle q1, CQRQuaternionHandle q2 );    

    /*  CQRDivide -- Divide a quaternion (q1) by quaternion (q2)  */
    
    int CQRDivide (CQRQuaternionHandle quaternion,  CQRQuaternionHandle q1, CQRQuaternionHandle q2 );

    /*  CQRScalarMultiply -- multiply a quaternion (q) by scalar (s)  */
    
    int CQRScalarMultiply (CQRQuaternionHandle quaternion,  CQRQuaternionHandle q, double s );

    /*  CQREqual -- return 0 if quaternion q1 == q2  */
    
    int CQREqual (CQRQuaternionHandle q1, CQRQuaternionHandle q2 );
    
    /*  CQRConjugate -- Form the conjugate of a quaternion qconj */

    int CQRConjugate (CQRQuaternionHandle qconjugate, CQRQuaternionHandle quaternion);
    
    /*  CQRNormsq -- Form the normsquared of a quaternion */
    
    int CQRNormsq (double * normsq, CQRQuaternionHandle quaternion ) ;
    
    /*  CQRNorm -- Form the norm of a quaternion */
    
    int CQRNorm (double * norm, CQRQuaternionHandle quaternion ) ;

    /*  CQRDistsq -- Form the distance squared between two quaternions */
    
    int CQRDistsq (double CQR_FAR * distsq, CQRQuaternionHandle q1, CQRQuaternionHandle q2) ;
    
    /*  CQRDist -- Form the distance between two quaternions */
    
    int CQRDist (double CQR_FAR * dist, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ) ;
    
    /*  CQRInverse -- Form the inverse of a quaternion */
    
    int CQRInverse (CQRQuaternionHandle inversequaternion, CQRQuaternionHandle quaternion );
    
    /* CQRRotateByQuaternion -- Rotate a vector by a Quaternion, w = qvq* */
    
    int CQRRotateByQuaternion(double * w, CQRQuaternionHandle rotquaternion, double * v);        
    
    /* CQRAxis2Quaternion -- Form the quaternion for a rotation around axis v  by angle theta */
    
    int CQRAxis2Quaternion (CQRQuaternionHandle rotquaternion, double * v, double theta);
    
    /* CQRMatrix2Quaterion -- Form the quaternion from a 3x3 rotation matrix R */
    
    int CQRMatrix2Quaternion (CQRQuaternionHandle rotquaternion, double R[3][3]);
    
    /* CQRQuaternion2Matrix -- Form the 3x3 rotation matrix from a quaternion */
    
    int CQRQuaternion2Matrix (double R[3][3], CQRQuaternionHandle rotquaternion);
    
    /* CQRQuaternion2Angles -- Convert a Quaternion into Euler Angles for Rz(Ry(Rx))) convention */
    
    int CQRQuaternion2Angles (double * RotX, double * RotY, double * RotZ, CQRQuaternionHandle rotquaternion);
    
    /* CQRAngles2Quaternion -- Convert Euler Angles for Rz(Ry(Rx))) convention into a quaternion */
    
    int CQRAngles2Quaternion (CQRQuaternionHandle rotquaternion, double RotX, double RotY, double RotZ );

    /* Represent a 3-vector as a quaternion with w=0 */
    
    int CQRPoint2Quaternion( CQRQuaternionHandle quaternion, double v[3] );
    
    /*  SLERP -- Spherical Linear Interpolation   */
    
    int CQRSLERP (CQRQuaternionHandle quaternion, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2,
                  const double w1, const double w2);
    
    /*  HLERP -- Hemispherical Linear Interpolation   */
    
    int CQRHLERP (CQRQuaternionHandle quaternion, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2,
                  const double w1, const double w2);
    
    /*  CQRSLERPDist -- Spherical Linear Interpolation distance */
    
    int CQRSLERPDist (double CQR_FAR * dist, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2);
    
    /*  CQRHLERPDist -- Hemispherical Linear Interpolation distance */
    
    int CQRHLERPDist (double CQR_FAR * dist, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2);
and for C++ 

template< typename DistanceType=double, typename VectorType=double[3], typename MatrixType=double[9] >
class CPPQR
{

public:

     /* Constructors  */
         inline CPPQR( );  // default constructor
         inline CPPQR( const CPPQR& q ); // copy constructor
         inline CPPQR( const DistanceType& wi, const DistanceType& xi, const DistanceType& yi, const DistanceType& zi );

     /* Set -- set the values of an existing quaternion = w +ix+jy+kz */
         inline void Set ( const DistanceType& wi, const DistanceType& xi, const DistanceType& yi, const DistanceType& zi ); 

     /* Accessors */
         inline DistanceType GetW( void ) const;
         inline DistanceType GetX( void ) const;
         inline DistanceType GetY( void ) const;
         inline DistanceType GetZ( void ) const;
         inline CPPQR GetIm( void ) const;
         inline CPPQR GetAxis( void ) const;
         inline double GetAngle( void ) const;
         
     /* Operators */
         inline CPPQR operator+ ( const CPPQR& q ) const;
         inline CPPQR& operator+= ( const CPPQR& q );
         inline CPPQR& operator-= ( const CPPQR& q );
         inline CPPQR operator- ( const CPPQR& q ) const;
         inline CPPQR operator* ( const CPPQR& q ) const;
         inline CPPQR operator/ ( const CPPQR& q2 ) const;
         inline CPPQR operator* ( const DistanceType& d ) const;
         inline CPPQR operator/ ( const DistanceType& d ) const;
         inline CPPQR Conjugate ( void ) const;
         inline CPPQR& operator= ( const CPPQR& q );
         inline bool operator== ( const CPPQR& q ) const;
         inline bool operator!= ( const CPPQR& q ) const;
         inline VectorType& operator* ( const VectorType& v );
         DistanceType operator[] ( const int k ) const;

     /* Dot -- Dot product of 2 quaternions as 4-vectors */
         inline DistanceType Dot( const CPPQR& q) const;

     /* Normsq -- Form the normsquared of a quaternion */
         inline DistanceType Normsq ( void ) const;

     /* Norm -- Form the norm of a quaternion */
         inline DistanceType Norm ( void ) const;

     /* Distsq -- Form the distance squared from a quaternion */
         inline DistanceType Distsq ( const CPPQR& q ) const;

     /* Dist -- Form the distance from a quaternion */
         inline DistanceType Dist ( const CPPQR& q ) const;

     /* Inverse -- Form the inverse of a quaternion */
         inline CPPQR Inverse ( void ) const;
         
     /* log -- Get the natural logarithm of a quaternion */
         inline CPPQR log( void ) const;
         
     /* exp -- Get the exponential of a quaternion */
         inline CPPQR exp( void ) const;
         
     /* pow -- Take a power of a quaternion */
         template 
         inline CPPQR pow( const powertype p) const;
         inline CPPQR pow( const int p) const;
         
     /* root -- Take an integer root of a quaternion */
         inline CPPQR root( const int r, const int m) const;

     /* RotateByQuaternion -- Rotate a vector by a Quaternion, w = qvq* */
         inline void RotateByQuaternion(VectorType &w, const VectorType v );
         inline VectorType& RotateByQuaternion( const VectorType v );

     /* Axis2Quaternion -- Form the quaternion for a rotation around axis v  by angle theta */
         static inline CPPQR Axis2Quaternion ( const DistanceType& angle, const VectorType v );
         static inline CPPQR Axis2Quaternion ( const VectorType v, const DistanceType& angle  );

     /* Matrix2Quaterion -- Form the quaternion from a 3x3 rotation matrix R */
         static inline void Matrix2Quaternion ( CPPQR& rotquaternion, const MatrixType m );
         static inline void Matrix2Quaternion ( CPPQR& rotquaternion, const DistanceType R[3][3] );

     /* Quaternion2Matrix -- Form the 3x3 rotation matrix from a quaternion */    
         static inline void Quaternion2Matrix( MatrixType& m, const CPPQR q );
         static inline void Quaternion2Matrix( DistanceType m[3][3], const CPPQR q );

     /* Get a unit quaternion from a general one */
         inline CPPQR UnitQ( void ) const;

     /* Quaternion2Angles -- Convert a Quaternion into Euler Angles for Rz(Ry(Rx))) convention */  
         inline bool Quaternion2Angles ( DistanceType& rotX, DistanceType& rotY, DistanceType& rotZ ) const;

     /* Angles2Quaternion -- Convert Euler Angles for Rz(Ry(Rx))) convention into a quaternion */
         static inline CPPQR Angles2Quaternion ( const DistanceType& rotX, const DistanceType& rotY, const DistanceType& rotZ );
         static inline CPPQR Point2Quaternion( const DistanceType v[3] );

     /*  SLERP -- Spherical Linear Interpolation  */
         inline CPPQR SLERP (const CPPQR& q, DistanceType w1, DistanceType w2) const;

     /*  HLERP -- Hemispherical Linear Interpolation */
         inline CPPQR HLERP (const CPPQR& q, DistanceType w1, DistanceType w2) const;

     /*  SLERPDist -- Spherical Linear Interpolation distance */
         inline DistanceType SLERPDist (const CPPQR& q) const;

     /*  HLERPDist -- Hemispherical Linear Interpolation distance */
         inline DistanceType HLERPDist (const CPPQR& q) const;

         
     

}; // end class CPPQR

Description

The cqrlib.h header file defines the CQRQuaternionHandle type as a pointer to a struct of the CQRQuaternion type: 


    typedef struct {
        double w;
        double x;
        double y;
        double z; } CQRQuaternion;
representing w + xi +yj + zk. A quaternion may be declared directly using the CQRQuaternion type or dynamically allocated by CQRCreateQuaternion or CQRCreateEmptyQuaternion, in which case it is a user responsibility to eventually free the allocated memory with CQRFreeQuaternion. The components of an existing quaternion may be set by CQRSetQuaternion.

The rules of quaternion arithmetic are applied:

-1 = i*i = j*j = k*k, i*j=k=-j*i, j*k=i=-j*k, k*i=j=-i*k

by CQRAdd, CQRSubtract, CQRMultiply and CQRDivide. CQRScalarMultiply multiplies a quaternion by a scalar.

CQREqual returns 0 if quaternion q1 == q2, component by component. CQRConjugate computes a quaternion with the same scalar component and the negative of the vector component. CQRNormsq computes the sum of the squares of the components. CQRInverse computes the inverse of a non-zero quaternion.

The functions CQRGetQuaternionW, CQRGetQuaternionX, CQRGetQuaternionY and CQRGetQuaternionZ extract the 4 components of a quaternion. The function CQRGetQuaternionIm extract the imaginary part of a quaternion as a quaternion with w=0. The function CQRQGetQuaternion extracts the imaginary part and normalizes it to a unit vector. The function CQRGetQuaternionAngle extracts the angle for the polar representation of a quaternion as an exponential (see below).

In handling rotations, a right-handed system is assumed. CQRRotateByQuaternion rotates a vector by a quaternion, w = qvq*. CQRAxis2Quaternion forms the quaternion for a rotation around axis v by angle theta. CQRMatrix2Quaterion forms the quaternion equivalent a 3x3 rotation matrix R. CQRQuaternion2Matrix forms a 3x3 rotation matrix from a quaternion. CQRQuaternion2Angles converts a quaternion into Euler Angles for the Rz(Ry(Rx))) convention. CQRAngles2Quaternion convert Euler angles for the Rz(Ry(Rx))) convention into a quaternion.

The logarithm of a quaternion in CQRLog is based on the polar representation

q = r*cos(theta) + r*sin(theta) [ i*axis_x + j*axis_y +k*axis_z]
= r*exp(theta*[ i*axis_x + j*axis_y +k*axis_z])

with a unit axis. Then the natural logarithm is given by

log(q) = log(r) + theta*[ i*axis_x + j*axis_y +k*axis_z])

Note than any integer multiple of 2*PI could have been added to theta, so the logarithm is multivalued. The code only returns one of these values. The exponential in CQRExp is created by reversing the transformation. Taking a quaternion to a quaternion power is done by taking the log, multiplying by the power and then taking the exponential. Only one representative power is returned by CQRQuaternionPower. CQRDoublePower takes a quaternion to a double power by the same log-multiply-exp approach. CQRIntegerPower applies positive and negative integer powers by multiplication withou taking any logs or exponentials. CQRIntegerRoot applies the log-multiply-exp approach for integer roots. The second integer argument allow selection of one of the multiple roots. For roots of quaternions with a non-zero imaginary part, there are r roots, so m = 0, 1, 2, ... r-1 are meaningful. For roots of reals, there can be infinitely many alternate roots. In the case, m will cycle first through the i-based roots, then the j-based roots and then the k-based roots.

The SLERP and HLERP functions combine quaternions by speherical linear interpolation. SLERP take two quaternions and two weights and combine them following a great circle on the unit quaternion 4-D sphere and linear interpolation between the radii. SLERP keeps a quaternion separate from the negative of the same quaternion and is not appropriate for quaternions representing rotations. Use HLERP to apply SLERP to quaternions representing rotations.

If operating with __cplusplus defined, then the CPPQR template is defined allowing the creation of CPPQR quaternion objects. The template has three typename arguments: DistanceType, VectorType and MatrixType that default to double, double[3] and double[9]. Specializations are provided to support a double[3][3] MatrixType.

Returns

The CQRlib functions return 0 for normal completion, or the sum of one or more of the following non-zero error codes:

Error Return Numeric Value    Meaning
CQR_BAD_ARGUMENT    1    /* An argument is not valid */
CQR_NO_MEMORY    2    /* A call to allocate memory failed */
CQR_FAILED    4    /* Operation failed */

Examples

To create a quaternion dynamically from memory, initialized as the x vector with a zero scalar value, reporting failure to stderr: 


        #include <cqrlib.h>
        #include <stdio.h>
        ...
        CQRQuaternionHandle quathandle;
        ...
        if (CQRCreateQuaternion(&quathandle,0.,1.,0.,0.)) fprintf(stderr," CQRCreateQuaternion failed!!\n");

To create an x vector quaternion, a y vector quaternion, add then together and multiply by a z-vector, and print the result : 


        #include <cqrlib.h>
        #include <stdio.h>
        ...
        CQRQuaternion qx, qy, qz, qresult1, qresult2;
        ...
        if (CQRSetQuaternion(&qx,0.,1.,0.,0.)
          ||CQRSetQuaternion(&qy,0.,0.,1.,0.)
          ||CQRSetQuaternion(&qz,0.,0.,0.,1.)) fprintf(stderr," CQRSetQuaternion failed!!\n");
        if (CQRAdd(&qresult1,&qx,&qy)||CQRMultiply(&qresult2,&qresult1,&qz)) 
          fprintf(stderr," CQR Add or Multiply failed!!\n");
        fprintf(stdout,"Result = ((i+j)*k) = %g %+gi %+gj + %+gk\n",
          qresult2.w, qresult2.x, qresult2.y, qresult2.z);

The output should be "Result = ((i+j)*k) = 0 +1i -1j +0k".

To rotate the 3D vector [-1.,0.,1.] 90 degrees clockwise around the vector [1.,1.,1.]: 


        #include <cqrlib.h>
        #include <math.h>
        #include <stdio.h>
        ...
        double axis[3] = {1.,1.,1.};
        double vector[3] = {-1.,0.,1.};
        double result[3];
        CQRQuaternion rotquat;
        
        double PI;
        PI = 4.*atan2(1.,1.);

        CQRAxis2Quaternion(&rotquat,axis,PI/2);
        CQRRotateByQuaternion(result, &rotquat, vector);
        ...
        fprintf(stdout," [-1.,0.,1.] rotated 90 degrees clockwise"
        " around the vector [1.,1.,1.] = [%g, %g, %g]\n",
        result[0], result[1], result[2]);

The output should be "[-1.,0.,1.] rotated 90 degrees clockwise around the vector [1.,1.,1.] = [0.57735, -1.1547, 0.57735]".

See the test program CQRlibTest.c.

For examples of the use of the CPPQR template, see the C++ test program CPPQRTest.cpp.

Updated 29 April 2018
 cqrlib-CQRlib-1.1.4/README_CQRlib.txt000066400000000000000000000616601327146370000170110ustar00rootroot00000000000000 Get CQRlib at SourceForge.net. Fast, secure and Free Open Source software downloads ---------------------------------------------------------------------- CQRlib -- ANSI C API for Quaternion Rotations Release 1.1.4 29 Apr 2018 © 2008, 2009, 2010, 2014, 2018 Herbert J. Bernstein yayahjb at gmail dot com You may distribute the CQRlib API under the LGPL The 1.1.4 release is a documentation change to reflect a move of the source to github. The 1.1.3 release parenthesized uses of *this that caused errors from OSX clang. Thanks to Zack Settel for reporting the problem. The 1.1.2 release improved the portability of the code for Visual Studio. The 1.1.1 release relaxed some of the test constraints and parametrized the tests against DBL_EPSILON and added the Dist and Distsq functions. The 1.1 release added functions for log, exp, power and root, added a macro form of the norm and fixed the macro for inverse. The 1.0.6 release fixed an error in the CQRHLERPDist definition and comments. The 1.0.5 release added SLERP/HLERP support in C++ and C, moved from the vector project. The 1.0.4 release added a version of L. Andrews adaptation to a C++ template. The 1.0.3 release changed from use of a FAR macro to use of a CQR_FAR macro to avoid name conflicts. the macros for malloc, free, memmove and memset were also changed. The 1.0.2 release of 14 June 2009 corrected the Makefile for case-sensitive file systems and to include -lm in loading. Release 1.0.1 of 23 February 2009 was a minor documentation update to the original 1.0 release of 22 February 2009. CQRlib is an ANSI C implementation of a utility library for quaternion arithmetic and quaternion rotation math. See * "Quaternions and spatial rotation", Wikipedia http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation * K. Shoemake, "Quaternions", Department of Computer Science, University of Pennsylvania, Philadelphia, PA 19104, ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z * K. Shoemake, "Animating rotation with quaternion curves", ACM SIGGRAPH Computer Graphics, Vol 19, No. 3, pp 245--254, 1985. Work supported in part by NIH NIGMS under grant 1R15GM078077-01 and DOE under grant ER63601-1021466-0009501. Any opinions, findings, and conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the funding agencies. Installation The CQRlib package is available at https://github.com/yayahjb/cqrlib.git. A source zip file is available at https://github.com/yayahjb/cqrlib/archive/master.zip When the source is downloaded and unpacked, you should have a directory cqrlib or master. To build you may need to install the libtool-bin package. To see the current settings for a build execute make which should give the following information: PLEASE READ README_CQRlib.txt and lgpl.txt Before making the CQRlib library and example programs, check that the chosen settings are correct The current C and C++ compile commands are: libtool --mode=compile gcc -g -O2 -Wall -ansi -pedantic -I. -c libtool --mode=compile g++ -g -O2 -Wall -ansi -pedantic -DCQR_NOCCODE=1 -I. -c The current library C and C++ link commands are: libtool --mode=link gcc -version-info 3:0:1 -rpath /home/yaya/lib libtool --mode=link g++ -version-info 3:0:1 -rpath /home/yaya/lib The current C library local, dynamic and static build commands are: libtool --mode=link gcc -g -O2 -Wall -ansi -pedantic -I. libtool --mode=link gcc -g -O2 -Wall -ansi -pedantic -dynamic -I /home/yaya/include -L/home/yaya/lib libtool --mode=link gcc -g -O2 -Wall -ansi -pedantic -static -I /home/yaya/include -L/home/yaya/lib The current C++ template local, dynamic and static build commands are: libtool --mode=link g++ -g -O2 -Wall -ansi -pedantic -DCQR_NOCCODE=1 -I. libtool --mode=link g++ -g -O2 -Wall -ansi -pedantic -DCQR_NOCCODE=1 -dynamic -I /home/yaya/include -L/home/yaya/lib libtool --mode=link g++ -g -O2 -Wall -ansi -pedantic -DCQR_NOCCODE=1 -static -I /home/yaya/include -L/home/yaya/lib Before installing the CQRlib library and example programs, check that the install directory and install commands are correct: The current values are : /home/yaya libtool --mode=install cp To compile the CQRlib library and example programs type: make clean make all To run a set of tests type: make tests To clean up the directories type: make clean To install the library and binaries type: make install If these settings need to be changed, edit Makefile. On some systems, e.g. Mac OS X, the default libtool is not appropriate. In that case you should install a recent version of libtool. The CQRlib kit has been tested with libtool versions 1.3.5, 1.5.4 and 2.4.6. If the system libtool is not to be used, define the variable LIBTOOL to be the path to the libtool executable, e.g. in bash export LIBTOOL=$HOME/bin/libtool of in the Makefie LIBTOOL = $(HOME)/bin/libtool If you need to include local header files using #include "..." instead of #include <...>, define the variable USE_LOCAL_HEADERS. This definition is forced if _MSC_VER is defined, meaning that local headers will automatically be used for Visual Studio. Synopsis #include /* CQRCreateQuaternion -- create a quaternion = w +ix+jy+kz */ int CQRCreateQuaternion(CQRQuaternionHandle * quaternion, double w, double x, double y, double z); /* CQRCreateEmptyQuaternion -- create a quaternion = 0 +i0+j0+k0 */ int CQRCreateEmptyQuaternion(CQRQuaternionHandle * quaternion) ; /* CQRFreeQuaternion -- free a quaternion */ int CQRFreeQuaternion(CQRQuaternionHandle * quaternion); /* CQRSetQuaternion -- create an existing quaternion = w +ix+jy+kz */ int CQRSetQuaternion( CQRQuaternionHandle quaternion, double w, double x, double y, double z); /* CQRGetQuaternionW -- get the w component of a quaternion */ int CQRGetQuaternionW( double CQR_FAR * qw, CQRQuaternionHandle q ); /* CQRGetQuaternionX -- get the x component of a quaternion */ int CQRGetQuaternionX( double CQR_FAR * qx, CQRQuaternionHandle q ); /* CQRGetQuaternionY -- get the y component of a quaternion */ int CQRGetQuaternionY( double CQR_FAR * qy, CQRQuaternionHandle q ); /* CQRGetQuaternionZ -- get the z component of a quaternion */ int CQRGetQuaternionZ( double CQR_FAR * qz, CQRQuaternionHandle q ); /* CQRGetQuaternionIm -- get the imaginary component of a quaternion */ int CQRGetQuaternionIm( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ); /* CQRGetQuaternionAxis -- get the axis for the polar representation of a quaternion */ int CQRGetQuaternionAxis( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ); /* CQRGetQuaternionAngle -- get the angular component of the polar representation of aquaternion */ int CQRGetQuaternionAngle( double CQR_FAR * angle, CQRQuaternionHandle q ); /* CQRLog -- get the natural logarithm of a quaternion */ int CQRLog( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ); /* CQRExp -- get the exponential (exp) of a quaternion */ int CQRExp( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ); /* CQRQuaternionPower -- take a quarernion to a quaternion power */ int CQRQuaternionPower( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, CQRQuaternionHandle p); /* CQRDoublePower -- take a quarernion to a double power */ int CQRDoublePower( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, double p); /* CQRIntegerPower -- take a quaternion to an integer power */ int CQRIntegerPower( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, int p); /* CQRIntegerRoot -- take the given integer root of a quaternion, returning the indicated mth choice from among multiple roots. For reals the cycle runs through first the i-based roots, then the j-based roots and then the k-based roots, out of the infinite number of possible roots of reals. */ int CQRIntegerRoot( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, int r, int m); /* CQRAdd -- add a quaternion (q1) to a quaternion (q2) */ int CQRAdd (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ); /* CQRSubtract -- subtract a quaternion (q2) from a quaternion (q1) */ int CQRSubtract (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ); /* CQRMultiply -- multiply a quaternion (q1) by quaternion (q2) */ int CQRMultiply (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ); /* CQRDot -- dot product of quaternion (q1) by quaternion (q2) as 4-vectors */ int CQRDot (double CQR_FAR * dotprod, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ); /* CQRDivide -- Divide a quaternion (q1) by quaternion (q2) */ int CQRDivide (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ); /* CQRScalarMultiply -- multiply a quaternion (q) by scalar (s) */ int CQRScalarMultiply (CQRQuaternionHandle quaternion, CQRQuaternionHandle q, double s ); /* CQREqual -- return 0 if quaternion q1 == q2 */ int CQREqual (CQRQuaternionHandle q1, CQRQuaternionHandle q2 ); /* CQRConjugate -- Form the conjugate of a quaternion qconj */ int CQRConjugate (CQRQuaternionHandle qconjugate, CQRQuaternionHandle quaternion); /* CQRNormsq -- Form the normsquared of a quaternion */ int CQRNormsq (double * normsq, CQRQuaternionHandle quaternion ) ; /* CQRNorm -- Form the norm of a quaternion */ int CQRNorm (double * norm, CQRQuaternionHandle quaternion ) ; /* CQRDistsq -- Form the distance squared between two quaternions */ int CQRDistsq (double CQR_FAR * distsq, CQRQuaternionHandle q1, CQRQuaternionHandle q2) ; /* CQRDist -- Form the distance between two quaternions */ int CQRDist (double CQR_FAR * dist, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ) ; /* CQRInverse -- Form the inverse of a quaternion */ int CQRInverse (CQRQuaternionHandle inversequaternion, CQRQuaternionHandle quaternion ); /* CQRRotateByQuaternion -- Rotate a vector by a Quaternion, w = qvq* */ int CQRRotateByQuaternion(double * w, CQRQuaternionHandle rotquaternion, double * v); /* CQRAxis2Quaternion -- Form the quaternion for a rotation around axis v by angle theta */ int CQRAxis2Quaternion (CQRQuaternionHandle rotquaternion, double * v, double theta); /* CQRMatrix2Quaterion -- Form the quaternion from a 3x3 rotation matrix R */ int CQRMatrix2Quaternion (CQRQuaternionHandle rotquaternion, double R[3][3]); /* CQRQuaternion2Matrix -- Form the 3x3 rotation matrix from a quaternion */ int CQRQuaternion2Matrix (double R[3][3], CQRQuaternionHandle rotquaternion); /* CQRQuaternion2Angles -- Convert a Quaternion into Euler Angles for Rz(Ry(Rx))) convention */ int CQRQuaternion2Angles (double * RotX, double * RotY, double * RotZ, CQRQuaternionHandle rotquaternion); /* CQRAngles2Quaternion -- Convert Euler Angles for Rz(Ry(Rx))) convention into a quaternion */ int CQRAngles2Quaternion (CQRQuaternionHandle rotquaternion, double RotX, double RotY, double RotZ ); /* Represent a 3-vector as a quaternion with w=0 */ int CQRPoint2Quaternion( CQRQuaternionHandle quaternion, double v[3] ); /* SLERP -- Spherical Linear Interpolation */ int CQRSLERP (CQRQuaternionHandle quaternion, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2, const double w1, const double w2); /* HLERP -- Hemispherical Linear Interpolation */ int CQRHLERP (CQRQuaternionHandle quaternion, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2, const double w1, const double w2); /* CQRSLERPDist -- Spherical Linear Interpolation distance */ int CQRSLERPDist (double CQR_FAR * dist, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2); /* CQRHLERPDist -- Hemispherical Linear Interpolation distance */ int CQRHLERPDist (double CQR_FAR * dist, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2); and for C++ template< typename DistanceType=double, typename VectorType=double[3], typename MatrixType=double[9] > class CPPQR { public: /* Constructors */ inline CPPQR( ); // default constructor inline CPPQR( const CPPQR& q ); // copy constructor inline CPPQR( const DistanceType& wi, const DistanceType& xi, const DistanceType& yi, const DistanceType& zi ); /* Set -- set the values of an existing quaternion = w +ix+jy+kz */ inline void Set ( const DistanceType& wi, const DistanceType& xi, const DistanceType& yi, const DistanceType& zi ); /* Accessors */ inline DistanceType GetW( void ) const; inline DistanceType GetX( void ) const; inline DistanceType GetY( void ) const; inline DistanceType GetZ( void ) const; inline CPPQR GetIm( void ) const; inline CPPQR GetAxis( void ) const; inline double GetAngle( void ) const; /* Operators */ inline CPPQR operator+ ( const CPPQR& q ) const; inline CPPQR& operator+= ( const CPPQR& q ); inline CPPQR& operator-= ( const CPPQR& q ); inline CPPQR operator- ( const CPPQR& q ) const; inline CPPQR operator* ( const CPPQR& q ) const; inline CPPQR operator/ ( const CPPQR& q2 ) const; inline CPPQR operator* ( const DistanceType& d ) const; inline CPPQR operator/ ( const DistanceType& d ) const; inline CPPQR Conjugate ( void ) const; inline CPPQR& operator= ( const CPPQR& q ); inline bool operator== ( const CPPQR& q ) const; inline bool operator!= ( const CPPQR& q ) const; inline VectorType& operator* ( const VectorType& v ); DistanceType operator[] ( const int k ) const; /* Dot -- Dot product of 2 quaternions as 4-vectors */ inline DistanceType Dot( const CPPQR& q) const; /* Normsq -- Form the normsquared of a quaternion */ inline DistanceType Normsq ( void ) const; /* Norm -- Form the norm of a quaternion */ inline DistanceType Norm ( void ) const; /* Distsq -- Form the distance squared from a quaternion */ inline DistanceType Distsq ( const CPPQR& q ) const; /* Dist -- Form the distance from a quaternion */ inline DistanceType Dist ( const CPPQR& q ) const; /* Inverse -- Form the inverse of a quaternion */ inline CPPQR Inverse ( void ) const; /* log -- Get the natural logarithm of a quaternion */ inline CPPQR log( void ) const; /* exp -- Get the exponential of a quaternion */ inline CPPQR exp( void ) const; /* pow -- Take a power of a quaternion */ template inline CPPQR pow( const powertype p) const; inline CPPQR pow( const int p) const; /* root -- Take an integer root of a quaternion */ inline CPPQR root( const int r, const int m) const; /* RotateByQuaternion -- Rotate a vector by a Quaternion, w = qvq* */ inline void RotateByQuaternion(VectorType &w, const VectorType v ); inline VectorType& RotateByQuaternion( const VectorType v ); /* Axis2Quaternion -- Form the quaternion for a rotation around axis v by angle theta */ static inline CPPQR Axis2Quaternion ( const DistanceType& angle, const VectorType v ); static inline CPPQR Axis2Quaternion ( const VectorType v, const DistanceType& angle ); /* Matrix2Quaterion -- Form the quaternion from a 3x3 rotation matrix R */ static inline void Matrix2Quaternion ( CPPQR& rotquaternion, const MatrixType m ); static inline void Matrix2Quaternion ( CPPQR& rotquaternion, const DistanceType R[3][3] ); /* Quaternion2Matrix -- Form the 3x3 rotation matrix from a quaternion */ static inline void Quaternion2Matrix( MatrixType& m, const CPPQR q ); static inline void Quaternion2Matrix( DistanceType m[3][3], const CPPQR q ); /* Get a unit quaternion from a general one */ inline CPPQR UnitQ( void ) const; /* Quaternion2Angles -- Convert a Quaternion into Euler Angles for Rz(Ry(Rx))) convention */ inline bool Quaternion2Angles ( DistanceType& rotX, DistanceType& rotY, DistanceType& rotZ ) const; /* Angles2Quaternion -- Convert Euler Angles for Rz(Ry(Rx))) convention into a quaternion */ static inline CPPQR Angles2Quaternion ( const DistanceType& rotX, const DistanceType& rotY, const DistanceType& rotZ ); static inline CPPQR Point2Quaternion( const DistanceType v[3] ); /* SLERP -- Spherical Linear Interpolation */ inline CPPQR SLERP (const CPPQR& q, DistanceType w1, DistanceType w2) const; /* HLERP -- Hemispherical Linear Interpolation */ inline CPPQR HLERP (const CPPQR& q, DistanceType w1, DistanceType w2) const; /* SLERPDist -- Spherical Linear Interpolation distance */ inline DistanceType SLERPDist (const CPPQR& q) const; /* HLERPDist -- Hemispherical Linear Interpolation distance */ inline DistanceType HLERPDist (const CPPQR& q) const; }; // end class CPPQR Description The cqrlib.h header file defines the CQRQuaternionHandle type as a pointer to a struct of the CQRQuaternion type: typedef struct { double w; double x; double y; double z; } CQRQuaternion; representing w + xi +yj + zk. A quaternion may be declared directly using the CQRQuaternion type or dynamically allocated by CQRCreateQuaternion or CQRCreateEmptyQuaternion, in which case it is a user responsibility to eventually free the allocated memory with CQRFreeQuaternion. The components of an existing quaternion may be set by CQRSetQuaternion. The rules of quaternion arithmetic are applied: -1 = i*i = j*j = k*k, i*j=k=-j*i, j*k=i=-j*k, k*i=j=-i*k by CQRAdd, CQRSubtract, CQRMultiply and CQRDivide. CQRScalarMultiply multiplies a quaternion by a scalar. CQREqual returns 0 if quaternion q1 == q2, component by component. CQRConjugate computes a quaternion with the same scalar component and the negative of the vector component. CQRNormsq computes the sum of the squares of the components. CQRInverse computes the inverse of a non-zero quaternion. The functions CQRGetQuaternionW, CQRGetQuaternionX, CQRGetQuaternionY and CQRGetQuaternionZ extract the 4 components of a quaternion. The function CQRGetQuaternionIm extract the imaginary part of a quaternion as a quaternion with w=0. The function CQRQGetQuaternion extracts the imaginary part and normalizes it to a unit vector. The function CQRGetQuaternionAngle extracts the angle for the polar representation of a quaternion as an exponential (see below). In handling rotations, a right-handed system is assumed. CQRRotateByQuaternion rotates a vector by a quaternion, w = qvq*. CQRAxis2Quaternion forms the quaternion for a rotation around axis v by angle theta. CQRMatrix2Quaterion forms the quaternion equivalent a 3x3 rotation matrix R. CQRQuaternion2Matrix forms a 3x3 rotation matrix from a quaternion. CQRQuaternion2Angles converts a quaternion into Euler Angles for the Rz(Ry(Rx))) convention. CQRAngles2Quaternion convert Euler angles for the Rz(Ry(Rx))) convention into a quaternion. The logarithm of a quaternion in CQRLog is based on the polar representation q = r*cos(theta) + r*sin(theta) [ i*axis_x + j*axis_y +k*axis_z] = r*exp(theta*[ i*axis_x + j*axis_y +k*axis_z]) with a unit axis. Then the natural logarithm is given by log(q) = log(r) + theta*[ i*axis_x + j*axis_y +k*axis_z]) Note than any integer multiple of 2*PI could have been added to theta, so the logarithm is multivalued. The code only returns one of these values. The exponential in CQRExp is created by reversing the transformation. Taking a quaternion to a quaternion power is done by taking the log, multiplying by the power and then taking the exponential. Only one representative power is returned by CQRQuaternionPower. CQRDoublePower takes a quaternion to a double power by the same log-multiply-exp approach. CQRIntegerPower applies positive and negative integer powers by multiplication withou taking any logs or exponentials. CQRIntegerRoot applies the log-multiply-exp approach for integer roots. The second integer argument allow selection of one of the multiple roots. For roots of quaternions with a non-zero imaginary part, there are r roots, so m = 0, 1, 2, ... r-1 are meaningful. For roots of reals, there can be infinitely many alternate roots. In the case, m will cycle first through the i-based roots, then the j-based roots and then the k-based roots. The SLERP and HLERP functions combine quaternions by speherical linear interpolation. SLERP take two quaternions and two weights and combine them following a great circle on the unit quaternion 4-D sphere and linear interpolation between the radii. SLERP keeps a quaternion separate from the negative of the same quaternion and is not appropriate for quaternions representing rotations. Use HLERP to apply SLERP to quaternions representing rotations. If operating with __cplusplus defined, then the CPPQR template is defined allowing the creation of CPPQR quaternion objects. The template has three typename arguments: DistanceType, VectorType and MatrixType that default to double, double[3] and double[9]. Specializations are provided to support a double[3][3] MatrixType. Returns The CQRlib functions return 0 for normal completion, or the sum of one or more of the following non-zero error codes: Error Return Numeric Value Meaning CQR_BAD_ARGUMENT 1 /* An argument is not valid */ CQR_NO_MEMORY 2 /* A call to allocate memory failed */ CQR_FAILED 4 /* Operation failed */ Examples To create a quaternion dynamically from memory, initialized as the x vector with a zero scalar value, reporting failure to stderr: #include #include ... CQRQuaternionHandle quathandle; ... if (CQRCreateQuaternion(&quathandle,0.,1.,0.,0.)) fprintf(stderr," CQRCreateQuaternion failed!!\n"); To create an x vector quaternion, a y vector quaternion, add then together and multiply by a z-vector, and print the result : #include #include ... CQRQuaternion qx, qy, qz, qresult1, qresult2; ... if (CQRSetQuaternion(&qx,0.,1.,0.,0.) ||CQRSetQuaternion(&qy,0.,0.,1.,0.) ||CQRSetQuaternion(&qz,0.,0.,0.,1.)) fprintf(stderr," CQRSetQuaternion failed!!\n"); if (CQRAdd(&qresult1,&qx,&qy)||CQRMultiply(&qresult2,&qresult1,&qz)) fprintf(stderr," CQR Add or Multiply failed!!\n"); fprintf(stdout,"Result = ((i+j)*k) = %g %+gi %+gj + %+gk\n", qresult2.w, qresult2.x, qresult2.y, qresult2.z); The output should be "Result = ((i+j)*k) = 0 +1i -1j +0k". To rotate the 3D vector [-1.,0.,1.] 90 degrees clockwise around the vector [1.,1.,1.]: #include #include #include ... double axis[3] = {1.,1.,1.}; double vector[3] = {-1.,0.,1.}; double result[3]; CQRQuaternion rotquat; double PI; PI = 4.*atan2(1.,1.); CQRAxis2Quaternion(&rotquat,axis,PI/2); CQRRotateByQuaternion(result, &rotquat, vector); ... fprintf(stdout," [-1.,0.,1.] rotated 90 degrees clockwise" " around the vector [1.,1.,1.] = [%g, %g, %g]\n", result[0], result[1], result[2]); The output should be "[-1.,0.,1.] rotated 90 degrees clockwise around the vector [1.,1.,1.] = [0.57735, -1.1547, 0.57735]". See the test program CQRlibTest.c. For examples of the use of the CPPQR template, see the C++ test program CPPQRTest.cpp. ---------------------------------------------------------------------- Updated 29 April 2018 yayahjb at gmail dot com cqrlib-CQRlib-1.1.4/cqrlib.c000066400000000000000000001125141327146370000155320ustar00rootroot00000000000000/* * cqrlib.c * * * Created by Herbert J. Bernstein on 2/15/09. * Copyright 2009 Herbert J. Bernstein. All rights reserved. * * Revised, 8 July 2009 for CQR_FAR macro -- HJB */ /* Work supported in part by NIH NIGMS under grant 1R15GM078077-01 and DOE under grant ER63601-1021466-0009501. Any opinions, findings, and conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the funding agencies. */ /********************************************************************** * * * YOU MAY REDISTRIBUTE THE CQRlib API UNDER THE TERMS OF THE LGPL * * * **********************************************************************/ /************************* LGPL NOTICES ******************************* * * * This library is free software; you can redistribute it and/or * * modify it under the terms of the GNU Lesser General Public * * License as published by the Free Software Foundation; either * * version 2.1 of the License, or (at your option) any later version. * * * * This library is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * * Lesser General Public License for more details. * * * * You should have received a copy of the GNU Lesser General Public * * License along with this library; if not, write to the Free * * Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, * * MA 02110-1301 USA * * * **********************************************************************/ /* A utility library for quaternion arithmetic and quaternion rotation math. See "Quaternions and spatial rotation", Wikipedia http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation K. Shoemake, "Quaternions", Department of Computer Science, University of Pennsylvania, Philadelphia, PA 19104, ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z K. Shoemake, "Animating rotation with quaternion curves", ACM SIGGRAPH Computer Graphics, Vol 19, No. 3, pp 245--254, 1985. */ #ifdef __cplusplus extern "C" { #endif #ifdef _MSC_VER #define USE_LOCAL_HEADERS #endif #ifndef USE_LOCAL_HEADERS #include #else #include "cqrlib.h" #endif /* CQRCreateQuaternion -- create a quaternion = w +ix+jy+kz */ int CQRCreateQuaternion(CQRQuaternionHandle CQR_FAR * quaternion, double w, double x, double y, double z) { *quaternion = (CQRQuaternionHandle)CQR_MALLOC(sizeof(CQRQuaternion)); if (!*quaternion) return CQR_NO_MEMORY; (*quaternion)->w = w; (*quaternion)->x = x; (*quaternion)->y = y; (*quaternion)->z = z; return CQR_SUCCESS; } /* CQRCreateEmptyQuaternion -- create a quaternion = 0 +i0+j0+k0 */ int CQRCreateEmptyQuaternion(CQRQuaternionHandle CQR_FAR * quaternion) { *quaternion = (CQRQuaternionHandle)CQR_MALLOC(sizeof(CQRQuaternion)); if (!*quaternion) return CQR_NO_MEMORY; (*quaternion)->w = 0; (*quaternion)->x = 0; (*quaternion)->y = 0; (*quaternion)->z = 0; return CQR_SUCCESS; } /* CQRFreeQuaternion -- free a quaternion */ int CQRFreeQuaternion(CQRQuaternionHandle CQR_FAR * quaternion) { if (!quaternion) return CQR_BAD_ARGUMENT; CQR_FREE(*quaternion); *quaternion = NULL; return CQR_SUCCESS; } /* CQRSetQuaternion -- create an existing quaternion = w +ix+jy+kz */ int CQRSetQuaternion( CQRQuaternionHandle quaternion, double w, double x, double y, double z) { if (!quaternion) return CQR_BAD_ARGUMENT; quaternion->w = w; quaternion->x = x; quaternion->y = y; quaternion->z = z; return CQR_SUCCESS; } /* CQRGetQuaternionW -- get the w component of a quaternion */ int CQRGetQuaternionW( double CQR_FAR * qw, CQRQuaternionHandle q ) { if (!qw || ! q) return CQR_BAD_ARGUMENT; *qw = q->w; return CQR_SUCCESS; } /* CQRGetQuaternionX -- get the x component of a quaternion */ int CQRGetQuaternionX( double CQR_FAR * qx, CQRQuaternionHandle q ) { if (!qx || ! q) return CQR_BAD_ARGUMENT; *qx = q->x; return CQR_SUCCESS; } /* CQRGetQuaternionY -- get the y component of a quaternion */ int CQRGetQuaternionY( double CQR_FAR * qy, CQRQuaternionHandle q ) { if (!qy || ! q) return CQR_BAD_ARGUMENT; *qy = q->y; return CQR_SUCCESS; } /* CQRGetQuaternionZ -- get the z component of a quaternion */ int CQRGetQuaternionZ( double CQR_FAR * qz, CQRQuaternionHandle q ) { if (!qz || ! q) return CQR_BAD_ARGUMENT; *qz = q->z; return CQR_SUCCESS; } /* CQRGetQuaternionIm -- get the imaginary component of a quaternion */ int CQRGetQuaternionIm( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ) { if (!quaternion || ! q) return CQR_BAD_ARGUMENT; quaternion->w = 0.; quaternion->x = q->x; quaternion->y = q->y; quaternion->z = q->z; return CQR_SUCCESS; } /* CQRGetQuaternionAxis -- get the axis for the polar representation of a quaternion */ int CQRGetQuaternionAxis( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ) { double anorm; if (!quaternion || ! q) return CQR_BAD_ARGUMENT; anorm = sqrt((q->x)*(q->x)+(q->y)*(q->y)+(q->z)*(q->z)); if (anorm < DBL_MIN) { quaternion->w = 0.; quaternion->x = 0.; quaternion->y = 0.; quaternion->z = 0.; } else { quaternion->w = 0.; quaternion->x = q->x/anorm; quaternion->y = q->y/anorm; quaternion->z = q->z/anorm; } return CQR_SUCCESS; } /* CQRGetQuaternionAngle -- get the angular component of the polar representation of aquaternion */ int CQRGetQuaternionAngle( double CQR_FAR * angle, CQRQuaternionHandle q ) { double cosangle, sinangle; double radius, anorm; if (!angle || ! q) return CQR_BAD_ARGUMENT; radius = sqrt( (q->w)*(q->w) + (q->x)*(q->x) + (q->y)*(q->y) + (q->z)*(q->z) ); anorm = sqrt( (q->x)*(q->x) + (q->y)*(q->y) + (q->z)*(q->z) ); if ( anorm <= DBL_MIN) { return CQR_SUCCESS; } cosangle = (q->w)/radius; sinangle = anorm/radius; *angle = atan2(sinangle,cosangle); return CQR_SUCCESS; } /* CQRLog -- get the natural logarithm of a quaternion */ int CQRLog( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ) { CQRQuaternion axis; double angle; double ipnormsq; double PI; if (!quaternion || ! q) return CQR_BAD_ARGUMENT; CQRGetQuaternionAxis(&axis,q); CQRGetQuaternionAngle(&angle,q); if (q->w < 0.) { ipnormsq = (q->x)*(q->x) + (q->y)*(q->y) + (q->z)*(q->z); if (ipnormsq <= DBL_MIN ) { PI = atan2(1.,1.)*4.; CQRMSet(axis,0.,1.,0.,0.); angle = PI; } } quaternion->w = log(sqrt((q->w)*(q->w) + (q->x)*(q->x) + (q->y)*(q->y) + (q->z)*(q->z) )); quaternion->x = axis.x*angle; quaternion->y = axis.y*angle; quaternion->z = axis.z*angle; return CQR_SUCCESS; } /* CQRExp -- get the exponential (exp) of a quaternion */ int CQRExp( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ) { CQRQuaternion impart = {0,0,0,0}; double angle; double rat; if (!quaternion || ! q) return CQR_BAD_ARGUMENT; CQRGetQuaternionIm(&impart,q); CQRMNorm(angle,impart); if (angle <= DBL_MIN) { quaternion->w = cos(angle)*exp(q->w); quaternion->x = 0.; quaternion->y = 0.; quaternion->z = 0.; } else { rat = exp(q->w)*sin(angle)/angle; quaternion->w = cos(angle)*exp(q->w); quaternion->x = rat*q->x; quaternion->y = rat*q->y; quaternion->z = rat*q->z; } return CQR_SUCCESS; } /* CQRQuaternionPower -- take a quaternion to a quaternion power */ int CQRQuaternionPower( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, CQRQuaternionHandle p) { CQRQuaternion qlog,qlogtimesp; CQRLog(&qlog,q); CQRMMultiply(qlogtimesp,qlog,*p); CQRExp(quaternion,&qlogtimesp); return CQR_SUCCESS; } /* CQRDoublePower -- take a quarernion to a double power */ int CQRDoublePower( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, double p) { CQRQuaternion qlog,qlogtimesp; CQRLog(&qlog,q); CQRMScalarMultiply(qlogtimesp,qlog,p); CQRExp(quaternion,&qlogtimesp); return CQR_SUCCESS; } /* CQRIntegerPower -- take a quaternion to an integer power */ int CQRIntegerPower( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, int p) { CQRQuaternion qtemp, qsq, qprod; unsigned int ptemp; CQRMSet(*quaternion,1.0,0.,0.,0.); if ( p == 0 ) return CQR_SUCCESS; else if ( p > 0 ) { CQRMCopy(qtemp,*q); ptemp = p; } else { CQRMInverse(qtemp,*q); ptemp = -p; } while(ptemp) { if ((ptemp&1)!= 0) { CQRMMultiply(qprod,*quaternion,qtemp); CQRMCopy(*quaternion,qprod); } ptemp >>= 1; if (ptemp==0) break; CQRMMultiply(qsq,qtemp,qtemp); CQRMCopy(qtemp,qsq) } return CQR_SUCCESS; } /* CQRIntegerRoot -- take the given integer root of a quaternion, returning the indicated mth choice from among multiple roots. For reals the cycle runs through first the i-based roots, then the j-based roots and then the k-based roots, out of the infinite number of possible roots of reals. */ int CQRIntegerRoot( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, int r, int m) { double PI; CQRQuaternion qlog,qlogoverr,qaxis; double recip, qaxisnormsq; int cycle; PI = 4.*atan2(1.,1.); if (r == 0) return CQR_BAD_ARGUMENT; recip = 1./((double)r); CQRLog(&qlog,q); if (m != 0) { CQRGetQuaternionAxis(&qaxis,q); CQRMNormsq(qaxisnormsq,qaxis); if (qaxisnormsq <= DBL_MIN) { cycle = (m/r)%3; switch (cycle) { case 1: CQRMSet(qaxis,0.,0.,1.,0.); break; case 2: CQRMSet(qaxis,0.,0.,0.,1.); break; default: CQRMSet(qaxis,0.,1.,0.,0.); break; } } CQRMScalarMultiply(qaxis,qaxis,2.*PI*((double)m)); CQRMAdd(qlog,qlog,qaxis); } CQRMScalarMultiply(qlogoverr,qlog,recip); CQRExp(quaternion,&qlogoverr); return CQR_SUCCESS; } /* CQRAdd -- add a quaternion (q1) to a quaternion (q2) */ int CQRAdd (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ) { if (!quaternion || !q1 || !q2 ) return CQR_BAD_ARGUMENT; CQRMAdd(*quaternion,*q1,*q2); return CQR_SUCCESS; } /* CQRSubtract -- subtract a quaternion (q2) from a quaternion (q1) */ int CQRSubtract (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ) { if (!quaternion || !q1 || !q2 ) return CQR_BAD_ARGUMENT; CQRMSubtract(*quaternion,*q1,*q2); return CQR_SUCCESS; } /* CQRMultiply -- multiply a quaternion (q1) by quaternion (q2) */ int CQRMultiply (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ) { if (!quaternion || !q1 || !q2 ) return CQR_BAD_ARGUMENT; CQRMMultiply(*quaternion,*q1,*q2); return CQR_SUCCESS; } /* CQRDot -- dot product of quaternion (q1) by quaternion (q2) as 4-vectors */ int CQRDot (double CQR_FAR * dotprod, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ) { if (!dotprod || !q1 || !q2 ) return CQR_BAD_ARGUMENT; *dotprod = q1->w*q2->w + q1->x*q2->x + q1->y*q2->y +q1->z*q2->z; return CQR_SUCCESS; } /* CQRDivide -- divide a quaternion (q1) by quaternion (q2) */ int CQRDivide (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ) { double norm2sq; CQRQuaternion q; if (!quaternion || !q1 || !q2 ) return CQR_BAD_ARGUMENT; norm2sq = q2->w*q2->w+q2->x*q2->x+q2->y*q2->y+q2->z*q2->z; if (norm2sq==0.) return CQR_BAD_ARGUMENT; q.w = q1->z*q2->z + q1->y*q2->y + q1->x*q2->x + q1->w*q2->w; q.x = -q1->y*q2->z + q1->z*q2->y - q1->w*q2->x + q1->x*q2->w; q.y = q1->x*q2->z - q1->w*q2->y - q1->z*q2->x + q1->y*q2->w; q.z = -q1->w*q2->z - q1->x*q2->y + q1->y*q2->x + q1->z*q2->w; quaternion->w=q.w/norm2sq; quaternion->x=q.x/norm2sq; quaternion->y=q.y/norm2sq; quaternion->z=q.z/norm2sq; return CQR_SUCCESS; } /* CQRScalarMultiply -- multiply a quaternion (q) by scalar (s) */ int CQRScalarMultiply (CQRQuaternionHandle quaternion, CQRQuaternionHandle q, double s ) { if (!quaternion || !q ) return CQR_BAD_ARGUMENT; CQRMScalarMultiply(*quaternion,*q,s); return CQR_SUCCESS; } /* CQREqual -- return 0 if quaternion q1 == q2 */ int CQREqual (CQRQuaternionHandle q1, CQRQuaternionHandle q2 ) { if ( !q1 || !q2 ) return CQR_BAD_ARGUMENT; return ((q1->w==q2->w)&&(q1->x==q2->x)&&(q1->y==q2->y)&&(q1->z==q2->z))?CQR_SUCCESS:CQR_FAILED; } /* CQRConjugate -- Form the conjugate of a quaternion qconj */ int CQRConjugate (CQRQuaternionHandle qconjugate, CQRQuaternionHandle quaternion) { if (!quaternion || !qconjugate ) return CQR_BAD_ARGUMENT; CQRMConjugate(*qconjugate,*quaternion); return CQR_SUCCESS; } /* CQRNormsq -- Form the normsquared of a quaternion */ int CQRNormsq (double CQR_FAR * normsq, CQRQuaternionHandle quaternion ) { if (!quaternion || !normsq ) return CQR_BAD_ARGUMENT; CQRMNormsq(*normsq,*quaternion); return CQR_SUCCESS; } /* CQRNorm -- Form the norm of a quaternion */ int CQRNorm (double CQR_FAR * norm, CQRQuaternionHandle quaternion ) { if (!quaternion || !norm ) return CQR_BAD_ARGUMENT; CQRMNorm(*norm,*quaternion); return CQR_SUCCESS; } /* CQRDistsq -- Form the distance squared between two quaternions */ int CQRDistsq (double CQR_FAR * distsq, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ) { if (!q1 || !q2 || !distsq ) return CQR_BAD_ARGUMENT; CQRMDistsq(*distsq,*q1,*q2); return CQR_SUCCESS; } /* CQRDist -- Form the distance between two quaternions */ int CQRDist (double CQR_FAR * dist, CQRQuaternionHandle q1, CQRQuaternionHandle q2) { if (!q1 || !q2 || !dist ) return CQR_BAD_ARGUMENT; CQRMDist(*dist,*q1,*q2); return CQR_SUCCESS; } /* CQRInverse -- Form the inverse of a quaternion */ int CQRInverse (CQRQuaternionHandle inversequaternion, CQRQuaternionHandle quaternion ) { double normsq; if (!quaternion || !inversequaternion ) return CQR_BAD_ARGUMENT; CQRMConjugate(*inversequaternion,*quaternion); CQRMNormsq(normsq,*quaternion); if (normsq > 0.) { CQRMScalarMultiply(*inversequaternion,*inversequaternion,1./normsq); return CQR_SUCCESS; } else return CQR_BAD_ARGUMENT; } /* CQRRotateByQuaternion -- Rotate a vector by a Quaternion, w = qvq* */ int CQRRotateByQuaternion(double CQR_FAR * w, CQRQuaternionHandle rotquaternion, double CQR_FAR * v) { CQRQuaternion vquat, wquat, qconj; if (!w || !rotquaternion || !v ) return CQR_BAD_ARGUMENT; CQRMSet(vquat,0,v[0],v[1],v[2]); CQRMMultiply(wquat,*rotquaternion,vquat); CQRMConjugate(qconj,*rotquaternion); CQRMMultiply(vquat,wquat,qconj); w[0] = vquat.x; w[1] = vquat.y; w[2] = vquat.z; return CQR_SUCCESS; } /* CQRAxis2Quaternion -- Form the quaternion for a rotation around axis v by angle theta */ int CQRAxis2Quaternion (CQRQuaternionHandle rotquaternion, double CQR_FAR * v, double theta) { double normsq, norm; if (!rotquaternion || !v ) return CQR_BAD_ARGUMENT; normsq = v[0]*v[0] + v[1]*v[1] + v[2]*v[2]; if (normsq == 0.) return CQR_BAD_ARGUMENT; if (normsq == 1.) { CQRMSet(*rotquaternion,cos(theta/2),sin(theta/2)*v[0],sin(theta/2)*v[1],sin(theta/2)*v[2]); } else { norm = sqrt(normsq); CQRMSet(*rotquaternion,cos(theta/2),sin(theta/2)*v[0]/norm,sin(theta/2)*v[1]/norm,sin(theta/2)*v[2]/norm); } return CQR_SUCCESS; } /* CQRMatrix2Quaternion -- Form the quaternion from a 3x3 rotation matrix R */ int CQRMatrix2Quaternion (CQRQuaternionHandle rotquaternion, double R[3][3]) { double trace, recip, fourxsq, fourysq, fourzsq; trace = R[0][0] + R[1][1] + R[2][2]; if (trace > -.75) { rotquaternion->w = sqrt((1.+trace))/2.; recip = .25/rotquaternion->w; rotquaternion->z = (R[1][0]-R[0][1])*recip; rotquaternion->y = (R[0][2]-R[2][0])*recip; rotquaternion->x = (R[2][1]-R[1][2])*recip; return CQR_SUCCESS; } fourxsq = 1.+ R[0][0] - R[1][1] - R[2][2]; if (fourxsq >= .25) { rotquaternion->x = sqrt(fourxsq)/2.; recip = .25/rotquaternion->x; rotquaternion->y = (R[1][0]+R[0][1])*recip; rotquaternion->z = (R[0][2]+R[2][0])*recip; rotquaternion->w = (R[2][1]-R[1][2])*recip; return CQR_SUCCESS; } fourysq = 1.+ R[1][1] - R[0][0] - R[2][2]; if (fourysq >= .25) { rotquaternion->y = sqrt(fourysq)/2.; recip = .25/rotquaternion->y; rotquaternion->x = (R[1][0]+R[0][1])*recip; rotquaternion->w = (R[0][2]-R[2][0])*recip; rotquaternion->z = (R[2][1]+R[1][2])*recip; return CQR_SUCCESS; } fourzsq = 1.+ R[2][2] - R[0][0] - R[1][1]; if (fourzsq >= .25) { rotquaternion->z = sqrt(fourzsq)/2.; recip = .25/rotquaternion->z; rotquaternion->w = (R[1][0]-R[0][1])*recip; rotquaternion->x = (R[0][2]+R[2][0])*recip; rotquaternion->y = (R[2][1]+R[1][2])*recip; return CQR_SUCCESS; } return CQR_BAD_ARGUMENT; } /* CQRQuaternion2Matrix -- Form the 3x3 rotation matrix from a quaternion */ int CQRQuaternion2Matrix (double R[3][3], CQRQuaternionHandle rotquaternion) { double twoxy, twoyz, twoxz, twowx, twowy, twowz; double ww, xx, yy, zz; if (!R || !rotquaternion) return CQR_BAD_ARGUMENT; ww = (rotquaternion->w)*(rotquaternion->w); xx = (rotquaternion->x)*(rotquaternion->x); yy = (rotquaternion->y)*(rotquaternion->y); zz = (rotquaternion->z)*(rotquaternion->z); R[0][0] = ww + xx - yy - zz; R[1][1] = ww - xx + yy - zz; R[2][2] = ww - xx - yy + zz; twoxy = 2.*(rotquaternion->x)*(rotquaternion->y); twoyz = 2.*(rotquaternion->y)*(rotquaternion->z); twoxz = 2.*(rotquaternion->x)*(rotquaternion->z); twowx = 2.*(rotquaternion->w)*(rotquaternion->x); twowy = 2.*(rotquaternion->w)*(rotquaternion->y); twowz = 2.*(rotquaternion->w)*(rotquaternion->z); R[0][1] = twoxy - twowz; R[0][2] = twoxz + twowy; R[1][0] = twoxy + twowz; R[1][2] = twoyz - twowx; R[2][0] = twoxz - twowy; R[2][1] = twoyz + twowx; return CQR_SUCCESS; } /* CQRQuaternion2Angles -- Convert a Quaternion into Euler Angles for Rz(Ry(Rx))) convention */ int CQRQuaternion2Angles (double CQR_FAR * RotX, double CQR_FAR * RotY, double CQR_FAR * RotZ, CQRQuaternionHandle rotquaternion) { double SRX, SRY, SRZ, TRX, TRY, TRZ; double NSum; double TSum; double RMX0, RMX1, RMY0, RMY1, RMZ0, RMZ1, RMZ2; double PI; PI = 4.*atan2(1.,1.); if (!rotquaternion || !RotX || !RotY || !RotZ) return CQR_BAD_ARGUMENT; RMX0 = rotquaternion->w*rotquaternion->w + rotquaternion->x*rotquaternion->x - rotquaternion->y*rotquaternion->y - rotquaternion->z*rotquaternion->z; RMX1 = 2.*(rotquaternion->x*rotquaternion->y-rotquaternion->w*rotquaternion->z); RMY0 = 2.*(rotquaternion->x*rotquaternion->y+rotquaternion->w*rotquaternion->z); RMY1 = rotquaternion->w*rotquaternion->w - rotquaternion->x*rotquaternion->x + rotquaternion->y*rotquaternion->y - rotquaternion->z*rotquaternion->z; RMZ0 = 2.*(rotquaternion->x*rotquaternion->z-rotquaternion->w*rotquaternion->y); RMZ1 = 2.*(rotquaternion->w*rotquaternion->x+rotquaternion->y*rotquaternion->z); RMZ2 = rotquaternion->w*rotquaternion->w - rotquaternion->x*rotquaternion->x - rotquaternion->y*rotquaternion->y + rotquaternion->z*rotquaternion->z; if (RMZ0 < 1. ) { if (RMZ0 > -1.) { SRY = asin(-RMZ0); } else { SRY = .5*PI; } } else { SRY = -.5*PI; } if (RMZ0 > .9999995) { SRX = atan2(-RMX1,RMY1); SRZ = 0; } else { if (RMZ0 < -.9999995 ) { SRX = atan2(RMX1,RMY1); SRZ = 0; } else { SRX = atan2(RMZ1,RMZ2); SRZ = atan2(RMY0,RMX0); } } TRX = PI+SRX; if ( TRX > 2.*PI ) TRX -= 2.*PI; TRY = PI+SRY; if ( TRY > 2.*PI ) TRY -= 2.*PI; TRZ = PI+SRZ; if ( TRZ > 2.*PI ) TRZ -= 2.*PI; NSum = 0; TSum = 0; NSum += fabs(cos(SRX)-cos(*RotX)) + fabs(sin(SRX)-sin(*RotX)) + fabs(cos(SRY)-cos(*RotY)) + fabs(sin(SRY)-sin(*RotY)) + fabs(cos(SRZ)-cos(*RotZ)) + fabs(sin(SRZ)-sin(*RotZ)); TSum += fabs(cos(TRX)-cos(*RotX)) + fabs(sin(TRX)-sin(*RotX)) + fabs(cos(TRY)-cos(*RotY)) + fabs(sin(TRY)-sin(*RotY)) + fabs(cos(TRZ)-cos(*RotZ)) + fabs(sin(TRZ)-sin(*RotZ)); if (NSum < TSum) { *RotX = SRX; *RotY = SRY; *RotZ = SRZ; } else { *RotX = TRX; *RotY = TRY; *RotZ = TRZ; } return CQR_SUCCESS; } /* CQRAngles2Quaternion -- Convert Euler Angles for Rz(Ry(Rx))) convention into a quaternion */ int CQRAngles2Quaternion (CQRQuaternionHandle rotquaternion, double RotX, double RotY, double RotZ ) { double cx, cy, cz, sx, sy, sz; if (!rotquaternion) return CQR_BAD_ARGUMENT; cx = cos(RotX/2); sx = sin(RotX/2); cy = cos(RotY/2); sy = sin(RotY/2); cz = cos(RotZ/2); sz = sin(RotZ/2); rotquaternion->w = cx*cy*cz + sx*sy*sz; rotquaternion->x = sx*cy*cz - cx*sy*sz; rotquaternion->y = cx*sy*cz + sx*cy*sz; rotquaternion->z = cx*cy*sz - sx*sy*cz; return CQR_SUCCESS; } /* Represent a 3-vector as a quaternion with w=0 */ int CQRPoint2Quaternion( CQRQuaternionHandle quaternion, double v[3] ) { quaternion->w = 0.; quaternion->x = v[0]; quaternion->y = v[1]; quaternion->z = v[2]; return CQR_SUCCESS; } /* SLERP -- Spherical Linear Interpolation Take two quaternions and two weights and combine them following a great circle on the unit quaternion 4-D sphere and linear interpolation between the radii This version keeps a quaternion separate from the negative of the same quaternion and is not appropriate for quaternions representing rotations. Use CQRHLERP to apply SLERP to quaternions representing rotations */ int CQRSLERP (CQRQuaternionHandle quaternion, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2, const double w1, const double w2) { CQRQuaternion s1; CQRQuaternion s2; CQRQuaternion st1; CQRQuaternion st2; CQRQuaternion sout; CQRQuaternion sout2; double normsq, norm1sq, norm2sq; double r1,r2, cosomega,sinomega, omega; double t, t1, t2; if (!quaternion || !q1 || !q2 ) return CQR_BAD_ARGUMENT; CQRMNormsq(norm1sq,*q1); CQRMNormsq(norm2sq,*q2); t = w1/(w1+w2); if (norm1sq <= DBL_MIN) { CQRMScalarMultiply(*quaternion,*q2,(1-t)); return CQR_SUCCESS; } if (norm2sq <= DBL_MIN) { CQRMScalarMultiply(*quaternion,*q1,t); return CQR_SUCCESS; } if (fabs(norm1sq-1.)<= DBL_MIN) { r1 = 1.; CQRMCopy(s1,*q1); } else { r1 = sqrt(norm1sq); CQRMScalarMultiply(s1,*q1,(1/r1)); } if (fabs(norm2sq-1.)<= DBL_MIN) { r2 = 1.; CQRMCopy(s2,*q2); } else { r2 = sqrt(norm1sq); CQRMScalarMultiply(s2,*q2,(1/r2)); } CQRMDot(cosomega,s1,s2); if (cosomega>=1. || cosomega<=-1.) { sinomega = 0.; } else { sinomega=sqrt(1.-cosomega*cosomega); } omega=atan2(sinomega,cosomega); if (sinomega <= 0.05) { t1=t*(1-t*t*omega*omega/6.); t2=(1-t)*(1.-(1-t)*(1-t)*omega*omega/6.); CQRMScalarMultiply(st1,s1,t1); CQRMScalarMultiply(st2,s2,t2); if (cosomega >=0.) { CQRMAdd(sout,st1,st2); } else { if (sinomega <= 0.00001) { sout.w = -st1.x+st2.x; sout.x = st1.w-st2.w; sout.y = st1.z-st2.z; sout.z = -st1.y+st2.y; } else { CQRMAdd(sout,s1,s2); } CQRMNormsq(normsq,sout); CQRMScalarMultiply(sout,sout,1/sqrt(normsq)); if (t >= 0.5) { CQRSLERP(&sout2,&sout,&s1,2-2.*t,2.*t-1.); CQRMCopy(sout,sout2); }else { CQRSLERP(&sout2,&sout,&s2,2.*t,1.-2.*t-1.); CQRMCopy(sout,sout2); } } CQRMNormsq(normsq,sout); if (normsq <= DBL_MIN) { CQRMSet(*quaternion,0.,0.,0.,0.); } else { CQRMScalarMultiply(*quaternion,sout,(t*r1+(1-t)*r2)/sqrt(normsq)); } return CQR_SUCCESS; } t1 = sin(t*omega); t2 = sin((1-t)*omega); CQRMScalarMultiply(st1,s1,t1); CQRMScalarMultiply(st2,s2,t2); CQRMAdd(sout,st1,st2); CQRMNormsq(normsq,sout); CQRMScalarMultiply(*quaternion,sout,(r1*t+r2*(1-t))/sqrt(normsq)); return CQR_SUCCESS; } /* HLERP -- Hemispherical Linear Interpolation Take two quaternions and two weights and combine them following a great circle on the unit quaternion 4-D sphere and linear interpolation between the radii This is the hemispherical version, for use with quaternions representing rotations. Use SLERP for full spherical interpolation. */ int CQRHLERP (CQRQuaternionHandle quaternion, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2, const double w1, const double w2) { CQRQuaternion s1; CQRQuaternion s2; CQRQuaternion st1; CQRQuaternion st2; CQRQuaternion sout; double normsq, norm1sq, norm2sq; double r1,r2, cosomega,sinomega, omega; double t, t1, t2; if (!quaternion || !q1 || !q2 ) return CQR_BAD_ARGUMENT; CQRMNormsq(norm1sq,*q1); CQRMNormsq(norm2sq,*q2); t = w1/(w1+w2); if (norm1sq <= DBL_MIN) { CQRMScalarMultiply(*quaternion,*q2,(1-t)); return CQR_SUCCESS; } if (norm2sq <= DBL_MIN) { CQRMScalarMultiply(*quaternion,*q1,t); return CQR_SUCCESS; } if (fabs(norm1sq-1.)<= DBL_MIN) { r1 = 1.; CQRMCopy(s1,*q1); } else { r1 = sqrt(norm1sq); CQRMScalarMultiply(s1,*q1,(1/r1)); } if (fabs(norm2sq-1.)<= DBL_MIN) { r2 = 1.; CQRMCopy(s2,*q2); } else { r2 = sqrt(norm1sq); CQRMScalarMultiply(s2,*q2,(1/r2)); } CQRMDot(cosomega,s1,s2); if (cosomega>=1. || cosomega<=-1.) { sinomega = 0.; } else { sinomega=sqrt(1.-cosomega*cosomega); } if (cosomega < 0.) { if (t < 0.5) { s1.w=-s1.w;s1.x=-s1.x;s1.y=-s1.y;s1.z=-s1.z; } else { s2.w=-s2.w;s2.x=-s2.x;s2.y=-s2.y;s2.z=-s2.z; } cosomega = -cosomega; } omega=atan2(sinomega,cosomega); if (sinomega <= 0.05) { t1=t*(1-t*t*omega*omega/6.); t2=(1-t)*(1.-(1-t)*(1-t)*omega*omega/6.); CQRMScalarMultiply(st1,s1,t1); CQRMScalarMultiply(st2,s2,t2); CQRMAdd(sout,st1,st2); if (sout.w < 0.) { sout.w = -sout.w; sout.x = -sout.x; sout.y = -sout.y; sout.z = -sout.z; } CQRMNormsq(normsq,sout); if (normsq <= DBL_MIN) { CQRMSet(*quaternion,0.,0.,0.,0.); } else { CQRMScalarMultiply(*quaternion,sout,(t*r1+(1-t)*r2)/sqrt(normsq)); } return CQR_SUCCESS; } t1 = sin(t*omega); t2 = sin((1-t)*omega); CQRMScalarMultiply(st1,s1,t1); CQRMScalarMultiply(st2,s2,t2); CQRMAdd(sout,st1,st2); if (sout.w < 0.) { sout.w = -sout.w; sout.x = -sout.x; sout.y = -sout.y; sout.z = -sout.z; } CQRMNormsq(normsq,sout); CQRMScalarMultiply(*quaternion,sout,(r1*t+r2*(1-t))/sqrt(normsq)); return CQR_SUCCESS; } /* CQRSLERPDist -- Spherical Linear Interpolation distance Form the distance between two quaternions by summing the difference in the magnitude of the radii and the great circle distance along the sphere of the smaller quaternion. This version keeps a quaternion separate from the negative of the same quaternion and is not appropriate for quaternions representing rotations. Use CQRHLERPDist to apply SLERPDist to quaternions representing rotations */ int CQRSLERPDist (double CQR_FAR * dist, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2) { CQRQuaternion s1; CQRQuaternion s2; double norm1sq, norm2sq; double r1,r2, cosomega,sinomega, omega; if (!dist || !q1 || !q2) return CQR_BAD_ARGUMENT; CQRMNormsq(norm1sq,*q1); CQRMNormsq(norm2sq,*q2); if (norm1sq <= DBL_MIN) {*dist = sqrt(norm2sq); return CQR_SUCCESS;} if (norm2sq <= DBL_MIN) {*dist = sqrt(norm1sq); return CQR_SUCCESS;} if (fabs(norm1sq-1.)<= DBL_MIN) { r1 = 1.; CQRMCopy(s1,*q1); } else { r1 = sqrt(norm1sq); CQRMScalarMultiply(s1,*q1,(1/r1)); } if (fabs(norm2sq-1.)<= DBL_MIN) { r2 = 1.; CQRMCopy(s2,*q2); } else { r2 = sqrt(norm1sq); CQRMScalarMultiply(s2,*q2,(1/r2)); } CQRMDot(cosomega,s1,s2); if (cosomega>=1. || cosomega<=-1.) { sinomega = 0.; } else { sinomega=sqrt(1.-cosomega*cosomega); } omega=atan2(sinomega,cosomega); if (r1 <= r2) *dist = (r2-r1)+r1*fabs(omega); else *dist = (r1-r2)+r2*fabs(omega); return CQR_SUCCESS; } /* CQRHLERPDist -- Hemispherical Linear Interpolation distance Form the distance between two quaternions by summing the difference in the magnitude of the radii and the great circle distance along the sphere of the smaller quaternion. This version keeps a quaternion separate from the negative of the same quaternion and is not appropriate for quaternions representing rotations. Use CQRHLERPDist to apply SLERPDist to quaternions representing rotations */ int CQRHLERPDist (double CQR_FAR * dist, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2) { CQRQuaternion s1; CQRQuaternion s2; double norm1sq, norm2sq; double r1,r2, cosomega,sinomega, omega; if (!dist || !q1 || !q2) return CQR_BAD_ARGUMENT; CQRMNormsq(norm1sq,*q1); CQRMNormsq(norm2sq,*q2); if (norm1sq <= DBL_MIN) {*dist = sqrt(norm2sq); return CQR_SUCCESS;} if (norm2sq <= DBL_MIN) {*dist = sqrt(norm1sq); return CQR_SUCCESS;} if (fabs(norm1sq-1.)<= DBL_MIN) { r1 = 1.; CQRMCopy(s1,*q1); } else { r1 = sqrt(norm1sq); CQRMScalarMultiply(s1,*q1,(1/r1)); } if (fabs(norm2sq-1.)<= DBL_MIN) { r2 = 1.; CQRMCopy(s2,*q2); } else { r2 = sqrt(norm1sq); CQRMScalarMultiply(s2,*q2,(1/r2)); } CQRMDot(cosomega,s1,s2); if (cosomega>=1. || cosomega<=-1.) { sinomega = 0.; } else { sinomega=sqrt(1.-cosomega*cosomega); } if (cosomega < 0.) { cosomega = -cosomega; } omega=atan2(sinomega,cosomega); if (r1 <= r2) *dist = (r2-r1)+r1*fabs(omega); else *dist = (r1-r2)+r2*fabs(omega); return CQR_SUCCESS; } #ifdef __cplusplus } #endif cqrlib-CQRlib-1.1.4/cqrlib.h000066400000000000000000001344121327146370000155400ustar00rootroot00000000000000/* * cqrlib.h * * * Created by Herbert J. Bernstein on 2/15/09. * Copyright 2009 Herbert J. Bernstein. All rights reserved. * * Revised, 8 July 2009 for CQR_FAR macro -- HJB */ /* Work supported in part by NIH NIGMS under grant 1R15GM078077-01 and DOE under grant ER63601-1021466-0009501. Any opinions, findings, and conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the funding agencies. */ /********************************************************************** * * * YOU MAY REDISTRIBUTE THE CQRlib API UNDER THE TERMS OF THE LGPL * * * **********************************************************************/ /************************* LGPL NOTICES ******************************* * * * This library is free software; you can redistribute it and/or * * modify it under the terms of the GNU Lesser General Public * * License as published by the Free Software Foundation; either * * version 2.1 of the License, or (at your option) any later version. * * * * This library is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * * Lesser General Public License for more details. * * * * You should have received a copy of the GNU Lesser General Public * * License along with this library; if not, write to the Free * * Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, * * MA 02110-1301 USA * * * **********************************************************************/ /* A utility library for quaternion arithmetic and quaternion rotation math. See "Quaternions and spatial rotation", Wikipedia http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation K. Shoemake, "Quaternions", Department of Computer Science, University of Pennsylvania, Philadelphia, PA 19104, ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z K. Shoemake, "Animating rotation with quaternion curves", ACM SIGGRAPH Computer Graphics, Vol 19, No. 3, pp 245--254, 1985. */ #ifndef CQRLIB_H_INCLUDED #define CQRLIB_H_INCLUDED #ifdef __cplusplus #include #include #include template< typename DistanceType=double, typename VectorType=double[3], typename MatrixType=double[9] > class CPPQR { private: DistanceType w, x, y, z; public: /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR( ) : // default constructor w( DistanceType( 0.0 ) ), x( DistanceType( 0.0 ) ), y( DistanceType( 0.0 ) ), z( DistanceType( 0.0 ) ) { } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR( const CPPQR& q ) // copy constructor { if ( this != &q ) { w = q.w; x = q.x; y = q.y; z = q.z; } } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR( // constructor const DistanceType& wi, const DistanceType& xi, const DistanceType& yi, const DistanceType& zi ) : w( wi ), x( xi ), y( yi ), z( zi ) { } /* Set -- set the values of an existing quaternion = w +ix+jy+kz */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline void Set ( const DistanceType& wi, const DistanceType& xi, const DistanceType& yi, const DistanceType& zi ) { w = wi; x = xi; y = yi; z = zi; } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline DistanceType GetW( void ) const { return( w ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline DistanceType GetX( void ) const { return( x ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline DistanceType GetY( void ) const { return( y ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline DistanceType GetZ( void ) const { return( z ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR GetIm( void ) const { return( CPPQR(0.,x,y,z) ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR GetAxis( void ) const { DistanceType anorm=sqrt((double)(x*x + y*y + z*z)); if (anorm < DBL_MIN) return(CPPQR(0.,0.,0.,0.)); return( CPPQR(0.,x/anorm,y/anorm,z/anorm) ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline double GetAngle( void ) const { double cosangle, sinangle; DistanceType radius, anorm; radius = sqrt( (double) (w*w + x*x + y*y + z*z) ); anorm = sqrt( (double) (x*x + y*y + z*z) ); if ( anorm <= DBL_MIN) { return 0.; } cosangle = w/radius; sinangle = anorm/radius; return(atan2(sinangle,cosangle)); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR log( void ) const { CPPQR axis = (*this).GetAxis(); double PI; double ipnormsq; double angle = (*this).GetAngle(); if ( w < 0. ) { ipnormsq = (double) (x*x + y*y + z*z); if (ipnormsq <= DBL_MIN ) { PI = std::atan2(1.,1.)*4.; axis = CPPQR(0.,1.,0.,0.); angle = PI; } } return (CPPQR(std::log((double)((*this).Norm())),axis.x*angle,axis.y*angle,axis.z*angle)); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR exp( void ) const { const CPPQR impart = (*this).GetIm( ); const double angle = (double)impart.Norm(); if (angle <= DBL_MIN) { return (CPPQR(cos(angle)*std::exp(w),0.,0.,0.)); } const double rat = std::exp(w)*sin(angle)/angle; return(CPPQR(cos(angle)*std::exp(w),rat*x,rat*y,rat*z)); } template inline CPPQR pow( const powertype p) const { return(((*this).log()*p).exp()); } inline CPPQR pow( const int p) const { CPPQR qtemp, qaccum; unsigned int ptemp; if ( p == 0 ) return (CPPQR(1.0,0.,0.,0.)); else if ( p > 0 ) { qtemp = *this; ptemp = p; } else { qtemp = (*this).Inverse(); ptemp = -p; } qaccum = CPPQR(1.0,0.,0.,0.); while(1) { if ((ptemp&1)!= 0) { qaccum *= qtemp; } ptemp >>= 1; if (ptemp==0) break; qtemp *= qtemp; } return qaccum; } /* root -- take the given integer root of a quaternion, returning the indicated mth choice from among multiple roots. For reals the cycle runs through first the i-based roots, then the j-based roots and then the k-based roots, out of the infinite number of possible roots of negative reals. */ inline CPPQR root( const int r, const int m) const { const double PI = 4.*atan2(1.,1.); CPPQR qlog,qlogoverr,qaxis; double recip, qaxisnormsq; int cycle; if (r == 0) return CPPQR(DBL_MAX,DBL_MAX,DBL_MAX,DBL_MAX); recip = 1./((double)r); qlog = (*this).log(); if (m != 0) { qaxis =(*this).GetAxis(); qaxisnormsq = qaxis.Normsq(); if (qaxisnormsq <= DBL_MIN) { cycle = (m/r)%3; switch (cycle) { case 1: qaxis = CPPQR(0.,0.,1.,0.); break; case 2: qaxis = CPPQR(0.,0.,0.,1.); break; default: qaxis = CPPQR(0.,1.,0.,0.); break; } } qaxis *= 2.*PI*((double)m); qlog += qaxis; } qlogoverr = qlog*recip; return qlogoverr.exp(); } /* Dot product of 2 quaternions as 4-vectors */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline DistanceType Dot( const CPPQR& q) const { return (w*q.w+x*q.x+y*q.y+z*q.z); } /* Add -- add a quaternion (q1) to a quaternion (q2) */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR operator+ ( const CPPQR& q ) const { CPPQR temp; temp.w = w + q.w; temp.x = x + q.x; temp.y = y + q.y; temp.z = z + q.z; return( temp ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR& operator+= ( const CPPQR& q ) { w += q.w; x += q.x; y += q.y; z += q.z; return (*this); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR& operator-= ( const CPPQR& q ) { w -= q.w; x -= q.x; y -= q.y; z -= q.z; return (*this); } /* Subtract -- subtract a quaternion (q2) from a quaternion (q1) */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR operator- ( const CPPQR& q ) const { CPPQR temp; temp.w = w - q.w; temp.x = x - q.x; temp.y = y - q.y; temp.z = z - q.z; return( temp ); } /* Unary minus -- negate a quaterion */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR operator- ( void ) const { CPPQR temp; temp.w = -w; temp.x = -x; temp.y = -y; temp.z = -z; return( temp ); } /* Multiply -- multiply a quaternion (q1) by quaternion (q2) */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR operator* ( const CPPQR& q ) const // multiply two quaternions { CPPQR temp; temp.w = -z*q.z - y*q.y - x*q.x + w*q.w; temp.x = y*q.z - z*q.y + w*q.x + x*q.w; temp.y = -x*q.z + w*q.y + z*q.x + y*q.w; temp.z = w*q.z + x*q.y - y*q.x + z*q.w; return( temp ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR& operator*= ( const CPPQR& q ) { CPPQR temp; temp.w = -z*q.z - y*q.y - x*q.x + w*q.w; temp.x = y*q.z - z*q.y + w*q.x + x*q.w; temp.y = -x*q.z + w*q.y + z*q.x + y*q.w; temp.z = w*q.z + x*q.y - y*q.x + z*q.w; w = temp.w; x = temp.x; y = temp.y; z = temp.z; return (*this); } /* Divide -- Divide a quaternion (q1) by quaternion (q2) */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR operator/ ( const CPPQR& q2 ) const { const DistanceType norm2sq = q2.w*q2.w + q2.x*q2.x + q2.y*q2.y + q2.z*q2.z; if ( norm2sq == 0.0 ) { return( CPPQR( DBL_MAX, DBL_MAX, DBL_MAX, DBL_MAX ) ); } CPPQR q; q.w = z*q2.z + y*q2.y + x*q2.x + w*q2.w; q.x = -y*q2.z + z*q2.y - w*q2.x + x*q2.w; q.y = x*q2.z - w*q2.y - z*q2.x + y*q2.w; q.z = -w*q2.z - x*q2.y + y*q2.x + z*q2.w; return( q / norm2sq ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR& operator/= ( const CPPQR& q2 ) { const DistanceType norm2sq = q2.w*q2.w + q2.x*q2.x + q2.y*q2.y + q2.z*q2.z; if ( norm2sq == 0.0 ) { return( CPPQR( DBL_MAX, DBL_MAX, DBL_MAX, DBL_MAX ) ); } CPPQR q; q.w = z*q2.z + y*q2.y + x*q2.x + w*q2.w; q.x = -y*q2.z + z*q2.y - w*q2.x + x*q2.w; q.y = x*q2.z - w*q2.y - z*q2.x + y*q2.w; q.z = -w*q2.z - x*q2.y + y*q2.x + z*q2.w; w = q.w/norm2sq; x = q.x/norm2sq; y = q.y/norm2sq; z = q.z/norm2sq; return (*this); } /* ScalarMultiply -- multiply a quaternion (q) by scalar (s) */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR operator* ( const DistanceType& d ) const // multiply by a constant { CPPQR temp; temp.w = w*d; temp.x = x*d; temp.y = y*d; temp.z = z*d; return( temp ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR& operator*= ( const DistanceType& d ) { w *= d; x *= d; y *= d; z *= d; return (*this); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR& operator/= ( const DistanceType& d ) { if ( std::abs((double)d) <= DBL_MIN ) { w = DBL_MAX; x = DBL_MAX; y = DBL_MAX; z = DBL_MAX; } else { w /= d; x /= d; y /= d; z /= d; } return (*this); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR operator/ ( const DistanceType& d ) const // divide by a constant { CPPQR temp; temp.w = w/d; temp.x = x/d; temp.y = y/d; temp.z = z/d; return( temp ); } /* Conjugate -- Form the conjugate of a quaternion qconj */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR Conjugate ( void ) const { CPPQR conjugate; conjugate.w = w; conjugate.x = -x; conjugate.y = -y; conjugate.z = -z; return( conjugate ); } /* Normsq -- Form the normsquared of a quaternion */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline DistanceType Normsq ( void ) const { return( w*w + x*x + y*y + z*z ); } /* Norm -- Form the norm of a quaternion */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline DistanceType Norm ( void ) const { return sqrt( w*w + x*x + y*y + z*z ); } /* Distsq -- Form the distance squared from a quaternion */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline DistanceType Distsq ( const CPPQR& q ) const { return( (w-q.w)*(w-q.w) + (x-q.x)*(x-q.x) + (y-q.y)*(y-q.y) + (z-q.z)*(z-q.z) ); } /* Dist -- Form the distance from a quaternion */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline DistanceType Dist ( const CPPQR& q ) const { return sqrt( (w-q.w)*(w-q.w) + (x-q.x)*(x-q.x) + (y-q.y)*(y-q.y) + (z-q.z)*(z-q.z) ); } /* Inverse -- Form the inverse of a quaternion */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR Inverse ( void ) const { CPPQR inversequaternion = (*this).Conjugate(); const DistanceType normsq = (*this).Normsq( ); if ( normsq > DistanceType( 0.0 ) ) { inversequaternion = inversequaternion * DistanceType( 1.0 ) / normsq; } else { inversequaternion = CPPQR( DBL_MAX, DBL_MAX, DBL_MAX, DBL_MAX ); } return( inversequaternion ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR& operator= ( const CPPQR& q ) { if ( this != &q ) { w = q.w; x = q.x; y = q.y; z = q.z; } return( *this ); } /* Equal */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline bool operator== ( const CPPQR& q ) const { return( w==q.w && x==q.x && y==q.y && z==q.z ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline bool operator!= ( const CPPQR& q ) const { return( (w!=q.w) || (x!=q.x) || (y!=q.y) | (z!=q.z) ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline VectorType& operator* ( const VectorType& v ) { return( RotateByQuaternion( v ) ); } /* RotateByQuaternion -- Rotate a vector by a Quaternion, w = qvq* */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline void RotateByQuaternion(VectorType &w, const VectorType v ) { CPPQR vquat( 0.0, v[0], v[1], v[2] ); const CPPQR wquat = (*this)*vquat; const CPPQR qconj = (*this).Conjugate( ); vquat = wquat * qconj; w[0] = vquat.x; w[1] = vquat.y; w[2] = vquat.z; return; } inline VectorType& RotateByQuaternion(const VectorType v ) { CPPQR vquat( 0.0, v[0], v[1], v[2] ); const CPPQR wquat = (*this)*vquat; const CPPQR qconj = (*this).Conjugate( ); vquat = wquat * qconj; return VectorType(vquat.x, vquat.y, vquat.z); } /* Axis2Quaternion -- Form the quaternion for a rotation around axis v by angle theta */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ static inline CPPQR Axis2Quaternion ( const DistanceType& angle, const VectorType v ) { return( Axis2Quaternion( v, angle ) ); } /* Axis2Quaternion -- Form the quaternion for a rotation around axis v by angle theta */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ static inline CPPQR Axis2Quaternion ( const VectorType v, const DistanceType& angle ) { const DistanceType norm2sq = v[0]*v[0]+v[1]*v[1]+v[2]*v[2]; if ( norm2sq == DistanceType(0.0) ) { return( CPPQR( DBL_MAX, DBL_MAX, DBL_MAX, DBL_MAX ) ); } const DistanceType sinOverNorm = sin(angle/2.0)/sqrt(norm2sq); const CPPQR q( cos(angle/2.0), sinOverNorm*v[0], sinOverNorm*v[1], sinOverNorm*v[2]); return( q ); } /* Matrix2Quaterion -- Form the quaternion from a 3x3 rotation matrix R */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ static inline void Matrix2Quaternion ( CPPQR& rotquaternion, const MatrixType m ) { const DistanceType trace = m[0] + m[4] + m[8]; if ( trace > -0.75 ) { rotquaternion.w = sqrt( (1.0+trace)) / 2.0; const DistanceType recip = 0.25 / rotquaternion.w; rotquaternion.z = (m[3]-m[1]) * recip; rotquaternion.y = (m[2]-m[6]) * recip; rotquaternion.x = (m[7]-m[5]) * recip; return; } const DistanceType fourxsq = 1.0 + m[0] - m[4] - m[8]; if ( fourxsq >= 0.25 ) { rotquaternion.x = sqrt( fourxsq ) / 2.0; const DistanceType recip = 0.25 /rotquaternion.x; rotquaternion.y = (m[3] + m[1])*recip; rotquaternion.z = (m[2] + m[6])*recip; rotquaternion.w = (m[7] - m[5])*recip; return; } const DistanceType fourysq = 1.0 + m[4] - m[0] - m[8]; if ( fourysq >= 0.25 ) { rotquaternion.y = sqrt( fourysq ) / 2.0; const DistanceType recip = 0.25 / rotquaternion.y; rotquaternion.x = (m[3] + m[1])*recip; rotquaternion.w = (m[2] - m[6])*recip; rotquaternion.z = (m[7] + m[5])*recip; return; } const DistanceType fourzsq = 1. + m[8] - m[0] - m[4]; if ( fourzsq >= 0.25 ) { rotquaternion.z = sqrt( fourzsq ) / 2.0; const DistanceType recip = 0.25 / rotquaternion.z; rotquaternion.w = (m[3] - m[1])*recip; rotquaternion.x = (m[2] + m[6])*recip; rotquaternion.y = (m[7] + m[5])*recip; return; } rotquaternion.x = rotquaternion.y = rotquaternion.z = rotquaternion.w = 0; return; } static inline void Matrix2Quaternion (CPPQR& rotquaternion, const DistanceType R[3][3] ) { const DistanceType trace = R[0][0] + R[1][1] + R[2][2]; if ( trace > -0.75 ) { rotquaternion.w = sqrt( (1.0+trace)) / 2.0; const DistanceType recip = 0.25 / rotquaternion.w; rotquaternion.z = (R[1][0]-R[0][1]) * recip; rotquaternion.y = (R[0][2]-R[2][0]) * recip; rotquaternion.x = (R[2][1]-R[1][2]) * recip; return; } const DistanceType fourxsq = 1.0 + R[0][0] - R[1][1] - R[2][2]; if ( fourxsq >= 0.25 ) { rotquaternion.x = sqrt( fourxsq ) / 2.0; const DistanceType recip = 0.25 /rotquaternion.x; rotquaternion.y = (R[1][0]+R[0][1])*recip; rotquaternion.z = (R[0][2]+R[2][0])*recip; rotquaternion.w = (R[2][1]-R[1][2])*recip; return; } const DistanceType fourysq = 1.0 + R[1][1] - R[0][0] - R[2][2]; if ( fourysq >= 0.25 ) { rotquaternion.y = sqrt( fourysq ) / 2.0; const DistanceType recip = 0.25 / rotquaternion.y; rotquaternion.x = (R[1][0]+R[0][1])*recip; rotquaternion.w = (R[0][2]-R[2][0])*recip; rotquaternion.z = (R[2][1]+R[1][2])*recip; return; } const DistanceType fourzsq = 1. + R[2][2] - R[0][0] - R[1][1]; if ( fourzsq >= 0.25 ) { rotquaternion.z = sqrt( fourzsq ) / 2.0; const DistanceType recip = 0.25 / rotquaternion.z; rotquaternion.w = (R[1][0]-R[0][1])*recip; rotquaternion.x = (R[0][2]+R[2][0])*recip; rotquaternion.y = (R[2][1]+R[1][2])*recip; return; } rotquaternion.x = rotquaternion.y = rotquaternion.z = rotquaternion.w = 0; return; } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ DistanceType operator[] ( const int k ) const { const int i = (k<0) ? 0 : ( (k>3)?3:k ) ; if ( i==0 ) return w; if ( i==1 ) return x; if ( i==2 ) return y; if ( i==3 ) return z; return( 0 ); // just to keep compilers happy } /* Quaternion2Matrix -- Form the 3x3 rotation matrix from a quaternion */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ static inline void Quaternion2Matrix(MatrixType& m, const CPPQR q ) { const DistanceType ww = q.w*q.w; const DistanceType xx = q.x*q.x; const DistanceType yy = q.y*q.y; const DistanceType zz = q.z*q.z; const DistanceType twoxy = 2.0 * q.x*q.y; const DistanceType twoyz = 2.0 * q.y*q.z; const DistanceType twoxz = 2.0 * q.x*q.z; const DistanceType twowx = 2.0 * q.w*q.x; const DistanceType twowy = 2.0 * q.w*q.y; const DistanceType twowz = 2.0 * q.w*q.z; m[0] = ww+xx-yy-zz; m[1] = twoxy - twowz; m[2] = twoxz + twowy; m[3] = twoxy + twowz; m[4] = ww-xx+yy-zz; m[5] = twoyz - twowx; m[6] = twoxz - twowy; m[7] = twoyz + twowx; m[8] = ww-xx-yy+zz; return; } static inline void Quaternion2Matrix( DistanceType m[3][3], const CPPQR q ) { const DistanceType ww = q.w*q.w; const DistanceType xx = q.x*q.x; const DistanceType yy = q.y*q.y; const DistanceType zz = q.z*q.z; const DistanceType twoxy = 2.0 * q.x*q.y; const DistanceType twoyz = 2.0 * q.y*q.z; const DistanceType twoxz = 2.0 * q.x*q.z; const DistanceType twowx = 2.0 * q.w*q.x; const DistanceType twowy = 2.0 * q.w*q.y; const DistanceType twowz = 2.0 * q.w*q.z; m[0][0] = ww+xx-yy-zz; m[0][1] = twoxy - twowz; m[0][2] = twoxz + twowy; m[1][0] = twoxy + twowz; m[1][1] = ww-xx+yy-zz; m[0][2] = twoyz - twowx; m[2][0] = twoxz - twowy; m[2][1] = twoyz + twowx; m[2][2] = ww-xx-yy+zz; return; } // end Quaternion2Matrix /* Get a unit quaternion from a general one */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR UnitQ( void ) const { const DistanceType normsq = Normsq( ); if ( normsq == 0.0 ) { return( CPPQR( DBL_MAX, DBL_MAX, DBL_MAX, DBL_MAX ) ); } else { return( (*this) / sqrt( Normsq( ) ) ); } } // end UnitQ /* Quaternion2Angles -- Convert a Quaternion into Euler Angles for Rz(Ry(Rx))) convention */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline bool Quaternion2Angles ( DistanceType& rotX, DistanceType& rotY, DistanceType& rotZ ) const { const DistanceType PI = 4.0 * atan( 1.0 ); const DistanceType rmx0 = w*w + x*x - y*y - z*z; const DistanceType rmx1 = 2.0 * ( x*y - w*z ); const DistanceType rmy0 = 2.0 * ( x*y + w*z ); const DistanceType rmy1 = w*w - x*x + y*y - z*z; const DistanceType rmz0 = 2.0 * (x*z - w*y ); const DistanceType rmz1 = 2.0 * (w*x + y*z ); const DistanceType rmz2 = w*w - x*x - y*y + z*z; DistanceType srx, sry, srz, trX, trY, trZ; if ( rmz0 >= 1.0 ) { sry = -0.5 * PI; } else if ( rmz0 <= -1.0 ) { sry = 0.5 * PI; } else { sry = asin( -rmz0 ); } if ( rmz0 > 0.9999995 ) { srx = atan2( -rmx1, rmy1 ); srz = 0.0; } else if ( rmz0 < -0.9999995 ) { srx = atan2( rmx1, rmy1 ); srz = 0.0; } else { srx = atan2( rmz1, rmz2 ); srz = atan2( rmy0, rmx0 ); } trX = PI + srx; if ( trX > 2.0 * PI ) trX -= 2.0 * PI; trY = PI + sry; if ( trY > 2.0 * PI ) trY -= 2.0 * PI; trZ = PI + srz; if ( trZ > 2.0 * PI ) trZ -= 2.0 * PI; const DistanceType nsum = fabs( cos(srx)-cos(rotX)) + fabs( sin(srx)-sin(rotX)) + fabs( cos(sry)-cos(rotY)) + fabs( sin(sry)-sin(rotY)) + fabs( cos(srz)-cos(rotZ)) + fabs( sin(srz)-sin(rotZ)); const DistanceType tsum = fabs( cos(trX)-cos(rotX)) + fabs( sin(trX)-sin(rotX)) + fabs( cos(trY)-cos(rotY)) + fabs( sin(trY)-sin(rotY)) + fabs( cos(trZ)-cos(rotZ)) + fabs( sin(trZ)-sin(rotZ)); if ( nsum < tsum ) { rotX = srx; rotY = sry; rotZ = srz; } else { rotX = trX; rotY = trY; rotZ = trZ; } return( true ); } // end Quaternion2Angles /* Angles2Quaternion -- Convert Euler Angles for Rz(Ry(Rx))) convention into a quaternion */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ static inline CPPQR Angles2Quaternion ( const DistanceType& rotX, const DistanceType& rotY, const DistanceType& rotZ ) { const DistanceType cx = cos( rotX / 2.0 ); const DistanceType sx = sin( rotX / 2.0 ); const DistanceType cy = cos( rotY / 2.0 ); const DistanceType sy = sin( rotY / 2.0 ); const DistanceType cz = cos( rotZ / 2.0 ); const DistanceType sz = sin( rotZ / 2.0 ); const CPPQR q( cx*cy*cz + sx*sy*sz, sx*cy*cz - cx*sy*sz, cx*sy*cz + sx*cy*sz, cx*cy*sz - sx*sy*cz ); return( q ); } // end Angles2Quaternion static inline CPPQR Point2Quaternion( const DistanceType v[3] ) { return( CPPQR( 0.0, v[0], v[1], v[2] ) ); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ /* SLERP -- Spherical Linear Interpolation Take two quaternions and two weights and combine them following a great circle on the unit quaternion 4-D sphere and linear interpolation between the radii This version keeps a quaternion separate from the negative of the same quaternion and is not appropriate for quaternions representing rotations. Use CQRHLERP to apply SLERP to quaternions representing rotations */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR SLERP (const CPPQR& q, DistanceType w1, DistanceType w2) const { CPPQR s1; CPPQR s2; CPPQR st1; CPPQR st2; CPPQR sout; DistanceType normsq; const DistanceType norm1sq=(*this).Normsq(); const DistanceType norm2sq=q.Normsq(); DistanceType r1,r2; DistanceType cosomega,sinomega; DistanceType omega; DistanceType t, t1, t2; t = w1/(w1+w2); if (norm1sq <= DBL_MIN) return q*(1-t); if (norm2sq <= DBL_MIN) return (*this)*t; if (fabs(norm1sq-1.)<= DBL_MIN) { r1 = 1.; s1 = *this; } else { r1 = sqrt(norm1sq); s1 = (*this)*(1/r1); } if (fabs(norm2sq-1.)<= DBL_MIN) { r2 = 1.; s2 = q; } else { r2 = sqrt(norm1sq); s2 = q*(1./r2); } cosomega = s1.Dot(s2); if (cosomega>=1. || cosomega<=-1.) { sinomega = 0.; } else { sinomega=sqrt(1.-cosomega*cosomega); } omega=atan2(sinomega,cosomega); if (sinomega <= 0.05) { t1=t*(1-t*t*omega*omega/6.); t2=(1-t)*(1.-(1-t)*(1-t)*omega*omega/6.); st1=s1*t1; st2=s2*t2; if (cosomega >=0.) { sout=st1+st2; } else { if (sinomega <= 0.00001) { sout=CPPQR(-st1.x,st1.w,st1.z,-st1.y)-CPPQR(-st2.x,st2.w,st2.z,-st2.y); } else { sout = s1+s2; } sout=sout*(1/sout.Norm()); if (t >= 0.5) { sout=sout.SLERP(s1,2-2.*t,2.*t-1.); }else { sout=sout.SLERP(s2,2.*t,1.-2.*t); } } normsq = sout.Normsq(); if (normsq <= DBL_MIN) { return CPPQR(0.,0.,0.,0.); } else { return CPPQR(sout*(t*r1+(1-t)*r2)/sqrt(normsq)); } } t1 = sin(t*omega); t2 = sin((1-t)*omega); st1=s1*t1; st2=s2*t2; sout=st1+st2; normsq = sout.Normsq(); return CPPQR(sout*((r1*t+r2*(1-t))/sqrt(normsq))); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ /* HLERP -- Hemispherical Linear Interpolation Take two quaternions and two weights and combine them following a great circle on the unit quaternion 4-D sphere and linear interpolation between the radii This is the hemispherical version, for use with quaternions representing rotations. Use SLERP for full spherical interpolation. */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline CPPQR HLERP (const CPPQR& q, DistanceType w1, DistanceType w2) const { CPPQR s1; CPPQR s2; CPPQR st1; CPPQR st2; CPPQR sout; DistanceType normsq; const DistanceType norm1sq=(*this).Normsq(); const DistanceType norm2sq=q.Normsq(); DistanceType r1,r2; DistanceType cosomega,sinomega; DistanceType omega; DistanceType t, t1, t2; t = w1/(w1+w2); if (norm1sq <= DBL_MIN) return q*(1-t); if (norm2sq <= DBL_MIN) return (*this)*t; if (fabs(norm1sq-1.)<= DBL_MIN) { r1 = 1.; s1 = *this; } else { r1 = sqrt(norm1sq); s1 = (*this)*(1/r1); } if (fabs(norm2sq-1.)<= DBL_MIN) { r2 = 1.; s2 = q; } else { r2 = sqrt(norm1sq); s2 = q*(1./r2); } cosomega = s1.Dot(s2); if (cosomega>=1. || cosomega<=-1.) { sinomega = 0.; } else { sinomega=sqrt(1.-cosomega*cosomega); } if (cosomega < 0.) { if (t < 0.5) { s1.w=-s1.w;s1.x=-s1.x;s1.y=-s1.y;s1.z=-s1.z; } else { s2.w=-s2.w;s2.x=-s2.x;s2.y=-s2.y;s2.z=-s2.z; } cosomega = -cosomega; } omega=atan2(sinomega,cosomega); if (sinomega <= 0.05) { t1=t*(1-t*t*omega*omega/6.); t2=(1-t)*(1.-(1-t)*(1-t)*omega*omega/6.); st1=s1*t1; st2=s2*t2; sout=st1+st2; if (sout.w < 0.) { sout.w = -sout.w; sout.x = -sout.x; sout.y = -sout.y; sout.z = -sout.z; } normsq = sout.Normsq(); if (normsq <= DBL_MIN) { return CPPQR(0.,0.,0.,0.); } else { return CPPQR(sout*(t*r1+(1-t)*r2)/sqrt(normsq)); } } t1 = sin(t*omega); t2 = sin((1-t)*omega); st1=s1*t1; st2=s2*t2; sout=st1+st2; if (sout.w < 0.) { sout = -sout; } normsq = sout.Normsq(); return CPPQR(sout*((r1*t+r2*(1-t))/sqrt(normsq))); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ /* SLERPDist -- Spherical Linear Interpolation distance Form the distance between two quaternions by summing the difference in the magnitude of the radii and the great circle distance along the sphere of the smaller quaternion. This version keeps a quaternion separate from the negative of the same quaternion and is not appropriate for quaternions representing rotations. Use CQRHLERPDist to apply SLERPDist to quaternions representing rotations */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline DistanceType SLERPDist (const CPPQR& q) const { CPPQR s1; CPPQR s2; CPPQR st1; CPPQR st2; CPPQR sout; const DistanceType norm1sq=(*this).Normsq(); const DistanceType norm2sq=q.Normsq(); DistanceType r1,r2; DistanceType cosomega,sinomega; DistanceType omega; if (norm1sq <= DBL_MIN) return sqrt(norm2sq); if (norm2sq <= DBL_MIN) return sqrt(norm1sq); if (fabs(norm1sq-1.)<= DBL_MIN) { r1 = 1.; s1 = *this; } else { r1 = sqrt(norm1sq); s1 = (*this)*(1/r1); } if (fabs(norm2sq-1.)<= DBL_MIN) { r2 = 1.; s2 = q; } else { r2 = sqrt(norm1sq); s2 = q*(1./r2); } cosomega = s1.Dot(s2); if (cosomega>=1. || cosomega<=-1.) { sinomega = 0.; } else { sinomega=sqrt(1.-cosomega*cosomega); } omega=atan2(sinomega,cosomega); if (r1 <= r2) return (r2-r1)+r1*fabs(omega); else return (r1-r2)+r2*fabs(omega); } /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ /* HLERPDist -- Hemispherical Linear Interpolation distance Form the distance between two quaternions by summing the difference in the magnitude of the radii and the great circle distance along the sphere of the smaller quaternion. */ /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ inline DistanceType HLERPDist (const CPPQR& q) const { CPPQR s1; CPPQR s2; CPPQR st1; CPPQR st2; CPPQR sout; const DistanceType norm1sq=(*this).Normsq(); const DistanceType norm2sq=q.Normsq(); DistanceType r1,r2; DistanceType cosomega,sinomega; DistanceType omega; if (norm1sq <= DBL_MIN) return sqrt(norm2sq); if (norm2sq <= DBL_MIN) return sqrt(norm1sq); if (fabs(norm1sq-1.)<= DBL_MIN) { r1 = 1.; s1 = *this; } else { r1 = sqrt(norm1sq); s1 = (*this)*(1/r1); } if (fabs(norm2sq-1.)<= DBL_MIN) { r2 = 1.; s2 = q; } else { r2 = sqrt(norm1sq); s2 = q*(1./r2); } cosomega = s1.Dot(s2); if (cosomega>=1. || cosomega<=-1.) { sinomega = 0.; } else { sinomega=sqrt(1.-cosomega*cosomega); } if (cosomega < 0.) { cosomega = -cosomega; } omega=atan2(sinomega,cosomega); if (r1 <= r2) return (r2-r1)+r1*fabs(omega); else return (r1-r2)+r2*fabs(omega); } }; // end class CPPQR #ifndef CQR_NOCCODE extern "C" { #endif #endif #ifndef __cplusplus #ifndef CQR_NOCCODE #include #include #endif #endif #ifndef CQR_NOCCODE #ifdef CQR_USE_FAR #include #define CQR_FAR __far #define CQR_MALLOC _fmalloc #define CQR_FREE _ffree #define CQR_MEMSET _fmemset #define CQR_MEMMOVE _fmemmove #else #include #define CQR_FAR #define CQR_MALLOC malloc #define CQR_FREE free #define CQR_MEMSET memset #define CQR_MEMMOVE memmove #endif #define CQR_FAILED 4 #define CQR_NO_MEMORY 2 #define CQR_BAD_ARGUMENT 1 #define CQR_SUCCESS 0 typedef struct { double w; double x; double y; double z; } CQRQuaternion; typedef CQRQuaternion CQR_FAR * CQRQuaternionHandle; /* CQR Macros */ #define CQRMIm(impart,q) \ (impart).w = 0.; (impart).x = (q).x; (impart).y = (q).y; (impart).z = (q).z; #define CQRMCopy(copy,orig) \ (copy).w = (orig).w; (copy).x = (orig).x; (copy).y = (orig).y; (copy).z = (orig).z; #define CQRMSet(q,qw,qx,qy,qz) \ (q).w = (qw); (q).x = (qx); (q).y = (qy); (q).z = (qz); #define CQRMAdd(sum,q1,q2) \ (sum).w = (q1).w + (q2).w; (sum).x = (q1).x + (q2).x; (sum).y = (q1).y + (q2).y; (sum).z = (q1).z + (q2).z; #define CQRMSubtract(sum,q1,q2) \ (sum).w = (q1).w - (q2).w; (sum).x = (q1).x - (q2).x; (sum).y = (q1).y - (q2).y; (sum).z = (q1).z - (q2).z; #define CQRMMultiply(product,q1,q2 ) \ (product).w = -(q1).z*(q2).z - (q1).y*(q2).y - (q1).x*(q2).x + (q1).w*(q2).w; \ (product).x = (q1).y*(q2).z - (q1).z*(q2).y + (q1).w*(q2).x + (q1).x*(q2).w; \ (product).y = -(q1).x*(q2).z + (q1).w*(q2).y + (q1).z*(q2).x + (q1).y*(q2).w; \ (product).z = (q1).w*(q2).z + (q1).x*(q2).y - (q1).y*(q2).x + (q1).z*(q2).w; #define CQRMDot(dotprod,q1,q2 ) \ dotprod = (q1).w*(q2).w + (q1).x*(q2).x + (q1).y*(q2).y + (q1).z*(q2).z; #define CQRMScalarMultiply(product,q,s ) \ (product).w = (q).w*s; \ (product).x = (q).x*s; \ (product).y = (q).y*s; \ (product).z = (q).z*s; #define CQRMConjugate(conjugate,q ) \ (conjugate).w = (q).w; \ (conjugate).x = -(q).x; \ (conjugate).y = -(q).y; \ (conjugate).z = -(q).z; #define CQRMNormsq(normsq,q) \ normsq = (q).w*(q).w + (q).x*(q).x + (q).y*(q).y + (q).z*(q).z; #define CQRMNorm(norm,q) \ norm = sqrt((q).w*(q).w + (q).x*(q).x + (q).y*(q).y + (q).z*(q).z); #define CQRMDistsq(distsq,q1,q2) \ distsq = ((q1).w-(q2).w)*((q1).w-(q2).w) + ((q1).x-(q2).x)*((q1).x-(q2).x) + ((q1).y-(q2).y)*((q1).y-(q2).y) + ((q1).z-(q2).z)*((q1).z-(q2).z); #define CQRMDist(dist,q1,q2) \ dist = sqrt(((q1).w-(q2).w)*((q1).w-(q2).w) + ((q1).x-(q2).x)*((q1).x-(q2).x) + ((q1).y-(q2).y)*((q1).y-(q2).y) + ((q1).z-(q2).z)*((q1).z-(q2).z)); #define CQRMInverse(inverseq,q) \ { double normsq; \ CQRMConjugate(inverseq,q); \ CQRMNormsq(normsq,q); \ if (normsq > 0.) { \ CQRMScalarMultiply(inverseq,inverseq,1./normsq); \ } \ } /* CQRCreateQuaternion -- create a quaternion = w +ix+jy+kz */ int CQRCreateQuaternion(CQRQuaternionHandle CQR_FAR * quaternion, double w, double x, double y, double z); /* CQRCreateEmptyQuaternion -- create a quaternion = 0 +i0+j0+k0 */ int CQRCreateEmptyQuaternion(CQRQuaternionHandle CQR_FAR * quaternion) ; /* CQRFreeQuaternion -- free a quaternion */ int CQRFreeQuaternion(CQRQuaternionHandle CQR_FAR * quaternion); /* CQRSetQuaternion -- create an existing quaternion = w +ix+jy+kz */ int CQRSetQuaternion( CQRQuaternionHandle quaternion, double w, double x, double y, double z); /* CQRGetQuaternionW -- get the w component of a quaternion */ int CQRGetQuaternionW( double CQR_FAR * qw, CQRQuaternionHandle q ); /* CQRGetQuaternionX -- get the x component of a quaternion */ int CQRGetQuaternionX( double CQR_FAR * qx, CQRQuaternionHandle q ); /* CQRGetQuaternionY -- get the y component of a quaternion */ int CQRGetQuaternionY( double CQR_FAR * qy, CQRQuaternionHandle q ); /* CQRGetQuaternionZ -- get the z component of a quaternion */ int CQRGetQuaternionZ( double CQR_FAR * qz, CQRQuaternionHandle q ); /* CQRGetQuaternionIm -- get the imaginary component of a quaternion */ int CQRGetQuaternionIm( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ); /* CQRGetQuaternionAxis -- get the axis for the polar representation of a quaternion */ int CQRGetQuaternionAxis( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ); /* CQRGetQuaternionAngle -- get the angular component of the polar representation of aquaternion */ int CQRGetQuaternionAngle( double CQR_FAR * angle, CQRQuaternionHandle q ); /* CQRLog -- get the natural logarithm of a quaternion */ int CQRLog( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ); /* CQRExp -- get the exponential (exp) of a quaternion */ int CQRExp( CQRQuaternionHandle quaternion, CQRQuaternionHandle q ); /* CQRQuaternionPower -- take a quarernion to a quaternion power */ int CQRQuaternionPower( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, CQRQuaternionHandle p); /* CQRDoublePower -- take a quarernion to a double power */ int CQRDoublePower( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, double p); /* CQRIntegerPower -- take a quaternion to an integer power */ int CQRIntegerPower( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, int p); /* CQRIntegerRoot -- take the given integer root of a quaternion, returning the indicated mth choice from among multiple roots. For reals the cycle runs through first the i-based roots, then the j-based roots and then the k-based roots, out of the infinite number of possible roots of reals. */ int CQRIntegerRoot( CQRQuaternionHandle quaternion, CQRQuaternionHandle q, int r, int m); /* CQRAdd -- add a quaternion (q1) to a quaternion (q2) */ int CQRAdd (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ); /* CQRSubtract -- subtract a quaternion (q2) from a quaternion (q1) */ int CQRSubtract (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ); /* CQRMultiply -- multiply a quaternion (q1) by quaternion (q2) */ int CQRMultiply (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ); /* CQRDot -- dot product of quaternion (q1) by quaternion (q2) as 4-vectors */ int CQRDot (double CQR_FAR * dotprod, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ); /* CQRDivide -- Divide a quaternion (q1) by quaternion (q2) */ int CQRDivide (CQRQuaternionHandle quaternion, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ); /* CQRScalarMultiply -- multiply a quaternion (q) by scalar (s) */ int CQRScalarMultiply (CQRQuaternionHandle quaternion, CQRQuaternionHandle q, double s ); /* CQREqual -- return 0 if quaternion q1 == q2 */ int CQREqual (CQRQuaternionHandle q1, CQRQuaternionHandle q2 ); /* CQRConjugate -- Form the conjugate of a quaternion qconj */ int CQRConjugate (CQRQuaternionHandle qconjgate, CQRQuaternionHandle quaternion); /* CQRNormsq -- Form the normsquared of a quaternion */ int CQRNormsq (double CQR_FAR * normsq, CQRQuaternionHandle quaternion ) ; /* CQRNorm -- Form the norm of a quaternion */ int CQRNorm (double CQR_FAR * norm, CQRQuaternionHandle quaternion ) ; /* CQRDistsq -- Form the distance squared between two quaternions */ int CQRDistsq (double CQR_FAR * distsq, CQRQuaternionHandle q1, CQRQuaternionHandle q2) ; /* CQRDist -- Form the distance between two quaternions */ int CQRDist (double CQR_FAR * dist, CQRQuaternionHandle q1, CQRQuaternionHandle q2 ) ; /* CQRInverse -- Form the inverse of a quaternion */ int CQRInverse (CQRQuaternionHandle inversequaternion, CQRQuaternionHandle quaternion ); /* CQRRotateByQuaternion -- Rotate a vector by a Quaternion, w = qvq* */ int CQRRotateByQuaternion(double CQR_FAR * w, CQRQuaternionHandle rotquaternion, double CQR_FAR * v); /* CQRAxis2Quaternion -- Form the quaternion for a rotation around axis v by angle theta */ int CQRAxis2Quaternion (CQRQuaternionHandle rotquaternion, double CQR_FAR * v, double theta); /* CQRMatrix2Quaterion -- Form the quaternion from a 3x3 rotation matrix R */ int CQRMatrix2Quaternion (CQRQuaternionHandle rotquaternion, double R[3][3]); /* CQRQuaternion2Matrix -- Form the 3x3 rotation matrix from a quaternion */ int CQRQuaternion2Matrix (double R[3][3], CQRQuaternionHandle rotquaternion); /* CQRQuaternion2Angles -- Convert a Quaternion into Euler Angles for Rz(Ry(Rx))) convention */ int CQRQuaternion2Angles (double CQR_FAR * RotX, double CQR_FAR * RotY, double CQR_FAR * RotZ, CQRQuaternionHandle rotquaternion); /* CQRAngles2Quaternion -- Convert Euler Angles for Rz(Ry(Rx))) convention into a quaternion */ int CQRAngles2Quaternion (CQRQuaternionHandle rotquaternion, double RotX, double RotY, double RotZ ); /* Represent a 3-vector as a quaternion with w=0 */ int CQRPoint2Quaternion( CQRQuaternionHandle quaternion, double v[3] ); /* SLERP -- Spherical Linear Interpolation Take two quaternions and two weights and combine them following a great circle on the unit quaternion 4-D sphere and linear interpolation between the radii This version keeps a quaternion separate from the negative of the same quaternion and is not appropriate for quaternions representing rotations. Use CQRHLERP to apply SLERP to quaternions representing rotations */ int CQRSLERP (CQRQuaternionHandle quaternion, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2, const double w1, const double w2); /* HLERP -- Hemispherical Linear Interpolation Take two quaternions and two weights and combine them following a great circle on the unit quaternion 4-D sphere and linear interpolation between the radii This is the hemispherical version, for use with quaternions representing rotations. Use SLERP for full spherical interpolation. */ int CQRHLERP (CQRQuaternionHandle quaternion, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2, const double w1, const double w2); /* SLERPDist -- Spherical Linear Interpolation distance Form the distance between two quaternions by summing the difference in the magnitude of the radii and the great circle distance along the sphere of the smaller quaternion. This version keeps a quaternion separate from the negative of the same quaternion and is not appropriate for quaternions representing rotations. Use CQRHLERPDist to apply SLERPDist to quaternions representing rotations */ int CQRSLERPDist (double CQR_FAR * dist, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2); /* HLERPDist -- Hemispherical Linear Interpolation distance Form the distance between two quaternions by summing the difference in the magnitude of the radii and the great circle distance along the sphere of the smaller quaternion. This version keeps a quaternion separate from the negative of the same quaternion and is not appropriate for quaternions representing rotations. Use CQRHLERPDist to apply SLERPDist to quaternions representing rotations */ int CQRHLERPDist (double CQR_FAR * dist, const CQRQuaternionHandle q1, const CQRQuaternionHandle q2); #ifdef __cplusplus } #endif #endif #endif cqrlib-CQRlib-1.1.4/index.html000077700000000000000000000000001327146370000211152README_CQRlib.htmlustar00rootroot00000000000000cqrlib-CQRlib-1.1.4/lgpl.txt000066400000000000000000000635001327146370000156110ustar00rootroot00000000000000 GNU LESSER GENERAL PUBLIC LICENSE Version 2.1, February 1999 Copyright (C) 1991, 1999 Free Software Foundation, Inc. 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. [This is the first released version of the Lesser GPL. It also counts as the successor of the GNU Library Public License, version 2, hence the version number 2.1.] Preamble The licenses for most software are designed to take away your freedom to share and change it. By contrast, the GNU General Public Licenses are intended to guarantee your freedom to share and change free software--to make sure the software is free for all its users. This license, the Lesser General Public License, applies to some specially designated software packages--typically libraries--of the Free Software Foundation and other authors who decide to use it. You can use it too, but we suggest you first think carefully about whether this license or the ordinary General Public License is the better strategy to use in any particular case, based on the explanations below. When we speak of free software, we are referring to freedom of use, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for this service if you wish); that you receive source code or can get it if you want it; that you can change the software and use pieces of it in new free programs; and that you are informed that you can do these things. To protect your rights, we need to make restrictions that forbid distributors to deny you these rights or to ask you to surrender these rights. These restrictions translate to certain responsibilities for you if you distribute copies of the library or if you modify it. For example, if you distribute copies of the library, whether gratis or for a fee, you must give the recipients all the rights that we gave you. You must make sure that they, too, receive or can get the source code. If you link other code with the library, you must provide complete object files to the recipients, so that they can relink them with the library after making changes to the library and recompiling it. And you must show them these terms so they know their rights. We protect your rights with a two-step method: (1) we copyright the library, and (2) we offer you this license, which gives you legal permission to copy, distribute and/or modify the library. To protect each distributor, we want to make it very clear that there is no warranty for the free library. Also, if the library is modified by someone else and passed on, the recipients should know that what they have is not the original version, so that the original author's reputation will not be affected by problems that might be introduced by others. Finally, software patents pose a constant threat to the existence of any free program. We wish to make sure that a company cannot effectively restrict the users of a free program by obtaining a restrictive license from a patent holder. Therefore, we insist that any patent license obtained for a version of the library must be consistent with the full freedom of use specified in this license. Most GNU software, including some libraries, is covered by the ordinary GNU General Public License. This license, the GNU Lesser General Public License, applies to certain designated libraries, and is quite different from the ordinary General Public License. We use this license for certain libraries in order to permit linking those libraries into non-free programs. When a program is linked with a library, whether statically or using a shared library, the combination of the two is legally speaking a combined work, a derivative of the original library. The ordinary General Public License therefore permits such linking only if the entire combination fits its criteria of freedom. The Lesser General Public License permits more lax criteria for linking other code with the library. We call this license the "Lesser" General Public License because it does Less to protect the user's freedom than the ordinary General Public License. It also provides other free software developers Less of an advantage over competing non-free programs. These disadvantages are the reason we use the ordinary General Public License for many libraries. However, the Lesser license provides advantages in certain special circumstances. For example, on rare occasions, there may be a special need to encourage the widest possible use of a certain library, so that it becomes a de-facto standard. To achieve this, non-free programs must be allowed to use the library. A more frequent case is that a free library does the same job as widely used non-free libraries. In this case, there is little to gain by limiting the free library to free software only, so we use the Lesser General Public License. In other cases, permission to use a particular library in non-free programs enables a greater number of people to use a large body of free software. For example, permission to use the GNU C Library in non-free programs enables many more people to use the whole GNU operating system, as well as its variant, the GNU/Linux operating system. Although the Lesser General Public License is Less protective of the users' freedom, it does ensure that the user of a program that is linked with the Library has the freedom and the wherewithal to run that program using a modified version of the Library. The precise terms and conditions for copying, distribution and modification follow. Pay close attention to the difference between a "work based on the library" and a "work that uses the library". The former contains code derived from the library, whereas the latter must be combined with the library in order to run. GNU LESSER GENERAL PUBLIC LICENSE TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 0. This License Agreement applies to any software library or other program which contains a notice placed by the copyright holder or other authorized party saying it may be distributed under the terms of this Lesser General Public License (also called "this License"). Each licensee is addressed as "you". A "library" means a collection of software functions and/or data prepared so as to be conveniently linked with application programs (which use some of those functions and data) to form executables. The "Library", below, refers to any such software library or work which has been distributed under these terms. A "work based on the Library" means either the Library or any derivative work under copyright law: that is to say, a work containing the Library or a portion of it, either verbatim or with modifications and/or translated straightforwardly into another language. (Hereinafter, translation is included without limitation in the term "modification".) "Source code" for a work means the preferred form of the work for making modifications to it. For a library, complete source code means all the source code for all modules it contains, plus any associated interface definition files, plus the scripts used to control compilation and installation of the library. Activities other than copying, distribution and modification are not covered by this License; they are outside its scope. The act of running a program using the Library is not restricted, and output from such a program is covered only if its contents constitute a work based on the Library (independent of the use of the Library in a tool for writing it). Whether that is true depends on what the Library does and what the program that uses the Library does. 1. You may copy and distribute verbatim copies of the Library's complete source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice and disclaimer of warranty; keep intact all the notices that refer to this License and to the absence of any warranty; and distribute a copy of this License along with the Library. You may charge a fee for the physical act of transferring a copy, and you may at your option offer warranty protection in exchange for a fee. 2. You may modify your copy or copies of the Library or any portion of it, thus forming a work based on the Library, and copy and distribute such modifications or work under the terms of Section 1 above, provided that you also meet all of these conditions: a) The modified work must itself be a software library. b) You must cause the files modified to carry prominent notices stating that you changed the files and the date of any change. c) You must cause the whole of the work to be licensed at no charge to all third parties under the terms of this License. d) If a facility in the modified Library refers to a function or a table of data to be supplied by an application program that uses the facility, other than as an argument passed when the facility is invoked, then you must make a good faith effort to ensure that, in the event an application does not supply such function or table, the facility still operates, and performs whatever part of its purpose remains meaningful. (For example, a function in a library to compute square roots has a purpose that is entirely well-defined independent of the application. Therefore, Subsection 2d requires that any application-supplied function or table used by this function must be optional: if the application does not supply it, the square root function must still compute square roots.) These requirements apply to the modified work as a whole. If identifiable sections of that work are not derived from the Library, and can be reasonably considered independent and separate works in themselves, then this License, and its terms, do not apply to those sections when you distribute them as separate works. But when you distribute the same sections as part of a whole which is a work based on the Library, the distribution of the whole must be on the terms of this License, whose permissions for other licensees extend to the entire whole, and thus to each and every part regardless of who wrote it. Thus, it is not the intent of this section to claim rights or contest your rights to work written entirely by you; rather, the intent is to exercise the right to control the distribution of derivative or collective works based on the Library. In addition, mere aggregation of another work not based on the Library with the Library (or with a work based on the Library) on a volume of a storage or distribution medium does not bring the other work under the scope of this License. 3. You may opt to apply the terms of the ordinary GNU General Public License instead of this License to a given copy of the Library. To do this, you must alter all the notices that refer to this License, so that they refer to the ordinary GNU General Public License, version 2, instead of to this License. (If a newer version than version 2 of the ordinary GNU General Public License has appeared, then you can specify that version instead if you wish.) Do not make any other change in these notices. Once this change is made in a given copy, it is irreversible for that copy, so the ordinary GNU General Public License applies to all subsequent copies and derivative works made from that copy. This option is useful when you wish to copy part of the code of the Library into a program that is not a library. 4. You may copy and distribute the Library (or a portion or derivative of it, under Section 2) in object code or executable form under the terms of Sections 1 and 2 above provided that you accompany it with the complete corresponding machine-readable source code, which must be distributed under the terms of Sections 1 and 2 above on a medium customarily used for software interchange. If distribution of object code is made by offering access to copy from a designated place, then offering equivalent access to copy the source code from the same place satisfies the requirement to distribute the source code, even though third parties are not compelled to copy the source along with the object code. 5. A program that contains no derivative of any portion of the Library, but is designed to work with the Library by being compiled or linked with it, is called a "work that uses the Library". Such a work, in isolation, is not a derivative work of the Library, and therefore falls outside the scope of this License. However, linking a "work that uses the Library" with the Library creates an executable that is a derivative of the Library (because it contains portions of the Library), rather than a "work that uses the library". The executable is therefore covered by this License. Section 6 states terms for distribution of such executables. When a "work that uses the Library" uses material from a header file that is part of the Library, the object code for the work may be a derivative work of the Library even though the source code is not. Whether this is true is especially significant if the work can be linked without the Library, or if the work is itself a library. The threshold for this to be true is not precisely defined by law. If such an object file uses only numerical parameters, data structure layouts and accessors, and small macros and small inline functions (ten lines or less in length), then the use of the object file is unrestricted, regardless of whether it is legally a derivative work. (Executables containing this object code plus portions of the Library will still fall under Section 6.) Otherwise, if the work is a derivative of the Library, you may distribute the object code for the work under the terms of Section 6. Any executables containing that work also fall under Section 6, whether or not they are linked directly with the Library itself. 6. As an exception to the Sections above, you may also combine or link a "work that uses the Library" with the Library to produce a work containing portions of the Library, and distribute that work under terms of your choice, provided that the terms permit modification of the work for the customer's own use and reverse engineering for debugging such modifications. You must give prominent notice with each copy of the work that the Library is used in it and that the Library and its use are covered by this License. You must supply a copy of this License. If the work during execution displays copyright notices, you must include the copyright notice for the Library among them, as well as a reference directing the user to the copy of this License. Also, you must do one of these things: a) Accompany the work with the complete corresponding machine-readable source code for the Library including whatever changes were used in the work (which must be distributed under Sections 1 and 2 above); and, if the work is an executable linked with the Library, with the complete machine-readable "work that uses the Library", as object code and/or source code, so that the user can modify the Library and then relink to produce a modified executable containing the modified Library. (It is understood that the user who changes the contents of definitions files in the Library will not necessarily be able to recompile the application to use the modified definitions.) b) Use a suitable shared library mechanism for linking with the Library. A suitable mechanism is one that (1) uses at run time a copy of the library already present on the user's computer system, rather than copying library functions into the executable, and (2) will operate properly with a modified version of the library, if the user installs one, as long as the modified version is interface-compatible with the version that the work was made with. c) Accompany the work with a written offer, valid for at least three years, to give the same user the materials specified in Subsection 6a, above, for a charge no more than the cost of performing this distribution. d) If distribution of the work is made by offering access to copy from a designated place, offer equivalent access to copy the above specified materials from the same place. e) Verify that the user has already received a copy of these materials or that you have already sent this user a copy. For an executable, the required form of the "work that uses the Library" must include any data and utility programs needed for reproducing the executable from it. However, as a special exception, the materials to be distributed need not include anything that is normally distributed (in either source or binary form) with the major components (compiler, kernel, and so on) of the operating system on which the executable runs, unless that component itself accompanies the executable. It may happen that this requirement contradicts the license restrictions of other proprietary libraries that do not normally accompany the operating system. Such a contradiction means you cannot use both them and the Library together in an executable that you distribute. 7. You may place library facilities that are a work based on the Library side-by-side in a single library together with other library facilities not covered by this License, and distribute such a combined library, provided that the separate distribution of the work based on the Library and of the other library facilities is otherwise permitted, and provided that you do these two things: a) Accompany the combined library with a copy of the same work based on the Library, uncombined with any other library facilities. This must be distributed under the terms of the Sections above. b) Give prominent notice with the combined library of the fact that part of it is a work based on the Library, and explaining where to find the accompanying uncombined form of the same work. 8. You may not copy, modify, sublicense, link with, or distribute the Library except as expressly provided under this License. Any attempt otherwise to copy, modify, sublicense, link with, or distribute the Library is void, and will automatically terminate your rights under this License. However, parties who have received copies, or rights, from you under this License will not have their licenses terminated so long as such parties remain in full compliance. 9. You are not required to accept this License, since you have not signed it. However, nothing else grants you permission to modify or distribute the Library or its derivative works. These actions are prohibited by law if you do not accept this License. Therefore, by modifying or distributing the Library (or any work based on the Library), you indicate your acceptance of this License to do so, and all its terms and conditions for copying, distributing or modifying the Library or works based on it. 10. Each time you redistribute the Library (or any work based on the Library), the recipient automatically receives a license from the original licensor to copy, distribute, link with or modify the Library subject to these terms and conditions. You may not impose any further restrictions on the recipients' exercise of the rights granted herein. You are not responsible for enforcing compliance by third parties with this License. 11. If, as a consequence of a court judgment or allegation of patent infringement or for any other reason (not limited to patent issues), conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot distribute so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not distribute the Library at all. For example, if a patent license would not permit royalty-free redistribution of the Library by all those who receive copies directly or indirectly through you, then the only way you could satisfy both it and this License would be to refrain entirely from distribution of the Library. If any portion of this section is held invalid or unenforceable under any particular circumstance, the balance of the section is intended to apply, and the section as a whole is intended to apply in other circumstances. It is not the purpose of this section to induce you to infringe any patents or other property right claims or to contest validity of any such claims; this section has the sole purpose of protecting the integrity of the free software distribution system which is implemented by public license practices. Many people have made generous contributions to the wide range of software distributed through that system in reliance on consistent application of that system; it is up to the author/donor to decide if he or she is willing to distribute software through any other system and a licensee cannot impose that choice. This section is intended to make thoroughly clear what is believed to be a consequence of the rest of this License. 12. If the distribution and/or use of the Library is restricted in certain countries either by patents or by copyrighted interfaces, the original copyright holder who places the Library under this License may add an explicit geographical distribution limitation excluding those countries, so that distribution is permitted only in or among countries not thus excluded. In such case, this License incorporates the limitation as if written in the body of this License. 13. The Free Software Foundation may publish revised and/or new versions of the Lesser General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns. Each version is given a distinguishing version number. If the Library specifies a version number of this License which applies to it and "any later version", you have the option of following the terms and conditions either of that version or of any later version published by the Free Software Foundation. If the Library does not specify a license version number, you may choose any version ever published by the Free Software Foundation. 14. If you wish to incorporate parts of the Library into other free programs whose distribution conditions are incompatible with these, write to the author to ask for permission. For software which is copyrighted by the Free Software Foundation, write to the Free Software Foundation; we sometimes make exceptions for this. Our decision will be guided by the two goals of preserving the free status of all derivatives of our free software and of promoting the sharing and reuse of software generally. NO WARRANTY 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. END OF TERMS AND CONDITIONS How to Apply These Terms to Your New Libraries If you develop a new library, and you want it to be of the greatest possible use to the public, we recommend making it free software that everyone can redistribute and change. You can do so by permitting redistribution under these terms (or, alternatively, under the terms of the ordinary General Public License). To apply these terms, attach the following notices to the library. It is safest to attach them to the start of each source file to most effectively convey the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. Copyright (C) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA Also add information on how to contact you by electronic and paper mail. You should also get your employer (if you work as a programmer) or your school, if any, to sign a "copyright disclaimer" for the library, if necessary. Here is a sample; alter the names: Yoyodyne, Inc., hereby disclaims all copyright interest in the library `Frob' (a library for tweaking knobs) written by James Random Hacker. , 1 April 1990 Ty Coon, President of Vice That's all there is to it!