diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 44745c5c2d..34a3207f47 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1731,6 +1731,14 @@ glm::mat4 Rig::getJointTransform(int jointIndex) const { } } +AnimPose Rig::getJointPose(int jointIndex) const { + if (isIndexValid(jointIndex)) { + return _internalPoseSet._absolutePoses[jointIndex]; + } else { + return AnimPose::identity; + } +} + void Rig::copyJointsIntoJointData(QVector& jointDataVec) const { const AnimPose geometryToRigPose(_geometryToRigTransform); diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 1ec4d9527f..7d7b55f4a8 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -159,6 +159,7 @@ public: // rig space glm::mat4 getJointTransform(int jointIndex) const; + AnimPose getJointPose(int jointIndex) const; // Start or stop animations as needed. void computeMotionAnimationState(float deltaTime, const glm::vec3& worldPosition, const glm::vec3& worldVelocity, const glm::quat& worldRotation, CharacterControllerState ccState); diff --git a/libraries/render-utils/src/CauterizedModel.cpp b/libraries/render-utils/src/CauterizedModel.cpp index dbb82ab638..36b06b1035 100644 --- a/libraries/render-utils/src/CauterizedModel.cpp +++ b/libraries/render-utils/src/CauterizedModel.cpp @@ -9,6 +9,7 @@ #include "CauterizedModel.h" #include +#include #include "AbstractViewStateInterface.h" #include "MeshPartPayload.h" @@ -109,30 +110,78 @@ void CauterizedModel::updateClusterMatrices() { const FBXMesh& mesh = geometry.meshes.at(i); for (int j = 0; j < mesh.clusters.size(); j++) { const FBXCluster& cluster = mesh.clusters.at(j); + + /* AJT: TODO REMOVE auto jointMatrix = _rig.getJointTransform(cluster.jointIndex); glm_mat4u_mul(jointMatrix, cluster.inverseBindMatrix, state.clusterMatrices[j]); + */ + // AJT: TODO OPTOMIZE + AnimPose jointPose = _rig.getJointPose(cluster.jointIndex); + AnimPose result = jointPose * AnimPose(cluster.inverseBindMatrix); + + // pack scale rotation and translation into a mat4. + state.clusterMatrices[j][0].x = result.scale().x; + state.clusterMatrices[j][0].y = result.scale().y; + state.clusterMatrices[j][0].z = result.scale().z; + + DualQuaternion dq(result.rot(), result.trans()); + state.clusterMatrices[j][1].x = dq.real().x; + state.clusterMatrices[j][1].y = dq.real().y; + state.clusterMatrices[j][1].z = dq.real().z; + state.clusterMatrices[j][1].w = dq.real().w; + state.clusterMatrices[j][2].x = dq.imag().x; + state.clusterMatrices[j][2].y = dq.imag().y; + state.clusterMatrices[j][2].z = dq.imag().z; + state.clusterMatrices[j][2].w = dq.imag().w; } } // as an optimization, don't build cautrizedClusterMatrices if the boneSet is empty. if (!_cauterizeBoneSet.empty()) { + static const glm::mat4 zeroScale( glm::vec4(0.0f, 0.0f, 0.0f, 0.0f), glm::vec4(0.0f, 0.0f, 0.0f, 0.0f), glm::vec4(0.0f, 0.0f, 0.0f, 0.0f), glm::vec4(0.0f, 0.0f, 0.0f, 1.0f)); auto cauterizeMatrix = _rig.getJointTransform(geometry.neckJointIndex) * zeroScale; + auto cauterizePose = AnimPose(cauterizeMatrix); for (int i = 0; i < _cauterizeMeshStates.size(); i++) { Model::MeshState& state = _cauterizeMeshStates[i]; const FBXMesh& mesh = geometry.meshes.at(i); for (int j = 0; j < mesh.clusters.size(); j++) { const FBXCluster& cluster = mesh.clusters.at(j); + + // AJT: TODO REMOVE: + /* auto jointMatrix = _rig.getJointTransform(cluster.jointIndex); if (_cauterizeBoneSet.find(cluster.jointIndex) != _cauterizeBoneSet.end()) { jointMatrix = cauterizeMatrix; } glm_mat4u_mul(jointMatrix, cluster.inverseBindMatrix, state.clusterMatrices[j]); + */ + + auto jointPose = _rig.getJointPose(cluster.jointIndex); + if (_cauterizeBoneSet.find(cluster.jointIndex) != _cauterizeBoneSet.end()) { + jointPose = cauterizePose; + } + AnimPose result = jointPose * AnimPose(cluster.inverseBindMatrix); + + // pack scale rotation and translation into a mat4. + state.clusterMatrices[j][0].x = result.scale().x; + state.clusterMatrices[j][0].y = result.scale().y; + state.clusterMatrices[j][0].z = result.scale().z; + + DualQuaternion dq(result.rot(), result.trans()); + state.clusterMatrices[j][1].x = dq.real().x; + state.clusterMatrices[j][1].y = dq.real().y; + state.clusterMatrices[j][1].z = dq.real().z; + state.clusterMatrices[j][1].w = dq.real().w; + state.clusterMatrices[j][2].x = dq.imag().x; + state.clusterMatrices[j][2].y = dq.imag().y; + state.clusterMatrices[j][2].z = dq.imag().z; + state.clusterMatrices[j][2].w = dq.imag().w; } } } diff --git a/libraries/render-utils/src/MeshPartPayload.cpp b/libraries/render-utils/src/MeshPartPayload.cpp index 2a59c7d3c5..eb3866df21 100644 --- a/libraries/render-utils/src/MeshPartPayload.cpp +++ b/libraries/render-utils/src/MeshPartPayload.cpp @@ -12,6 +12,7 @@ #include "MeshPartPayload.h" #include +#include #include "DeferredLightingEffect.h" @@ -330,7 +331,10 @@ ModelMeshPartPayload::ModelMeshPartPayload(ModelPointer model, int meshIndex, in updateTransform(transform, offsetTransform); Transform renderTransform = transform; if (state.clusterMatrices.size() == 1) { - renderTransform = transform.worldTransform(Transform(state.clusterMatrices[0])); + // convert form dual quaternion representation to a Transform. + glm::vec3 scale = glm::vec3(state.clusterMatrices[0][0]); + DualQuaternion dq(state.clusterMatrices[0][1], state.clusterMatrices[0][2]); + renderTransform = transform.worldTransform(Transform(dq.getRotation(), scale, dq.getTranslation())); } updateTransformForSkinnedMesh(renderTransform, transform); diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp index b91f4dd405..6ff86516fc 100644 --- a/libraries/render-utils/src/Model.cpp +++ b/libraries/render-utils/src/Model.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include "AbstractViewStateInterface.h" #include "MeshPartPayload.h" @@ -1176,8 +1177,32 @@ void Model::updateClusterMatrices() { const FBXMesh& mesh = geometry.meshes.at(i); for (int j = 0; j < mesh.clusters.size(); j++) { const FBXCluster& cluster = mesh.clusters.at(j); + + // AJT: TODO REMOVE + /* auto jointMatrix = _rig.getJointTransform(cluster.jointIndex); glm_mat4u_mul(jointMatrix, cluster.inverseBindMatrix, state.clusterMatrices[j]); + */ + AnimPose jointPose = _rig.getJointPose(cluster.jointIndex); + + // AJT: TODO: store inverseBindMatrix as an animpose + // AJT: TODO: optimize AnimPose::operator* + AnimPose result = jointPose * AnimPose(cluster.inverseBindMatrix); + + // pack scale rotation and translation into a mat4. + state.clusterMatrices[j][0].x = result.scale().x; + state.clusterMatrices[j][0].y = result.scale().y; + state.clusterMatrices[j][0].z = result.scale().z; + + DualQuaternion dq(result.rot(), result.trans()); + state.clusterMatrices[j][1].x = dq.real().x; + state.clusterMatrices[j][1].y = dq.real().y; + state.clusterMatrices[j][1].z = dq.real().z; + state.clusterMatrices[j][1].w = dq.real().w; + state.clusterMatrices[j][2].x = dq.imag().x; + state.clusterMatrices[j][2].y = dq.imag().y; + state.clusterMatrices[j][2].z = dq.imag().z; + state.clusterMatrices[j][2].w = dq.imag().w; } } diff --git a/libraries/render-utils/src/Skinning.slh b/libraries/render-utils/src/Skinning.slh index 2d1f010029..10dc4042b2 100644 --- a/libraries/render-utils/src/Skinning.slh +++ b/libraries/render-utils/src/Skinning.slh @@ -18,52 +18,108 @@ layout(std140) uniform skinClusterBuffer { mat4 clusterMatrices[MAX_CLUSTERS]; }; -void skinPosition(ivec4 skinClusterIndex, vec4 skinClusterWeight, vec4 inPosition, out vec4 skinnedPosition) { - vec4 newPosition = vec4(0.0, 0.0, 0.0, 0.0); +vec4 quatConj(vec4 v) { + return vec4(-v.x, -v.y, -v.z, v.w); +} +vec4 quatMul(vec4 q1, vec4 q2) { + return vec4( q1.x * q2.w + q1.y * q2.z - q1.z * q2.y + q1.w * q2.x, + -q1.x * q2.z + q1.y * q2.w + q1.z * q2.x + q1.w * q2.y, + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w + q1.w * q2.z, + -q1.x * q2.x - q1.y * q2.y - q1.z * q2.z + q1.w * q2.w); +} + +vec3 rotateByQuat(vec4 q, vec3 p) { + return vec3(quatMul(quatMul(q, vec4(p.x, p.y, p.z, 0.0)), quatConj(q))); +} + +void dqMul(vec4 lhsReal, vec4 lhsImag, vec4 rhsReal, vec4 rhsImag, out vec4 realOut, out vec4 imagOut) { + realOut = quatMul(lhsReal, rhsReal); + imagOut = quatMul(lhsReal, rhsImag) + quatMul(lhsImag, rhsReal); +} + +void blendClusters(ivec4 skinClusterIndex, vec4 skinClusterWeight, out vec3 scaleOut, out vec4 rotOut, out vec3 posOut) { + vec3 scale = vec3(0.0, 0.0, 0.0); + vec4 dqReal = vec4(0.0, 0.0, 0.0, 0.0); + vec4 dqImag = vec4(0.0, 0.0, 0.0, 0.0); for (int i = 0; i < INDICES_PER_VERTEX; i++) { mat4 clusterMatrix = clusterMatrices[(skinClusterIndex[i])]; float clusterWeight = skinClusterWeight[i]; - newPosition += clusterMatrix * inPosition * clusterWeight; + + vec3 s = vec3(clusterMatrix[0][0], clusterMatrix[0][1], clusterMatrix[0][2]); + vec4 dqr = vec4(clusterMatrix[1][0], clusterMatrix[1][1], clusterMatrix[1][2], clusterMatrix[1][3]); + vec4 dqi = vec4(clusterMatrix[2][0], clusterMatrix[2][1], clusterMatrix[2][2], clusterMatrix[2][3]); + + scale += s * clusterWeight; + dqReal += dqr * clusterWeight; + dqImag += dqi * clusterWeight; } - skinnedPosition = newPosition; + scaleOut = scale; + + float dqLen = length(dqReal); + dqReal *= 1.0 / dqLen; + dqImag *= 1.0 / dqLen; + + rotOut = dqReal; + + vec4 invReal = quatConj(dqReal); + posOut.xyz = 2.0 * quatMul(dqImag, invReal).xyz; +} + +void blendClusters_rigid(ivec4 skinClusterIndex, vec4 skinClusterWeight, out vec3 scaleOut, out vec4 rotOut, out vec3 posOut) { + float maxWeight = 0.0; + for (int i = 0; i < INDICES_PER_VERTEX; i++) { + mat4 clusterMatrix = clusterMatrices[(skinClusterIndex[i])]; + float clusterWeight = skinClusterWeight[i]; + + vec3 s = vec3(clusterMatrix[0][0], clusterMatrix[0][1], clusterMatrix[0][2]); + vec4 dqr = vec4(clusterMatrix[1][0], clusterMatrix[1][1], clusterMatrix[1][2], clusterMatrix[1][3]); + vec4 dqi = vec4(clusterMatrix[2][0], clusterMatrix[2][1], clusterMatrix[2][2], clusterMatrix[2][3]); + + if (clusterWeight > maxWeight) { + maxWeight = clusterWeight; + scaleOut = s; + rotOut = dqr; + vec4 invReal = quatConj(dqr); + posOut = 2.0 * quatMul(dqi, invReal).xyz; + } + } +} + +void skinPosition(ivec4 skinClusterIndex, vec4 skinClusterWeight, vec4 inPosition, out vec4 skinnedPosition) { + + vec3 scale, pos; + vec4 rot; + blendClusters(skinClusterIndex, skinClusterWeight, scale, rot, pos); + + skinnedPosition.xyz = rotateByQuat(rot, (vec3(inPosition) * scale)) + pos; + skinnedPosition.w = 1; } void skinPositionNormal(ivec4 skinClusterIndex, vec4 skinClusterWeight, vec4 inPosition, vec3 inNormal, out vec4 skinnedPosition, out vec3 skinnedNormal) { - vec4 newPosition = vec4(0.0, 0.0, 0.0, 0.0); - vec4 newNormal = vec4(0.0, 0.0, 0.0, 0.0); - for (int i = 0; i < INDICES_PER_VERTEX; i++) { - mat4 clusterMatrix = clusterMatrices[(skinClusterIndex[i])]; - float clusterWeight = skinClusterWeight[i]; - newPosition += clusterMatrix * inPosition * clusterWeight; - newNormal += clusterMatrix * vec4(inNormal.xyz, 0.0) * clusterWeight; - } + vec3 scale, pos; + vec4 rot; + blendClusters(skinClusterIndex, skinClusterWeight, scale, rot, pos); - skinnedPosition = newPosition; - skinnedNormal = newNormal.xyz; + skinnedNormal.xyz = rotateByQuat(rot, inNormal * scale); } void skinPositionNormalTangent(ivec4 skinClusterIndex, vec4 skinClusterWeight, vec4 inPosition, vec3 inNormal, vec3 inTangent, out vec4 skinnedPosition, out vec3 skinnedNormal, out vec3 skinnedTangent) { - vec4 newPosition = vec4(0.0, 0.0, 0.0, 0.0); - vec4 newNormal = vec4(0.0, 0.0, 0.0, 0.0); - vec4 newTangent = vec4(0.0, 0.0, 0.0, 0.0); - for (int i = 0; i < INDICES_PER_VERTEX; i++) { - mat4 clusterMatrix = clusterMatrices[(skinClusterIndex[i])]; - float clusterWeight = skinClusterWeight[i]; - newPosition += clusterMatrix * inPosition * clusterWeight; - newNormal += clusterMatrix * vec4(inNormal.xyz, 0.0) * clusterWeight; - newTangent += clusterMatrix * vec4(inTangent.xyz, 0.0) * clusterWeight; - } + vec3 scale, pos; + vec4 rot; + blendClusters(skinClusterIndex, skinClusterWeight, scale, rot, pos); - skinnedPosition = newPosition; - skinnedNormal = newNormal.xyz; - skinnedTangent = newTangent.xyz; + skinnedPosition.xyz = rotateByQuat(rot, (vec3(inPosition) * scale)) + pos; + skinnedPosition.w = 1; + + skinnedNormal = rotateByQuat(rot, inNormal * scale); + skinnedTangent = rotateByQuat(rot, inTangent * scale); } -<@endif@> \ No newline at end of file +<@endif@> diff --git a/libraries/shared/src/DualQuaternion.cpp b/libraries/shared/src/DualQuaternion.cpp new file mode 100644 index 0000000000..3b5aaf6d6d --- /dev/null +++ b/libraries/shared/src/DualQuaternion.cpp @@ -0,0 +1,81 @@ +// +// DualQuaternion.cpp +// +// Created by Anthony J. Thibault on Dec 13th 2017. +// Copyright (c) 2017 High Fidelity, Inc. All rights reserved. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include "DualQuaternion.h" +#include "GLMHelpers.h" + +// delegating constructor +DualQuaternion::DualQuaternion(const glm::mat4& m) : DualQuaternion(glmExtractRotation(m), extractTranslation(m)) { +} + +DualQuaternion::DualQuaternion(const glm::quat& real, const glm::quat& imag) : _real(real), _imag(imag) { +} + +DualQuaternion::DualQuaternion(const glm::vec4& real, const glm::vec4& imag) : + _real(real.w, real.x, real.y, real.z), + _imag(imag.w, imag.x, imag.y, imag.z) { +} + +DualQuaternion::DualQuaternion(const glm::quat& rotation, const glm::vec3& translation) { + _real = rotation; + _imag = glm::quat(0, 0.5f * translation.x, 0.5f * translation.y, 0.5f * translation.z) * rotation; +} + +DualQuaternion DualQuaternion::operator*(const DualQuaternion& rhs) const { + return DualQuaternion(_real * rhs._real, _real * rhs._imag + _imag * rhs._real); +} + +DualQuaternion DualQuaternion::operator*(float scalar) const { + return DualQuaternion(_real * scalar, _imag * scalar); +} + +DualQuaternion DualQuaternion::operator+(const DualQuaternion& rhs) const { + return DualQuaternion(_real + rhs._real, _imag + rhs._imag); +} + +glm::vec3 DualQuaternion::xformPoint(const glm::vec3& rhs) const { + DualQuaternion v(glm::quat(), glm::quat(0.0f, rhs.x, rhs.y, rhs.z)); + DualQuaternion dualConj(glm::conjugate(_real), -glm::conjugate(_imag)); + DualQuaternion result = *this * v * dualConj; + return vec3(result._imag.x, result._imag.y, result._imag.z); +} + +glm::quat DualQuaternion::getRotation() const { + return _real; +} + +glm::vec3 DualQuaternion::getTranslation() const { + glm::quat result = 2.0f * (_imag * glm::inverse(_real)); + return glm::vec3(result.x, result.y, result.z); +} + + +glm::vec3 DualQuaternion::xformVector(const glm::vec3& rhs) const { + return _real * rhs; +} + +DualQuaternion DualQuaternion::inverse() const { + glm::quat invReal = glm::inverse(_real); + return DualQuaternion(invReal, - invReal * _imag * invReal); +} + +DualQuaternion DualQuaternion::conjugate() const { + return DualQuaternion(glm::conjugate(_real), glm::conjugate(_imag)); +} + +float DualQuaternion::length() const { + DualQuaternion result = *this * conjugate(); + return sqrtf(result._real.w); +} + +DualQuaternion DualQuaternion::normalize() const { + float invLen = 1.0f / length(); + return *this * invLen; +} diff --git a/libraries/shared/src/DualQuaternion.h b/libraries/shared/src/DualQuaternion.h new file mode 100644 index 0000000000..ed8cdf54ff --- /dev/null +++ b/libraries/shared/src/DualQuaternion.h @@ -0,0 +1,58 @@ +// +// DualQuaternion.h +// +// Created by Anthony J. Thibault on Dec 13th 2017. +// Copyright (c) 2017 High Fidelity, Inc. All rights reserved. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + + +#ifndef hifi_DualQuaternion +#define hifi_DualQuaternion + +#include +#include +#include +#include + +class DualQuaternion { +public: + explicit DualQuaternion(const glm::mat4& m); + DualQuaternion(const glm::quat& real, const glm::quat& imag); + DualQuaternion(const glm::quat& rotation, const glm::vec3& translation); + DualQuaternion(const glm::vec4& real, const glm::vec4& imag); + DualQuaternion operator*(const DualQuaternion& rhs) const; + DualQuaternion operator*(float scalar) const; + DualQuaternion operator+(const DualQuaternion& rhs) const; + + const glm::quat& real() const { return _real; } + glm::quat& real() { return _real; } + + const glm::quat& imag() const { return _imag; } + glm::quat& imag() { return _imag; } + + glm::quat getRotation() const; + glm::vec3 getTranslation() const; + + glm::vec3 xformPoint(const glm::vec3& rhs) const; + glm::vec3 xformVector(const glm::vec3& rhs) const; + + DualQuaternion inverse() const; + DualQuaternion conjugate() const; + float length() const; + DualQuaternion normalize() const; + +protected: + friend QDebug operator<<(QDebug debug, const DualQuaternion& pose); + glm::quat _real; + glm::quat _imag; +}; + +inline QDebug operator<<(QDebug debug, const DualQuaternion& dq) { + debug << "AnimPose, real = (" << dq._real.x << dq._real.y << dq._real.z << dq._real.w << "), imag = (" << dq._imag.x << dq._imag.y << dq._imag.z << dq._imag.w << ")"; + return debug; +} + +#endif diff --git a/tests/shared/src/DualQuaternionTests.cpp b/tests/shared/src/DualQuaternionTests.cpp new file mode 100644 index 0000000000..8187ed78bd --- /dev/null +++ b/tests/shared/src/DualQuaternionTests.cpp @@ -0,0 +1,138 @@ +// +// DualQuaternionTests.cpp +// tests/shared/src +// +// Copyright 2017 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include + +#include "DualQuaternionTests.h" + +#include +#include +#include +#include + +#include <../GLMTestUtils.h> +#include <../QTestExtensions.h> + +QTEST_MAIN(DualQuaternionTests) + +static void quatComp(const glm::quat& q1, const glm::quat& q2) { + QCOMPARE_WITH_ABS_ERROR(q1.x, q2.x, EPSILON); + QCOMPARE_WITH_ABS_ERROR(q1.y, q2.y, EPSILON); + QCOMPARE_WITH_ABS_ERROR(q1.z, q2.z, EPSILON); + QCOMPARE_WITH_ABS_ERROR(q1.w, q2.w, EPSILON); +} + +void DualQuaternionTests::ctor() { + glm::quat real = angleAxis(PI / 2.0f, Vectors::UNIT_Y); + glm::quat imag(0.0f, 1.0f, 2.0f, 3.0f); + + DualQuaternion dq(real, imag); + quatComp(real, dq.real()); + quatComp(imag, dq.imag()); + + glm::quat rotation = angleAxis(PI / 3.0f, Vectors::UNIT_X); + glm::vec3 translation(1.0, 2.0f, 3.0f); + dq = DualQuaternion(rotation, translation); + quatComp(rotation, dq.getRotation()); + QCOMPARE_WITH_ABS_ERROR(translation, dq.getTranslation(), EPSILON); + + rotation = angleAxis(-2.0f * PI / 7.0f, Vectors::UNIT_Z); + translation = glm::vec3(-1.0, 12.0f, 2.0f); + glm::mat4 m = createMatFromQuatAndPos(rotation, translation); + dq = DualQuaternion(m); + quatComp(rotation, dq.getRotation()); + QCOMPARE_WITH_ABS_ERROR(translation, dq.getTranslation(), EPSILON); +} + +void DualQuaternionTests::mult() { + + glm::quat rotation = angleAxis(PI / 3.0f, Vectors::UNIT_X); + glm::vec3 translation(1.0, 2.0f, 3.0f); + glm::mat4 m1 = createMatFromQuatAndPos(rotation, translation); + DualQuaternion dq1(m1); + + rotation = angleAxis(-2.0f * PI / 7.0f, Vectors::UNIT_Z); + translation = glm::vec3(-1.0, 12.0f, 2.0f); + glm::mat4 m2 = createMatFromQuatAndPos(rotation, translation); + DualQuaternion dq2(m2); + + DualQuaternion dq3 = dq1 * dq2; + glm::mat4 m3 = m1 * m2; + + rotation = glmExtractRotation(m3); + translation = extractTranslation(m3); + + quatComp(rotation, dq3.getRotation()); + QCOMPARE_WITH_ABS_ERROR(translation, dq3.getTranslation(), EPSILON); +} + +void DualQuaternionTests::xform() { + + glm::quat rotation = angleAxis(PI / 3.0f, Vectors::UNIT_X); + glm::vec3 translation(1.0, 2.0f, 3.0f); + glm::mat4 m1 = createMatFromQuatAndPos(rotation, translation); + DualQuaternion dq1(m1); + + rotation = angleAxis(-2.0f * PI / 7.0f, Vectors::UNIT_Z); + translation = glm::vec3(-1.0, 12.0f, 2.0f); + glm::mat4 m2 = createMatFromQuatAndPos(rotation, translation); + DualQuaternion dq2(m2); + + DualQuaternion dq3 = dq1 * dq2; + glm::mat4 m3 = m1 * m2; + + glm::vec3 p(1.0f, 2.0f, 3.0f); + + glm::vec3 p1 = transformPoint(m3, p); + glm::vec3 p2 = dq3.xformPoint(p); + + QCOMPARE_WITH_ABS_ERROR(p1, p2, 0.001f); + + p1 = transformVectorFast(m3, p); + p2 = dq3.xformVector(p); + + QCOMPARE_WITH_ABS_ERROR(p1, p2, 0.001f); +} + +void DualQuaternionTests::trans() { + glm::vec3 t1 = glm::vec3(); + DualQuaternion dq1(Quaternions::IDENTITY, t1); + glm::vec3 t2 = glm::vec3(1.0f, 2.0f, 3.0f); + DualQuaternion dq2(angleAxis(PI / 3.0f, Vectors::UNIT_X), t2); + glm::vec3 t3 = glm::vec3(3.0f, 2.0f, 1.0f); + DualQuaternion dq3(angleAxis(PI / 5.0f, Vectors::UNIT_Y), t3); + + QCOMPARE_WITH_ABS_ERROR(t1, dq1.getTranslation(), 0.001f); + QCOMPARE_WITH_ABS_ERROR(t2, dq2.getTranslation(), 0.001f); + QCOMPARE_WITH_ABS_ERROR(t3, dq3.getTranslation(), 0.001f); +} + +// Dual Quaternion Linear Blending test +void DualQuaternionTests::dlb() { + DualQuaternion dq1(Quaternions::IDENTITY, glm::vec3()); + DualQuaternion dq2(angleAxis(PI / 2.0f, Vectors::UNIT_X), glm::vec3(0.0f, 1.0f, 0.0f)); + + qDebug() << "dq1 =" << dq1; + qDebug() << "dq1.length = " << dq1.length(); + qDebug() << "dq2 =" << dq2; + qDebug() << "dq2.length = " << dq2.length(); + + // linear blend between dq1 and dq2 + DualQuaternion dq3 = dq1 * 0.5f + dq2 * 0.5f; + DualQuaternion dq4 = dq3.normalize(); + + qDebug() << "dq3 =" << dq3; + qDebug() << "dq3.length = " << dq3.length(); + qDebug() << "dq3.trans =" << dq3.getTranslation(); + + qDebug() << "dq4 =" << dq4; + qDebug() << "dq4.length = " << dq4.length(); + qDebug() << "dq4.trans =" << dq4.getTranslation(); +} diff --git a/tests/shared/src/DualQuaternionTests.h b/tests/shared/src/DualQuaternionTests.h new file mode 100644 index 0000000000..988973b689 --- /dev/null +++ b/tests/shared/src/DualQuaternionTests.h @@ -0,0 +1,26 @@ +// +// DualQuaternionTests.h +// tests/shared/src +// +// Copyright 2017 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_DualQuaternionTests_h +#define hifi_DualQuaternionTests_h + +#include + +class DualQuaternionTests : public QObject { + Q_OBJECT +private slots: + void ctor(); + void mult(); + void xform(); + void trans(); + void dlb(); +}; + +#endif // hifi_DualQuaternionTests_h