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
#	User	Rev	Content
1	tim	1254	#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