quat.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2016-2017, the mcl_3dl authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef MCL_3DL_QUAT_H
31 #define MCL_3DL_QUAT_H
32 
33 #include <algorithm>
34 #include <cmath>
35 
36 #include <mcl_3dl/vec3.h>
37 
38 namespace mcl_3dl
39 {
40 class Quat
41 {
42 public:
43  float x_;
44  float y_;
45  float z_;
46  float w_;
47 
48  inline constexpr Quat(const float& x, const float& y, const float& z, const float& w)
49  : x_(x)
50  , y_(y)
51  , z_(z)
52  , w_(w)
53  {
54  }
55  inline Quat(const Vec3& axis, const float& ang)
56  {
57  setAxisAng(axis, ang);
58  }
59  inline Quat(const Vec3& forward, const Vec3& up_raw)
60  {
61  const Vec3 xv = forward.normalized();
62  const Vec3 yv = up_raw.cross(xv).normalized();
63  const Vec3 zv = xv.cross(yv).normalized();
64 
65  w_ = std::sqrt(std::max(0.0, 1.0 + xv.x_ + yv.y_ + zv.z_)) / 2.0;
66  x_ = std::sqrt(std::max(0.0, 1.0 + xv.x_ - yv.y_ - zv.z_)) / 2.0;
67  y_ = std::sqrt(std::max(0.0, 1.0 - xv.x_ + yv.y_ - zv.z_)) / 2.0;
68  z_ = std::sqrt(std::max(0.0, 1.0 - xv.x_ - yv.y_ + zv.z_)) / 2.0;
69  if (zv.y_ - yv.z_ > 0)
70  x_ = -x_;
71  if (xv.z_ - zv.x_ > 0)
72  y_ = -y_;
73  if (yv.x_ - xv.y_ > 0)
74  z_ = -z_;
75  }
76  inline explicit Quat(const Vec3& rpy)
77  {
78  setRPY(rpy);
79  }
80  inline constexpr Quat()
81  : x_(0)
82  , y_(0)
83  , z_(0)
84  , w_(1)
85  {
86  }
87  inline constexpr float dot(const Quat& q) const
88  {
89  return x_ * q.x_ + y_ * q.y_ + z_ * q.z_ + w_ * q.w_;
90  }
91  inline float norm() const
92  {
93  return std::sqrt(dot(*this));
94  }
95  inline constexpr bool operator==(const Quat& q) const
96  {
97  return x_ == q.x_ && y_ == q.y_ && z_ == q.z_ && w_ == q.w_;
98  }
99  inline constexpr bool operator!=(const Quat& q) const
100  {
101  return !operator==(q);
102  }
103  inline Quat operator+(const Quat& q) const
104  {
105  return Quat(x_ + q.x_, y_ + q.y_, z_ + q.z_, w_ + q.w_);
106  }
107  inline Quat operator+=(const Quat& q)
108  {
109  x_ += q.x_;
110  y_ += q.y_;
111  z_ += q.z_;
112  w_ += q.w_;
113  return *this;
114  }
115  inline Quat operator-(const Quat& q) const
116  {
117  return Quat(x_ - q.x_, y_ - q.y_, z_ - q.z_, w_ - q.w_);
118  }
119  inline Quat operator-=(const Quat& q)
120  {
121  x_ -= q.x_;
122  y_ -= q.y_;
123  z_ -= q.z_;
124  w_ -= q.w_;
125  return *this;
126  }
127  inline constexpr Quat operator-() const
128  {
129  return Quat(-x_, -y_, -z_, -w_);
130  }
131  inline constexpr Quat operator*(const Quat& q) const
132  {
133  return Quat(
134  w_ * q.x_ + x_ * q.w_ + y_ * q.z_ - z_ * q.y_,
135  w_ * q.y_ + y_ * q.w_ + z_ * q.x_ - x_ * q.z_,
136  w_ * q.z_ + z_ * q.w_ + x_ * q.y_ - y_ * q.x_,
137  w_ * q.w_ - x_ * q.x_ - y_ * q.y_ - z_ * q.z_);
138  }
139  inline Vec3 operator*(const Vec3& v) const
140  {
141  const Quat ret = *this * Quat(v.x_, v.y_, v.z_, 0.0) * conj();
142  return Vec3(ret.x_, ret.y_, ret.z_);
143  }
144  inline constexpr Quat operator*(const float& s) const
145  {
146  return Quat(x_ * s, y_ * s, z_ * s, w_ * s);
147  }
148  inline constexpr Quat operator/(const float& s) const
149  {
150  return operator*(1.0 / s);
151  }
152  inline Quat operator*=(const float& s)
153  {
154  x_ *= s;
155  y_ *= s;
156  z_ *= s;
157  w_ *= s;
158  return *this;
159  }
160  inline Quat operator/=(const float& s)
161  {
162  x_ /= s;
163  y_ /= s;
164  z_ /= s;
165  w_ /= s;
166  return *this;
167  }
168  inline Quat weighted(const float& s) const
169  {
170  Vec3 axis;
171  float ang;
172  getAxisAng(axis, ang);
173  return Quat(axis, ang * s);
174  }
175  inline Quat normalized() const
176  {
177  return (*this) / norm();
178  }
179  inline void normalize()
180  {
181  *this = normalized();
182  }
183  inline constexpr Quat conj() const
184  {
185  return Quat(-x_, -y_, -z_, w_);
186  }
187  inline constexpr Quat inv() const
188  {
189  return conj() / dot(*this);
190  }
191  inline constexpr Vec3 getRPY() const
192  {
193  const float ysq = y_ * y_;
194  const float t0 = -2.0 * (ysq + z_ * z_) + 1.0;
195  const float t1 = +2.0 * (x_ * y_ + w_ * z_);
196  const float t2 = std::max(-1.0, std::min(1.0, -2.0 * (x_ * z_ - w_ * y_)));
197  const float t3 = +2.0 * (y_ * z_ + w_ * x_);
198  const float t4 = -2.0 * (x_ * x_ + ysq) + 1.0;
199 
200  return Vec3(std::atan2(t3, t4), std::asin(t2), std::atan2(t1, t0));
201  }
202  inline void setRPY(const Vec3& rpy)
203  {
204  const float t2 = std::cos(rpy.x_ / 2);
205  const float t3 = std::sin(rpy.x_ / 2);
206  const float t4 = std::cos(rpy.y_ / 2);
207  const float t5 = std::sin(rpy.y_ / 2);
208  const float t0 = std::cos(rpy.z_ / 2);
209  const float t1 = std::sin(rpy.z_ / 2);
210 
211  x_ = t0 * t3 * t4 - t1 * t2 * t5;
212  y_ = t0 * t2 * t5 + t1 * t3 * t4;
213  z_ = t1 * t2 * t4 - t0 * t3 * t5;
214  w_ = t0 * t2 * t4 + t1 * t3 * t5;
215  }
216  inline void setAxisAng(const Vec3& axis, const float& ang)
217  {
218  const Vec3 a = axis / axis.norm();
219  const float s = std::sin(ang / 2);
220  x_ = a.x_ * s;
221  y_ = a.y_ * s;
222  z_ = a.z_ * s;
223  w_ = std::cos(ang / 2);
224  normalize();
225  }
226  inline void getAxisAng(Vec3& axis, float& ang) const
227  {
228  if (fabs(w_) >= 1.0 - 0.000001)
229  {
230  ang = 0.0;
231  axis = Vec3(0.0, 0.0, 1.0);
232  return;
233  }
234  ang = std::acos(w_) * 2.0;
235  if (ang > M_PI)
236  ang -= 2.0 * M_PI;
237  const float wsq = 1.0 - w_ * w_;
238  axis = Vec3(x_, y_, z_) / std::sqrt(wsq);
239  }
240  inline void rotateAxis(const Quat& r)
241  {
242  Vec3 axis;
243  float ang;
244  getAxisAng(axis, ang);
245  setAxisAng(r * axis, ang);
246  }
247 };
248 } // namespace mcl_3dl
249 
250 #endif // MCL_3DL_QUAT_H
float z_
Definition: vec3.h:42
void setAxisAng(const Vec3 &axis, const float &ang)
Definition: quat.h:216
Quat operator/=(const float &s)
Definition: quat.h:160
float y_
Definition: quat.h:44
XmlRpcServer s
constexpr Quat operator*(const Quat &q) const
Definition: quat.h:131
float norm() const
Definition: vec3.h:153
constexpr Quat conj() const
Definition: quat.h:183
Quat operator-=(const Quat &q)
Definition: quat.h:119
Quat operator+(const Quat &q) const
Definition: quat.h:103
constexpr Quat()
Definition: quat.h:80
constexpr Quat operator*(const float &s) const
Definition: quat.h:144
Quat(const Vec3 &rpy)
Definition: quat.h:76
Quat(const Vec3 &forward, const Vec3 &up_raw)
Definition: quat.h:59
constexpr Quat operator/(const float &s) const
Definition: quat.h:148
constexpr Vec3 getRPY() const
Definition: quat.h:191
void normalize()
Definition: quat.h:179
constexpr Quat(const float &x, const float &y, const float &z, const float &w)
Definition: quat.h:48
float w_
Definition: quat.h:46
float y_
Definition: vec3.h:41
constexpr Quat inv() const
Definition: quat.h:187
void getAxisAng(Vec3 &axis, float &ang) const
Definition: quat.h:226
Quat operator*=(const float &s)
Definition: quat.h:152
constexpr Quat operator-() const
Definition: quat.h:127
Quat normalized() const
Definition: quat.h:175
constexpr bool operator!=(const Quat &q) const
Definition: quat.h:99
constexpr bool operator==(const Quat &q) const
Definition: quat.h:95
float x_
Definition: vec3.h:40
Quat(const Vec3 &axis, const float &ang)
Definition: quat.h:55
Vec3 normalized() const
Definition: vec3.h:157
Vec3 operator*(const Vec3 &v) const
Definition: quat.h:139
void setRPY(const Vec3 &rpy)
Definition: quat.h:202
Quat operator+=(const Quat &q)
Definition: quat.h:107
constexpr Vec3 cross(const Vec3 &q) const
Definition: vec3.h:143
Quat weighted(const float &s) const
Definition: quat.h:168
Quat operator-(const Quat &q) const
Definition: quat.h:115
constexpr float dot(const Quat &q) const
Definition: quat.h:87
float norm() const
Definition: quat.h:91
float z_
Definition: quat.h:45
float x_
Definition: quat.h:43
void rotateAxis(const Quat &r)
Definition: quat.h:240


mcl_3dl
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:16:29