diff options
Diffstat (limited to 'src/Runtime/Source/platformspecific/windows/libs/nv_math/nv_quat.cpp')
| -rw-r--r-- | src/Runtime/Source/platformspecific/windows/libs/nv_math/nv_quat.cpp | 199 |
1 files changed, 199 insertions, 0 deletions
diff --git a/src/Runtime/Source/platformspecific/windows/libs/nv_math/nv_quat.cpp b/src/Runtime/Source/platformspecific/windows/libs/nv_math/nv_quat.cpp new file mode 100644 index 00000000..9626eb8c --- /dev/null +++ b/src/Runtime/Source/platformspecific/windows/libs/nv_math/nv_quat.cpp @@ -0,0 +1,199 @@ +/**************************************************************************** +** +** Copyright (C) 2009-2011 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 "nv_quat.h" +#include <stdlib.h> +#include <string.h> + +void NvQuatCopy(GLfloat r[4], const GLfloat q[4]) +{ + memcpy(r, q, 4 * sizeof(GLfloat)); +} + +void NvQuatConvertTo3x3Mat(GLfloat r[3][3], const GLfloat q[4]) +{ + // Assumes that the quaternion is normalized! + GLfloat x2 = q[0] * 2.0f; + GLfloat y2 = q[1] * 2.0f; + GLfloat z2 = q[2] * 2.0f; + GLfloat xSq2 = x2 * q[0]; + GLfloat ySq2 = y2 * q[1]; + GLfloat zSq2 = z2 * q[2]; + GLfloat xy2 = x2 * q[1]; + GLfloat xz2 = x2 * q[2]; + GLfloat xw2 = x2 * q[3]; + GLfloat yz2 = y2 * q[2]; + GLfloat yw2 = y2 * q[3]; + GLfloat zw2 = z2 * q[3]; + + /* Matrix is + * | 1 - 2y^2 - 2z^2 2xy - 2zw 2xz + 2yw | + * | 2xy + 2zw 1 - 2x^2 - 2z^2 2yz - 2xw | + * | 2xz - 2yw 2yz + 2xw 1 - 2x^2 - 2y^2 | + */ + r[0][0] = 1.0f - ySq2 - zSq2; + r[0][1] = xy2 - zw2; + r[0][2] = xz2 + yw2; + + r[1][0] = xy2 + zw2; + r[1][1] = 1.0f - xSq2 - zSq2; + r[1][2] = yz2 - xw2; + + r[2][0] = xz2 - yw2; + r[2][1] = yz2 + xw2; + r[2][2] = 1.0f - xSq2 - ySq2; +} + +void NvQuatIdentity(GLfloat r[4]) +{ + r[0] = 0.0f; + r[1] = 0.0f; + r[2] = 0.0f; + r[3] = 1.0f; +} + +void NvQuatFromAngleAxis(GLfloat r[4], GLfloat radians, const GLfloat axis[3]) +{ + GLfloat cosa = cosf(radians * 0.5f); + GLfloat sina = sinf(radians * 0.5f); + + r[0] = axis[0] * sina; + r[1] = axis[1] * sina; + r[2] = axis[2] * sina; + r[3] = cosa; +} + +void NvQuatX(GLfloat r[4], GLfloat radians) +{ + GLfloat cosa = cosf(radians * 0.5f); + GLfloat sina = sinf(radians * 0.5f); + + r[0] = sina; + r[1] = 0.0f; + r[2] = 0.0f; + r[3] = cosa; +} + +void NvQuatY(GLfloat r[4], GLfloat radians) +{ + GLfloat cosa = cosf(radians * 0.5f); + GLfloat sina = sinf(radians * 0.5f); + + r[0] = 0.0f; + r[1] = sina; + r[2] = 0.0f; + r[3] = cosa; +} + +void NvQuatZ(GLfloat r[4], GLfloat radians) +{ + GLfloat cosa = cosf(radians * 0.5f); + GLfloat sina = sinf(radians * 0.5f); + + r[0] = 0.0f; + r[1] = 0.0f; + r[2] = sina; + r[3] = cosa; +} + +void NvQuatFromEuler(GLfloat r[4], GLfloat heading, GLfloat pitch, GLfloat roll) +{ + GLfloat h[4], p[4], ro[4]; + + NvQuatY(h, heading); + NvQuatX(p, pitch); + NvQuatZ(ro, roll); + + NvQuatMult(r, h, p); + NvQuatMult(r, ro, r); +} + +void NvQuatFromEulerReverse(GLfloat r[4], GLfloat heading, GLfloat pitch, GLfloat roll) +{ + GLfloat h[4], p[4], ro[4]; + + NvQuatZ(ro, roll); + NvQuatX(p, pitch); + NvQuatY(h, heading); + + NvQuatMult(r, p, h); + NvQuatMult(r, r, ro); +} + +GLfloat NvQuatDot(const GLfloat q1[4], const GLfloat q2[4]) +{ + return q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3]; +} + +void NvQuatMult(GLfloat r[4], const GLfloat q1[4], const GLfloat q2[4]) +{ + const GLfloat q1x = q1[0]; + const GLfloat q1y = q1[1]; + const GLfloat q1z = q1[2]; + const GLfloat q1w = q1[3]; + const GLfloat q2x = q2[0]; + const GLfloat q2y = q2[1]; + const GLfloat q2z = q2[2]; + const GLfloat q2w = q2[3]; + + r[0] = q1y * q2z - q2y * q1z + q1w * q2x + q2w * q1x; + r[1] = q1z * q2x - q2z * q1x + q1w * q2y + q2w * q1y; + r[2] = q1x * q2y - q2x * q1y + q1w * q2z + q2w * q1z; + r[3] = q1w * q2w - q1x * q2x - q1y * q2y - q1z * q2z; +} + +void NvQuatNLerp(GLfloat r[4], const GLfloat q1[4], const GLfloat q2[4], GLfloat t) +{ + GLfloat omt = 1.0f - t; + + if (NvQuatDot(q1, q2) < 0.0f) { + r[0] = -q1[0] * omt + q2[0] * t; + r[1] = -q1[1] * omt + q2[1] * t; + r[2] = -q1[2] * omt + q2[2] * t; + r[3] = -q1[3] * omt + q2[3] * t; + } else { + r[0] = q1[0] * omt + q2[0] * t; + r[1] = q1[1] * omt + q2[1] * t; + r[2] = q1[2] * omt + q2[2] * t; + r[3] = q1[3] * omt + q2[3] * t; + } + + NvQuatNormalize(r, r); +} + +void NvQuatNormalize(GLfloat r[4], const GLfloat q[4]) +{ + GLfloat invLength = 1.0f / sqrtf(NvQuatDot(q, q)); + + r[0] = invLength * q[0]; + r[1] = invLength * q[1]; + r[2] = invLength * q[2]; + r[3] = invLength * q[3]; +} |