1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/include/miniassimp/quaternion.inl	Mon Jan 28 18:19:26 2019 +0200
     1.3 @@ -0,0 +1,286 @@
     1.4 +/*
     1.5 +---------------------------------------------------------------------------
     1.6 +Open Asset Import Library (assimp)
     1.7 +---------------------------------------------------------------------------
     1.8 +
     1.9 +Copyright (c) 2006-2018, assimp team
    1.10 +
    1.11 +
    1.12 +
    1.13 +All rights reserved.
    1.14 +
    1.15 +Redistribution and use of this software in source and binary forms,
    1.16 +with or without modification, are permitted provided that the following
    1.17 +conditions are met:
    1.18 +
    1.19 +* Redistributions of source code must retain the above
    1.20 +  copyright notice, this list of conditions and the
    1.21 +  following disclaimer.
    1.22 +
    1.23 +* Redistributions in binary form must reproduce the above
    1.24 +  copyright notice, this list of conditions and the
    1.25 +  following disclaimer in the documentation and/or other
    1.26 +  materials provided with the distribution.
    1.27 +
    1.28 +* Neither the name of the assimp team, nor the names of its
    1.29 +  contributors may be used to endorse or promote products
    1.30 +  derived from this software without specific prior
    1.31 +  written permission of the assimp team.
    1.32 +
    1.33 +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    1.34 +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    1.35 +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
    1.36 +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
    1.37 +OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
    1.38 +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
    1.39 +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
    1.40 +DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
    1.41 +THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
    1.42 +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
    1.43 +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
    1.44 +---------------------------------------------------------------------------
    1.45 +*/
    1.46 +
    1.47 +/** @file  quaternion.inl
    1.48 + *  @brief Inline implementation of aiQuaterniont<TReal> operators
    1.49 + */
    1.50 +#pragma once
    1.51 +#ifndef AI_QUATERNION_INL_INC
    1.52 +#define AI_QUATERNION_INL_INC
    1.53 +
    1.54 +#ifdef __cplusplus
    1.55 +#include "quaternion.h"
    1.56 +
    1.57 +#include <cmath>
    1.58 +
    1.59 +// ---------------------------------------------------------------------------
    1.60 +template<typename TReal>
    1.61 +bool aiQuaterniont<TReal>::operator== (const aiQuaterniont& o) const
    1.62 +{
    1.63 +    return x == o.x && y == o.y && z == o.z && w == o.w;
    1.64 +}
    1.65 +
    1.66 +// ---------------------------------------------------------------------------
    1.67 +template<typename TReal>
    1.68 +bool aiQuaterniont<TReal>::operator!= (const aiQuaterniont& o) const
    1.69 +{
    1.70 +    return !(*this == o);
    1.71 +}
    1.72 +
    1.73 +// ---------------------------------------------------------------------------
    1.74 +template<typename TReal>
    1.75 +inline bool aiQuaterniont<TReal>::Equal(const aiQuaterniont& o, TReal epsilon) const {
    1.76 +    return
    1.77 +        std::abs(x - o.x) <= epsilon &&
    1.78 +        std::abs(y - o.y) <= epsilon &&
    1.79 +        std::abs(z - o.z) <= epsilon &&
    1.80 +        std::abs(w - o.w) <= epsilon;
    1.81 +}
    1.82 +
    1.83 +// ---------------------------------------------------------------------------
    1.84 +// Constructs a quaternion from a rotation matrix
    1.85 +template<typename TReal>
    1.86 +inline aiQuaterniont<TReal>::aiQuaterniont( const aiMatrix3x3t<TReal> &pRotMatrix)
    1.87 +{
    1.88 +    TReal t = pRotMatrix.a1 + pRotMatrix.b2 + pRotMatrix.c3;
    1.89 +
    1.90 +    // large enough
    1.91 +    if( t > static_cast<TReal>(0))
    1.92 +    {
    1.93 +        TReal s = std::sqrt(1 + t) * static_cast<TReal>(2.0);
    1.94 +        x = (pRotMatrix.c2 - pRotMatrix.b3) / s;
    1.95 +        y = (pRotMatrix.a3 - pRotMatrix.c1) / s;
    1.96 +        z = (pRotMatrix.b1 - pRotMatrix.a2) / s;
    1.97 +        w = static_cast<TReal>(0.25) * s;
    1.98 +    } // else we have to check several cases
    1.99 +    else if( pRotMatrix.a1 > pRotMatrix.b2 && pRotMatrix.a1 > pRotMatrix.c3 )
   1.100 +    {
   1.101 +        // Column 0:
   1.102 +        TReal s = std::sqrt( static_cast<TReal>(1.0) + pRotMatrix.a1 - pRotMatrix.b2 - pRotMatrix.c3) * static_cast<TReal>(2.0);
   1.103 +        x = static_cast<TReal>(0.25) * s;
   1.104 +        y = (pRotMatrix.b1 + pRotMatrix.a2) / s;
   1.105 +        z = (pRotMatrix.a3 + pRotMatrix.c1) / s;
   1.106 +        w = (pRotMatrix.c2 - pRotMatrix.b3) / s;
   1.107 +    }
   1.108 +    else if( pRotMatrix.b2 > pRotMatrix.c3)
   1.109 +    {
   1.110 +        // Column 1:
   1.111 +        TReal s = std::sqrt( static_cast<TReal>(1.0) + pRotMatrix.b2 - pRotMatrix.a1 - pRotMatrix.c3) * static_cast<TReal>(2.0);
   1.112 +        x = (pRotMatrix.b1 + pRotMatrix.a2) / s;
   1.113 +        y = static_cast<TReal>(0.25) * s;
   1.114 +        z = (pRotMatrix.c2 + pRotMatrix.b3) / s;
   1.115 +        w = (pRotMatrix.a3 - pRotMatrix.c1) / s;
   1.116 +    } else
   1.117 +    {
   1.118 +        // Column 2:
   1.119 +        TReal s = std::sqrt( static_cast<TReal>(1.0) + pRotMatrix.c3 - pRotMatrix.a1 - pRotMatrix.b2) * static_cast<TReal>(2.0);
   1.120 +        x = (pRotMatrix.a3 + pRotMatrix.c1) / s;
   1.121 +        y = (pRotMatrix.c2 + pRotMatrix.b3) / s;
   1.122 +        z = static_cast<TReal>(0.25) * s;
   1.123 +        w = (pRotMatrix.b1 - pRotMatrix.a2) / s;
   1.124 +    }
   1.125 +}
   1.126 +
   1.127 +// ---------------------------------------------------------------------------
   1.128 +// Construction from euler angles
   1.129 +template<typename TReal>
   1.130 +inline aiQuaterniont<TReal>::aiQuaterniont( TReal fPitch, TReal fYaw, TReal fRoll )
   1.131 +{
   1.132 +    const TReal fSinPitch(std::sin(fPitch*static_cast<TReal>(0.5)));
   1.133 +    const TReal fCosPitch(std::cos(fPitch*static_cast<TReal>(0.5)));
   1.134 +    const TReal fSinYaw(std::sin(fYaw*static_cast<TReal>(0.5)));
   1.135 +    const TReal fCosYaw(std::cos(fYaw*static_cast<TReal>(0.5)));
   1.136 +    const TReal fSinRoll(std::sin(fRoll*static_cast<TReal>(0.5)));
   1.137 +    const TReal fCosRoll(std::cos(fRoll*static_cast<TReal>(0.5)));
   1.138 +    const TReal fCosPitchCosYaw(fCosPitch*fCosYaw);
   1.139 +    const TReal fSinPitchSinYaw(fSinPitch*fSinYaw);
   1.140 +    x = fSinRoll * fCosPitchCosYaw     - fCosRoll * fSinPitchSinYaw;
   1.141 +    y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw;
   1.142 +    z = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw;
   1.143 +    w = fCosRoll * fCosPitchCosYaw     + fSinRoll * fSinPitchSinYaw;
   1.144 +}
   1.145 +
   1.146 +// ---------------------------------------------------------------------------
   1.147 +// Returns a matrix representation of the quaternion
   1.148 +template<typename TReal>
   1.149 +inline aiMatrix3x3t<TReal> aiQuaterniont<TReal>::GetMatrix() const
   1.150 +{
   1.151 +    aiMatrix3x3t<TReal> resMatrix;
   1.152 +    resMatrix.a1 = static_cast<TReal>(1.0) - static_cast<TReal>(2.0) * (y * y + z * z);
   1.153 +    resMatrix.a2 = static_cast<TReal>(2.0) * (x * y - z * w);
   1.154 +    resMatrix.a3 = static_cast<TReal>(2.0) * (x * z + y * w);
   1.155 +    resMatrix.b1 = static_cast<TReal>(2.0) * (x * y + z * w);
   1.156 +    resMatrix.b2 = static_cast<TReal>(1.0) - static_cast<TReal>(2.0) * (x * x + z * z);
   1.157 +    resMatrix.b3 = static_cast<TReal>(2.0) * (y * z - x * w);
   1.158 +    resMatrix.c1 = static_cast<TReal>(2.0) * (x * z - y * w);
   1.159 +    resMatrix.c2 = static_cast<TReal>(2.0) * (y * z + x * w);
   1.160 +    resMatrix.c3 = static_cast<TReal>(1.0) - static_cast<TReal>(2.0) * (x * x + y * y);
   1.161 +
   1.162 +    return resMatrix;
   1.163 +}
   1.164 +
   1.165 +// ---------------------------------------------------------------------------
   1.166 +// Construction from an axis-angle pair
   1.167 +template<typename TReal>
   1.168 +inline aiQuaterniont<TReal>::aiQuaterniont( aiVector3t<TReal> axis, TReal angle)
   1.169 +{
   1.170 +    axis.Normalize();
   1.171 +
   1.172 +    const TReal sin_a = std::sin( angle / 2 );
   1.173 +    const TReal cos_a = std::cos( angle / 2 );
   1.174 +    x    = axis.x * sin_a;
   1.175 +    y    = axis.y * sin_a;
   1.176 +    z    = axis.z * sin_a;
   1.177 +    w    = cos_a;
   1.178 +}
   1.179 +// ---------------------------------------------------------------------------
   1.180 +// Construction from am existing, normalized quaternion
   1.181 +template<typename TReal>
   1.182 +inline aiQuaterniont<TReal>::aiQuaterniont( aiVector3t<TReal> normalized)
   1.183 +{
   1.184 +    x = normalized.x;
   1.185 +    y = normalized.y;
   1.186 +    z = normalized.z;
   1.187 +
   1.188 +    const TReal t = static_cast<TReal>(1.0) - (x*x) - (y*y) - (z*z);
   1.189 +
   1.190 +    if (t < static_cast<TReal>(0.0)) {
   1.191 +        w = static_cast<TReal>(0.0);
   1.192 +    }
   1.193 +    else w = std::sqrt (t);
   1.194 +}
   1.195 +
   1.196 +// ---------------------------------------------------------------------------
   1.197 +// Performs a spherical interpolation between two quaternions
   1.198 +// Implementation adopted from the gmtl project. All others I found on the net fail in some cases.
   1.199 +// Congrats, gmtl!
   1.200 +template<typename TReal>
   1.201 +inline void aiQuaterniont<TReal>::Interpolate( aiQuaterniont& pOut, const aiQuaterniont& pStart, const aiQuaterniont& pEnd, TReal pFactor)
   1.202 +{
   1.203 +    // calc cosine theta
   1.204 +    TReal cosom = pStart.x * pEnd.x + pStart.y * pEnd.y + pStart.z * pEnd.z + pStart.w * pEnd.w;
   1.205 +
   1.206 +    // adjust signs (if necessary)
   1.207 +    aiQuaterniont end = pEnd;
   1.208 +    if( cosom < static_cast<TReal>(0.0))
   1.209 +    {
   1.210 +        cosom = -cosom;
   1.211 +        end.x = -end.x;   // Reverse all signs
   1.212 +        end.y = -end.y;
   1.213 +        end.z = -end.z;
   1.214 +        end.w = -end.w;
   1.215 +    }
   1.216 +
   1.217 +    // Calculate coefficients
   1.218 +    TReal sclp, sclq;
   1.219 +    if( (static_cast<TReal>(1.0) - cosom) > static_cast<TReal>(0.0001)) // 0.0001 -> some epsillon
   1.220 +    {
   1.221 +        // Standard case (slerp)
   1.222 +        TReal omega, sinom;
   1.223 +        omega = std::acos( cosom); // extract theta from dot product's cos theta
   1.224 +        sinom = std::sin( omega);
   1.225 +        sclp  = std::sin( (static_cast<TReal>(1.0) - pFactor) * omega) / sinom;
   1.226 +        sclq  = std::sin( pFactor * omega) / sinom;
   1.227 +    } else
   1.228 +    {
   1.229 +        // Very close, do linear interp (because it's faster)
   1.230 +        sclp = static_cast<TReal>(1.0) - pFactor;
   1.231 +        sclq = pFactor;
   1.232 +    }
   1.233 +
   1.234 +    pOut.x = sclp * pStart.x + sclq * end.x;
   1.235 +    pOut.y = sclp * pStart.y + sclq * end.y;
   1.236 +    pOut.z = sclp * pStart.z + sclq * end.z;
   1.237 +    pOut.w = sclp * pStart.w + sclq * end.w;
   1.238 +}
   1.239 +
   1.240 +// ---------------------------------------------------------------------------
   1.241 +template<typename TReal>
   1.242 +inline aiQuaterniont<TReal>& aiQuaterniont<TReal>::Normalize()
   1.243 +{
   1.244 +    // compute the magnitude and divide through it
   1.245 +    const TReal mag = std::sqrt(x*x + y*y + z*z + w*w);
   1.246 +    if (mag)
   1.247 +    {
   1.248 +        const TReal invMag = static_cast<TReal>(1.0)/mag;
   1.249 +        x *= invMag;
   1.250 +        y *= invMag;
   1.251 +        z *= invMag;
   1.252 +        w *= invMag;
   1.253 +    }
   1.254 +    return *this;
   1.255 +}
   1.256 +
   1.257 +// ---------------------------------------------------------------------------
   1.258 +template<typename TReal>
   1.259 +inline aiQuaterniont<TReal> aiQuaterniont<TReal>::operator* (const aiQuaterniont& t) const
   1.260 +{
   1.261 +    return aiQuaterniont(w*t.w - x*t.x - y*t.y - z*t.z,
   1.262 +        w*t.x + x*t.w + y*t.z - z*t.y,
   1.263 +        w*t.y + y*t.w + z*t.x - x*t.z,
   1.264 +        w*t.z + z*t.w + x*t.y - y*t.x);
   1.265 +}
   1.266 +
   1.267 +// ---------------------------------------------------------------------------
   1.268 +template<typename TReal>
   1.269 +inline aiQuaterniont<TReal>& aiQuaterniont<TReal>::Conjugate ()
   1.270 +{
   1.271 +    x = -x;
   1.272 +    y = -y;
   1.273 +    z = -z;
   1.274 +    return *this;
   1.275 +}
   1.276 +
   1.277 +// ---------------------------------------------------------------------------
   1.278 +template<typename TReal>
   1.279 +inline aiVector3t<TReal> aiQuaterniont<TReal>::Rotate (const aiVector3t<TReal>& v)
   1.280 +{
   1.281 +    aiQuaterniont q2(0.f,v.x,v.y,v.z), q = *this, qinv = q;
   1.282 +    qinv.Conjugate();
   1.283 +
   1.284 +    q = q*q2*qinv;
   1.285 +    return aiVector3t<TReal>(q.x,q.y,q.z);
   1.286 +}
   1.287 +
   1.288 +#endif
   1.289 +#endif // AI_QUATERNION_INL_INC