| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * Copyright (c) 2000-2022 Inria | ||
| 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 notice, | ||
| 9 | * this list of conditions and the following disclaimer. | ||
| 10 | * * Redistributions in binary form must reproduce the above copyright notice, | ||
| 11 | * this list of conditions and the following disclaimer in the documentation | ||
| 12 | * and/or other materials provided with the distribution. | ||
| 13 | * * Neither the name of the ALICE Project-Team nor the names of its | ||
| 14 | * contributors may be used to endorse or promote products derived from this | ||
| 15 | * 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 HOLDER 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 | * Contact: Bruno Levy | ||
| 30 | * | ||
| 31 | * https://www.inria.fr/fr/bruno-levy | ||
| 32 | * | ||
| 33 | * Inria, | ||
| 34 | * Domaine de Voluceau, | ||
| 35 | * 78150 Le Chesnay - Rocquencourt | ||
| 36 | * FRANCE | ||
| 37 | * | ||
| 38 | */ | ||
| 39 | |||
| 40 | #include <geogram/basic/quaternion.h> | ||
| 41 | |||
| 42 | namespace { | ||
| 43 | using namespace GEO; | ||
| 44 | |||
| 45 | static const double SMALL = .00001 ; | ||
| 46 | } | ||
| 47 | |||
| 48 | namespace GEO { | ||
| 49 | |||
| 50 | ✗ | mat4 Quaternion::to_matrix() const { | |
| 51 | double t, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz; | ||
| 52 | ✗ | t = 2.0 / (dot(v_, v_) + (s_ * s_)); | |
| 53 | |||
| 54 | ✗ | xs = v_.x * t ; | |
| 55 | ✗ | ys = v_.y * t ; | |
| 56 | ✗ | zs = v_.z * t ; | |
| 57 | |||
| 58 | ✗ | wx = s_ * xs ; | |
| 59 | ✗ | wy = s_ * ys ; | |
| 60 | ✗ | wz = s_ * zs ; | |
| 61 | |||
| 62 | ✗ | xx = v_.x * xs ; | |
| 63 | ✗ | xy = v_.x * ys ; | |
| 64 | ✗ | xz = v_.x * zs ; | |
| 65 | |||
| 66 | ✗ | yy = v_.y * ys ; | |
| 67 | ✗ | yz = v_.y * zs ; | |
| 68 | ✗ | zz = v_.z * zs ; | |
| 69 | |||
| 70 | ✗ | mat4 matrix ; | |
| 71 | ✗ | matrix(0,0) = 1.0 - (yy+zz) ; | |
| 72 | ✗ | matrix(1,0) = xy + wz ; | |
| 73 | ✗ | matrix(2,0) = xz - wy ; | |
| 74 | ✗ | matrix(0,1) = xy - wz ; | |
| 75 | ✗ | matrix(1,1) = 1.0 - (xx+zz) ; | |
| 76 | ✗ | matrix(2,1) = yz+wx ; | |
| 77 | ✗ | matrix(0,2) = xz + wy ; | |
| 78 | ✗ | matrix(1,2) = yz - wx ; | |
| 79 | ✗ | matrix(2,2) = 1.0 - (xx+yy) ; | |
| 80 | ✗ | return matrix; | |
| 81 | } | ||
| 82 | |||
| 83 | ✗ | vec3 Quaternion::axis() const { | |
| 84 | double scale; | ||
| 85 | ✗ | scale = ::sin( ::acos( s_ ) ); | |
| 86 | ✗ | if ( scale < SMALL && scale > -SMALL ) { | |
| 87 | ✗ | return vec3( 0.0, 0.0, 0.0 ); | |
| 88 | } else { | ||
| 89 | ✗ | return v_ / scale; | |
| 90 | } | ||
| 91 | } | ||
| 92 | |||
| 93 | ✗ | Quaternion Quaternion::spherical_interpolation( | |
| 94 | const Quaternion& from, const Quaternion& to, | ||
| 95 | double t | ||
| 96 | ) { | ||
| 97 | ✗ | Quaternion to1; | |
| 98 | |||
| 99 | |||
| 100 | // calculate cosine | ||
| 101 | ✗ | double cosom = dot(from.v(),to.v()) + from.s() + to.s(); | |
| 102 | |||
| 103 | // Adjust signs (if necessary) | ||
| 104 | ✗ | if ( cosom < 0.0 ) { | |
| 105 | ✗ | cosom = -cosom; | |
| 106 | ✗ | to1 = -to; | |
| 107 | } else { | ||
| 108 | ✗ | to1 = to; | |
| 109 | } | ||
| 110 | |||
| 111 | double scale0, scale1; | ||
| 112 | |||
| 113 | // Calculate coefficients | ||
| 114 | ✗ | if ((1.0 - cosom) > SMALL ) { | |
| 115 | // standard case (slerp) | ||
| 116 | ✗ | double omega = acos( cosom ); | |
| 117 | ✗ | double sinom = sin( omega ); | |
| 118 | ✗ | scale0 = sin((1.0 - t) * omega) / sinom; | |
| 119 | ✗ | scale1 = sin(t * omega) / sinom; | |
| 120 | } else { | ||
| 121 | // 'from' and 'to' are very close - just do linear interpolation | ||
| 122 | ✗ | scale0 = 1.0 - t; | |
| 123 | ✗ | scale1 = t; | |
| 124 | } | ||
| 125 | ✗ | return scale0 * from + scale1 * to1; | |
| 126 | } | ||
| 127 | } | ||
| 128 |