*** empty log message ***
This commit is contained in:
jiri
parent
20a34fbc52
commit
faf719660c
@ -18,6 +18,8 @@
|
||||
|
||||
#include "quaternion.h"
|
||||
|
||||
using namespace LA_Quaternion;
|
||||
|
||||
|
||||
//do not replicate this code in each object file, therefore not in .h
|
||||
//and instantize the templates for the types needed
|
||||
@ -154,7 +156,6 @@ return derivative;
|
||||
|
||||
//optionally skip this for microcontrollers if not needed
|
||||
//note that C++ standard headers should use float version of the functions for T=float
|
||||
#ifndef AVOID_GONIOM_FUNC
|
||||
template<typename T>
|
||||
void Quaternion<T>::normquat2euler(T *e) const
|
||||
{
|
||||
@ -184,7 +185,6 @@ axis[0]= q[1]*s;
|
||||
axis[1]= q[2]*s;
|
||||
axis[2]= q[3]*s;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@ -192,18 +192,10 @@ axis[2]= q[3]*s;
|
||||
#define INSTANTIZE(T) \
|
||||
template class Quaternion<T>; \
|
||||
|
||||
#define INSTANTIZE2(T) \
|
||||
|
||||
|
||||
INSTANTIZE(float)
|
||||
#ifndef QUAT_NO_DOUBLE
|
||||
INSTANTIZE(double)
|
||||
#endif
|
||||
|
||||
#ifndef AVOID_GONIOM_FUNC
|
||||
INSTANTIZE2(float)
|
||||
#ifndef QUAT_NO_DOUBLE
|
||||
INSTANTIZE2(double)
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
||||
67
quaternion.h
67
quaternion.h
@ -21,13 +21,16 @@
|
||||
#ifndef _QUATERNION_H_
|
||||
#define _QUATERNION_H_
|
||||
|
||||
|
||||
#ifndef AVOID_STDSTREAM
|
||||
#include <iostream>
|
||||
#endif
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <complex>
|
||||
#include <math.h>
|
||||
|
||||
namespace LA_Quaternion {
|
||||
|
||||
template <typename T>
|
||||
class Quaternion
|
||||
@ -73,17 +76,25 @@ public:
|
||||
T dot(const Quaternion &rhs) const {return q[0]*rhs.q[0] + q[1]*rhs.q[1] + q[2]*rhs.q[2] + q[3]*rhs.q[3];};
|
||||
T normsqr(void) const {return dot(*this);};
|
||||
T norm(void) const {return sqrt(normsqr());};
|
||||
Quaternion& normalize(bool unique_sign=false) {*this /= norm(); if(unique_sign && q[0]<0) *this *= (T)-1; return *this;};
|
||||
Quaternion& normalize(T *getnorm=NULL, bool unique_sign=false) {T nn=norm(); if(getnorm) *getnorm=nn; *this /= nn; if(unique_sign && q[0]<0) *this *= (T)-1; return *this;};
|
||||
Quaternion inverse(void) const {return Quaternion(*this).conjugateme()/normsqr();};
|
||||
const Quaternion operator/(const Quaternion &rhs) const {return *this * rhs.inverse();};
|
||||
Quaternion rotateby(const Quaternion &rhs); //conjugation-rotation of this by NORMALIZED rhs
|
||||
void rotate(T *to, const T *from, Quaternion<T> *grad=NULL) const; //rotate xyz vector by NORMALIZED *this
|
||||
Quaternion rotate_match(T *to, const T *from, const T *match) const; //gradient of quaternion rotation which should match a given vector by rotating the from vector
|
||||
Quaternion commutator(const Quaternion &rhs) const {return *this * rhs - rhs * *this;}; //could be made more efficient
|
||||
Quaternion anticommutator(const Quaternion &rhs) const {return *this * rhs + rhs * *this;}; //could be made more efficient
|
||||
|
||||
//some conversions
|
||||
void normquat2euler(T *) const; //"euler" or Tait-Bryan angles [corresponding to meul -r -T xyz -d -t -R]
|
||||
void axis2normquat(const T *axis, const T &angle);
|
||||
void normquat2axis(T *axis, T &angle) const;
|
||||
|
||||
//C-style IO
|
||||
void fprintf(FILE *f, const char *format) const {::fprintf(f,format,q[0],q[1],q[2],q[3]);};
|
||||
void sprintf(char *f, const char *format) const {::sprintf(f,format,q[0],q[1],q[2],q[3]);};
|
||||
int fscanf(FILE *f, const char *format) const {return ::fscanf(f,format,q[0],q[1],q[2],q[3]);};
|
||||
int sscanf(char *f, const char *format) const {return ::sscanf(f,format,q[0],q[1],q[2],q[3]);};
|
||||
};
|
||||
|
||||
|
||||
@ -243,7 +254,61 @@ if(a[0][1]-a[1][0]<0) q[3] = -q[3];
|
||||
|
||||
|
||||
|
||||
//Functions - cf. https://en.wikipedia.org/wiki/Quaternion
|
||||
|
||||
|
||||
template<typename T>
|
||||
Quaternion<T> exp(const Quaternion<T> &x)
|
||||
{
|
||||
Quaternion<T> r;
|
||||
T vnorm = sqrt(x[1]*x[1]+x[2]*x[2]+x[3]*x[3]);
|
||||
r[0] = cos(vnorm);
|
||||
vnorm = sin(vnorm)/vnorm;
|
||||
r[1] = x[1] * vnorm;
|
||||
r[2] = x[2] * vnorm;
|
||||
r[3] = x[3] * vnorm;
|
||||
r*= ::exp(x[0]);
|
||||
return r;
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
Quaternion<T> log(const Quaternion<T> &x)
|
||||
{
|
||||
Quaternion<T> r;
|
||||
T vnorm = x[1]*x[1]+x[2]*x[2]+x[3]*x[3];
|
||||
T xnorm = vnorm + x[0]*x[0];
|
||||
vnorm = sqrt(vnorm);
|
||||
xnorm = sqrt(xnorm);
|
||||
r[0] = ::log(xnorm);
|
||||
T tmp = acos(x[0]/xnorm)/vnorm;
|
||||
r[1] = x[1] * tmp;
|
||||
r[2] = x[2] * tmp;
|
||||
r[3] = x[3] * tmp;
|
||||
return r;
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
Quaternion<T> pow(const Quaternion<T> &x, const T &y)
|
||||
{
|
||||
Quaternion<T> r;
|
||||
T vnorm = x[1]*x[1]+x[2]*x[2]+x[3]*x[3];
|
||||
T xnorm = vnorm + x[0]*x[0];
|
||||
vnorm = sqrt(vnorm);
|
||||
xnorm = sqrt(xnorm);
|
||||
T phi = acos(x[0]/xnorm);
|
||||
r[0] = cos(y*phi);
|
||||
T tmp = sin(y*phi)/vnorm;
|
||||
r[1] = x[1] * tmp;
|
||||
r[2] = x[2] * tmp;
|
||||
r[3] = x[3] * tmp;
|
||||
r *= ::pow(xnorm,y);
|
||||
return r;
|
||||
}
|
||||
|
||||
|
||||
} //namespace
|
||||
|
||||
#endif /* _QUATERNION_H_ */
|
||||
|
||||
|
||||
10
t.cc
10
t.cc
@ -24,9 +24,12 @@
|
||||
#include "quaternion.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace LA_Vecmat3;
|
||||
using namespace LA_Quaternion;
|
||||
using namespace LA;
|
||||
|
||||
|
||||
|
||||
extern void test(const NRVec<double> &x);
|
||||
|
||||
|
||||
@ -1883,7 +1886,7 @@ cout<<r<<endl;
|
||||
Mat3<double> rotmat;
|
||||
quat2rotmat(r,rotmat);
|
||||
cout << rotmat[0][1]<<endl;
|
||||
r.normalize(true);
|
||||
r.normalize();
|
||||
NRMat<double> rotmat2(3,3),rotmat3(3,3);
|
||||
Quaternion<NRMat<double> > rotmatder;
|
||||
rotmatder[0].resize(3,3);
|
||||
@ -1921,6 +1924,11 @@ Quaternion<double> rrvec = rvec.rotateby(rrr.conjugate());
|
||||
cout <<rrvec<<endl;
|
||||
Vec3<double> rotvec;
|
||||
rrr.rotate(rotvec,vec);
|
||||
Quaternion<double> qq={1.5,2,-3,2.123};
|
||||
cout << " test "<<qq*qq<<endl;
|
||||
cout <<"exp " <<exp(qq)<<endl;
|
||||
cout <<"log " <<log(qq)<<endl;
|
||||
cout <<"pow " <<pow(qq,0.5)<<endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
15
vecmat3.h
15
vecmat3.h
@ -29,6 +29,9 @@
|
||||
#endif
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
namespace LA_Vecmat3 {
|
||||
|
||||
//forward declaration
|
||||
template <typename T> class Mat3;
|
||||
@ -81,7 +84,11 @@ public:
|
||||
r[2] = q[0]*rhs.q[0][2] + q[1]*rhs.q[1][2] + q[2]*rhs.q[2][2];
|
||||
return r;
|
||||
}; //matrix times vector
|
||||
|
||||
//C-style IO
|
||||
void fprintf(FILE *f, const char *format) const {::fprintf(f,format,q[0],q[1],q[2]);};
|
||||
void sprintf(char *f, const char *format) const {::sprintf(f,format,q[0],q[1],q[2]);};
|
||||
int fscanf(FILE *f, const char *format) const {return ::fscanf(f,format,q[0],q[1],q[2]);};
|
||||
int sscanf(char *f, const char *format) const {return ::sscanf(f,format,q[0],q[1],q[2]);};
|
||||
};
|
||||
|
||||
|
||||
@ -166,6 +173,10 @@ public:
|
||||
r[2][2]= q[1][1]*q[0][0]-q[1][0]*q[0][1];
|
||||
return r/determinant();
|
||||
};
|
||||
//C-style IO
|
||||
void fprintf(FILE *f, const char *format) const {::fprintf(f,format,q[0][0],q[0][1],q[0][2]); ::fprintf(f,format,q[1][0],q[1][1],q[1][2]); ::fprintf(f,format,q[2][0],q[2][1],q[2][2]);};
|
||||
int fscanf(FILE *f, const char *format) const {return ::fscanf(f,format,q[0][0],q[0][1],q[0][2]) + ::fscanf(f,format,q[1][0],q[1][1],q[1][2]) + ::fscanf(f,format,q[2][0],q[2][1],q[2][2]);};
|
||||
|
||||
};
|
||||
|
||||
|
||||
@ -213,6 +224,6 @@ return s;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
}//namespace
|
||||
#endif /* _VECMAT3_H_ */
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user