Subversion Repositories gelsvn

Rev

Rev 48 | Rev 51 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 48 Rev 49
1 
#ifndef __CGLA_QUATD_H__
 1 
#ifndef __CGLA_QUATD_H__
2 
#define __CGLA_QUATD_H__
 2 
#define __CGLA_QUATD_H__
3 
 
 3 
 
4 
#include "Vec3d.h"
 4 
#include "Vec3d.h"
5 
#include "Vec4d.h"
 5 
#include "Vec4d.h"
6 
#include "Mat3x3d.h"
 6 
#include "Mat3x3d.h"
7 
#include "Mat4x4d.h"
 7 
#include "Mat4x4d.h"
- 
 
 8 
#include <cmath>
8 
 
 9 
 
9 
namespace CGLA {
 10 
namespace CGLA {
10 
 
 11 
 
11 
  /** A Quaterinion class. Quaternions are algebraic entities
12 
  /** A Quaterinion class. Quaternions are algebraic entities
12 
      useful for rotation. */
 13 
      useful for rotation. */
13 
 
 14 
 
14 
  class Quatd
 15 
  class Quatd
15 
  {
 16 
  {
16 
  public:
 17 
  public:
17 
 
 18 
 
18 
    /// Vector part of quaternion
 19 
    /// Vector part of quaternion
19 
    Vec3d qv;
 20 
    Vec3d qv;
20 
 
 21 
 
21 
    /// Scalar part of quaternion
 22 
    /// Scalar part of quaternion
22 
    double qw;
 23 
    double qw;
23 
 
 24 
 
24 
    /// Construct undefined quaternion
 25 
    /// Construct undefined quaternion
25 
#ifndef NDEBUG
 26 
#ifndef NDEBUG
26 
    Quatd() : qw(CGLA_INIT_VALUE) {}
 27 
    Quatd() : qw(CGLA_INIT_VALUE) {}
27 
#else
 28 
#else
28 
    Quatd() {}
 29 
    Quatd() {}
29 
#endif
 30 
#endif
30 
 
 31 
 
31 
    /// Construct quaternion from vector and scalar
 32 
    /// Construct quaternion from vector and scalar
32 
    Quatd(const Vec3d& imaginary, double real = 1.0f) : qv(imaginary) , qw(real) {}
 33 
    Quatd(const Vec3d& imaginary, double real = 1.0f) : qv(imaginary) , qw(real) {}
33 
 
 34 
 
34 
    /// Construct quaternion from four scalars
 35 
    /// Construct quaternion from four scalars
35 
    Quatd(double x, double y, double z, double _qw) : qv(x,y,z), qw(_qw) {}
 36 
    Quatd(double x, double y, double z, double _qw) : qv(x,y,z), qw(_qw) {}
36 
 
 37 
 
37 
    /// Construct quaternion from a 4D vector
 38 
    /// Construct quaternion from a 4D vector
38 
    explicit Quatd(const Vec4d& v) : qv(v[0], v[1], v[2]), qw(v[3]) {}
 39 
    explicit Quatd(const Vec4d& v) : qv(v[0], v[1], v[2]), qw(v[3]) {}
39 
 
 40 
 
40 
    /// Assign values to a quaternion
 41 
    /// Assign values to a quaternion
41 
    void set(const Vec3d& imaginary, double real=1.0f)
 42 
    void set(const Vec3d& imaginary, double real=1.0f)
42 
    {
 43 
    {
43 
      qv = imaginary;
 44 
      qv = imaginary;
44 
      qw = real;
 45 
      qw = real;
45 
    }
 46 
    }
46 
 
 47 
 
47 
    void set(double x, double y, double z, double _qw)
48 
    void set(double x, double y, double z, double _qw)
48 
    {
 49 
    {
49 
      qv.set(x,y,z);
 50 
      qv.set(x,y,z);
50 
      qw = _qw;
 51 
      qw = _qw;
51 
    }
 52 
    }
52 
 
 53 
 
53 
    void set(const Vec4f& v)
 54 
    void set(const Vec4f& v)
54 
    {
 55 
    {
55 
      qv.set(v[0], v[1], v[2]);
 56 
      qv.set(v[0], v[1], v[2]);
56 
      qw = v[3];
57 
      qw = v[3];
57 
    }
 58 
    }
58 
 
 59 
 
59 
    /// Get values from a quaternion
 60 
    /// Get values from a quaternion
60 
    void get(double& x, double& y, double& z, double& _qw) const
 61 
    void get(double& x, double& y, double& z, double& _qw) const
61 
    {
 62 
    {
62 
      x  = qv[0];
 63 
      x  = qv[0];
63 
      y  = qv[1];
 64 
      y  = qv[1];
64 
      z  = qv[2];
 65 
      z  = qv[2];
65 
      _qw = qw;
 66 
      _qw = qw;
66 
    }
 67 
    }
67 
 
 68 
 
68 
    /// Get imaginary part of a quaternion
 69 
    /// Get imaginary part of a quaternion
69 
    Vec3d get_imaginary_part() const { return qv; }
 70 
    Vec3d get_imaginary_part() const { return qv; }
70 
 
 71 
 
71 
    /// Get real part of a quaternion
 72 
    /// Get real part of a quaternion
72 
    double get_real_part() const { return qw; }
 73 
    double get_real_part() const { return qw; }
73 
 
 74 
 
74 
    /// Get a 3x3 rotation matrix from a quaternion
 75 
    /// Get a 3x3 rotation matrix from a quaternion
75 
    Mat3x3d get_mat3x3d() const;
 76 
    Mat3x3d get_mat3x3d() const
- 
 
 77 
    {
- 
 
 78 
      double s = 2/norm();
- 
 
 79 
      // note that the all q_*q_ are used twice (optimize)
- 
 
 80 
      return Mat3x3d(Vec3d(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),
- 
 
 81 
			         s*(qv[0]*qv[1] - qw*qv[2]),
- 
 
 82 
			         s*(qv[0]*qv[2] + qw*qv[1])),
- 
 
 83 
		     Vec3d(      s*(qv[0]*qv[1] + qw*qv[2]),
- 
 
 84 
			   1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),
- 
 
 85 
			         s*(qv[1]*qv[2] - qw*qv[0])),
- 
 
 86 
		     Vec3d(      s*(qv[0]*qv[2] - qw*qv[1]),
- 
 
 87 
			         s*(qv[1]*qv[2] + qw*qv[0]),
- 
 
 88 
			   1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1])));
- 
 
 89 
    }
76 
 
 90 
 
77 
    /// Get a 4x4 rotation matrix from a quaternion
 91 
    /// Get a 4x4 rotation matrix from a quaternion
78 
    Mat4x4d get_mat4x4d() const;
 92 
    Mat4x4d get_mat4x4d() const
- 
 
 93 
    {
- 
 
 94 
      double s = 2/norm();
- 
 
 95 
      // note that the all q_*q_ are used twice (optimize?)
- 
 
 96 
      return Mat4x4d(Vec4d(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),
- 
 
 97 
			         s*(qv[0]*qv[1] - qw*qv[2]),
- 
 
 98 
			         s*(qv[0]*qv[2] + qw*qv[1]),
- 
 
 99 
			   0.0),
- 
 
 100 
		     Vec4d(      s*(qv[0]*qv[1] + qw*qv[2]),
- 
 
 101 
			   1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),
- 
 
 102 
			         s*(qv[1]*qv[2] - qw*qv[0]),
- 
 
 103 
			   0.0),
- 
 
 104 
		     Vec4d(      s*(qv[0]*qv[2] - qw*qv[1]),
- 
 
 105 
			         s*(qv[1]*qv[2] + qw*qv[0]),
- 
 
 106 
			   1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1]),
- 
 
 107 
			   0.0),
- 
 
 108 
		     Vec4d(0.0, 0.0, 0.0, 1.0));
- 
 
 109 
    }
79 
 
 110 
 
80 
    /// Obtain angle of rotation and axis
 111 
    /// Obtain angle of rotation and axis
81 
    void get_rot(double& angle, Vec3d& vec);
 112 
    void get_rot(double& angle, Vec3d& vec)
- 
 
 113 
    {
- 
 
 114 
      angle = 2*std::acos(qw);
- 
 
 115 
 
- 
 
 116 
      if(angle < TINY)
- 
 
 117 
	v = Vec3d(1.0, 0.0, 0.0);
- 
 
 118 
      else
- 
 
 119 
	v = qv*(1/std::sin(angle));
- 
 
 120 
 
- 
 
 121 
      if(angle > M_PI)
- 
 
 122 
	v = -v;
- 
 
 123 
 
- 
 
 124 
      v.normalize();
- 
 
 125 
    }
82 
 
 126 
 
83 
    /// Construct a Quaternion from an angle and axis of rotation.
 127 
    /// Construct a Quaternion from an angle and axis of rotation.
84 
    void make_rot(double angle, const Vec3d&);
 128 
    void make_rot(double angle, const Vec3d&)
- 
 
 129 
    {
- 
 
 130 
      angle /= 2.0;
- 
 
 131 
      qv = CGLA::normalize(v)*std::sin(angle);
- 
 
 132 
      qw = std::cos(angle);
- 
 
 133 
    }
85 
 
 134 
 
86 
    /** Construct a Quaternion rotating from the direction given
 135 
    /** Construct a Quaternion rotating from the direction given
87 
	by the first argument to the direction given by the second.*/
 136 
	by the first argument to the direction given by the second.*/
88 
    void make_rot(const Vec3d&,const Vec3d&);
 137 
    void make_rot(const Vec3d&,const Vec3d&)
- 
 
 138 
    {
- 
 
 139 
      double tmp = std::sqrt(2*(1 + dot(s, t)));
- 
 
 140 
      qv = cross(s, t)*(1.0/tmp);
- 
 
 141 
      qw = tmp/2.0;
- 
 
 142 
    }
89 
 
 143 
 
90 
    //----------------------------------------------------------------------
 144 
    //----------------------------------------------------------------------
91 
    // Binary operators
 145 
    // Binary operators
92 
    //----------------------------------------------------------------------
 146 
    //----------------------------------------------------------------------
93
147
94 
    bool operator==(const Quatd& q) const
 148 
    bool operator==(const Quatd& q) const
95 
    {
 149 
    {
96 
      return qw == q.qw && qv == q.qv;
 150 
      return qw == q.qw && qv == q.qv;
97 
    }
 151 
    }
98 
 
 152 
 
99 
    bool operator!=(const Quatd& q) const
 153 
    bool operator!=(const Quatd& q) const
100 
    {
 154 
    {
101 
      return qw != q.qw || qv != q.qv;
 155 
      return qw != q.qw || qv != q.qv;
102 
    }
 156 
    }
103 
 
 157 
 
104 
    /// Multiply two quaternions. (Combine their rotation)
 158 
    /// Multiply two quaternions. (Combine their rotation)
105 
    Quatd operator*(const Quatd& q) const
 159 
    Quatd operator*(const Quatd& q) const
106 
    {
 160 
    {
107 
      return Quatd(cross(qv, q.qv) + qv*q.qw + q.qv*qw,
161 
      return Quatd(cross(qv, q.qv) + qv*q.qw + q.qv*qw,
108 
		         qw*q.qw - dot(qv, q.qv));
162 
		         qw*q.qw - dot(qv, q.qv));
109 
    }
 163 
    }
110 
 
 164 
 
111 
    /// Multiply scalar onto quaternion.
 165 
    /// Multiply scalar onto quaternion.
112 
    Quatd operator*(double scalar) const
 166 
    Quatd operator*(double scalar) const
113 
    {
 167 
    {
114 
      return Quatd(qv*scalar, qw*scalar);
 168 
      return Quatd(qv*scalar, qw*scalar);
115 
    }
 169 
    }
116 
 
 170 
 
117 
    /// Add two quaternions.
 171 
    /// Add two quaternions.
118 
    Quatd operator+(const Quatd& q) const
 172 
    Quatd operator+(const Quatd& q) const
119 
    {
 173 
    {
120 
      return Quatd(qv + q.qv, qw + q.qw);
 174 
      return Quatd(qv + q.qv, qw + q.qw);
121 
    }
 175 
    }
122 
 
 176 
 
123 
    //----------------------------------------------------------------------
 177 
    //----------------------------------------------------------------------
124 
    // Unary operators
 178 
    // Unary operators
125 
    //----------------------------------------------------------------------
 179 
    //----------------------------------------------------------------------
126 
 
 180 
 
127 
    /// Compute the additive inverse of the quaternion
 181 
    /// Compute the additive inverse of the quaternion
128 
    Quatd operator-() const { return Quatd(-qv, -qw); }
 182 
    Quatd operator-() const { return Quatd(-qv, -qw); }
129 
 
 183 
 
130 
    /// Compute norm of quaternion
 184 
    /// Compute norm of quaternion
131 
    double norm() const { return dot(qv, qv) + qw*qw; }
 185 
    double norm() const { return dot(qv, qv) + qw*qw; }
132 
 
 186 
 
133 
    /// Return conjugate quaternion
 187 
    /// Return conjugate quaternion
134 
    Quatd conjugate() const { return Quatd(-qv, qw); }
 188 
    Quatd conjugate() const { return Quatd(-qv, qw); }
135 
 
 189 
 
136 
    /// Compute the multiplicative inverse of the quaternion
 190 
    /// Compute the multiplicative inverse of the quaternion
137 
    Quatd inverse() const { return Quatd(conjugate()*(1/norm())); }
 191 
    Quatd inverse() const { return Quatd(conjugate()*(1/norm())); }
138
192
139 
    /// Normalize quaternion.
 193 
    /// Normalize quaternion.
140 
    Quatd normalize() { return Quatd((*this)*(1/norm())); }
 194 
    Quatd normalize() { return Quatd((*this)*(1/norm())); }
141 
 
 195 
 
142 
    //----------------------------------------------------------------------
 196 
    //----------------------------------------------------------------------
143 
    // Application
 197 
    // Application
144 
    //----------------------------------------------------------------------
 198 
    //----------------------------------------------------------------------
145 
 
 199 
 
146 
    /// Rotate vector according to quaternion
 200 
    /// Rotate vector according to quaternion
147 
    Vec3d apply(const Vec3d& vec) const
201 
    Vec3d apply(const Vec3d& vec) const
148 
    {
 202 
    {
149 
      return ((*this)*Quatd(vec)*inverse()).qv;
 203 
      return ((*this)*Quatd(vec)*inverse()).qv;
150 
    }
 204 
    }
151 
 
 205 
 
152 
    /// Rotate vector according to unit quaternion
 206 
    /// Rotate vector according to unit quaternion
153 
    Vec3d apply_unit(const Vec3d& vec) const
 207 
    Vec3d apply_unit(const Vec3d& vec) const
154 
    {
 208 
    {
155 
      return ((*this)*Quatd(vec)*conjugate()).qv;
 209 
      return ((*this)*Quatd(vec)*conjugate()).qv;
156 
    }
 210 
    }
157 
  };
 211 
  };
158 
 
 212 
 
159 
  inline Quatd operator*(double scalar, const Quatd& q)
 213 
  inline Quatd operator*(double scalar, const Quatd& q)
160 
  {
 214 
  {
161 
    return q*scalar;
 215 
    return q*scalar;
162 
  }
 216 
  }
163 
 
 217 
 
164 
  /** Perform linear interpolation of two quaternions.
218 
  /** Perform linear interpolation of two quaternions.
165 
      The last argument is the parameter used to interpolate
 219 
      The last argument is the parameter used to interpolate
166 
      between the two first. SLERP - invented by Shoemake -
 220 
      between the two first. SLERP - invented by Shoemake -
167 
      is a good way to interpolate because the interpolation
 221 
      is a good way to interpolate because the interpolation
168 
      is performed on the unit sphere.
222 
      is performed on the unit sphere.
169 
  */
 223 
  */
170 
  inline Quatd slerp(Quatd q0, Quatd q1, double t);
 224 
  inline Quatd slerp(Quatd q0, Quatd q1, double t)
- 
 
 225 
  {
- 
 
 226 
    double angle = std::acos(q0.qv[0]*q1.qv[0] + q0.qv[1]*q1.qv[1]
- 
 
 227 
			     + q0.qv[2]*q1.qv[2] + q0.qw*q1.qw);
- 
 
 228 
    return (q0*std::sin((1 - t)*angle) + q1*std::sin(t*angle))*(1/std::sin(angle));
- 
 
 229 
  }
171 
 
 230 
 
172 
  /// Create an identity quaternion
 231 
  /// Create an identity quaternion
173 
  inline Quatd identity_Quatd()
 232 
  inline Quatd identity_Quatd()
174 
  {
 233 
  {
175 
    return Quatd(Vec3d(0.0));
 234 
    return Quatd(Vec3d(0.0));
176 
  }
 235 
  }
177 
 
 236 
 
178 
  /// Print quaternion to stream.
 237 
  /// Print quaternion to stream.
179 
  inline std::ostream& operator<<(std::ostream&os, const Quatd v)
 238 
  inline std::ostream& operator<<(std::ostream&os, const Quatd v)
180 
  {
 239 
  {
181 
    os << "[ ";
 240 
    os << "[ ";
182 
    for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";
 241 
    for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";
183 
    os << "~ " << v.qw << " ";
 242 
    os << "~ " << v.qw << " ";
184 
    os << "]";
 243 
    os << "]";
185 
 
 244 
 
186 
    return os;
 245 
    return os;
187 
  }
 246 
  }
188 
}
 247 
}
189 
#endif
 248 
#endif
190 
 
 249