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<JointData>& 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 <PerfStat.h>
+#include <DualQuaternion.h>
#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 <PerfStat.h>
+#include <DualQuaternion.h>
#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 <ViewFrustum.h>
#include <GLMHelpers.h>
#include <model-networking/SimpleMeshProxy.h>
+#include <DualQuaternion.h>
#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 <QtGlobal>
+#include <QDebug>
+#include <glm/glm.hpp>
+#include <glm/gtc/quaternion.hpp>
+
+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 <iostream>
+
+#include "DualQuaternionTests.h"
+
+#include <DualQuaternion.h>
+#include <GLMHelpers.h>
+#include <NumericalConstants.h>
+#include <StreamUtils.h>
+
+#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 <QtTest/QtTest>
+
+class DualQuaternionTests : public QObject {
+ Q_OBJECT
+private slots:
+ void ctor();
+ void mult();
+ void xform();
+ void trans();
+ void dlb();
+};
+
+#endif // hifi_DualQuaternionTests_h