OOPSE/libmdtools/Quaternion.cpp

#include <cmath>
#include "Euler3.hpp"
#include "Vector3d.hpp"
#include "Mat3x3d.hpp"
#include "Quaternion.hpp"

Quaternion::Quaternion(Mat3x3d& m){
    *this = m.toQuaternion();
}

Quaternion::Quaternion(const Euler3& e){
  quat[0] = cos(e.theta/2) * cos((e.phi + e.psi)/2);
  quat[1] = sin(e.theta/2) * cos((e.phi - e.psi)/2);
  quat[2] = sin(e.theta/2) * sin((e.phi - e.psi)/2);
  quat[3] = cos(e.theta/2) * sin((e.phi + e.psi)/2);
}

Mat3x3d Quaternion::toRotationMatrix(){

  return Mat3x3d(*this);
}

Euler3 Quaternion::toEuler(){
  Mat3x3d m;
  m = toRotationMatrix();
  return m.toEuler();
}

Revision:	1254
Committed:	Wed Jun 9 16:16:33 2004 UTC (20 years, 10 months ago) by tim
File size:	601 byte(s)
Log Message:	1. adding some useful math classes(Mat3x3d, Vector3d, Quaternion, Euler3) these classes use anonymous union and struct to support double[3], double[3][3] and double[4] 2. adding roll constraint algorithm
#	Content
1	#include <cmath>
2	#include "Euler3.hpp"
3	#include "Vector3d.hpp"
4	#include "Mat3x3d.hpp"
5	#include "Quaternion.hpp"
6
7	Quaternion::Quaternion(Mat3x3d& m){
8	*this = m.toQuaternion();
9	}
10
11	Quaternion::Quaternion(const Euler3& e){
12	quat[0] = cos(e.theta/2) * cos((e.phi + e.psi)/2);
13	quat[1] = sin(e.theta/2) * cos((e.phi - e.psi)/2);
14	quat[2] = sin(e.theta/2) * sin((e.phi - e.psi)/2);
15	quat[3] = cos(e.theta/2) * sin((e.phi + e.psi)/2);
16	}
17
18	Mat3x3d Quaternion::toRotationMatrix(){
19
20	return Mat3x3d(*this);
21	}
22
23	Euler3 Quaternion::toEuler(){
24	Mat3x3d m;
25	m = toRotationMatrix();
26	return m.toEuler();
27	}
28