MercuryDPM  Beta
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Quarternion Class Reference

#include <Quarternion.h>

Public Member Functions

 Quarternion ()
 Constructs a basic Quarternion. More...
 
 Quarternion (const Quarternion &other)
 Copy constructor. More...
 
 Quarternion (double x, double y, double z, double w)
 Constructs a a Quarternion using its four components. More...
 
 Quarternion (Vec3D axis, double angle)
 Constructs a Quarternion using a rotation around an axis. More...
 
void getAxisAndAngle (Vec3D &axis, double &angle) const
 Returns the orientation as represented by a rotation around an axis. More...
 
Matrix3D getRotationMatrix () const
 
void reset ()
 Resets a the Quarternion. More...
 
void normalise ()
 Normalises the Quarternion. More...
 
Quarternion operator* (const Quarternion &other) const
 Multiplication operator. More...
 
void integrate (const Vec3D &omega, double timeStep)
 Integration of the Quarternion using rotational velocity and timestep. More...
 
 Quarternion (const Vec3D &a)
 Constructs a Quaternion using the lab x-y-z sequence, where the object is. More...
 
Vec3D get3DAngles () const
 Constructs a representation of the quarternion using 3 rotations in the lab x-y-z sequence, where the object is first rotated along lab X-axis then rotated along lab Y-axis then rotated along lab Z-axis. More...
 

Private Attributes

Mdouble w_
 
Mdouble x_
 
Mdouble y_
 
Mdouble z_
 

Friends

std::ostream & operator<< (std::ostream &os, const Quarternion &q)
 
std::istream & operator>> (std::istream &is, Quarternion &q)
 

Detailed Description

Definition at line 36 of file Quarternion.h.

Constructor & Destructor Documentation

Quarternion::Quarternion ( )

Constructs a basic Quarternion.

Definition at line 32 of file Quarternion.cc.

References reset().

33 {
34  reset();
35 }
void reset()
Resets a the Quarternion.
Definition: Quarternion.cc:91
Quarternion::Quarternion ( const Quarternion other)

Copy constructor.

Definition at line 37 of file Quarternion.cc.

References w_, x_, y_, and z_.

38 {
39  x_ = other.x_;
40  y_ = other.y_;
41  z_ = other.z_;
42  w_ = other.w_;
43 }
Mdouble z_
Definition: Quarternion.h:114
Mdouble y_
Definition: Quarternion.h:114
Mdouble x_
Definition: Quarternion.h:114
Mdouble w_
Definition: Quarternion.h:114
Quarternion::Quarternion ( double  x,
double  y,
double  z,
double  w 
)

Constructs a a Quarternion using its four components.

Definition at line 45 of file Quarternion.cc.

References w_, x_, y_, and z_.

46 {
47  x_ = x;
48  y_ = y;
49  z_ = z;
50  w_ = w;
51 }
Mdouble z_
Definition: Quarternion.h:114
Mdouble y_
Definition: Quarternion.h:114
Mdouble x_
Definition: Quarternion.h:114
Mdouble w_
Definition: Quarternion.h:114
Quarternion::Quarternion ( Vec3D  axis,
double  angle 
)

Constructs a Quarternion using a rotation around an axis.

Definition at line 53 of file Quarternion.cc.

References Vec3D::normalize(), w_, Vec3D::X, x_, Vec3D::Y, y_, Vec3D::Z, and z_.

54 {
55  std::cout << "Axis=" << axis << " Angle=" << angle << std::endl;
56  axis.normalize();
57  x_ = std::sin(0.5 * angle) * axis.X;
58  y_ = std::sin(0.5 * angle) * axis.Y;
59  z_ = std::sin(0.5 * angle) * axis.Z;
60  w_ = std::cos(0.5 * angle);
61 }
Mdouble z_
Definition: Quarternion.h:114
Mdouble X
the vector components
Definition: Vector.h:52
void normalize()
Makes this Vec3D unit length.
Definition: Vector.cc:234
Mdouble y_
Definition: Quarternion.h:114
Mdouble x_
Definition: Quarternion.h:114
Mdouble w_
Definition: Quarternion.h:114
Mdouble Y
Definition: Vector.h:52
Mdouble Z
Definition: Vector.h:52
Quarternion::Quarternion ( const Vec3D a)

Constructs a Quaternion using the lab x-y-z sequence, where the object is.

first rotated along lab X-axis then rotated along lab Y-axis then rotated along lab Z-axis

Definition at line 63 of file Quarternion.cc.

References w_, Vec3D::X, x_, Vec3D::Y, y_, Vec3D::Z, and z_.

64 {
65  x_ = sin(0.5 * a.X) * cos(0.5 * a.Y) * cos(0.5 * a.Z) - cos(0.5 * a.X) * sin(0.5 * a.Y) * sin(0.5 * a.Z);
66  y_ = cos(0.5 * a.X) * sin(0.5 * a.Y) * cos(0.5 * a.Z) + sin(0.5 * a.X) * cos(0.5 * a.Y) * sin(0.5 * a.Z);
67  z_ = cos(0.5 * a.X) * cos(0.5 * a.Y) * sin(0.5 * a.Z) - sin(0.5 * a.X) * sin(0.5 * a.Y) * cos(0.5 * a.Z);
68  w_ = cos(0.5 * a.X) * cos(0.5 * a.Y) * cos(0.5 * a.Z) + sin(0.5 * a.X) * sin(0.5 * a.Y) * sin(0.5 * a.Z);
69 }
Mdouble z_
Definition: Quarternion.h:114
Mdouble X
the vector components
Definition: Vector.h:52
Mdouble y_
Definition: Quarternion.h:114
Mdouble x_
Definition: Quarternion.h:114
Mdouble w_
Definition: Quarternion.h:114
Mdouble Y
Definition: Vector.h:52
Mdouble Z
Definition: Vector.h:52

Member Function Documentation

Vec3D Quarternion::get3DAngles ( ) const

Constructs a representation of the quarternion using 3 rotations in the lab x-y-z sequence, where the object is first rotated along lab X-axis then rotated along lab Y-axis then rotated along lab Z-axis.

Definition at line 143 of file Quarternion.cc.

References constants::pi, w_, x_, y_, and z_.

144 {
145  double rotY, rotZ, rotX;
146  double sqrtX = x_ * x_;
147  double sqrtY = y_ * y_;
148  double sqrtZ = z_ * z_;
149  double sqrtW = w_ * w_;
150  double unit = sqrtX + sqrtY + sqrtZ + sqrtW;
151  double test = x_ * y_ + z_ * w_;
152  if (test > 0.499999 * unit) // singularity at north pole
153  {
154  rotY = 2.0 * std::atan2(x_, w_);
155  rotZ = constants::pi / 2;
156  rotX = 0;
157  }
158  else if (test < -0.499999 * unit) // singularity at south pole
159  {
160  rotY = -2.0 * atan2(x_, w_);
161  rotZ = -constants::pi / 2;
162  rotX = 0;
163  }
164  else
165  {
166  rotY = atan2(2.0 * y_ * w_ - 2.0 * x_ * z_, sqrtX - sqrtY - sqrtZ + sqrtW);
167  rotZ = asin(2.0 * test / unit);
168  rotX = atan2(2.0 * x_ * w_ - 2.0 * y_ * z_, -sqrtX + sqrtY - sqrtZ + sqrtW);
169  }
170  return Vec3D(rotX, rotY, rotZ);
171 }
Mdouble z_
Definition: Quarternion.h:114
Mdouble y_
Definition: Quarternion.h:114
const Mdouble pi
Definition: ExtendedMath.h:42
Mdouble x_
Definition: Quarternion.h:114
Mdouble w_
Definition: Quarternion.h:114
Implementation of a 3D vector (by Vitaliy).
Definition: Vector.h:45
void Quarternion::getAxisAndAngle ( Vec3D axis,
double &  angle 
) const

Returns the orientation as represented by a rotation around an axis.

Definition at line 71 of file Quarternion.cc.

References w_, x_, y_, and z_.

72  {
73  angle = 2.0 * std::acos(w_);
74  axis = Vec3D(x_ / std::sin(0.5 * angle), y_ / std::sin(0.5 * angle), z_ / std::sin(0.5 * angle));
75 }
Mdouble z_
Definition: Quarternion.h:114
Mdouble y_
Definition: Quarternion.h:114
Mdouble x_
Definition: Quarternion.h:114
Mdouble w_
Definition: Quarternion.h:114
Implementation of a 3D vector (by Vitaliy).
Definition: Vector.h:45
Matrix3D Quarternion::getRotationMatrix ( ) const

Definition at line 77 of file Quarternion.cc.

References w_, x_, y_, and z_.

78 {
79  double xx = w_ * w_ + x_ * x_ - y_ * y_ - z_ * z_;
80  double xy = 2.0 * (x_ * y_ - w_ * z_);
81  double xz = 2.0 * (w_ * y_ + x_ * z_);
82  double yx = 2.0 * (x_ * y_ + w_ * z_);
83  double yy = w_ * w_ - x_ * x_ + y_ * y_ - z_ * z_;
84  double yz = 2.0 * (y_ * z_ - w_ * x_);
85  double zx = 2.0 * (x_ * z_ - w_ * y_);
86  double zy = 2.0 * (w_ * x_ + y_ * z_);
87  double zz = w_ * w_ - x_ * x_ - y_ * y_ + z_ * z_;
88  return Matrix3D(xx, xy, xz, yx, yy, yz, zx, zy, zz);
89 }
Mdouble z_
Definition: Quarternion.h:114
Mdouble y_
Definition: Quarternion.h:114
Mdouble x_
Definition: Quarternion.h:114
Mdouble w_
Definition: Quarternion.h:114
Implementation of a 3D matrix.
Definition: Matrix.h:36
void Quarternion::integrate ( const Vec3D omega,
double  timeStep 
)

Integration of the Quarternion using rotational velocity and timestep.

Todo:
Are we sure this works? *this = deltaQ * *this
Author
weinhartt

Definition at line 118 of file Quarternion.cc.

References Vec3D::getLengthSquared(), w_, Vec3D::X, x_, Vec3D::Y, y_, Vec3D::Z, and z_.

119 {
120  Quarternion deltaQ;
121  Vec3D theta = 0.5 * omega * timeStep;
122  double thetaMagSq = theta.getLengthSquared();
123  double s;
124  if (thetaMagSq * thetaMagSq / 24.0 < 1e-10)
125  {
126  std::cout << "Warning small rotation, needs additional checking" << std::endl;
127  deltaQ.w_ = 1.0 - 0.5 * thetaMagSq;
128  s = 1.0 - thetaMagSq / 6.0;
129  }
130  else
131  {
132  double thetaMag = std::sqrt(thetaMagSq);
133  deltaQ.w_ = std::cos(thetaMag);
134  s = std::sin(thetaMag) / thetaMag;
135  }
136  deltaQ.x_ = theta.X * s;
137  deltaQ.y_ = theta.Y * s;
138  deltaQ.z_ = theta.Z * s;
140  *this = deltaQ * *this;
141 }
Mdouble z_
Definition: Quarternion.h:114
static Mdouble getLengthSquared(const Vec3D &a)
Calculates the squared length of a Vec3D: .
Definition: Vector.cc:304
Mdouble X
the vector components
Definition: Vector.h:52
Mdouble y_
Definition: Quarternion.h:114
Mdouble x_
Definition: Quarternion.h:114
Mdouble w_
Definition: Quarternion.h:114
Mdouble Y
Definition: Vector.h:52
Implementation of a 3D vector (by Vitaliy).
Definition: Vector.h:45
Mdouble Z
Definition: Vector.h:52
void Quarternion::normalise ( )

Normalises the Quarternion.

Definition at line 99 of file Quarternion.cc.

References w_, x_, y_, and z_.

100 {
101  double n = std::sqrt(x_ * x_ + y_ * y_ + z_ * z_ + w_ * w_);
102  x_ /= n;
103  y_ /= n;
104  z_ /= n;
105  w_ /= n;
106 }
Mdouble z_
Definition: Quarternion.h:114
Mdouble y_
Definition: Quarternion.h:114
Mdouble x_
Definition: Quarternion.h:114
Mdouble w_
Definition: Quarternion.h:114
Quarternion Quarternion::operator* ( const Quarternion other) const

Multiplication operator.

Definition at line 108 of file Quarternion.cc.

References w_, x_, y_, and z_.

109  {
110  Quarternion result;
111  result.w_ = w_ * other.w_ - x_ * other.x_ - y_ * other.y_ - z_ * other.z_;
112  result.x_ = w_ * other.x_ + x_ * other.w_ + y_ * other.z_ - z_ * other.y_;
113  result.y_ = w_ * other.y_ + y_ * other.w_ - x_ * other.z_ + z_ * other.x_;
114  result.z_ = w_ * other.z_ + z_ * other.w_ + x_ * other.y_ - y_ * other.x_;
115  return result;
116 }
Mdouble z_
Definition: Quarternion.h:114
Mdouble y_
Definition: Quarternion.h:114
Mdouble x_
Definition: Quarternion.h:114
Mdouble w_
Definition: Quarternion.h:114
void Quarternion::reset ( )

Resets a the Quarternion.

Definition at line 91 of file Quarternion.cc.

References w_, x_, y_, and z_.

Referenced by Quarternion().

92 {
93  x_ = 0.0;
94  y_ = 0.0;
95  z_ = 0.0;
96  w_ = 1.0;
97 }
Mdouble z_
Definition: Quarternion.h:114
Mdouble y_
Definition: Quarternion.h:114
Mdouble x_
Definition: Quarternion.h:114
Mdouble w_
Definition: Quarternion.h:114

Friends And Related Function Documentation

std::ostream& operator<< ( std::ostream &  os,
const Quarternion q 
)
friend

Definition at line 173 of file Quarternion.cc.

174 {
175  os << q.x_ << " " << q.y_ << " " << q.z_ << " " << q.w_;
176  return os;
177 }
Mdouble z_
Definition: Quarternion.h:114
Mdouble y_
Definition: Quarternion.h:114
Mdouble x_
Definition: Quarternion.h:114
Mdouble w_
Definition: Quarternion.h:114
std::istream& operator>> ( std::istream &  is,
Quarternion q 
)
friend

Definition at line 179 of file Quarternion.cc.

180 {
181  is >> q.x_ >> q.y_ >> q.z_ >> q.w_;
182  return is;
183 }
Mdouble z_
Definition: Quarternion.h:114
Mdouble y_
Definition: Quarternion.h:114
Mdouble x_
Definition: Quarternion.h:114
Mdouble w_
Definition: Quarternion.h:114

Member Data Documentation


The documentation for this class was generated from the following files: