1 #ifndef __JQUATERNION3D__
2 #define __JQUATERNION3D__
78 const double theta = atan2(
__u,
__a) *
y;
96 return in >> quaternion.
__a >> quaternion.
__u;
109 const JFormat format(out, getFormat<JQuaternion3D>(
JFormat_t(9, 6, std::ios::fixed | std::ios::showpos)));
111 return out << format << quaternion.
__a <<
' '
112 << format << quaternion.
__u;
125 return in >> quaternion.
__a >> quaternion.
__u;
138 return out << quaternion.
__a << quaternion.
__u;
304 public JMath <JQuaternion3D>,
305 public JMath <JQuaternion3D, JQuaternion3X>,
306 public JMath <JQuaternion3D, JQuaternion3Y>,
307 public JMath <JQuaternion3D, JQuaternion3Z>,
364 const double ct = cos(0.5*theta);
365 const double st = sin(0.5*theta);
533 return atan2(sqrt(this->
getB()*this->
getB() +
794 return *
this =
JQuaternion3D().setIdentity().mul(qz).mul(qy).mul(qx);
805 void rotate(
double& __x,
double& __y,
double& __z)
const
807 const double qx = 2.0 * (
__c*__z -
__d*__y);
808 const double qy = 2.0 * (
__d*__x -
__b*__z);
809 const double qz = 2.0 * (
__b*__y -
__c*__x);
826 const double qx = 2.0 * (
__d*__y -
__c*__z);
827 const double qy = 2.0 * (
__b*__z -
__d*__x);
828 const double qz = 2.0 * (
__c*__x -
__b*__y);
844 const double precision = std::numeric_limits<double>::min())
const
846 return (fabs(
getA() - quaternion.
getA()) <= precision &&
847 fabs(
getB() - quaternion.
getB()) <= precision &&
848 fabs(
getC() - quaternion.
getC()) <= precision &&
849 fabs(
getD() - quaternion.
getD()) <= precision);
859 bool isIdentity(
const double precision = std::numeric_limits<double>::min())
const
861 if (fabs(
getA()) <= precision) {
863 if (fabs(
getB()) <= precision)
864 return ((fabs(
getC()) <= precision && fabs(fabs(
getD()) - 1.0) <= precision) ||
865 (fabs(fabs(
getC()) - 1.0) <= precision && fabs(
getD()) <= precision));
867 return (fabs(fabs(
getB()) - 1.0) <= precision &&
868 fabs(
getC()) <= precision &&
869 fabs(
getD()) <= precision);
873 return (fabs(fabs(
getA()) - 1.0) <= precision &&
874 fabs(
getB()) <= precision &&
875 fabs(
getC()) <= precision &&
876 fabs(
getD()) <= precision);
911 return JQuaternion3D(quaternion).sub(*this).getLengthSquared();
987 const double theta = atan2(
v,
getA());
1007 const double MAXIMAL_DOT_PRODUCT = 0.9995;
1015 double dot = v0.
getDot(v1);
1023 double s0 = 1.0 - alpha;
1025 if (dot <= MAXIMAL_DOT_PRODUCT) {
1027 const double theta_0 = acos(dot);
1028 const double theta_1 = theta_0 * alpha;
1030 s1 = sin(theta_1) / sin(theta_0);
1031 s0 = cos(theta_1) - dot * s1;
1034 *
this = (s0 * v0) + (s1 * v1);
1049 in >> quaternion.
__a;
1050 in >> quaternion.
__b;
1051 in >> quaternion.
__c;
1052 in >> quaternion.
__d;
1069 const JFormat format(out, getFormat<JQuaternion3D>(
JFormat_t(9, 6, std::ios::fixed | std::ios::showpos)));
1071 out << format << quaternion.
getA() <<
' '
1072 << format << quaternion.
getB() <<
' '
1073 << format << quaternion.
getC() <<
' '
1074 << format << quaternion.
getD();
1089 in >> quaternion.
__a;
1090 in >> quaternion.
__b;
1091 in >> quaternion.
__c;
1092 in >> quaternion.
__d;
1107 out << quaternion.
__a;
1108 out << quaternion.
__b;
1109 out << quaternion.
__c;
1110 out << quaternion.
__d;
1168 double dot = (first.
getA() * second.
getA() +
1173 dot = 2.0 * dot * dot - 1.0;
1177 else if (dot < -1.0)
1180 return 0.5 * acos(dot) * 180.0 /
JMATH::PI;
Base class for data structures with artithmetic capabilities.
Data structure for unit quaternion in three dimensions.
JQuaternion3D(const JQuaternion3Y &qy)
Constructor.
JQuaternion3D & conjugate()
Conjugate quaternion.
JQuaternion3D(const JQuaternion3X &qx, const JQuaternion3Y &qy, const JQuaternion3Z &qz)
Constructor.
const JQuaternion3D & getQuaternion() const
Get quaternion.
static const JQuaternion3D & getIdentity()
Get identity quaternion.
friend JWriter & operator<<(JWriter &out, const JQuaternion3D &quaternion)
Write quaternion to output.
JQuaternion3D & mul(const JQuaternion3D &first, const JQuaternion3Y &second)
Quaternion multiplication.
double getDistanceSquared(const JQuaternion3D &quaternion) const
Get squared of distance to quaternion.
JQuaternion3D & mul(const JQuaternion3D &first, const JQuaternion3Z &second)
Quaternion multiplication.
JQuaternion3D & mul(const JQuaternion3X &qx, const JQuaternion3Y &qy, const JQuaternion3Z &qz)
Quaternion multiplication.
JQuaternion3D(const JQuaternion3Z &qz)
Constructor.
JQuaternion3D(const JQuaternion3X &qx)
Constructor.
JQuaternion3D()
Default constructor.
JQuaternion3D & interpolate(const JQuaternion3D &object, const double alpha)
Interpolation between quaternions.
JQuaternion3D & mul(const JQuaternion3D &first, const JQuaternion3X &second)
Quaternion multiplication.
void setQuaternion(const JQuaternion3D &quaternion)
Set quaternion.
JQuaternion3D & pow(const double y)
Raise quaternion to given power.
JQuaternion3D & div(const double factor)
Scale quaternion.
friend JReader & operator>>(JReader &in, JQuaternion3D &quaternion)
Read quaternion from input.
JQuaternion3D getConjugate() const
Get conjugate of this quaternion.
void rotate(double &__x, double &__y, double &__z) const
Rotate.
bool isIdentity(const double precision=std::numeric_limits< double >::min()) const
Test identity.
JQuaternion3D & setIdentity()
Set to identity quaternion.
double getAngle() const
Get rotation angle.
double getB() const
Get b value.
JQuaternion3D & normalise()
Normalise quaternion.
double getD() const
Get d value.
double getDistance(const JQuaternion3D &quaternion) const
Get distance to quaternion.
double getLength() const
Get length.
JQuaternion3D & getQuaternion()
Get quaternion.
void rotate_back(double &__x, double &__y, double &__z) const
Rotate back.
double getC() const
Get c value.
JQuaternion3D & mul(const double factor)
Scale quaternion.
JQuaternion3D(const double theta, const JVersor3D &dir)
Constructor.
JQuaternion3D & negate()
Negate quaternion.
JQuaternion3D(const double a, const double b, const double c, const double d)
Constructor.
JQuaternion3D & mul(const JQuaternion3D &first, const JQuaternion3D &second)
Quaternion multiplication.
JQuaternion3D & add(const JQuaternion3D &quaternion)
Add quaternion.
friend std::ostream & operator<<(std::ostream &out, const JQuaternion3D &quaternion)
Write quaternion to output.
bool equals(const JQuaternion3D &quaternion, const double precision=std::numeric_limits< double >::min()) const
Check equality.
double getDot(const JQuaternion3D &quaternion) const
Get dot product.
friend std::istream & operator>>(std::istream &in, JQuaternion3D &quaternion)
Write quaternion from input.
double getA() const
Get a value.
double getLengthSquared() const
Get length squared.
JQuaternion3D & sub(const JQuaternion3D &quaternion)
Subtract quaternion.
JQuaternion3D(const double w, const JVector3D &dir)
Constructor.
Data structure for vector in three dimensions.
double getY() const
Get y position.
double getZ() const
Get z position.
double getDot(const JVector3D &vector) const
Get dot product.
double getX() const
Get x position.
Data structure for normalised vector in three dimensions.
double getDY() const
Get y direction.
double getDX() const
Get x direction.
double getDZ() const
Get z direction.
Interface for binary input.
Interface for binary output.
Auxiliary classes and methods for 3D geometrical objects and operations.
double getAngle(const JQuaternion3D &first, const JQuaternion3D &second)
Get space angle between quanternions.
static const double PI
Mathematical constants.
This name space includes all other name spaces (except KM3NETDAQ, KM3NET and ANTARES).
This class represents a rotation.
friend std::istream & operator>>(std::istream &in, JQuaternion2D &quaternion)
Write quaternion from input.
double getA() const
Get a value.
friend std::ostream & operator<<(std::ostream &out, const JQuaternion2D &quaternion)
Write quaternion to output.
friend JWriter & operator<<(JWriter &out, const JQuaternion2D &quaternion)
Write quaternion to output.
JQuaternion2D & pow(const double y)
Raise quaternion to given power.
JQuaternion2D(const double theta)
Constructor.
JQuaternion2D()
Default constructor.
friend JReader & operator>>(JReader &in, JQuaternion2D &quaternion)
Read quaternion from input.
Auxiliary data structure for decomposition of quaternion in twist and swing quaternions.
JQuaternion3D getQuaternion() const
Get quaternion.
JQuaternion3D swing
rotation around perpendicular axis
JQuaternion3D twist
rotation around parallel axis
decomposition(const JQuaternion3D &Q, const JVector3D &dir)
Constructor.
This class represents a rotation around the x-axis.
JQuaternion3X(const double theta)
Constructor.
double getB() const
Get b value.
JQuaternion3X()
Default constructor.
JQuaternion3X & pow(const double y)
Raise quaternion to given power.
This class represents a rotation around the y-axis.
JQuaternion3Y()
Default constructor.
JQuaternion3Y & pow(const double y)
Raise quaternion to given power.
JQuaternion3Y(const double theta)
Constructor.
double getC() const
Get c value.
This class represents a rotation around the z-axis.
double getD() const
Get d value.
JQuaternion3Z & pow(const double y)
Raise quaternion to given power.
JQuaternion3Z()
Default constructor.
JQuaternion3Z(const double theta)
Constructor.
Template definition of auxiliary base class for comparison of data structures.
Auxiliary base class for aritmetic operations of derived class types.