diff options
Diffstat (limited to 'src/foundation/Qt3DSMathUtils.cpp')
| -rw-r--r-- | src/foundation/Qt3DSMathUtils.cpp | 181 |
1 files changed, 181 insertions, 0 deletions
diff --git a/src/foundation/Qt3DSMathUtils.cpp b/src/foundation/Qt3DSMathUtils.cpp new file mode 100644 index 0000000..7eee740 --- /dev/null +++ b/src/foundation/Qt3DSMathUtils.cpp @@ -0,0 +1,181 @@ +/**************************************************************************** +** +** Copyright (C) 2008-2012 NVIDIA Corporation. +** Copyright (C) 2017 The Qt Company Ltd. +** Contact: https://www.qt.io/licensing/ +** +** This file is part of Qt 3D Studio. +** +** $QT_BEGIN_LICENSE:GPL$ +** Commercial License Usage +** Licensees holding valid commercial Qt licenses may use this file in +** accordance with the commercial license agreement provided with the +** Software or, alternatively, in accordance with the terms contained in +** a written agreement between you and The Qt Company. For licensing terms +** and conditions see https://www.qt.io/terms-conditions. For further +** information use the contact form at https://www.qt.io/contact-us. +** +** GNU General Public License Usage +** Alternatively, this file may be used under the terms of the GNU +** General Public License version 3 or (at your option) any later version +** approved by the KDE Free Qt Foundation. The licenses are as published by +** the Free Software Foundation and appearing in the file LICENSE.GPL3 +** included in the packaging of this file. Please review the following +** information to ensure the GNU General Public License requirements will +** be met: https://www.gnu.org/licenses/gpl-3.0.html. +** +** $QT_END_LICENSE$ +** +****************************************************************************/ + +#include "foundation/Qt3DSMathUtils.h" +#include "foundation/Qt3DSUtilities.h" +#include "foundation/Qt3DSMat33.h" + +using namespace qt3ds; +using namespace qt3ds::foundation; +using namespace qt3ds::intrinsics; + +QT3DSQuat qt3ds::foundation::computeQuatFromNormal(const QT3DSVec3 &n) +{ + // parallel or anti-parallel + if (n.x > 0.9999f) { + // parallel + return QT3DSQuat::createIdentity(); + } else if (n.x < -0.9999f) { + // anti-parallel + // contactQuaternion.fromAngleAxisFast(PXD_PI, Vector3(0.0f, 1.0f, 0.0f)); + return QT3DSQuat(0.0f, 1.0f, 0.0f, 0.0f); + } else { + QT3DSVec3 rotVec(0.0f, -n.z, n.y); + + // Convert to quat + NVReal angle = rotVec.magnitude(); + rotVec *= 1.0f / angle; + // if(angle > 1.0f) angle = 1.0f; + angle = selectMin(angle, 1.0f); + + // djs: injudiciously imbecilic use of trig functions, good thing Adam is going to trample + // this path like a + // frustrated rhinoceros in mating season + + angle = NVAsin(angle); + + // if(n.x < 0) + // angle = NVPi - angle; + angle = fsel(n.x, angle, NVPi - angle); + + return QT3DSQuat(angle, rotVec); + } +} + +/** +\brief computes a oriented bounding box around the scaled basis. +\param basis Input = skewed basis, Output = (normalized) orthogonal basis. +\return Bounding box extent. +*/ +QT3DSVec3 qt3ds::foundation::optimizeBoundingBox(QT3DSMat33 &basis) +{ + QT3DSVec3 *QT3DS_RESTRICT vec = &basis[0]; // PT: don't copy vectors if not needed... + + // PT: since we store the magnitudes to memory, we can avoid the FCMNV afterwards + QT3DSVec3 magnitude(vec[0].magnitudeSquared(), vec[1].magnitudeSquared(), + vec[2].magnitudeSquared()); + +// find indices sorted by magnitude +#ifdef QT3DS_X360 + int i = (QT3DSU32 &)(magnitude[1]) > (QT3DSU32 &)(magnitude[0]) ? 1 : 0; + int j = (QT3DSU32 &)(magnitude[2]) > (QT3DSU32 &)(magnitude[1 - i]) ? 2 : 1 - i; +#else + int i = magnitude[1] > magnitude[0] ? 1 : 0; + int j = magnitude[2] > magnitude[1 - i] ? 2 : 1 - i; +#endif + const int k = 3 - i - j; +#ifdef QT3DS_X360 + if ((QT3DSU32 &)(magnitude[i]) < (QT3DSU32 &)(magnitude[j])) +#else + if (magnitude[i] < magnitude[j]) +#endif + swap(i, j); + + // ortho-normalize basis + + NVReal invSqrt = NVRecipSqrt(magnitude[i]); + magnitude[i] *= invSqrt; + vec[i] *= invSqrt; // normalize the first axis + NVReal dotij = vec[i].dot(vec[j]); + NVReal dotik = vec[i].dot(vec[k]); + magnitude[i] += NVAbs(dotij) + NVAbs(dotik); // elongate the axis by projection of the other two + vec[j] -= vec[i] * dotij; // orthogonize the two remaining axii relative to vec[i] + vec[k] -= vec[i] * dotik; + + magnitude[j] = vec[j].normalize(); + NVReal dotjk = vec[j].dot(vec[k]); + magnitude[j] += NVAbs(dotjk); // elongate the axis by projection of the other one + vec[k] -= vec[j] * dotjk; // orthogonize vec[k] relative to vec[j] + + magnitude[k] = vec[k].normalize(); + + return magnitude; +} + +QT3DSQuat qt3ds::foundation::slerp(const NVReal t, const QT3DSQuat &left, const QT3DSQuat &right) +{ + const NVReal quatEpsilon = (NVReal(1.0e-8f)); + + NVReal cosine = left.dot(right); + NVReal sign = NVReal(1); + if (cosine < 0) { + cosine = -cosine; + sign = NVReal(-1); + } + + NVReal sine = NVReal(1) - cosine * cosine; + + if (sine >= quatEpsilon * quatEpsilon) { + sine = NVSqrt(sine); + const NVReal angle = NVAtan2(sine, cosine); + const NVReal i_sin_angle = NVReal(1) / sine; + + const NVReal leftw = NVSin(angle * (NVReal(1) - t)) * i_sin_angle; + const NVReal rightw = NVSin(angle * t) * i_sin_angle * sign; + + return left * leftw + right * rightw; + } + + return left; +} + +void qt3ds::foundation::integrateTransform(const NVTransform &curTrans, const QT3DSVec3 &linvel, + const QT3DSVec3 &angvel, NVReal timeStep, NVTransform &result) +{ + result.p = curTrans.p + linvel * timeStep; + + // from void NVsDynamicsContext::integrateAtomPose(NVsRigidBody* atom, Cm::BitMap + // &shapeChangedMap) const: + // Integrate the rotation using closed form quaternion integrator + NVReal w = angvel.magnitudeSquared(); + + if (w != 0.0f) { + w = NVSqrt(w); + if (w != 0.0f) { + const NVReal v = timeStep * w * 0.5f; + const NVReal q = NVCos(v); + const NVReal s = NVSin(v) / w; + + const QT3DSVec3 pqr = angvel * s; + const QT3DSQuat quatVel(pqr.x, pqr.y, pqr.z, 0); + QT3DSQuat out; // need to have temporary, otherwise we may overwrite input if &curTrans == + // &result. + out = quatVel * curTrans.q; + out.x += curTrans.q.x * q; + out.y += curTrans.q.y * q; + out.z += curTrans.q.z * q; + out.w += curTrans.q.w * q; + result.q = out; + return; + } + } + // orientation stays the same - convert from quat to matrix: + result.q = curTrans.q; +} |