re #35

Adding the Qaut.cpp again

Code/Mantid/Geometry/src/Quat.cpp

+#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