jiri
/
LA_library
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

*** empty log message ***

This commit is contained in:
jiri 2020-01-04 21:58:49 +00:00
parent 2ab28bb5e8
commit 20a34fbc52
3 changed files with 47 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
quaternion.cc
View File

@ -24,17 +24,43 @@
template<typename T>
const Quaternion<T> Quaternion<T>::operator*(const Quaternion<T> &rhs) const
Quaternion<T> Quaternion<T>::operator*(const Quaternion<T> &rhs) const
{
return Quaternion<T>
(
this->q[0]*rhs.q[0]-this->q[1]*rhs.q[1]-this->q[2]*rhs.q[2]-this->q[3]*rhs.q[3],
this->q[0]*rhs.q[1]+this->q[1]*rhs.q[0]+this->q[2]*rhs.q[3]-this->q[3]*rhs.q[2],
this->q[0]*rhs.q[2]+this->q[2]*rhs.q[0]+this->q[3]*rhs.q[1]-this->q[1]*rhs.q[3],
this->q[0]*rhs.q[3]+this->q[3]*rhs.q[0]+this->q[1]*rhs.q[2]-this->q[2]*rhs.q[1]
q[0]*rhs.q[0]-q[1]*rhs.q[1]-q[2]*rhs.q[2]-q[3]*rhs.q[3],
q[0]*rhs.q[1]+q[1]*rhs.q[0]+q[2]*rhs.q[3]-q[3]*rhs.q[2],
q[0]*rhs.q[2]+q[2]*rhs.q[0]+q[3]*rhs.q[1]-q[1]*rhs.q[3],
q[0]*rhs.q[3]+q[3]*rhs.q[0]+q[1]*rhs.q[2]-q[2]*rhs.q[1]
);
};
template<typename T>
Quaternion<T> Quaternion<T>::times_vec3(const T *rhs) const
{
return Quaternion<T>
(
-q[1]*rhs[0]-q[2]*rhs[1]-q[3]*rhs[2],
q[0]*rhs[0]+q[2]*rhs[2]-q[3]*rhs[1],
q[0]*rhs[1]+q[3]*rhs[0]-q[1]*rhs[2],
q[0]*rhs[2]+q[1]*rhs[1]-q[2]*rhs[0]
);
};
template<typename T>
Quaternion<T> Quaternion<T>::vec3_times(const T *lhs) const
{
return Quaternion<T>
(
-lhs[0]*q[1]-lhs[1]*q[2]-lhs[2]*q[3],
lhs[0]*q[0]+lhs[1]*q[3]-lhs[2]*q[2],
lhs[1]*q[0]+lhs[2]*q[1]-lhs[0]*q[3],
lhs[2]*q[0]+lhs[0]*q[2]-lhs[1]*q[1]
);
};
//basically the same code as in normquat2rotmat, but avoiding extra storage
template<typename T>

16
quaternion.h
View File

@ -21,10 +21,11 @@
#ifndef _QUATERNION_H_
#define _QUATERNION_H_
#include <stdlib.h>
#ifndef AVOID_STDSTREAM
#include <iostream>
#endif
#include <string.h>
#include <complex>
#include <cstring>
#include <math.h>
@ -61,9 +62,12 @@ public:
const Quaternion operator-() const {Quaternion r(*this); r.q[0]= -r.q[0]; r.q[1]= -r.q[1]; r.q[2]= -r.q[2]; r.q[3]= -r.q[3]; return r;}; //unary minus
Quaternion& operator+=(const Quaternion &rhs) {q[0]+=rhs.q[0];q[1]+=rhs.q[1];q[2]+=rhs.q[2];q[3]+=rhs.q[3]; return *this;};
Quaternion& operator-=(const Quaternion &rhs) {q[0]-=rhs.q[0];q[1]-=rhs.q[1];q[2]-=rhs.q[2];q[3]-=rhs.q[3]; return *this;};
const Quaternion operator+(const Quaternion &rhs) const {return Quaternion(*this) += rhs;};
const Quaternion operator-(const Quaternion &rhs) const {return Quaternion(*this) -= rhs;};
const Quaternion operator*(const Quaternion &rhs) const;
Quaternion operator+(const Quaternion &rhs) const {return Quaternion(*this) += rhs;};
Quaternion operator-(const Quaternion &rhs) const {return Quaternion(*this) -= rhs;};
Quaternion operator*(const Quaternion &rhs) const; //regular product
Quaternion times_vec3(const T *rhs) const; //save flops for quaternions representing vectors
Quaternion vec3_times(const T *rhs) const; //save flops for quaternions representing vectors
Quaternion& conjugateme(void) {q[1] = -q[1]; q[2] = -q[2]; q[3] = -q[3]; return *this;}
Quaternion conjugate(void) const {return Quaternion(*this).conjugateme();}
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];};
@ -84,6 +88,7 @@ public:
//stream I/O
#ifndef AVOID_STDSTREAM
template <typename T>
std::istream& operator>>(std::istream &s, Quaternion<T> &x)
{
@ -102,6 +107,7 @@ s << x.q[2]<<" ";
s << x.q[3];
return s;
}
#endif

8
vecmat3.h
View File

@ -24,8 +24,10 @@
#define _VECMAT3_H_
#include <stdlib.h>
#ifndef AVOID_STDSTREAM
#include <iostream>
#include <cstring>
#endif
#include <string.h>
#include <math.h>
//forward declaration
@ -169,6 +171,7 @@ public:
//stream I/O
#ifndef AVOID_STDSTREAM
template <typename T>
std::istream& operator>>(std::istream &s, Vec3<T> &x)
{
@ -208,8 +211,7 @@ s << x.q[1][0]<<" "<< x.q[1][1]<<" " << x.q[1][2]<<std::endl;
s << x.q[2][0]<<" "<< x.q[2][1]<<" " << x.q[2][2]<<std::endl;
return s;
}
#endif
#endif /* _VECMAT3_H_ */