| 1 |
// |
| 2 |
// SPDX-License-Identifier: BSD-3-Clause |
| 3 |
// Copyright Contributors to the OpenEXR Project. |
| 4 |
// |
| 5 |
|
| 6 |
// |
| 7 |
// Euler angle representation of rotation/orientation |
| 8 |
// |
| 9 |
|
| 10 |
#ifndef INCLUDED_IMATHEULER_H |
| 11 |
#define INCLUDED_IMATHEULER_H |
| 12 |
|
| 13 |
#include "ImathExport.h" |
| 14 |
#include "ImathNamespace.h" |
| 15 |
|
| 16 |
#include "ImathMath.h" |
| 17 |
#include "ImathMatrix.h" |
| 18 |
#include "ImathQuat.h" |
| 19 |
#include "ImathVec.h" |
| 20 |
|
| 21 |
#include <iostream> |
| 22 |
|
| 23 |
IMATH_INTERNAL_NAMESPACE_HEADER_ENTER |
| 24 |
|
| 25 |
#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER |
| 26 |
// Disable MS VC++ warnings about conversion from double to float |
| 27 |
# pragma warning(push) |
| 28 |
# pragma warning(disable : 4244) |
| 29 |
#endif |
| 30 |
|
| 31 |
/// |
| 32 |
/// Template class `Euler<T>` |
| 33 |
/// |
| 34 |
/// The Euler class represents euler angle orientations. The class |
| 35 |
/// inherits from Vec3 to it can be freely cast. The additional |
| 36 |
/// information is the euler priorities rep. This class is |
| 37 |
/// essentially a rip off of Ken Shoemake's GemsIV code. It has |
| 38 |
/// been modified minimally to make it more understandable, but |
| 39 |
/// hardly enough to make it easy to grok completely. |
| 40 |
/// |
| 41 |
/// There are 24 possible combonations of Euler angle |
| 42 |
/// representations of which 12 are common in CG and you will |
| 43 |
/// probably only use 6 of these which in this scheme are the |
| 44 |
/// non-relative-non-repeating types. |
| 45 |
/// |
| 46 |
/// The representations can be partitioned according to two |
| 47 |
/// criteria: |
| 48 |
/// |
| 49 |
/// 1) Are the angles measured relative to a set of fixed axis |
| 50 |
/// or relative to each other (the latter being what happens |
| 51 |
/// when rotation matrices are multiplied together and is |
| 52 |
/// almost ubiquitous in the cg community) |
| 53 |
/// |
| 54 |
/// 2) Is one of the rotations repeated (ala XYX rotation) |
| 55 |
/// |
| 56 |
/// When you construct a given representation from scratch you |
| 57 |
/// must order the angles according to their priorities. So, the |
| 58 |
/// easiest is a softimage or aerospace (yaw/pitch/roll) ordering |
| 59 |
/// of ZYX. |
| 60 |
/// |
| 61 |
/// float x_rot = 1; |
| 62 |
/// float y_rot = 2; |
| 63 |
/// float z_rot = 3; |
| 64 |
/// |
| 65 |
/// Eulerf angles(z_rot, y_rot, x_rot, Eulerf::ZYX); |
| 66 |
/// |
| 67 |
/// or: |
| 68 |
/// |
| 69 |
/// Eulerf angles( V3f(z_rot,y_rot,z_rot), Eulerf::ZYX ); |
| 70 |
/// |
| 71 |
/// |
| 72 |
/// If instead, the order was YXZ for instance you would have to |
| 73 |
/// do this: |
| 74 |
/// |
| 75 |
/// float x_rot = 1; |
| 76 |
/// float y_rot = 2; |
| 77 |
/// float z_rot = 3; |
| 78 |
/// |
| 79 |
/// Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ); |
| 80 |
/// |
| 81 |
/// or: |
| 82 |
/// |
| 83 |
/// |
| 84 |
/// Eulerf angles( V3f(y_rot,x_rot,z_rot), Eulerf::YXZ ); |
| 85 |
/// |
| 86 |
/// Notice how the order you put the angles into the three slots |
| 87 |
/// should correspond to the enum (YXZ) ordering. The input angle |
| 88 |
/// vector is called the "ijk" vector -- not an "xyz" vector. The |
| 89 |
/// ijk vector order is the same as the enum. If you treat the |
| 90 |
/// Euler as a Vec3 (which it inherts from) you will find the |
| 91 |
/// angles are ordered in the same way, i.e.: |
| 92 |
/// |
| 93 |
/// V3f v = angles; |
| 94 |
/// v.x == y_rot, v.y == x_rot, v.z == z_rot |
| 95 |
/// |
| 96 |
/// If you just want the x, y, and z angles stored in a vector in |
| 97 |
/// that order, you can do this: |
| 98 |
/// |
| 99 |
/// V3f v = angles.toXYZVector() |
| 100 |
/// v.x == x_rot, v.y == y_rot, v.z == z_rot |
| 101 |
/// |
| 102 |
/// If you want to set the Euler with an XYZVector use the |
| 103 |
/// optional layout argument: |
| 104 |
/// |
| 105 |
/// Eulerf angles(x_rot, y_rot, z_rot, Eulerf::YXZ, Eulerf::XYZLayout); |
| 106 |
/// |
| 107 |
/// This is the same as: |
| 108 |
/// |
| 109 |
/// Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ); |
| 110 |
/// |
| 111 |
/// Note that this won't do anything intelligent if you have a |
| 112 |
/// repeated axis in the euler angles (e.g. XYX) |
| 113 |
/// |
| 114 |
/// If you need to use the "relative" versions of these, you will |
| 115 |
/// need to use the "r" enums. |
| 116 |
/// |
| 117 |
/// The units of the rotation angles are assumed to be radians. |
| 118 |
/// |
| 119 |
|
| 120 |
template <class T> class IMATH_EXPORT_TEMPLATE_TYPE Euler : public Vec3<T> |
| 121 |
{ |
| 122 |
public: |
| 123 |
using Vec3<T>::x; |
| 124 |
using Vec3<T>::y; |
| 125 |
using Vec3<T>::z; |
| 126 |
|
| 127 |
|
| 128 |
/// |
| 129 |
/// All 24 possible orderings |
| 130 |
/// |
| 131 |
enum IMATH_EXPORT_ENUM Order |
| 132 |
{ |
| 133 |
XYZ = 0x0101, // "usual" orderings |
| 134 |
XZY = 0x0001, |
| 135 |
YZX = 0x1101, |
| 136 |
YXZ = 0x1001, |
| 137 |
ZXY = 0x2101, |
| 138 |
ZYX = 0x2001, |
| 139 |
|
| 140 |
XZX = 0x0011, // first axis repeated |
| 141 |
XYX = 0x0111, |
| 142 |
YXY = 0x1011, |
| 143 |
YZY = 0x1111, |
| 144 |
ZYZ = 0x2011, |
| 145 |
ZXZ = 0x2111, |
| 146 |
|
| 147 |
XYZr = 0x2000, // relative orderings -- not common |
| 148 |
XZYr = 0x2100, |
| 149 |
YZXr = 0x1000, |
| 150 |
YXZr = 0x1100, |
| 151 |
ZXYr = 0x0000, |
| 152 |
ZYXr = 0x0100, |
| 153 |
|
| 154 |
XZXr = 0x2110, // relative first axis repeated |
| 155 |
XYXr = 0x2010, |
| 156 |
YXYr = 0x1110, |
| 157 |
YZYr = 0x1010, |
| 158 |
ZYZr = 0x0110, |
| 159 |
ZXZr = 0x0010, |
| 160 |
// |||| |
| 161 |
// VVVV |
| 162 |
// ABCD |
| 163 |
// Legend: |
| 164 |
// A -> Initial Axis (0==x, 1==y, 2==z) |
| 165 |
// B -> Parity Even (1==true) |
| 166 |
// C -> Initial Repeated (1==true) |
| 167 |
// D -> Frame Static (1==true) |
| 168 |
// |
| 169 |
|
| 170 |
Legal = XYZ | XZY | YZX | YXZ | ZXY | ZYX | XZX | XYX | YXY | YZY | ZYZ | ZXZ | XYZr | |
| 171 |
XZYr | YZXr | YXZr | ZXYr | ZYXr | XZXr | XYXr | YXYr | YZYr | ZYZr | ZXZr, |
| 172 |
|
| 173 |
Min = 0x0000, |
| 174 |
Max = 0x2111, |
| 175 |
Default = XYZ |
| 176 |
}; |
| 177 |
|
| 178 |
/// |
| 179 |
/// Axes |
| 180 |
/// |
| 181 |
enum IMATH_EXPORT_ENUM Axis |
| 182 |
{ |
| 183 |
X = 0, |
| 184 |
Y = 1, |
| 185 |
Z = 2 |
| 186 |
}; |
| 187 |
|
| 188 |
/// |
| 189 |
/// Layout |
| 190 |
/// |
| 191 |
|
| 192 |
enum IMATH_EXPORT_ENUM InputLayout |
| 193 |
{ |
| 194 |
XYZLayout, |
| 195 |
IJKLayout |
| 196 |
}; |
| 197 |
|
| 198 |
/// @{ |
| 199 |
/// @name Constructors |
| 200 |
/// |
| 201 |
/// All default to `ZYX` non-relative (ala Softimage 3D/Maya), |
| 202 |
/// where there is no argument to specify it. |
| 203 |
/// |
| 204 |
/// The Euler-from-matrix constructors assume that the matrix does |
| 205 |
/// not include shear or non-uniform scaling, but the constructors |
| 206 |
/// do not examine the matrix to verify this assumption. If necessary, |
| 207 |
/// you can adjust the matrix by calling the removeScalingAndShear() |
| 208 |
/// function, defined in ImathMatrixAlgo.h. |
| 209 |
|
| 210 |
/// No initialization by default |
| 211 |
IMATH_HOSTDEVICE constexpr Euler() IMATH_NOEXCEPT; |
| 212 |
|
| 213 |
/// Copy constructor |
| 214 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 Euler (const Euler&) IMATH_NOEXCEPT; |
| 215 |
|
| 216 |
/// Construct from given Order |
| 217 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 Euler (Order p) IMATH_NOEXCEPT; |
| 218 |
|
| 219 |
/// Construct from vector, order, layout |
| 220 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 Euler (const Vec3<T>& v, |
| 221 |
Order o = Default, |
| 222 |
InputLayout l = IJKLayout) IMATH_NOEXCEPT; |
| 223 |
/// Construct from explicit axes, order, layout |
| 224 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 |
| 225 |
Euler (T i, T j, T k, Order o = Default, InputLayout l = IJKLayout) IMATH_NOEXCEPT; |
| 226 |
|
| 227 |
/// Copy constructor with new Order |
| 228 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 Euler (const Euler<T>& euler, Order newp) IMATH_NOEXCEPT; |
| 229 |
|
| 230 |
/// Construct from Matrix33 |
| 231 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 Euler (const Matrix33<T>&, Order o = Default) IMATH_NOEXCEPT; |
| 232 |
|
| 233 |
/// Construct from Matrix44 |
| 234 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 Euler (const Matrix44<T>&, Order o = Default) IMATH_NOEXCEPT; |
| 235 |
|
| 236 |
/// Destructor |
| 237 |
~Euler () = default; |
| 238 |
|
| 239 |
/// @} |
| 240 |
|
| 241 |
/// @{ |
| 242 |
/// @name Query |
| 243 |
|
| 244 |
/// Return whether the given value is a legal Order |
| 245 |
IMATH_HOSTDEVICE constexpr static bool legal (Order) IMATH_NOEXCEPT; |
| 246 |
|
| 247 |
/// Return the order |
| 248 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 Order order() const IMATH_NOEXCEPT; |
| 249 |
|
| 250 |
/// Return frameStatic |
| 251 |
IMATH_HOSTDEVICE constexpr bool frameStatic() const { return _frameStatic; } |
| 252 |
|
| 253 |
/// Return intialRepeated |
| 254 |
IMATH_HOSTDEVICE constexpr bool initialRepeated() const { return _initialRepeated; } |
| 255 |
|
| 256 |
/// Return partityEven |
| 257 |
IMATH_HOSTDEVICE constexpr bool parityEven() const { return _parityEven; } |
| 258 |
|
| 259 |
/// Return initialAxis |
| 260 |
IMATH_HOSTDEVICE constexpr Axis initialAxis() const { return _initialAxis; } |
| 261 |
|
| 262 |
/// Unpack angles from ijk form |
| 263 |
IMATH_HOSTDEVICE void angleOrder (int& i, int& j, int& k) const IMATH_NOEXCEPT; |
| 264 |
|
| 265 |
/// Determine mapping from xyz to ijk (reshuffle the xyz to match the order) |
| 266 |
IMATH_HOSTDEVICE void angleMapping (int& i, int& j, int& k) const IMATH_NOEXCEPT; |
| 267 |
|
| 268 |
/// @} |
| 269 |
|
| 270 |
/// @{ |
| 271 |
/// @name Set Value |
| 272 |
|
| 273 |
/// Set the order. This does NOT convert the angles, but it |
| 274 |
/// does reorder the input vector. |
| 275 |
IMATH_HOSTDEVICE void setOrder (Order) IMATH_NOEXCEPT; |
| 276 |
|
| 277 |
/// Set the euler value: set the first angle to `v[0]`, the second to |
| 278 |
/// `v[1]`, the third to `v[2]`. |
| 279 |
IMATH_HOSTDEVICE void setXYZVector (const Vec3<T>&) IMATH_NOEXCEPT; |
| 280 |
|
| 281 |
/// Set the value. |
| 282 |
IMATH_HOSTDEVICE void set (Axis initial, bool relative, bool parityEven, bool firstRepeats) IMATH_NOEXCEPT; |
| 283 |
|
| 284 |
/// @} |
| 285 |
|
| 286 |
/// @{ |
| 287 |
/// @name Assignments and Conversions |
| 288 |
/// |
| 289 |
|
| 290 |
/// Assignment |
| 291 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 const Euler<T>& operator= (const Euler<T>&) IMATH_NOEXCEPT; |
| 292 |
|
| 293 |
/// Assignment |
| 294 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 const Euler<T>& operator= (const Vec3<T>&) IMATH_NOEXCEPT; |
| 295 |
|
| 296 |
/// Assign from Matrix33, assumed to be affine |
| 297 |
IMATH_HOSTDEVICE void extract (const Matrix33<T>&) IMATH_NOEXCEPT; |
| 298 |
|
| 299 |
/// Assign from Matrix44, assumed to be affine |
| 300 |
IMATH_HOSTDEVICE void extract (const Matrix44<T>&) IMATH_NOEXCEPT; |
| 301 |
|
| 302 |
/// Assign from Quaternion |
| 303 |
IMATH_HOSTDEVICE void extract (const Quat<T>&) IMATH_NOEXCEPT; |
| 304 |
|
| 305 |
/// Convert to Matrix33 |
| 306 |
IMATH_HOSTDEVICE Matrix33<T> toMatrix33() const IMATH_NOEXCEPT; |
| 307 |
|
| 308 |
/// Convert to Matrix44 |
| 309 |
IMATH_HOSTDEVICE Matrix44<T> toMatrix44() const IMATH_NOEXCEPT; |
| 310 |
|
| 311 |
/// Convert to Quat |
| 312 |
IMATH_HOSTDEVICE Quat<T> toQuat() const IMATH_NOEXCEPT; |
| 313 |
|
| 314 |
/// Reorder the angles so that the X rotation comes first, |
| 315 |
/// followed by the Y and Z in cases like XYX ordering, the |
| 316 |
/// repeated angle will be in the "z" component |
| 317 |
IMATH_HOSTDEVICE Vec3<T> toXYZVector() const IMATH_NOEXCEPT; |
| 318 |
|
| 319 |
/// @} |
| 320 |
|
| 321 |
/// @{ |
| 322 |
/// @name Utility Methods |
| 323 |
/// |
| 324 |
/// Utility methods for getting continuous rotations. None of these |
| 325 |
/// methods change the orientation given by its inputs (or at least |
| 326 |
/// that is the intent). |
| 327 |
|
| 328 |
/// Convert an angle to its equivalent in [-PI, PI] |
| 329 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 static float angleMod (T angle) IMATH_NOEXCEPT; |
| 330 |
|
| 331 |
/// Adjust xyzRot so that its components differ from targetXyzRot by no more than +/-PI |
| 332 |
IMATH_HOSTDEVICE static void simpleXYZRotation (Vec3<T>& xyzRot, const Vec3<T>& targetXyzRot) IMATH_NOEXCEPT; |
| 333 |
|
| 334 |
/// Adjust xyzRot so that its components differ from targetXyzRot by as little as possible. |
| 335 |
/// Note that xyz here really means ijk, because the order must be provided. |
| 336 |
IMATH_HOSTDEVICE static void |
| 337 |
nearestRotation (Vec3<T>& xyzRot, const Vec3<T>& targetXyzRot, Order order = XYZ) IMATH_NOEXCEPT; |
| 338 |
|
| 339 |
/// Adjusts "this" Euler so that its components differ from target |
| 340 |
/// by as little as possible. This method might not make sense for |
| 341 |
/// Eulers with different order and it probably doesn't work for |
| 342 |
/// repeated axis and relative orderings (TODO). |
| 343 |
IMATH_HOSTDEVICE void makeNear (const Euler<T>& target) IMATH_NOEXCEPT; |
| 344 |
|
| 345 |
/// @} |
| 346 |
|
| 347 |
protected: |
| 348 |
|
| 349 |
/// relative or static rotations |
| 350 |
bool _frameStatic : 1; |
| 351 |
|
| 352 |
/// init axis repeated as last |
| 353 |
bool _initialRepeated : 1; |
| 354 |
|
| 355 |
/// "parity of axis permutation" |
| 356 |
bool _parityEven : 1; |
| 357 |
|
| 358 |
#if defined _WIN32 || defined _WIN64 |
| 359 |
/// First axis of rotation |
| 360 |
Axis _initialAxis; |
| 361 |
#else |
| 362 |
/// First axis of rotation |
| 363 |
Axis _initialAxis : 2; |
| 364 |
#endif |
| 365 |
}; |
| 366 |
|
| 367 |
// |
| 368 |
// Convenient typedefs |
| 369 |
// |
| 370 |
|
| 371 |
/// Euler of type float |
| 372 |
typedef Euler<float> Eulerf; |
| 373 |
/// Euler of type double |
| 374 |
typedef Euler<double> Eulerd; |
| 375 |
|
| 376 |
// |
| 377 |
// Implementation |
| 378 |
// |
| 379 |
|
| 380 |
/// @cond Doxygen_Suppress |
| 381 |
|
| 382 |
template <class T> |
| 383 |
IMATH_HOSTDEVICE inline void |
| 384 |
Euler<T>::angleOrder (int& i, int& j, int& k) const IMATH_NOEXCEPT |
| 385 |
{ |
| 386 |
i = _initialAxis; |
| 387 |
j = _parityEven ? (i + 1) % 3 : (i > 0 ? i - 1 : 2); |
| 388 |
k = _parityEven ? (i > 0 ? i - 1 : 2) : (i + 1) % 3; |
| 389 |
} |
| 390 |
|
| 391 |
template <class T> |
| 392 |
IMATH_HOSTDEVICE inline void |
| 393 |
Euler<T>::angleMapping (int& i, int& j, int& k) const IMATH_NOEXCEPT |
| 394 |
{ |
| 395 |
int m[3]; |
| 396 |
|
| 397 |
m[_initialAxis] = 0; |
| 398 |
m[(_initialAxis + 1) % 3] = _parityEven ? 1 : 2; |
| 399 |
m[(_initialAxis + 2) % 3] = _parityEven ? 2 : 1; |
| 400 |
i = m[0]; |
| 401 |
j = m[1]; |
| 402 |
k = m[2]; |
| 403 |
} |
| 404 |
|
| 405 |
template <class T> |
| 406 |
IMATH_HOSTDEVICE inline void |
| 407 |
Euler<T>::setXYZVector (const Vec3<T>& v) IMATH_NOEXCEPT |
| 408 |
{ |
| 409 |
int i, j, k; |
| 410 |
angleMapping (i, j, k); |
| 411 |
(*this)[i] = v.x; |
| 412 |
(*this)[j] = v.y; |
| 413 |
(*this)[k] = v.z; |
| 414 |
} |
| 415 |
|
| 416 |
template <class T> |
| 417 |
IMATH_HOSTDEVICE inline Vec3<T> |
| 418 |
Euler<T>::toXYZVector() const IMATH_NOEXCEPT |
| 419 |
{ |
| 420 |
int i, j, k; |
| 421 |
angleMapping (i, j, k); |
| 422 |
return Vec3<T> ((*this)[i], (*this)[j], (*this)[k]); |
| 423 |
} |
| 424 |
|
| 425 |
template <class T> |
| 426 |
IMATH_HOSTDEVICE constexpr inline Euler<T>::Euler() IMATH_NOEXCEPT |
| 427 |
: Vec3<T> (0, 0, 0), |
| 428 |
_frameStatic (true), |
| 429 |
_initialRepeated (false), |
| 430 |
_parityEven (true), |
| 431 |
_initialAxis (X) |
| 432 |
{} |
| 433 |
|
| 434 |
template <class T> |
| 435 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 inline Euler<T>::Euler (typename Euler<T>::Order p) IMATH_NOEXCEPT |
| 436 |
: Vec3<T> (0, 0, 0), |
| 437 |
_frameStatic (true), |
| 438 |
_initialRepeated (false), |
| 439 |
_parityEven (true), |
| 440 |
_initialAxis (X) |
| 441 |
{ |
| 442 |
setOrder (p); |
| 443 |
} |
| 444 |
|
| 445 |
template <class T> |
| 446 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 inline Euler<T>::Euler (const Vec3<T>& v, |
| 447 |
typename Euler<T>::Order p, |
| 448 |
typename Euler<T>::InputLayout l) IMATH_NOEXCEPT |
| 449 |
{ |
| 450 |
setOrder (p); |
| 451 |
if (l == XYZLayout) |
| 452 |
setXYZVector (v); |
| 453 |
else |
| 454 |
{ |
| 455 |
x = v.x; |
| 456 |
y = v.y; |
| 457 |
z = v.z; |
| 458 |
} |
| 459 |
} |
| 460 |
|
| 461 |
template <class T> |
| 462 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 inline Euler<T>::Euler (const Euler<T>& euler) IMATH_NOEXCEPT |
| 463 |
{ |
| 464 |
operator= (euler); |
| 465 |
} |
| 466 |
|
| 467 |
template <class T> |
| 468 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 inline Euler<T>::Euler (const Euler<T>& euler, Order p) IMATH_NOEXCEPT |
| 469 |
{ |
| 470 |
setOrder (p); |
| 471 |
Matrix33<T> M = euler.toMatrix33(); |
| 472 |
extract (M); |
| 473 |
} |
| 474 |
|
| 475 |
template <class T> |
| 476 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 inline |
| 477 |
Euler<T>::Euler (T xi, T yi, T zi, typename Euler<T>::Order p, |
| 478 |
typename Euler<T>::InputLayout l) IMATH_NOEXCEPT |
| 479 |
{ |
| 480 |
setOrder (p); |
| 481 |
if (l == XYZLayout) |
| 482 |
setXYZVector (Vec3<T> (xi, yi, zi)); |
| 483 |
else |
| 484 |
{ |
| 485 |
x = xi; |
| 486 |
y = yi; |
| 487 |
z = zi; |
| 488 |
} |
| 489 |
} |
| 490 |
|
| 491 |
template <class T> |
| 492 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 inline Euler<T>::Euler (const Matrix33<T>& M, typename Euler::Order p) IMATH_NOEXCEPT |
| 493 |
{ |
| 494 |
setOrder (p); |
| 495 |
extract (M); |
| 496 |
} |
| 497 |
|
| 498 |
template <class T> |
| 499 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 inline Euler<T>::Euler (const Matrix44<T>& M, typename Euler::Order p) IMATH_NOEXCEPT |
| 500 |
{ |
| 501 |
setOrder (p); |
| 502 |
extract (M); |
| 503 |
} |
| 504 |
|
| 505 |
template <class T> |
| 506 |
IMATH_HOSTDEVICE inline void |
| 507 |
Euler<T>::extract (const Quat<T>& q) IMATH_NOEXCEPT |
| 508 |
{ |
| 509 |
extract (q.toMatrix33()); |
| 510 |
} |
| 511 |
|
| 512 |
template <class T> |
| 513 |
IMATH_HOSTDEVICE void |
| 514 |
Euler<T>::extract (const Matrix33<T>& M) IMATH_NOEXCEPT |
| 515 |
{ |
| 516 |
int i, j, k; |
| 517 |
angleOrder (i, j, k); |
| 518 |
|
| 519 |
if (_initialRepeated) |
| 520 |
{ |
| 521 |
// |
| 522 |
// Extract the first angle, x. |
| 523 |
// |
| 524 |
|
| 525 |
x = std::atan2 (M[j][i], M[k][i]); |
| 526 |
|
| 527 |
// |
| 528 |
// Remove the x rotation from M, so that the remaining |
| 529 |
// rotation, N, is only around two axes, and gimbal lock |
| 530 |
// cannot occur. |
| 531 |
// |
| 532 |
|
| 533 |
Vec3<T> r (0, 0, 0); |
| 534 |
r[i] = (_parityEven ? -x : x); |
| 535 |
|
| 536 |
Matrix44<T> N; |
| 537 |
N.rotate (r); |
| 538 |
|
| 539 |
N = N * Matrix44<T> (M[0][0], |
| 540 |
M[0][1], |
| 541 |
M[0][2], |
| 542 |
0, |
| 543 |
M[1][0], |
| 544 |
M[1][1], |
| 545 |
M[1][2], |
| 546 |
0, |
| 547 |
M[2][0], |
| 548 |
M[2][1], |
| 549 |
M[2][2], |
| 550 |
0, |
| 551 |
0, |
| 552 |
0, |
| 553 |
0, |
| 554 |
1); |
| 555 |
// |
| 556 |
// Extract the other two angles, y and z, from N. |
| 557 |
// |
| 558 |
|
| 559 |
T sy = std::sqrt (N[j][i] * N[j][i] + N[k][i] * N[k][i]); |
| 560 |
y = std::atan2 (sy, N[i][i]); |
| 561 |
z = std::atan2 (N[j][k], N[j][j]); |
| 562 |
} |
| 563 |
else |
| 564 |
{ |
| 565 |
// |
| 566 |
// Extract the first angle, x. |
| 567 |
// |
| 568 |
|
| 569 |
x = std::atan2 (M[j][k], M[k][k]); |
| 570 |
|
| 571 |
// |
| 572 |
// Remove the x rotation from M, so that the remaining |
| 573 |
// rotation, N, is only around two axes, and gimbal lock |
| 574 |
// cannot occur. |
| 575 |
// |
| 576 |
|
| 577 |
Vec3<T> r (0, 0, 0); |
| 578 |
r[i] = (_parityEven ? -x : x); |
| 579 |
|
| 580 |
Matrix44<T> N; |
| 581 |
N.rotate (r); |
| 582 |
|
| 583 |
N = N * Matrix44<T> (M[0][0], |
| 584 |
M[0][1], |
| 585 |
M[0][2], |
| 586 |
0, |
| 587 |
M[1][0], |
| 588 |
M[1][1], |
| 589 |
M[1][2], |
| 590 |
0, |
| 591 |
M[2][0], |
| 592 |
M[2][1], |
| 593 |
M[2][2], |
| 594 |
0, |
| 595 |
0, |
| 596 |
0, |
| 597 |
0, |
| 598 |
1); |
| 599 |
// |
| 600 |
// Extract the other two angles, y and z, from N. |
| 601 |
// |
| 602 |
|
| 603 |
T cy = std::sqrt (N[i][i] * N[i][i] + N[i][j] * N[i][j]); |
| 604 |
y = std::atan2 (-N[i][k], cy); |
| 605 |
z = std::atan2 (-N[j][i], N[j][j]); |
| 606 |
} |
| 607 |
|
| 608 |
if (!_parityEven) |
| 609 |
*this *= -1; |
| 610 |
|
| 611 |
if (!_frameStatic) |
| 612 |
{ |
| 613 |
T t = x; |
| 614 |
x = z; |
| 615 |
z = t; |
| 616 |
} |
| 617 |
} |
| 618 |
|
| 619 |
template <class T> |
| 620 |
IMATH_HOSTDEVICE void |
| 621 |
Euler<T>::extract (const Matrix44<T>& M) IMATH_NOEXCEPT |
| 622 |
{ |
| 623 |
int i, j, k; |
| 624 |
angleOrder (i, j, k); |
| 625 |
|
| 626 |
if (_initialRepeated) |
| 627 |
{ |
| 628 |
// |
| 629 |
// Extract the first angle, x. |
| 630 |
// |
| 631 |
|
| 632 |
x = std::atan2 (M[j][i], M[k][i]); |
| 633 |
|
| 634 |
// |
| 635 |
// Remove the x rotation from M, so that the remaining |
| 636 |
// rotation, N, is only around two axes, and gimbal lock |
| 637 |
// cannot occur. |
| 638 |
// |
| 639 |
|
| 640 |
Vec3<T> r (0, 0, 0); |
| 641 |
r[i] = (_parityEven ? -x : x); |
| 642 |
|
| 643 |
Matrix44<T> N; |
| 644 |
N.rotate (r); |
| 645 |
N = N * M; |
| 646 |
|
| 647 |
// |
| 648 |
// Extract the other two angles, y and z, from N. |
| 649 |
// |
| 650 |
|
| 651 |
T sy = std::sqrt (N[j][i] * N[j][i] + N[k][i] * N[k][i]); |
| 652 |
y = std::atan2 (sy, N[i][i]); |
| 653 |
z = std::atan2 (N[j][k], N[j][j]); |
| 654 |
} |
| 655 |
else |
| 656 |
{ |
| 657 |
// |
| 658 |
// Extract the first angle, x. |
| 659 |
// |
| 660 |
|
| 661 |
x = std::atan2 (M[j][k], M[k][k]); |
| 662 |
|
| 663 |
// |
| 664 |
// Remove the x rotation from M, so that the remaining |
| 665 |
// rotation, N, is only around two axes, and gimbal lock |
| 666 |
// cannot occur. |
| 667 |
// |
| 668 |
|
| 669 |
Vec3<T> r (0, 0, 0); |
| 670 |
r[i] = (_parityEven ? -x : x); |
| 671 |
|
| 672 |
Matrix44<T> N; |
| 673 |
N.rotate (r); |
| 674 |
N = N * M; |
| 675 |
|
| 676 |
// |
| 677 |
// Extract the other two angles, y and z, from N. |
| 678 |
// |
| 679 |
|
| 680 |
T cy = std::sqrt (N[i][i] * N[i][i] + N[i][j] * N[i][j]); |
| 681 |
y = std::atan2 (-N[i][k], cy); |
| 682 |
z = std::atan2 (-N[j][i], N[j][j]); |
| 683 |
} |
| 684 |
|
| 685 |
if (!_parityEven) |
| 686 |
*this *= -1; |
| 687 |
|
| 688 |
if (!_frameStatic) |
| 689 |
{ |
| 690 |
T t = x; |
| 691 |
x = z; |
| 692 |
z = t; |
| 693 |
} |
| 694 |
} |
| 695 |
|
| 696 |
template <class T> |
| 697 |
IMATH_HOSTDEVICE Matrix33<T> |
| 698 |
Euler<T>::toMatrix33() const IMATH_NOEXCEPT |
| 699 |
{ |
| 700 |
int i, j, k; |
| 701 |
angleOrder (i, j, k); |
| 702 |
|
| 703 |
Vec3<T> angles; |
| 704 |
|
| 705 |
if (_frameStatic) |
| 706 |
angles = (*this); |
| 707 |
else |
| 708 |
angles = Vec3<T> (z, y, x); |
| 709 |
|
| 710 |
if (!_parityEven) |
| 711 |
angles *= -1.0; |
| 712 |
|
| 713 |
T ci = std::cos (angles.x); |
| 714 |
T cj = std::cos (angles.y); |
| 715 |
T ch = std::cos (angles.z); |
| 716 |
T si = std::sin (angles.x); |
| 717 |
T sj = std::sin (angles.y); |
| 718 |
T sh = std::sin (angles.z); |
| 719 |
|
| 720 |
T cc = ci * ch; |
| 721 |
T cs = ci * sh; |
| 722 |
T sc = si * ch; |
| 723 |
T ss = si * sh; |
| 724 |
|
| 725 |
Matrix33<T> M; |
| 726 |
|
| 727 |
if (_initialRepeated) |
| 728 |
{ |
| 729 |
M[i][i] = cj; |
| 730 |
M[j][i] = sj * si; |
| 731 |
M[k][i] = sj * ci; |
| 732 |
M[i][j] = sj * sh; |
| 733 |
M[j][j] = -cj * ss + cc; |
| 734 |
M[k][j] = -cj * cs - sc; |
| 735 |
M[i][k] = -sj * ch; |
| 736 |
M[j][k] = cj * sc + cs; |
| 737 |
M[k][k] = cj * cc - ss; |
| 738 |
} |
| 739 |
else |
| 740 |
{ |
| 741 |
M[i][i] = cj * ch; |
| 742 |
M[j][i] = sj * sc - cs; |
| 743 |
M[k][i] = sj * cc + ss; |
| 744 |
M[i][j] = cj * sh; |
| 745 |
M[j][j] = sj * ss + cc; |
| 746 |
M[k][j] = sj * cs - sc; |
| 747 |
M[i][k] = -sj; |
| 748 |
M[j][k] = cj * si; |
| 749 |
M[k][k] = cj * ci; |
| 750 |
} |
| 751 |
|
| 752 |
return M; |
| 753 |
} |
| 754 |
|
| 755 |
template <class T> |
| 756 |
IMATH_HOSTDEVICE Matrix44<T> |
| 757 |
Euler<T>::toMatrix44() const IMATH_NOEXCEPT |
| 758 |
{ |
| 759 |
int i, j, k; |
| 760 |
angleOrder (i, j, k); |
| 761 |
|
| 762 |
Vec3<T> angles; |
| 763 |
|
| 764 |
if (_frameStatic) |
| 765 |
angles = (*this); |
| 766 |
else |
| 767 |
angles = Vec3<T> (z, y, x); |
| 768 |
|
| 769 |
if (!_parityEven) |
| 770 |
angles *= -1.0; |
| 771 |
|
| 772 |
T ci = std::cos (angles.x); |
| 773 |
T cj = std::cos (angles.y); |
| 774 |
T ch = std::cos (angles.z); |
| 775 |
T si = std::sin (angles.x); |
| 776 |
T sj = std::sin (angles.y); |
| 777 |
T sh = std::sin (angles.z); |
| 778 |
|
| 779 |
T cc = ci * ch; |
| 780 |
T cs = ci * sh; |
| 781 |
T sc = si * ch; |
| 782 |
T ss = si * sh; |
| 783 |
|
| 784 |
Matrix44<T> M; |
| 785 |
|
| 786 |
if (_initialRepeated) |
| 787 |
{ |
| 788 |
M[i][i] = cj; |
| 789 |
M[j][i] = sj * si; |
| 790 |
M[k][i] = sj * ci; |
| 791 |
M[i][j] = sj * sh; |
| 792 |
M[j][j] = -cj * ss + cc; |
| 793 |
M[k][j] = -cj * cs - sc; |
| 794 |
M[i][k] = -sj * ch; |
| 795 |
M[j][k] = cj * sc + cs; |
| 796 |
M[k][k] = cj * cc - ss; |
| 797 |
} |
| 798 |
else |
| 799 |
{ |
| 800 |
M[i][i] = cj * ch; |
| 801 |
M[j][i] = sj * sc - cs; |
| 802 |
M[k][i] = sj * cc + ss; |
| 803 |
M[i][j] = cj * sh; |
| 804 |
M[j][j] = sj * ss + cc; |
| 805 |
M[k][j] = sj * cs - sc; |
| 806 |
M[i][k] = -sj; |
| 807 |
M[j][k] = cj * si; |
| 808 |
M[k][k] = cj * ci; |
| 809 |
} |
| 810 |
|
| 811 |
return M; |
| 812 |
} |
| 813 |
|
| 814 |
template <class T> |
| 815 |
IMATH_HOSTDEVICE Quat<T> |
| 816 |
Euler<T>::toQuat() const IMATH_NOEXCEPT |
| 817 |
{ |
| 818 |
Vec3<T> angles; |
| 819 |
int i, j, k; |
| 820 |
angleOrder (i, j, k); |
| 821 |
|
| 822 |
if (_frameStatic) |
| 823 |
angles = (*this); |
| 824 |
else |
| 825 |
angles = Vec3<T> (z, y, x); |
| 826 |
|
| 827 |
if (!_parityEven) |
| 828 |
angles.y = -angles.y; |
| 829 |
|
| 830 |
T ti = angles.x * 0.5; |
| 831 |
T tj = angles.y * 0.5; |
| 832 |
T th = angles.z * 0.5; |
| 833 |
T ci = std::cos (ti); |
| 834 |
T cj = std::cos (tj); |
| 835 |
T ch = std::cos (th); |
| 836 |
T si = std::sin (ti); |
| 837 |
T sj = std::sin (tj); |
| 838 |
T sh = std::sin (th); |
| 839 |
T cc = ci * ch; |
| 840 |
T cs = ci * sh; |
| 841 |
T sc = si * ch; |
| 842 |
T ss = si * sh; |
| 843 |
|
| 844 |
T parity = _parityEven ? 1.0 : -1.0; |
| 845 |
|
| 846 |
Quat<T> q; |
| 847 |
Vec3<T> a; |
| 848 |
|
| 849 |
if (_initialRepeated) |
| 850 |
{ |
| 851 |
a[i] = cj * (cs + sc); |
| 852 |
a[j] = sj * (cc + ss) * parity, // NOSONAR - suppress SonarCloud bug report. |
| 853 |
a[k] = sj * (cs - sc); |
| 854 |
q.r = cj * (cc - ss); |
| 855 |
} |
| 856 |
else |
| 857 |
{ |
| 858 |
a[i] = cj * sc - sj * cs, |
| 859 |
a[j] = (cj * ss + sj * cc) * parity, // NOSONAR - suppress SonarCloud bug report. |
| 860 |
a[k] = cj * cs - sj * sc; |
| 861 |
q.r = cj * cc + sj * ss; |
| 862 |
} |
| 863 |
|
| 864 |
q.v = a; |
| 865 |
|
| 866 |
return q; |
| 867 |
} |
| 868 |
|
| 869 |
template <class T> |
| 870 |
IMATH_HOSTDEVICE constexpr inline bool |
| 871 |
Euler<T>::legal (typename Euler<T>::Order order) IMATH_NOEXCEPT |
| 872 |
{ |
| 873 |
return (order & ~Legal) ? false : true; |
| 874 |
} |
| 875 |
|
| 876 |
template <class T> |
| 877 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 typename Euler<T>::Order |
| 878 |
Euler<T>::order() const IMATH_NOEXCEPT |
| 879 |
{ |
| 880 |
int foo = (_initialAxis == Z ? 0x2000 : (_initialAxis == Y ? 0x1000 : 0)); |
| 881 |
|
| 882 |
if (_parityEven) |
| 883 |
foo |= 0x0100; |
| 884 |
if (_initialRepeated) |
| 885 |
foo |= 0x0010; |
| 886 |
if (_frameStatic) |
| 887 |
foo++; |
| 888 |
|
| 889 |
return (Order) foo; |
| 890 |
} |
| 891 |
|
| 892 |
template <class T> |
| 893 |
IMATH_HOSTDEVICE inline void |
| 894 |
Euler<T>::setOrder (typename Euler<T>::Order p) IMATH_NOEXCEPT |
| 895 |
{ |
| 896 |
set (p & 0x2000 ? Z : (p & 0x1000 ? Y : X), // initial axis |
| 897 |
!(p & 0x1), // static? |
| 898 |
!!(p & 0x100), // permutation even? |
| 899 |
!!(p & 0x10)); // initial repeats? |
| 900 |
} |
| 901 |
|
| 902 |
template <class T> |
| 903 |
IMATH_HOSTDEVICE inline void |
| 904 |
Euler<T>::set (typename Euler<T>::Axis axis, bool relative, bool parityEven, bool firstRepeats) IMATH_NOEXCEPT |
| 905 |
{ |
| 906 |
_initialAxis = axis; |
| 907 |
_frameStatic = !relative; |
| 908 |
_parityEven = parityEven; |
| 909 |
_initialRepeated = firstRepeats; |
| 910 |
} |
| 911 |
|
| 912 |
template <class T> |
| 913 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 inline const Euler<T>& |
| 914 |
Euler<T>::operator= (const Euler<T>& euler) IMATH_NOEXCEPT |
| 915 |
{ |
| 916 |
x = euler.x; |
| 917 |
y = euler.y; |
| 918 |
z = euler.z; |
| 919 |
_initialAxis = euler._initialAxis; |
| 920 |
_frameStatic = euler._frameStatic; |
| 921 |
_parityEven = euler._parityEven; |
| 922 |
_initialRepeated = euler._initialRepeated; |
| 923 |
return *this; |
| 924 |
} |
| 925 |
|
| 926 |
template <class T> |
| 927 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 inline const Euler<T>& |
| 928 |
Euler<T>::operator= (const Vec3<T>& v) IMATH_NOEXCEPT |
| 929 |
{ |
| 930 |
x = v.x; |
| 931 |
y = v.y; |
| 932 |
z = v.z; |
| 933 |
return *this; |
| 934 |
} |
| 935 |
|
| 936 |
template <class T> |
| 937 |
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 inline float |
| 938 |
Euler<T>::angleMod (T angle) IMATH_NOEXCEPT |
| 939 |
{ |
| 940 |
const T pi = static_cast<T> (M_PI); |
| 941 |
angle = fmod (T (angle), T (2 * pi)); |
| 942 |
|
| 943 |
if (angle < -pi) |
| 944 |
angle += 2 * pi; |
| 945 |
if (angle > +pi) |
| 946 |
angle -= 2 * pi; |
| 947 |
|
| 948 |
return angle; |
| 949 |
} |
| 950 |
|
| 951 |
template <class T> |
| 952 |
IMATH_HOSTDEVICE inline void |
| 953 |
Euler<T>::simpleXYZRotation (Vec3<T>& xyzRot, const Vec3<T>& targetXyzRot) IMATH_NOEXCEPT |
| 954 |
{ |
| 955 |
Vec3<T> d = xyzRot - targetXyzRot; |
| 956 |
xyzRot[0] = targetXyzRot[0] + angleMod (d[0]); |
| 957 |
xyzRot[1] = targetXyzRot[1] + angleMod (d[1]); |
| 958 |
xyzRot[2] = targetXyzRot[2] + angleMod (d[2]); |
| 959 |
} |
| 960 |
|
| 961 |
template <class T> |
| 962 |
IMATH_HOSTDEVICE void |
| 963 |
Euler<T>::nearestRotation (Vec3<T>& xyzRot, const Vec3<T>& targetXyzRot, Order order) IMATH_NOEXCEPT |
| 964 |
{ |
| 965 |
int i, j, k; |
| 966 |
Euler<T> e (0, 0, 0, order); |
| 967 |
e.angleOrder (i, j, k); |
| 968 |
|
| 969 |
simpleXYZRotation (xyzRot, targetXyzRot); |
| 970 |
|
| 971 |
Vec3<T> otherXyzRot; |
| 972 |
otherXyzRot[i] = M_PI + xyzRot[i]; |
| 973 |
otherXyzRot[j] = M_PI - xyzRot[j]; |
| 974 |
otherXyzRot[k] = M_PI + xyzRot[k]; |
| 975 |
|
| 976 |
simpleXYZRotation (otherXyzRot, targetXyzRot); |
| 977 |
|
| 978 |
Vec3<T> d = xyzRot - targetXyzRot; |
| 979 |
Vec3<T> od = otherXyzRot - targetXyzRot; |
| 980 |
T dMag = d.dot (d); |
| 981 |
T odMag = od.dot (od); |
| 982 |
|
| 983 |
if (odMag < dMag) |
| 984 |
{ |
| 985 |
xyzRot = otherXyzRot; |
| 986 |
} |
| 987 |
} |
| 988 |
|
| 989 |
template <class T> |
| 990 |
IMATH_HOSTDEVICE void |
| 991 |
Euler<T>::makeNear (const Euler<T>& target) IMATH_NOEXCEPT |
| 992 |
{ |
| 993 |
Vec3<T> xyzRot = toXYZVector(); |
| 994 |
Vec3<T> targetXyz; |
| 995 |
if (order() != target.order()) |
| 996 |
{ |
| 997 |
Euler<T> targetSameOrder = Euler<T> (target, order()); |
| 998 |
targetXyz = targetSameOrder.toXYZVector(); |
| 999 |
} |
| 1000 |
else |
| 1001 |
{ |
| 1002 |
targetXyz = target.toXYZVector(); |
| 1003 |
} |
| 1004 |
|
| 1005 |
nearestRotation (xyzRot, targetXyz, order()); |
| 1006 |
|
| 1007 |
setXYZVector (xyzRot); |
| 1008 |
} |
| 1009 |
|
| 1010 |
/// @endcond |
| 1011 |
|
| 1012 |
/// Stream ouput, as "(x y z i j k)" |
| 1013 |
template <class T> |
| 1014 |
std::ostream& |
| 1015 |
operator<< (std::ostream& o, const Euler<T>& euler) |
| 1016 |
{ |
| 1017 |
char a[3] = { 'X', 'Y', 'Z' }; |
| 1018 |
|
| 1019 |
const char* r = euler.frameStatic() ? "" : "r"; |
| 1020 |
int i, j, k; |
| 1021 |
euler.angleOrder (i, j, k); |
| 1022 |
|
| 1023 |
if (euler.initialRepeated()) |
| 1024 |
k = i; |
| 1025 |
|
| 1026 |
return o << "(" << euler.x << " " << euler.y << " " << euler.z << " " << a[i] << a[j] << a[k] |
| 1027 |
<< r << ")"; |
| 1028 |
} |
| 1029 |
|
| 1030 |
#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER |
| 1031 |
# pragma warning(pop) |
| 1032 |
#endif |
| 1033 |
|
| 1034 |
|
| 1035 |
IMATH_INTERNAL_NAMESPACE_HEADER_EXIT |
| 1036 |
|
| 1037 |
#endif // INCLUDED_IMATHEULER_H |