From 51a7727fc3f1292f64b587ae8592cd0862d866ee Mon Sep 17 00:00:00 2001
From: Laurent Chapon <laurent.chapon@stfc.ac.uk>
Date: Wed, 7 Nov 2007 10:23:35 +0000
Subject: [PATCH] re #35 Adding the Qaut.cpp again
---
Code/Mantid/Geometry/src/Quat.cpp | 339 ++++++++++++++++++++++++++++++
1 file changed, 339 insertions(+)
create mode 100644 Code/Mantid/Geometry/src/Quat.cpp
diff --git a/Code/Mantid/Geometry/src/Quat.cpp b/Code/Mantid/Geometry/src/Quat.cpp
new file mode 100644
index 00000000000..0fc2741620f
--- /dev/null
+++ b/Code/Mantid/Geometry/src/Quat.cpp
@@ -0,0 +1,339 @@
+#include "Quat.h"
+#include "V3D.h"
+#include <cmath>
+#include <boost/test/floating_point_comparison.hpp>
+#include <stdexcept>
+
+namespace Mantid
+{
+namespace Geometry
+{
+
+// Use boost float comparison
+boost::test_tools::close_at_tolerance<double> quat_tol(boost::test_tools::percent_tolerance(1e-6));
+
+
+Quat::Quat():w(1),a(0),b(0),c(0)
+/*! Null Constructor
+ * Initialize the quaternion with the identity q=1.0+0i+0j+0k;
+ */
+{
+}
+Quat::Quat(const double _w,const double _a, const double _b, const double _c):w(_w),a(_a),b(_b),c(_c)
+//! Constructor with values
+{
+}
+
+Quat::Quat(const Quat& _q)
+//! Copy constructor
+{
+ w=_q.w;
+ a=_q.a;
+ b=_q.b;
+ c=_q.c;
+}
+
+Quat::Quat(const double _deg,const V3D& _axis)
+/*! Constructor from an angle and axis.
+ * \param _deg :: angle of rotation
+ * \param _axis :: axis to rotate about
+ *
+ * This construct a quaternion to represent a rotation
+ * of an angle _deg around the _axis. The _axis does not need to be a unit vector
+ * */
+{
+ setAngleAxis(_deg,_axis);
+}
+Quat& Quat::operator=(const Quat& q)
+{
+ w=q.w;
+ a=q.a;
+ b=q.b;
+ c=q.c;
+ return *this;
+}
+void Quat::set(const double ww, const double aa, const double bb, const double cc)
+{
+ w=ww;
+ a=aa;
+ b=bb;
+ c=cc;
+ return;
+}
+void Quat::setAngleAxis(const double _deg, const V3D& _axis)
+/*! Constructor from an angle and axis.
+ * \param _deg :: angle of rotation
+ * \param _axis :: axis to rotate about
+ *
+ * This construct a quaternion to represent a rotation
+ * of an angle _deg around the _axis. The _axis does not need to be a unit vector
+ * */
+{
+ double deg2rad=M_PI/180.0;
+ w=cos(0.5*_deg*deg2rad);
+ double s=sin(0.5*_deg*deg2rad);
+ V3D temp(_axis);
+ temp.normalize();
+ a=s*temp[0];
+ b=s*temp[1];
+ c=s*temp[2];
+ return;
+}
+void Quat::operator()(const double ww, const double aa, const double bb, const double cc)
+{
+ this->set(ww,aa,bb,cc);
+}
+void Quat::operator()(const double angle, const V3D& axis)
+{
+ this->setAngleAxis(angle,axis);
+}
+Quat::~Quat()
+//! Destructor
+{}
+
+void Quat::init()
+/*! Re-initialise a quaternion to identity.
+ */
+{
+ w=1.0;
+ a=b=c=0.0;
+ return;
+}
+
+Quat Quat::operator+(const Quat& _q) const
+/*! Quaternion addition operator
+ * \param _q :: the quaternion to add
+ * \return *this+_q
+ */
+{
+ return Quat(w+_q.w,a+_q.a,b+_q.b,c+_q.c);
+}
+
+Quat& Quat::operator+=(const Quat& _q)
+/*! Quaternion self-addition operator
+ * \param _q :: the quaternion to add
+ * \return *this+=_q
+ */
+{
+ w+=_q.w;a+=_q.a;b+=_q.b;c+=_q.c;
+ return *this;
+}
+
+Quat Quat::operator-(const Quat& _q) const
+/*! Quaternion subtraction operator
+ * \param _q :: the quaternion to add
+ * \return *this-_q
+ */
+
+{
+ return Quat(w-_q.w,a-_q.a,b-_q.b,c-_q.c);
+}
+
+Quat& Quat::operator-=(const Quat& _q)
+/*! Quaternion self-substraction operator
+ * \param _q :: the quaternion to add
+ * \return *this-=_q
+ */
+{
+ w-=_q.w;
+ a-=_q.a;
+ b-=_q.b;
+ c-=_q.c;
+ return *this;
+}
+
+Quat Quat::operator*(const Quat& _q) const
+/*! Quaternion multiplication operator
+ * \param _q :: the quaternion to multiply
+ * \return *this*_q
+ *
+ * Quaternion multiplication is non commutative
+ * in the same way multiplication of rotation matrices
+ * isn't.
+ */
+{
+ double w1,a1,b1,c1;
+ w1=w*_q.w-a*_q.a-b*_q.b-c*_q.c;
+ a1=w*_q.a+_q.w*a+b*_q.c-_q.b*c;
+ b1=w*_q.b+_q.w*b-a*_q.c+c*_q.a;
+ c1=w*_q.c+_q.w*c+a*_q.b-_q.a*b;
+ return Quat(w1,a1,b1,c1);
+}
+
+Quat& Quat::operator*=(const Quat& _q)
+/*! Quaternion self-multiplication operator
+ * \param _q :: the quaternion to multiply
+ * \return *this*=_q
+ */
+{
+ double w1,a1,b1,c1;
+ w1=w*_q.w-a*_q.a-b*_q.b-c*_q.c;
+ a1=w*_q.a+_q.w*a+b*_q.c-_q.b*c;
+ b1=w*_q.b+_q.w*b-a*_q.c+c*_q.a;
+ c1=w*_q.c+_q.w*c+a*_q.b-_q.a*b;
+ w=w1;a=a1;b=b1;c=c1;
+ return (*this);
+}
+
+bool Quat::operator==(const Quat& q) const
+/*! Quaternion equal operator
+ * \param _q :: the quaternion to compare
+ *
+ * Compare two quaternions at 1e-6%tolerance.
+ * Use boost close_at_tolerance method
+ */
+{
+ return (quat_tol(w,q.w) && quat_tol(a,q.a) && quat_tol(b,q.b) && quat_tol(c,q.c));
+}
+
+bool Quat::operator!=(const Quat& _q) const
+{
+/*! Quaternion non-equal operator
+ * \param _q :: the quaternion to compare
+ *
+ * Compare two quaternions at 1e-6%tolerance.
+ * Use boost close_at_tolerance method
+ */
+ return (!operator==(_q));
+}
+
+void Quat::normalize()
+/*! Quaternion normalization
+ *
+ * Divide all elements by the quaternion norm
+ */
+{
+ double overnorm=1.0/len2();
+ w*=overnorm;
+ a*=overnorm;
+ b*=overnorm;
+ c*=overnorm;
+ return;
+}
+
+void Quat::conjugate()
+/*! Quaternion complex conjugate
+ *
+ * Reverse the sign of the 3 imaginary components of the
+ * quaternion
+ */
+{
+ a*=-1.0;
+ b*=-1.0;
+ c*=-1.0;
+ return;
+}
+
+double Quat::len() const
+/*! Quaternion length
+ *
+ */
+{
+ return sqrt(len2());
+}
+
+double Quat::len2() const
+/*! Quaternion norm (length squared)
+ *
+ */
+{
+ return (w*w+a*a+b*b+c*c);
+}
+
+void Quat::inverse()
+/*! Inverse a quaternion
+ *
+ */
+{
+ conjugate();
+ normalize();
+ return;
+}
+void Quat::rotate(V3D& v) const
+/*! Rotate a vector.
+ * \param v :: the vector to be rotated
+ *
+ * The quaternion needs to be normalized beforehand to
+ * represent a rotation. If q is thequaternion, the rotation
+ * is represented by q.v.q-1 where q-1 is the inverse of
+ * v.
+ */
+ {
+ Quat qinvert(*this);
+ qinvert.inverse();
+ Quat pos(0.0,v[0],v[1],v[2]);
+ pos*=qinvert;
+ pos=(*this)*pos;
+ v[0]=pos[1];
+ v[1]=pos[2];
+ v[2]=pos[3];
+ }
+void Quat::GLMatrix(double mat[16])
+/*!
+ */
+{
+ double aa = a * a;
+ double ab = a * b;
+ double ac = a * c;
+ double aw = a * w;
+ double bb = b * b;
+ double bc = b * c;
+ double bw = b * w;
+ double cc = c * c;
+ double cw = c * w;
+ mat[0] = 1.0 - 2.0 * ( bb + cc );
+ mat[4] = 2.0 * ( ab - cw );
+ mat[8] = 2.0 * ( ac + bw );
+ mat[1] = 2.0 * ( ab + cw );
+ mat[5] = 1.0 - 2.0 * ( aa + cc );
+ mat[9] = 2.0 * ( bc - aw );
+ mat[2] = 2.0 * ( ac - bw );
+ mat[6] = 2.0 * ( bc + aw );
+ mat[10] = 1.0 - 2.0 * ( aa + bb );
+ mat[12] = mat[13] = mat[14] = mat[3] = mat[7] = mat[11] = 0.0;
+ mat[15] = 1.0;
+ return;
+}
+
+const double& Quat::operator[](const int Index) const
+{
+ switch (Index)
+ {
+ case 0: return w;
+ case 1: return a;
+ case 2: return b;
+ case 3: return c;
+ default:
+ throw std::runtime_error("Quat::operator[] range error");
+ }
+}
+
+double& Quat::operator[](const int Index)
+{
+ switch (Index)
+ {
+ case 0: return w;
+ case 1: return a;
+ case 2: return b;
+ case 3: return c;
+ default:
+ throw std::runtime_error("Quat::operator[] range error");
+ }
+}
+
+void Quat::printSelf(std::ostream& os) const
+{
+ os << "[" << w << "," << a << "," << b << "," << c << "]";
+ return;
+
+}
+
+std::ostream& operator<<(std::ostream& os,const Quat& q)
+{
+ q.printSelf(os);
+ return os;
+}
+
+} // Namespace Geometry
+
+} // Namespce Mantid
--
GitLab