| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright Contributors to the OpenVDB Project | ||
| 2 | // SPDX-License-Identifier: MPL-2.0 | ||
| 3 | |||
| 4 | #ifndef OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED | ||
| 5 | #define OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED | ||
| 6 | |||
| 7 | #include "Mat.h" | ||
| 8 | #include "Mat3.h" | ||
| 9 | #include "Math.h" | ||
| 10 | #include "Vec3.h" | ||
| 11 | #include <openvdb/Exceptions.h> | ||
| 12 | #include <cmath> | ||
| 13 | #include <iostream> | ||
| 14 | #include <sstream> | ||
| 15 | #include <string> | ||
| 16 | |||
| 17 | |||
| 18 | namespace openvdb { | ||
| 19 | OPENVDB_USE_VERSION_NAMESPACE | ||
| 20 | namespace OPENVDB_VERSION_NAME { | ||
| 21 | namespace math { | ||
| 22 | |||
| 23 | template<typename T> class Quat; | ||
| 24 | |||
| 25 | /// Linear interpolation between the two quaternions | ||
| 26 | template <typename T> | ||
| 27 | Quat<T> slerp(const Quat<T> &q1, const Quat<T> &q2, T t, T tolerance=0.00001) | ||
| 28 | { | ||
| 29 | T qdot, angle, sineAngle; | ||
| 30 | |||
| 31 | qdot = q1.dot(q2); | ||
| 32 | |||
| 33 | if (fabs(qdot) >= 1.0) { | ||
| 34 | angle = 0; // not necessary but suppresses compiler warning | ||
| 35 | sineAngle = 0; | ||
| 36 | } else { | ||
| 37 | angle = acos(qdot); | ||
| 38 | sineAngle = sin(angle); | ||
| 39 | } | ||
| 40 | |||
| 41 | // | ||
| 42 | // Denominator close to 0 corresponds to the case where the | ||
| 43 | // two quaternions are close to the same rotation. In this | ||
| 44 | // case linear interpolation is used but we normalize to | ||
| 45 | // guarantee unit length | ||
| 46 | // | ||
| 47 | if (sineAngle <= tolerance) { | ||
| 48 | T s = 1.0 - t; | ||
| 49 | |||
| 50 | Quat<T> qtemp(s * q1[0] + t * q2[0], s * q1[1] + t * q2[1], | ||
| 51 | s * q1[2] + t * q2[2], s * q1[3] + t * q2[3]); | ||
| 52 | // | ||
| 53 | // Check the case where two close to antipodal quaternions were | ||
| 54 | // blended resulting in a nearly zero result which can happen, | ||
| 55 | // for example, if t is close to 0.5. In this case it is not safe | ||
| 56 | // to project back onto the sphere. | ||
| 57 | // | ||
| 58 | double lengthSquared = qtemp.dot(qtemp); | ||
| 59 | |||
| 60 | if (lengthSquared <= tolerance * tolerance) { | ||
| 61 | qtemp = (t < 0.5) ? q1 : q2; | ||
| 62 | } else { | ||
| 63 | qtemp *= 1.0 / sqrt(lengthSquared); | ||
| 64 | } | ||
| 65 | return qtemp; | ||
| 66 | } else { | ||
| 67 | |||
| 68 | T sine = 1.0 / sineAngle; | ||
| 69 | T a = sin((1.0 - t) * angle) * sine; | ||
| 70 | T b = sin(t * angle) * sine; | ||
| 71 | return Quat<T>(a * q1[0] + b * q2[0], a * q1[1] + b * q2[1], | ||
| 72 | a * q1[2] + b * q2[2], a * q1[3] + b * q2[3]); | ||
| 73 | } | ||
| 74 | |||
| 75 | } | ||
| 76 | |||
| 77 | template<typename T> | ||
| 78 | class Quat | ||
| 79 | { | ||
| 80 | public: | ||
| 81 | using value_type = T; | ||
| 82 | using ValueType = T; | ||
| 83 | static const int size = 4; | ||
| 84 | |||
| 85 | /// Trivial constructor, the quaternion is NOT initialized | ||
| 86 | #if OPENVDB_ABI_VERSION_NUMBER >= 8 | ||
| 87 | /// @note destructor, copy constructor, assignment operator and | ||
| 88 | /// move constructor are left to be defined by the compiler (default) | ||
| 89 | Quat() = default; | ||
| 90 | #else | ||
| 91 | Quat() {} | ||
| 92 | |||
| 93 | /// Copy constructor | ||
| 94 | Quat(const Quat &q) | ||
| 95 | { | ||
| 96 | mm[0] = q.mm[0]; | ||
| 97 | mm[1] = q.mm[1]; | ||
| 98 | mm[2] = q.mm[2]; | ||
| 99 | mm[3] = q.mm[3]; | ||
| 100 | |||
| 101 | } | ||
| 102 | |||
| 103 | /// Assignment operator | ||
| 104 | Quat& operator=(const Quat &q) | ||
| 105 | { | ||
| 106 | mm[0] = q.mm[0]; | ||
| 107 | mm[1] = q.mm[1]; | ||
| 108 | mm[2] = q.mm[2]; | ||
| 109 | mm[3] = q.mm[3]; | ||
| 110 | |||
| 111 | return *this; | ||
| 112 | } | ||
| 113 | #endif | ||
| 114 | |||
| 115 | /// Constructor with four arguments, e.g. Quatf q(1,2,3,4); | ||
| 116 | 665 | Quat(T x, T y, T z, T w) | |
| 117 | { | ||
| 118 | 665 | mm[0] = x; | |
| 119 | 665 | mm[1] = y; | |
| 120 | 665 | mm[2] = z; | |
| 121 |
8/17✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
|
10 | mm[3] = w; |
| 122 | |||
| 123 | } | ||
| 124 | |||
| 125 | /// Constructor with array argument, e.g. float a[4]; Quatf q(a); | ||
| 126 | Quat(T *a) | ||
| 127 | { | ||
| 128 | mm[0] = a[0]; | ||
| 129 | mm[1] = a[1]; | ||
| 130 | mm[2] = a[2]; | ||
| 131 | mm[3] = a[3]; | ||
| 132 | |||
| 133 | } | ||
| 134 | |||
| 135 | /// Constructor given rotation as axis and angle, the axis must be | ||
| 136 | /// unit vector | ||
| 137 | 1 | Quat(const Vec3<T> &axis, T angle) | |
| 138 | { | ||
| 139 | // assert( REL_EQ(axis.length(), 1.) ); | ||
| 140 | |||
| 141 | 1 | T s = T(sin(angle*T(0.5))); | |
| 142 | |||
| 143 | 1 | mm[0] = axis.x() * s; | |
| 144 | 1 | mm[1] = axis.y() * s; | |
| 145 | 1 | mm[2] = axis.z() * s; | |
| 146 | |||
| 147 | 1 | mm[3] = T(cos(angle*T(0.5))); | |
| 148 | |||
| 149 | 1 | } | |
| 150 | |||
| 151 | /// Constructor given rotation as axis and angle | ||
| 152 | 1 | Quat(math::Axis axis, T angle) | |
| 153 | { | ||
| 154 | T s = T(sin(angle*T(0.5))); | ||
| 155 | |||
| 156 | 1 | mm[0] = (axis==math::X_AXIS) * s; | |
| 157 | 1 | mm[1] = (axis==math::Y_AXIS) * s; | |
| 158 | 1 | mm[2] = (axis==math::Z_AXIS) * s; | |
| 159 | |||
| 160 | 1 | mm[3] = T(cos(angle*T(0.5))); | |
| 161 | } | ||
| 162 | |||
| 163 | /// Constructor given a rotation matrix | ||
| 164 | template<typename T1> | ||
| 165 | 3 | Quat(const Mat3<T1> &rot) { | |
| 166 | |||
| 167 | // verify that the matrix is really a rotation | ||
| 168 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
|
3 | if(!isUnitary(rot)) { // unitary is reflection or rotation |
| 169 | ✗ | OPENVDB_THROW(ArithmeticError, | |
| 170 | "A non-rotation matrix can not be used to construct a quaternion"); | ||
| 171 | } | ||
| 172 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | if (!isApproxEqual(rot.det(), T1(1))) { // rule out reflection |
| 173 | ✗ | OPENVDB_THROW(ArithmeticError, | |
| 174 | "A reflection matrix can not be used to construct a quaternion"); | ||
| 175 | } | ||
| 176 | |||
| 177 | T trace(rot.trace()); | ||
| 178 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | if (trace > 0) { |
| 179 | |||
| 180 | ✗ | T q_w = 0.5 * std::sqrt(trace+1); | |
| 181 | ✗ | T factor = 0.25 / q_w; | |
| 182 | |||
| 183 | ✗ | mm[0] = factor * (rot(1,2) - rot(2,1)); | |
| 184 | ✗ | mm[1] = factor * (rot(2,0) - rot(0,2)); | |
| 185 | ✗ | mm[2] = factor * (rot(0,1) - rot(1,0)); | |
| 186 | ✗ | mm[3] = q_w; | |
| 187 |
2/4✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
|
3 | } else if (rot(0,0) > rot(1,1) && rot(0,0) > rot(2,2)) { |
| 188 | |||
| 189 | ✗ | T q_x = 0.5 * sqrt(rot(0,0)- rot(1,1)-rot(2,2)+1); | |
| 190 | ✗ | T factor = 0.25 / q_x; | |
| 191 | |||
| 192 | ✗ | mm[0] = q_x; | |
| 193 | ✗ | mm[1] = factor * (rot(0,1) + rot(1,0)); | |
| 194 | ✗ | mm[2] = factor * (rot(2,0) + rot(0,2)); | |
| 195 | ✗ | mm[3] = factor * (rot(1,2) - rot(2,1)); | |
| 196 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | } else if (rot(1,1) > rot(2,2)) { |
| 197 | |||
| 198 | ✗ | T q_y = 0.5 * sqrt(rot(1,1)-rot(0,0)-rot(2,2)+1); | |
| 199 | ✗ | T factor = 0.25 / q_y; | |
| 200 | |||
| 201 | ✗ | mm[0] = factor * (rot(0,1) + rot(1,0)); | |
| 202 | ✗ | mm[1] = q_y; | |
| 203 | ✗ | mm[2] = factor * (rot(1,2) + rot(2,1)); | |
| 204 | ✗ | mm[3] = factor * (rot(2,0) - rot(0,2)); | |
| 205 | } else { | ||
| 206 | |||
| 207 | 3 | T q_z = 0.5 * sqrt(rot(2,2)-rot(0,0)-rot(1,1)+1); | |
| 208 | 3 | T factor = 0.25 / q_z; | |
| 209 | |||
| 210 | 3 | mm[0] = factor * (rot(2,0) + rot(0,2)); | |
| 211 | 3 | mm[1] = factor * (rot(1,2) + rot(2,1)); | |
| 212 | 3 | mm[2] = q_z; | |
| 213 | 3 | mm[3] = factor * (rot(0,1) - rot(1,0)); | |
| 214 | } | ||
| 215 | 3 | } | |
| 216 | |||
| 217 | /// Reference to the component, e.g. q.x() = 4.5f; | ||
| 218 | T& x() { return mm[0]; } | ||
| 219 | T& y() { return mm[1]; } | ||
| 220 | T& z() { return mm[2]; } | ||
| 221 | T& w() { return mm[3]; } | ||
| 222 | |||
| 223 | /// Get the component, e.g. float f = q.w(); | ||
| 224 | T x() const { return mm[0]; } | ||
| 225 | T y() const { return mm[1]; } | ||
| 226 | T z() const { return mm[2]; } | ||
| 227 | T w() const { return mm[3]; } | ||
| 228 | |||
| 229 | // Number of elements | ||
| 230 | static unsigned numElements() { return 4; } | ||
| 231 | |||
| 232 | /// Array style reference to the components, e.g. q[3] = 1.34f; | ||
| 233 | T& operator[](int i) { return mm[i]; } | ||
| 234 | |||
| 235 | /// Array style constant reference to the components, e.g. float f = q[1]; | ||
| 236 | T operator[](int i) const { return mm[i]; } | ||
| 237 | |||
| 238 | /// Cast to T* | ||
| 239 | operator T*() { return mm; } | ||
| 240 | operator const T*() const { return mm; } | ||
| 241 | |||
| 242 | /// Alternative indexed reference to the elements | ||
| 243 | T& operator()(int i) { return mm[i]; } | ||
| 244 | |||
| 245 | /// Alternative indexed constant reference to the elements, | ||
| 246 | T operator()(int i) const { return mm[i]; } | ||
| 247 | |||
| 248 | /// Return angle of rotation | ||
| 249 | 2 | T angle() const | |
| 250 | { | ||
| 251 | 2 | T sqrLength = mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2]; | |
| 252 | |||
| 253 |
1/2✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
2 | if ( sqrLength > 1.0e-8 ) { |
| 254 | |||
| 255 | 2 | return T(T(2.0) * acos(mm[3])); | |
| 256 | |||
| 257 | } else { | ||
| 258 | |||
| 259 | return T(0.0); | ||
| 260 | } | ||
| 261 | } | ||
| 262 | |||
| 263 | /// Return axis of rotation | ||
| 264 | 2 | Vec3<T> axis() const | |
| 265 | { | ||
| 266 | 2 | T sqrLength = mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2]; | |
| 267 | |||
| 268 |
1/2✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
2 | if ( sqrLength > 1.0e-8 ) { |
| 269 | |||
| 270 | 2 | T invLength = T(T(1)/sqrt(sqrLength)); | |
| 271 | |||
| 272 | 2 | return Vec3<T>( mm[0]*invLength, mm[1]*invLength, mm[2]*invLength ); | |
| 273 | } else { | ||
| 274 | |||
| 275 | ✗ | return Vec3<T>(1,0,0); | |
| 276 | } | ||
| 277 | } | ||
| 278 | |||
| 279 | |||
| 280 | /// "this" quaternion gets initialized to [x, y, z, w] | ||
| 281 | Quat& init(T x, T y, T z, T w) | ||
| 282 | { | ||
| 283 | mm[0] = x; mm[1] = y; mm[2] = z; mm[3] = w; | ||
| 284 | return *this; | ||
| 285 | } | ||
| 286 | |||
| 287 | /// "this" quaternion gets initialized to identity, same as setIdentity() | ||
| 288 | Quat& init() { return setIdentity(); } | ||
| 289 | |||
| 290 | /// Set "this" quaternion to rotation specified by axis and angle, | ||
| 291 | /// the axis must be unit vector | ||
| 292 | 1 | Quat& setAxisAngle(const Vec3<T>& axis, T angle) | |
| 293 | { | ||
| 294 | |||
| 295 | 1 | T s = T(sin(angle*T(0.5))); | |
| 296 | |||
| 297 | 1 | mm[0] = axis.x() * s; | |
| 298 | 1 | mm[1] = axis.y() * s; | |
| 299 | 1 | mm[2] = axis.z() * s; | |
| 300 | |||
| 301 | 1 | mm[3] = T(cos(angle*T(0.5))); | |
| 302 | |||
| 303 | 1 | return *this; | |
| 304 | } // axisAngleTest | ||
| 305 | |||
| 306 | /// Set "this" vector to zero | ||
| 307 | Quat& setZero() | ||
| 308 | { | ||
| 309 | mm[0] = mm[1] = mm[2] = mm[3] = 0; | ||
| 310 | return *this; | ||
| 311 | } | ||
| 312 | |||
| 313 | /// Set "this" vector to identity | ||
| 314 | Quat& setIdentity() | ||
| 315 | { | ||
| 316 | mm[0] = mm[1] = mm[2] = 0; | ||
| 317 | mm[3] = 1; | ||
| 318 | return *this; | ||
| 319 | } | ||
| 320 | |||
| 321 | /// Returns vector of x,y,z rotational components | ||
| 322 | 8 | Vec3<T> eulerAngles(RotationOrder rotationOrder) const | |
| 323 | 8 | { return math::eulerAngles(Mat3<T>(*this), rotationOrder); } | |
| 324 | |||
| 325 | /// Equality operator, does exact floating point comparisons | ||
| 326 | bool operator==(const Quat &q) const | ||
| 327 | { | ||
| 328 |
2/10✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
|
6 | return (isExactlyEqual(mm[0],q.mm[0]) && |
| 329 |
2/10✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
|
6 | isExactlyEqual(mm[1],q.mm[1]) && |
| 330 |
4/20✗ Branch 0 not taken.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 5 times.
✗ Branch 19 not taken.
|
12 | isExactlyEqual(mm[2],q.mm[2]) && |
| 331 | isExactlyEqual(mm[3],q.mm[3]) ); | ||
| 332 | } | ||
| 333 | |||
| 334 | /// Test if "this" is equivalent to q with tolerance of eps value | ||
| 335 |
1/2✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
2 | bool eq(const Quat &q, T eps=1.0e-7) const |
| 336 | { | ||
| 337 |
2/4✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | return isApproxEqual(mm[0],q.mm[0],eps) && isApproxEqual(mm[1],q.mm[1],eps) && |
| 338 |
2/4✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
|
4 | isApproxEqual(mm[2],q.mm[2],eps) && isApproxEqual(mm[3],q.mm[3],eps) ; |
| 339 | } // trivial | ||
| 340 | |||
| 341 | /// Add quaternion q to "this" quaternion, e.g. q += q1; | ||
| 342 | Quat& operator+=(const Quat &q) | ||
| 343 | { | ||
| 344 | mm[0] += q.mm[0]; | ||
| 345 | mm[1] += q.mm[1]; | ||
| 346 | mm[2] += q.mm[2]; | ||
| 347 | mm[3] += q.mm[3]; | ||
| 348 | |||
| 349 | return *this; | ||
| 350 | } | ||
| 351 | |||
| 352 | /// Subtract quaternion q from "this" quaternion, e.g. q -= q1; | ||
| 353 | Quat& operator-=(const Quat &q) | ||
| 354 | { | ||
| 355 | mm[0] -= q.mm[0]; | ||
| 356 | mm[1] -= q.mm[1]; | ||
| 357 | mm[2] -= q.mm[2]; | ||
| 358 | mm[3] -= q.mm[3]; | ||
| 359 | |||
| 360 | return *this; | ||
| 361 | } | ||
| 362 | |||
| 363 | /// Scale "this" quaternion by scalar, e.g. q *= scalar; | ||
| 364 | Quat& operator*=(T scalar) | ||
| 365 | { | ||
| 366 | 1 | mm[0] *= scalar; | |
| 367 | 1 | mm[1] *= scalar; | |
| 368 | 1 | mm[2] *= scalar; | |
| 369 | 1 | mm[3] *= scalar; | |
| 370 | |||
| 371 | return *this; | ||
| 372 | } | ||
| 373 | |||
| 374 | /// Return (this+q), e.g. q = q1 + q2; | ||
| 375 | Quat operator+(const Quat &q) const | ||
| 376 | { | ||
| 377 | return Quat<T>(mm[0]+q.mm[0], mm[1]+q.mm[1], mm[2]+q.mm[2], mm[3]+q.mm[3]); | ||
| 378 | } | ||
| 379 | |||
| 380 | /// Return (this-q), e.g. q = q1 - q2; | ||
| 381 | Quat operator-(const Quat &q) const | ||
| 382 | { | ||
| 383 | return Quat<T>(mm[0]-q.mm[0], mm[1]-q.mm[1], mm[2]-q.mm[2], mm[3]-q.mm[3]); | ||
| 384 | } | ||
| 385 | |||
| 386 | /// Return (this*q), e.g. q = q1 * q2; | ||
| 387 | 2 | Quat operator*(const Quat &q) const | |
| 388 | { | ||
| 389 | Quat<T> prod; | ||
| 390 | |||
| 391 | 2 | prod.mm[0] = mm[3]*q.mm[0] + mm[0]*q.mm[3] + mm[1]*q.mm[2] - mm[2]*q.mm[1]; | |
| 392 | 2 | prod.mm[1] = mm[3]*q.mm[1] + mm[1]*q.mm[3] + mm[2]*q.mm[0] - mm[0]*q.mm[2]; | |
| 393 | 2 | prod.mm[2] = mm[3]*q.mm[2] + mm[2]*q.mm[3] + mm[0]*q.mm[1] - mm[1]*q.mm[0]; | |
| 394 | 2 | prod.mm[3] = mm[3]*q.mm[3] - mm[0]*q.mm[0] - mm[1]*q.mm[1] - mm[2]*q.mm[2]; | |
| 395 | |||
| 396 | 2 | return prod; | |
| 397 | |||
| 398 | } | ||
| 399 | |||
| 400 | /// Assigns this to (this*q), e.g. q *= q1; | ||
| 401 | Quat operator*=(const Quat &q) | ||
| 402 | { | ||
| 403 | *this = *this * q; | ||
| 404 | return *this; | ||
| 405 | } | ||
| 406 | |||
| 407 | /// Return (this*scalar), e.g. q = q1 * scalar; | ||
| 408 | Quat operator*(T scalar) const | ||
| 409 | { | ||
| 410 | return Quat<T>(mm[0]*scalar, mm[1]*scalar, mm[2]*scalar, mm[3]*scalar); | ||
| 411 | } | ||
| 412 | |||
| 413 | /// Return (this/scalar), e.g. q = q1 / scalar; | ||
| 414 | Quat operator/(T scalar) const | ||
| 415 | { | ||
| 416 | 1 | return Quat<T>(mm[0]/scalar, mm[1]/scalar, mm[2]/scalar, mm[3]/scalar); | |
| 417 | } | ||
| 418 | |||
| 419 | /// Negation operator, e.g. q = -q; | ||
| 420 | Quat operator-() const | ||
| 421 | { return Quat<T>(-mm[0], -mm[1], -mm[2], -mm[3]); } | ||
| 422 | |||
| 423 | /// this = q1 + q2 | ||
| 424 | /// "this", q1 and q2 need not be distinct objects, e.g. q.add(q1,q); | ||
| 425 | Quat& add(const Quat &q1, const Quat &q2) | ||
| 426 | { | ||
| 427 | mm[0] = q1.mm[0] + q2.mm[0]; | ||
| 428 | mm[1] = q1.mm[1] + q2.mm[1]; | ||
| 429 | mm[2] = q1.mm[2] + q2.mm[2]; | ||
| 430 | mm[3] = q1.mm[3] + q2.mm[3]; | ||
| 431 | |||
| 432 | return *this; | ||
| 433 | } | ||
| 434 | |||
| 435 | /// this = q1 - q2 | ||
| 436 | /// "this", q1 and q2 need not be distinct objects, e.g. q.sub(q1,q); | ||
| 437 | Quat& sub(const Quat &q1, const Quat &q2) | ||
| 438 | { | ||
| 439 | mm[0] = q1.mm[0] - q2.mm[0]; | ||
| 440 | mm[1] = q1.mm[1] - q2.mm[1]; | ||
| 441 | mm[2] = q1.mm[2] - q2.mm[2]; | ||
| 442 | mm[3] = q1.mm[3] - q2.mm[3]; | ||
| 443 | |||
| 444 | return *this; | ||
| 445 | } | ||
| 446 | |||
| 447 | /// this = q1 * q2 | ||
| 448 | /// q1 and q2 must be distinct objects than "this", e.g. q.mult(q1,q2); | ||
| 449 | Quat& mult(const Quat &q1, const Quat &q2) | ||
| 450 | { | ||
| 451 | mm[0] = q1.mm[3]*q2.mm[0] + q1.mm[0]*q2.mm[3] + | ||
| 452 | q1.mm[1]*q2.mm[2] - q1.mm[2]*q2.mm[1]; | ||
| 453 | mm[1] = q1.mm[3]*q2.mm[1] + q1.mm[1]*q2.mm[3] + | ||
| 454 | q1.mm[2]*q2.mm[0] - q1.mm[0]*q2.mm[2]; | ||
| 455 | mm[2] = q1.mm[3]*q2.mm[2] + q1.mm[2]*q2.mm[3] + | ||
| 456 | q1.mm[0]*q2.mm[1] - q1.mm[1]*q2.mm[0]; | ||
| 457 | mm[3] = q1.mm[3]*q2.mm[3] - q1.mm[0]*q2.mm[0] - | ||
| 458 | q1.mm[1]*q2.mm[1] - q1.mm[2]*q2.mm[2]; | ||
| 459 | |||
| 460 | return *this; | ||
| 461 | } | ||
| 462 | |||
| 463 | /// this = scalar*q, q need not be distinct object than "this", | ||
| 464 | /// e.g. q.scale(1.5,q1); | ||
| 465 | Quat& scale(T scale, const Quat &q) | ||
| 466 | { | ||
| 467 | mm[0] = scale * q.mm[0]; | ||
| 468 | mm[1] = scale * q.mm[1]; | ||
| 469 | mm[2] = scale * q.mm[2]; | ||
| 470 | mm[3] = scale * q.mm[3]; | ||
| 471 | |||
| 472 | return *this; | ||
| 473 | } | ||
| 474 | |||
| 475 | /// Dot product | ||
| 476 | T dot(const Quat &q) const | ||
| 477 | { | ||
| 478 |
2/4✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
4 | return (mm[0]*q.mm[0] + mm[1]*q.mm[1] + mm[2]*q.mm[2] + mm[3]*q.mm[3]); |
| 479 | } | ||
| 480 | |||
| 481 | /// Return the quaternion rate corrsponding to the angular velocity omega | ||
| 482 | /// and "this" current rotation | ||
| 483 | Quat derivative(const Vec3<T>& omega) const | ||
| 484 | { | ||
| 485 | return Quat<T>( +w()*omega.x() -z()*omega.y() +y()*omega.z() , | ||
| 486 | +z()*omega.x() +w()*omega.y() -x()*omega.z() , | ||
| 487 | -y()*omega.x() +x()*omega.y() +w()*omega.z() , | ||
| 488 | -x()*omega.x() -y()*omega.y() -z()*omega.z() ); | ||
| 489 | } | ||
| 490 | |||
| 491 | /// this = normalized this | ||
| 492 | 1 | bool normalize(T eps = T(1.0e-8)) | |
| 493 | { | ||
| 494 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | T d = T(sqrt(mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2] + mm[3]*mm[3])); |
| 495 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if( isApproxEqual(d, T(0.0), eps) ) return false; |
| 496 | 1 | *this *= ( T(1)/d ); | |
| 497 | 1 | return true; | |
| 498 | } | ||
| 499 | |||
| 500 | /// this = normalized this | ||
| 501 | Quat unit() const | ||
| 502 | { | ||
| 503 | T d = sqrt(mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2] + mm[3]*mm[3]); | ||
| 504 | if( isExactlyEqual(d , T(0.0) ) ) | ||
| 505 | OPENVDB_THROW(ArithmeticError, | ||
| 506 | "Normalizing degenerate quaternion"); | ||
| 507 | return *this / d; | ||
| 508 | } | ||
| 509 | |||
| 510 | /// returns inverse of this | ||
| 511 | 1 | Quat inverse(T tolerance = T(0)) const | |
| 512 | { | ||
| 513 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | T d = mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2] + mm[3]*mm[3]; |
| 514 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if( isApproxEqual(d, T(0.0), tolerance) ) |
| 515 | ✗ | OPENVDB_THROW(ArithmeticError, | |
| 516 | "Cannot invert degenerate quaternion"); | ||
| 517 | 1 | Quat result = *this/-d; | |
| 518 | 1 | result.mm[3] = -result.mm[3]; | |
| 519 | 1 | return result; | |
| 520 | } | ||
| 521 | |||
| 522 | |||
| 523 | /// Return the conjugate of "this", same as invert without | ||
| 524 | /// unit quaternion test | ||
| 525 | Quat conjugate() const | ||
| 526 | { | ||
| 527 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | return Quat<T>(-mm[0], -mm[1], -mm[2], mm[3]); |
| 528 | } | ||
| 529 | |||
| 530 | /// Return rotated vector by "this" quaternion | ||
| 531 | Vec3<T> rotateVector(const Vec3<T> &v) const | ||
| 532 | { | ||
| 533 | Mat3<T> m(*this); | ||
| 534 | return m.transform(v); | ||
| 535 | } | ||
| 536 | |||
| 537 | /// Predefined constants, e.g. Quat q = Quat::identity(); | ||
| 538 | 327 | static Quat zero() { return Quat<T>(0,0,0,0); } | |
| 539 | static Quat identity() { return Quat<T>(0,0,0,1); } | ||
| 540 | |||
| 541 | /// @return string representation of Classname | ||
| 542 | ✗ | std::string str() const | |
| 543 | { | ||
| 544 | ✗ | std::ostringstream buffer; | |
| 545 | |||
| 546 | ✗ | buffer << "["; | |
| 547 | |||
| 548 | // For each column | ||
| 549 | ✗ | for (unsigned j(0); j < 4; j++) { | |
| 550 | ✗ | if (j) buffer << ", "; | |
| 551 | ✗ | buffer << mm[j]; | |
| 552 | } | ||
| 553 | |||
| 554 | ✗ | buffer << "]"; | |
| 555 | |||
| 556 | ✗ | return buffer.str(); | |
| 557 | } | ||
| 558 | |||
| 559 | /// Output to the stream, e.g. std::cout << q << std::endl; | ||
| 560 | ✗ | friend std::ostream& operator<<(std::ostream &stream, const Quat &q) | |
| 561 | { | ||
| 562 | ✗ | stream << q.str(); | |
| 563 | ✗ | return stream; | |
| 564 | } | ||
| 565 | |||
| 566 | friend Quat slerp<>(const Quat &q1, const Quat &q2, T t, T tolerance); | ||
| 567 | |||
| 568 | void write(std::ostream& os) const { os.write(static_cast<char*>(&mm), sizeof(T) * 4); } | ||
| 569 | void read(std::istream& is) { is.read(static_cast<char*>(&mm), sizeof(T) * 4); } | ||
| 570 | |||
| 571 | protected: | ||
| 572 | T mm[4]; | ||
| 573 | }; | ||
| 574 | |||
| 575 | /// Multiply each element of the given quaternion by @a scalar and return the result. | ||
| 576 | template <typename S, typename T> | ||
| 577 | Quat<T> operator*(S scalar, const Quat<T> &q) { return q*scalar; } | ||
| 578 | |||
| 579 | |||
| 580 | /// @brief Interpolate between m1 and m2. | ||
| 581 | /// Converts to quaternion form and uses slerp | ||
| 582 | /// m1 and m2 must be rotation matrices! | ||
| 583 | template <typename T, typename T0> | ||
| 584 | Mat3<T> slerp(const Mat3<T0> &m1, const Mat3<T0> &m2, T t) | ||
| 585 | { | ||
| 586 | using MatType = Mat3<T>; | ||
| 587 | |||
| 588 | Quat<T> q1(m1); | ||
| 589 | Quat<T> q2(m2); | ||
| 590 | |||
| 591 | if (q1.dot(q2) < 0) q2 *= -1; | ||
| 592 | |||
| 593 | Quat<T> qslerp = slerp<T>(q1, q2, static_cast<T>(t)); | ||
| 594 | MatType m = rotation<MatType>(qslerp); | ||
| 595 | return m; | ||
| 596 | } | ||
| 597 | |||
| 598 | |||
| 599 | |||
| 600 | /// Interpolate between m1 and m4 by converting m1 ... m4 into | ||
| 601 | /// quaternions and treating them as control points of a Bezier | ||
| 602 | /// curve using slerp in place of lerp in the De Castlejeau evaluation | ||
| 603 | /// algorithm. Just like a cubic Bezier curve, this will interpolate | ||
| 604 | /// m1 at t = 0 and m4 at t = 1 but in general will not pass through | ||
| 605 | /// m2 and m3. Unlike a standard Bezier curve this curve will not have | ||
| 606 | /// the convex hull property. | ||
| 607 | /// m1 ... m4 must be rotation matrices! | ||
| 608 | template <typename T, typename T0> | ||
| 609 | Mat3<T> bezLerp(const Mat3<T0> &m1, const Mat3<T0> &m2, | ||
| 610 | const Mat3<T0> &m3, const Mat3<T0> &m4, | ||
| 611 | T t) | ||
| 612 | { | ||
| 613 | Mat3<T> m00, m01, m02, m10, m11; | ||
| 614 | |||
| 615 | m00 = slerp(m1, m2, t); | ||
| 616 | m01 = slerp(m2, m3, t); | ||
| 617 | m02 = slerp(m3, m4, t); | ||
| 618 | |||
| 619 | m10 = slerp(m00, m01, t); | ||
| 620 | m11 = slerp(m01, m02, t); | ||
| 621 | |||
| 622 | return slerp(m10, m11, t); | ||
| 623 | } | ||
| 624 | |||
| 625 | using Quats = Quat<float>; | ||
| 626 | using Quatd = Quat<double>; | ||
| 627 | |||
| 628 | #if OPENVDB_ABI_VERSION_NUMBER >= 8 | ||
| 629 | OPENVDB_IS_POD(Quats) | ||
| 630 | OPENVDB_IS_POD(Quatd) | ||
| 631 | #endif | ||
| 632 | |||
| 633 | } // namespace math | ||
| 634 | |||
| 635 | |||
| 636 | template<> inline math::Quats zeroVal<math::Quats >() { return math::Quats::zero(); } | ||
| 637 | 324 | template<> inline math::Quatd zeroVal<math::Quatd >() { return math::Quatd::zero(); } | |
| 638 | |||
| 639 | } // namespace OPENVDB_VERSION_NAME | ||
| 640 | } // namespace openvdb | ||
| 641 | |||
| 642 | #endif //OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED | ||
| 643 |