Skip to content

Commit

Permalink
DualQuaternion
Browse files Browse the repository at this point in the history
  • Loading branch information
@zhouhang95
zhouhang95 committed Aug 14, 2024
1 parent b615390 commit 6410369
Show file tree
Hide file tree
Showing 3 changed files with 228 additions and 11 deletions.
131 changes: 131 additions & 0 deletions projects/FBX/DualQuaternion.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,131 @@
#include "DualQuaternion.h"
#include <cmath>
#include <zeno/utils/bit_operations.h>

DualQuaternion operator+(const DualQuaternion& l, const DualQuaternion& r) {
return DualQuaternion(l.real + r.real, l.dual + r.dual);
}

DualQuaternion operator*(const DualQuaternion& dq, float f) {
return DualQuaternion(dq.real * f, dq.dual * f);
}

bool operator==(const DualQuaternion& l, const DualQuaternion& r) {
return l.real == r.real && l.dual == r.dual;
}

bool operator!=(const DualQuaternion& l, const DualQuaternion& r) {
return l.real != r.real || l.dual != r.dual;
}

// Remember, multiplication order is left to right.
// This is the opposite of matrix and quaternion multiplication order
DualQuaternion operator*(const DualQuaternion& l, const DualQuaternion& r) {
DualQuaternion lhs = normalized(l);
DualQuaternion rhs = normalized(r);
// DualQuaternion lhs = l;
// DualQuaternion rhs = r;

return DualQuaternion(lhs.real * rhs.real, lhs.real * rhs.dual + lhs.dual * rhs.real);
}

float dot(const DualQuaternion& l, const DualQuaternion& r) {
return dot(l.real, r.real);
}

DualQuaternion conjugate(const DualQuaternion& dq) {
return DualQuaternion(conjugate(dq.real), conjugate(dq.dual));
}

DualQuaternion normalized(const DualQuaternion& dq) {
float magSq = dot(dq.real, dq.real);
if (magSq < 0.000001f) {
return DualQuaternion();
}
float invMag = 1.0f / sqrtf(magSq);

return DualQuaternion(dq.real * invMag, dq.dual * invMag);
}

void normalize(DualQuaternion& dq) {
float magSq = dot(dq.real, dq.real);
if (magSq < 0.000001f) {
return;
}
float invMag = 1.0f / sqrtf(magSq);

dq.real = dq.real * invMag;
dq.dual = dq.dual * invMag;
}

static void decomposeMtx(const glm::mat4& m, glm::vec3& pos, glm::quat& rot, glm::vec3& scale)
{
pos = m[3];
for(int i = 0; i < 3; i++)
scale[i] = glm::length(glm::vec3(m[i]));
const glm::mat3 rotMtx(
glm::vec3(m[0]) / scale[0],
glm::vec3(m[1]) / scale[1],
glm::vec3(m[2]) / scale[2]);
rot = glm::quat_cast(rotMtx);
}

constexpr glm::quat dual_quat(const glm::quat& q,const glm::vec3& t) {

auto qx = q.x;
auto qy = q.y;
auto qz = q.z;
auto qw = q.w;
auto tx = t[0];
auto ty = t[1];
auto tz = t[2];

glm::quat qd;
qd.w = -0.5*( tx*qx + ty*qy + tz*qz); // qd.w
qd.x = 0.5*( tx*qw + ty*qz - tz*qy); // qd.x
qd.y = 0.5*(-tx*qz + ty*qw + tz*qx); // qd.y
qd.z = 0.5*( tx*qy - ty*qx + tz*qw); // qd.z

return qd;
}
DualQuaternion mat4ToDualQuat2(const glm::mat4& transformation) {
glm::vec3 scale;
glm::quat rotation;
glm::vec3 translation;
decomposeMtx(transformation, translation, rotation, scale);
glm::quat qr = rotation;
glm::quat qd = dual_quat(qr, translation);
return DualQuaternion(qr, qd);
}

glm::mat4 dualQuatToMat4(const DualQuaternion& dq) {
glm::mat4 rotation = glm::toMat4(dq.real);

glm::quat d = conjugate(dq.real) * (dq.dual * 2.0f);
glm::mat4 position = glm::translate(glm::vec3(d.x, d.y, d.z));

glm::mat4 result = position * rotation;
return result;
}

glm::vec3 transformVector(const DualQuaternion& dq, const glm::vec3& v) {
return dq.real * v;
}

glm::vec3 transformPoint2(const DualQuaternion& dq, const glm::vec3& v){
auto d0 = glm::vec3(dq.real.x, dq.real.y, dq.real.z);
auto de = glm::vec3(dq.dual.x, dq.dual.y, dq.dual.z);
auto a0 = dq.real.w;
auto ae = dq.dual.w;

return v + 2.0f * cross(d0,cross(d0,v) + a0*v) + 2.0f *(a0*de - ae*d0 + cross(d0,de));
}


zeno::vec3f transformVector(const DualQuaternion& dq, const zeno::vec3f& v) {
return zeno::bit_cast<zeno::vec3f>(transformVector(dq, zeno::bit_cast<glm::vec3>(v)));
}

zeno::vec3f transformPoint2(const DualQuaternion& dq, const zeno::vec3f& v) {
return zeno::bit_cast<zeno::vec3f>(transformPoint2(dq, zeno::bit_cast<glm::vec3>(v)));
}
40 changes: 40 additions & 0 deletions projects/FBX/DualQuaternion.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
#ifndef _H_DUALQUATERNION_
#define _H_DUALQUATERNION_

#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/euler_angles.hpp>
#include <glm/gtx/quaternion.hpp>
#include <glm/gtx/transform.hpp>
#include <zeno/utils/vec.h>

struct DualQuaternion {
glm::quat real = {1, 0, 0, 0};
glm::quat dual = {0, 0, 0, 0};
inline DualQuaternion() { }
inline DualQuaternion(const glm::quat& r, const glm::quat& d) :
real(r), dual(d) { }
};

DualQuaternion operator+(const DualQuaternion& l, const DualQuaternion& r);
DualQuaternion operator*(const DualQuaternion& dq, float f);
// Multiplication order is left to right
// Left to right is the OPPOSITE of matrices and quaternions
DualQuaternion operator*(const DualQuaternion& l, const DualQuaternion& r);
bool operator==(const DualQuaternion& l, const DualQuaternion& r);
bool operator!=(const DualQuaternion& l, const DualQuaternion& r);

float dot(const DualQuaternion& l, const DualQuaternion& r);
DualQuaternion conjugate(const DualQuaternion& dq);
DualQuaternion normalized(const DualQuaternion& dq);
void normalize(DualQuaternion& dq);

DualQuaternion mat4ToDualQuat2(const glm::mat4& t);
glm::mat4 dualQuatToMat4(const DualQuaternion& dq);

glm::vec3 transformVector(const DualQuaternion& dq, const glm::vec3& v);
zeno::vec3f transformVector(const DualQuaternion& dq, const zeno::vec3f& v);
zeno::vec3f transformPoint2(const DualQuaternion& dq, const zeno::vec3f& v);

#endif
68 changes: 57 additions & 11 deletions projects/FBX/FBXSDK.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <glm/gtx/euler_angles.hpp>
#include <glm/gtx/quaternion.hpp>
#include <glm/gtx/transform.hpp>
#include "DualQuaternion.h"

#ifdef ZENO_FBXSDK
#include <fbxsdk.h>
Expand Down Expand Up @@ -1009,6 +1010,7 @@ struct NewFBXImportAnimation : INode {
ud.set2("boneName_count", int(bone_names.size()));
for (auto i = 0; i < bone_names.size(); i++) {
ud.set2(zeno::format("boneName_{}", i), bone_names[i]);
zeno::log_info("boneName: {}", bone_names[i]);
}
}

Expand Down Expand Up @@ -1387,6 +1389,7 @@ struct NewFBXBoneDeform : INode {
return mapping;
}
virtual void apply() override {
auto usingDualQuaternion = get_input2<std::string>("SkinningMethod") == "DualQuaternion";
auto geometryToDeform = get_input2<PrimitiveObject>("GeometryToDeform");
auto geometryToDeformBoneNames = getBoneNames(geometryToDeform.get());
auto restPointTransformsPrim = get_input2<PrimitiveObject>("RestPointTransforms");
Expand All @@ -1400,6 +1403,8 @@ struct NewFBXBoneDeform : INode {

std::vector<glm::mat4> matrixs;
matrixs.reserve(geometryToDeformBoneNames.size());
std::vector<DualQuaternion> dqs;
dqs.reserve(geometryToDeformBoneNames.size());
for (auto i = 0; i < geometryToDeformBoneNames.size(); i++) {
glm::mat4 res_inv_matrix = glm::mat4(1);
glm::mat4 deform_matrix = glm::mat4(1);
Expand All @@ -1409,6 +1414,7 @@ struct NewFBXBoneDeform : INode {
}
auto matrix = deform_matrix * res_inv_matrix;
matrixs.push_back(matrix);
dqs.push_back(mat4ToDualQuat2(matrix));
}

auto prim = std::dynamic_pointer_cast<PrimitiveObject>(geometryToDeform->clone());
Expand All @@ -1425,19 +1431,30 @@ struct NewFBXBoneDeform : INode {
for (auto i = 0; i < vert_count; i++) {
auto opos = prim->verts[i];
vec3f pos = {};
DualQuaternion dq_acc({0, 0, 0, 0}, {0, 0, 0, 0});
float w = 0;
for (auto j = 0; j < maxnum_boneWeight; j++) {
auto index = bi[j]->operator[](i);
if (index < 0) {
continue;
}
auto matrix = matrixs[index];
auto weight = bw[j]->operator[](i);
pos += transform_pos(matrix, opos) * weight;
if (usingDualQuaternion) {
dq_acc = dq_acc + dqs[index] * weight;
}
else {
pos += transform_pos(matrixs[index], opos) * weight;
}
w += weight;
}
if (w > 0) {
prim->verts[i] = pos / w;
if (usingDualQuaternion) {
dq_acc = normalized(dq_acc);
prim->verts[i] = transformPoint2(dq_acc, opos);
}
else {
prim->verts[i] = pos / w;
}
}
}
auto vectors_str = get_input2<std::string>("vectors");
Expand All @@ -1450,19 +1467,33 @@ struct NewFBXBoneDeform : INode {
#pragma omp parallel for
for (auto i = 0; i < vert_count; i++) {
glm::mat4 matrix(0);
DualQuaternion dq_acc({0, 0, 0, 0}, {0, 0, 0, 0});
float w = 0;
for (auto j = 0; j < maxnum_boneWeight; j++) {
auto index = bi[j]->operator[](i);
if (index < 0) {
continue;
}
auto weight = bw[j]->operator[](i);
matrix += matrixs[index] * weight;
if (usingDualQuaternion) {
dq_acc = dq_acc + dqs[index] * weight;
}
else {
matrix += matrixs[index] * weight;
}
w += weight;
}
matrix = matrix / w;
auto nrm = transform_nrm(matrix, nrms[i]);
nrms[i] = zeno::normalize(nrm);
if (w > 0) {
if (usingDualQuaternion) {
dq_acc = normalized(dq_acc);
nrms[i] = transformVector(dq_acc, nrms[i]);
}
else {
matrix = matrix / w;
nrms[i] = transform_nrm(matrix, nrms[i]);
}
nrms[i] = zeno::normalize(nrms[i]);
}
}
}
if (prim->loops.attr_is<vec3f>(vector)) {
Expand All @@ -1471,19 +1502,33 @@ struct NewFBXBoneDeform : INode {
for (auto i = 0; i < prim->loops.size(); i++) {
auto vi = prim->loops[i];
glm::mat4 matrix(0);
DualQuaternion dq_acc({0, 0, 0, 0}, {0, 0, 0, 0});
float w = 0;
for (auto j = 0; j < maxnum_boneWeight; j++) {
auto index = bi[j]->operator[](vi);
if (index < 0) {
continue;
}
auto weight = bw[j]->operator[](vi);
matrix += matrixs[index] * weight;
if (usingDualQuaternion) {
dq_acc = dq_acc + dqs[index] * weight;
}
else {
matrix += matrixs[index] * weight;
}
w += weight;
}
matrix = matrix / w;
auto nrm = transform_nrm(matrix, nrms[i]);
nrms[i] = zeno::normalize(nrm);
if (w > 0) {
if (usingDualQuaternion) {
dq_acc = normalized(dq_acc);
nrms[i] = transformVector(dq_acc, nrms[i]);
}
else {
matrix = matrix / w;
nrms[i] = transform_nrm(matrix, nrms[i]);
}
nrms[i] = zeno::normalize(nrms[i]);
}
}
}
}
Expand All @@ -1498,6 +1543,7 @@ ZENDEFNODE(NewFBXBoneDeform, {
"GeometryToDeform",
"RestPointTransforms",
"DeformPointTransforms",
{"enum Linear DualQuaternion", "SkinningMethod", "Linear"},
{"string", "vectors", "nrm,"},
},
{
Expand Down

0 comments on commit 6410369

Please sign in to comment.