WIP first attempt at dual quat skinning

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);

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);

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;
             }
         }
     }

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);
 

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;
         }
     }
 

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@>
+<@endif@>

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;
+}

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

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();
+}

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