MercuryDPM  Beta
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Quarternion.cc
Go to the documentation of this file.
1 //Copyright (c) 2013-2014, The MercuryDPM Developers Team. All rights reserved.
2 //For the list of developers, see <http://www.MercuryDPM.org/Team>.
3 //
4 //Redistribution and use in source and binary forms, with or without
5 //modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name MercuryDPM nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 //ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 //WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 //DISCLAIMED. IN NO EVENT SHALL THE MERCURYDPM DEVELOPERS TEAM BE LIABLE FOR ANY
19 //DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 //(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 //LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 //ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 //(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 //SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 
26 #include "Quarternion.h"
27 
28 #include "Vector.h"
29 #include "Matrix.h"
30 #include "ExtendedMath.h"
31 
33 {
34  reset();
35 }
36 
38 {
39  x_ = other.x_;
40  y_ = other.y_;
41  z_ = other.z_;
42  w_ = other.w_;
43 }
44 
45 Quarternion::Quarternion(double x, double y, double z, double w)
46 {
47  x_ = x;
48  y_ = y;
49  z_ = z;
50  w_ = w;
51 }
52 
53 Quarternion::Quarternion(Vec3D axis, double angle)
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 }
62 
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 }
70 
71 void Quarternion::getAxisAndAngle(Vec3D& axis, double& angle) const
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 }
76 
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 }
90 
92 {
93  x_ = 0.0;
94  y_ = 0.0;
95  z_ = 0.0;
96  w_ = 1.0;
97 }
98 
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 }
107 
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 }
117 
118 void Quarternion::integrate(const Vec3D& omega, double timeStep)
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 }
142 
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 }
172 
173 std::ostream& operator<<(std::ostream& os, const Quarternion &q)
174 {
175  os << q.x_ << " " << q.y_ << " " << q.z_ << " " << q.w_;
176  return os;
177 }
178 
179 std::istream& operator>>(std::istream& is, Quarternion &q)
180 {
181  is >> q.x_ >> q.y_ >> q.z_ >> q.w_;
182  return is;
183 }
184 
Mdouble z_
Definition: Quarternion.h:114
void getAxisAndAngle(Vec3D &axis, double &angle) const
Returns the orientation as represented by a rotation around an axis.
Definition: Quarternion.cc:71
static Mdouble getLengthSquared(const Vec3D &a)
Calculates the squared length of a Vec3D: .
Definition: Vector.cc:304
Vec3D get3DAngles() const
Constructs a representation of the quarternion using 3 rotations in the lab x-y-z sequence...
Definition: Quarternion.cc:143
Mdouble X
the vector components
Definition: Vector.h:52
std::ostream & operator<<(std::ostream &os, const Quarternion &q)
Definition: Quarternion.cc:173
void normalize()
Makes this Vec3D unit length.
Definition: Vector.cc:234
void reset()
Resets a the Quarternion.
Definition: Quarternion.cc:91
Quarternion operator*(const Quarternion &other) const
Multiplication operator.
Definition: Quarternion.cc:108
Mdouble y_
Definition: Quarternion.h:114
const Mdouble pi
Definition: ExtendedMath.h:42
Mdouble x_
Definition: Quarternion.h:114
Quarternion()
Constructs a basic Quarternion.
Definition: Quarternion.cc:32
std::istream & operator>>(std::istream &is, Quarternion &q)
Definition: Quarternion.cc:179
Mdouble w_
Definition: Quarternion.h:114
Mdouble Y
Definition: Vector.h:52
Matrix3D getRotationMatrix() const
Definition: Quarternion.cc:77
Implementation of a 3D matrix.
Definition: Matrix.h:36
Implementation of a 3D vector (by Vitaliy).
Definition: Vector.h:45
void normalise()
Normalises the Quarternion.
Definition: Quarternion.cc:99
void integrate(const Vec3D &omega, double timeStep)
Integration of the Quarternion using rotational velocity and timestep.
Definition: Quarternion.cc:118
Mdouble Z
Definition: Vector.h:52