mirror of https://github.com/CGAL/cgal
87 lines
1.9 KiB
C++
87 lines
1.9 KiB
C++
#ifndef CGAL_DSRPDB_QUATERNION_H_
|
|
#define CGAL_DSRPDB_QUATERNION_H_
|
|
#include <CGAL/PDB/basic.h>
|
|
#include <cmath>
|
|
#include <CGAL/PDB/Point.h>
|
|
#include <CGAL/PDB/Transform.h>
|
|
#include <iostream>
|
|
|
|
CGAL_PDB_BEGIN_NAMESPACE
|
|
//! \cond
|
|
class Quaternion {
|
|
protected:
|
|
typedef double NT;
|
|
NT q_[4];
|
|
|
|
public:
|
|
Quaternion(){
|
|
q_[0]=0;
|
|
q_[1]=0;
|
|
q_[2]=0;
|
|
q_[3]=0;
|
|
};
|
|
//! Construct it from the four doubles.
|
|
Quaternion(NT a, NT b, NT c, NT d){
|
|
q_[0]=a;
|
|
q_[1]=b;
|
|
q_[2]=c;
|
|
q_[3]=d;
|
|
};
|
|
|
|
|
|
//! Construct from 3 doubles (the first quaternion value is implicit)
|
|
Quaternion(NT b, NT c, NT d){
|
|
q_[1]=b;
|
|
q_[2]=c;
|
|
q_[3]=d;
|
|
q_[0]=std::sqrt(1-b*b-c*c-d*d);
|
|
};
|
|
|
|
//! Construct a quaternion for a rotation by angle around the axis
|
|
Quaternion(const Vector &axis, double angle) {
|
|
const double mypi= 3.14159265358979323846264338327950288;
|
|
double f= angle/(2*mypi);
|
|
double sf= std::sqrt(f);
|
|
double n2= axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2];
|
|
double n= std::sqrt(n2);
|
|
q_[0]= std::sqrt(1-f);
|
|
q_[1]= sf*axis[0]/n;
|
|
q_[2]= sf*axis[1]/n;
|
|
q_[3]= sf*axis[2]/n;
|
|
assert(q_[0]*q_[0]+ q_[1]*q_[1] + q_[2]*q_[2] + q_[3]*q_[3] < 1.1);
|
|
assert(q_[0]*q_[0]+ q_[1]*q_[1] + q_[2]*q_[2] + q_[3]*q_[3] > 0.9);
|
|
}
|
|
|
|
template <class Arr>
|
|
Quaternion(Arr va){
|
|
for (int i=0; i<4; ++i){
|
|
q_[i]=va[i];
|
|
}
|
|
}
|
|
|
|
//Quaternion(const double m[3][3]);
|
|
Quaternion(Transform t);
|
|
|
|
NT& operator[](unsigned int i) {
|
|
assert(i<4);
|
|
return q_[i];
|
|
}
|
|
|
|
NT operator[](unsigned int i) const {
|
|
assert(i<4);
|
|
return q_[i];
|
|
}
|
|
|
|
Transform transform() const;
|
|
Transform transform(Vector translation) const;
|
|
};
|
|
|
|
inline std::ostream &operator<<(std::ostream &out, const Quaternion &q) {
|
|
out << "(" << q[0] << ", " << q[1] << ", " << q[2] << ", " << q[3] << ")";
|
|
return out;
|
|
}
|
|
|
|
//! \endcond
|
|
CGAL_PDB_END_NAMESPACE
|
|
#endif
|