From 6d6b543a03c2649b3623e5374bd00d2c38f8cbe5 Mon Sep 17 00:00:00 2001 From: zhouhang95 <765229842@qq.com> Date: Fri, 23 Aug 2024 20:57:45 +0800 Subject: [PATCH 1/8] xinxin ik code --- projects/FBX/FBXSDK.cpp | 168 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 168 insertions(+) diff --git a/projects/FBX/FBXSDK.cpp b/projects/FBX/FBXSDK.cpp index dbc62d6e32..c49688f68d 100644 --- a/projects/FBX/FBXSDK.cpp +++ b/projects/FBX/FBXSDK.cpp @@ -1996,4 +1996,172 @@ ZENDEFNODE(PrimAttrFlat, { {"debug"}, }); +#if 0 +float length(std::vector &b) +{ + float l = 0; + for(int i=0;i> &A, std::vector &b, std::vector&x, + int max_iter, float tol) + { + int iter=0; + float b_nrm = length(b); + while(iter &index, + std::vector &J, + std::vector &r, + //skeleton * skel_ptr, + vec3f e_curr + ) + { + J.resize(index.size()); + for(int i=0;i &J, std::vector> &JTJ, float alpha) + { + JTJ.resize(J.size()); + for(int i=0;i &index, + std::vector &dtheta, + std::vector &theta, + float &dist) +{ + dtheta.resize(theta.size()); + dtheta.assign(0); + //skeleton = FK(theta); + //e_curr = getJointPos(id, skeleton); + //de = e - tarPos; + dist = length(de); + if(dist<0.0001) + return; + //computeJointJacobian(); + //computeJTJ(..,..,0.01); + auto b = std::vector(index.size()); + for(int i=0;i(0); + GaussSeidelSolve(JTJ, b, x, 100, 0.00001); + for(int i=0;i & theta, + std::vector & theta_constraints, + std::vector &targets, + std::vector endEffectorIDs, + std::vector startIDs, + int iter_max + ) +{ + std::vector> dtheta; + dtheta.resize(endEffectorIDs.size()); + int iter = 0; + std::vector old_theta; + old_theta = theta; + while(iter w; + w.resize(theta.size()); + w.assign(0); + std::vector total_dtheta; + total_dtheta.resize(theta.size()); + total_dtheta.assign(0); + + for(int j=0;j 0 ? 1 : 0; + } + + + for (int i = 0; i < theta.size(); ++i) { + theta[i] = theta[i] + total_dtheta[i] / (w[i] > 0 ? w[i] : 1); + theta[i] = clamp(theta[i], theta_constraints[i][0], theta_constraints[i][1]); + } + + float max_dtheta = 0; + for(int i=0;i Date: Wed, 28 Aug 2024 20:44:02 +0800 Subject: [PATCH 2/8] temp --- projects/FBX/FBXSDK.cpp | 129 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 129 insertions(+) diff --git a/projects/FBX/FBXSDK.cpp b/projects/FBX/FBXSDK.cpp index c49688f68d..4736a559d2 100644 --- a/projects/FBX/FBXSDK.cpp +++ b/projects/FBX/FBXSDK.cpp @@ -1461,6 +1461,7 @@ static std::vector getBoneNames(PrimitiveObject *prim) { } return boneNames; } + static std::vector TopologicalSorting(std::map bone_connects, zeno::PrimitiveObject* skeleton) { std::vector ordering; std::set ordering_set; @@ -1996,6 +1997,134 @@ ZENDEFNODE(PrimAttrFlat, { {"debug"}, }); +struct IKChainsItemObject : PrimitiveObject { + std::string RootName; + std::string MidName; + std::string TipName; + bool MatchByName = true; + std::string TwistName; + std::string GoalName; + float Blend = 1; + bool OrientTip = true; +}; +struct IKChainsItem : INode { + virtual void apply() override { + auto item = std::make_shared(); + item->RootName = get_input2("RootName"); + item->MidName = get_input2("MidName"); + item->TipName = get_input2("TipName"); + item->MatchByName = get_input2("MatchByName"); + item->TwistName = get_input2("TwistName"); + item->Blend = get_input2("Blend"); + item->OrientTip = get_input2("OrientTip"); + + set_output2("poseItem", std::move(item)); + } +}; + +ZENDEFNODE(IKChainsItem, { + { + {"string", "RootName", ""}, + {"string", "MidName", ""}, + {"string", "TipName", ""}, + {"bool", "MatchByName", "1"}, + {"string", "TwistName", ""}, + {"string", "GoalName", ""}, + {"float", "Blend", "1"}, + {"bool", "OrientTip", "1"}, + }, + { + "poseItem", + }, + {}, + {"Animation"}, +}); + +float sqr(float v) { + return v * v; +} +// return: mid, tip +std::pair twoBoneIK( + vec3f root + , vec3f joint + , vec3f end + , vec3f jointTarget + , vec3f effector +) { + vec3f output_joint = {}; + vec3f output_end = {}; + + auto root_to_effect = effector - root; + auto root_to_jointTarget = jointTarget - root; + + auto upper_limb_length = zeno::length(root - joint); + auto lower_limb_length = zeno::length(joint - end); + auto desired_length = zeno::length(root_to_effect); + if (desired_length < abs(upper_limb_length - lower_limb_length)) { + zeno::log_info("A"); + output_joint = root + normalize(root_to_effect) * abs(upper_limb_length - lower_limb_length); + output_end = root + normalize(root_to_effect) * upper_limb_length; + } + else if (desired_length > upper_limb_length + lower_limb_length) { + zeno::log_info("B"); + + output_joint = root + normalize(root_to_effect) * upper_limb_length; + output_end = root + normalize(root_to_effect) * (upper_limb_length + lower_limb_length); + } + else { + zeno::log_info("C"); + + vec3f to_pole = normalize(cross(cross(root_to_effect, root_to_jointTarget), root_to_effect)); + float cos_theta = (sqr(upper_limb_length) + sqr(desired_length) - sqr(lower_limb_length)) / (2.0f * upper_limb_length * desired_length); + float sin_theta = sqrt(1 - sqr(cos_theta)); + output_joint = root + normalize(root_to_effect) * cos_theta + to_pole * sin_theta; + output_end = effector; + } + + return {output_joint, output_end}; +} + +struct IKChains : INode { + virtual void apply() override { + auto skeleton = get_input2("Skeleton"); + auto ikDrivers = get_input2("IK Drivers"); + auto items = get_input("items")->getRaw(); + auto skeletonBoneNameMapping = getBoneNameMapping(skeleton.get()); + auto ikDriversBoneNameMapping = getBoneNameMapping(ikDrivers.get()); + + for (auto item: items) { + std::string TwistName = item->MatchByName? item->MidName: item->TwistName; + std::string GoalName = item->MatchByName? item->TipName: item->GoalName; + vec3f root = skeleton->verts[skeletonBoneNameMapping[item->RootName]]; + vec3f &joint = skeleton->verts[skeletonBoneNameMapping[item->MidName]]; + vec3f &end = skeleton->verts[skeletonBoneNameMapping[item->TipName]]; + vec3f jointTarget = ikDrivers->verts[ikDriversBoneNameMapping[TwistName]]; + vec3f effector = ikDrivers->verts[ikDriversBoneNameMapping[GoalName]]; + zeno::log_info("{} {} {}", root, joint, end); + zeno::log_info("{} {}", jointTarget, effector); + auto [midPos, tipPos] = twoBoneIK(root, joint, end, jointTarget, effector); + // set ... + joint = midPos; + end = tipPos; + } + + set_output("Skeleton", skeleton); + } +}; + +ZENDEFNODE(IKChains, { + { + "Skeleton", + "IK Drivers", + {"list", "items"}, + }, + { + "Skeleton", + }, + {}, + {"Animation"}, +}); + #if 0 float length(std::vector &b) { From f4d2a4c74e1c1011390a38d45163ed56b511d0e3 Mon Sep 17 00:00:00 2001 From: zhouhang95 <765229842@qq.com> Date: Thu, 29 Aug 2024 19:34:08 +0800 Subject: [PATCH 3/8] remove unused bit_cast --- zeno/include/zeno/utils/type_traits.h | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/zeno/include/zeno/utils/type_traits.h b/zeno/include/zeno/utils/type_traits.h index 1ff175ec34..cf9d1a6bc0 100644 --- a/zeno/include/zeno/utils/type_traits.h +++ b/zeno/include/zeno/utils/type_traits.h @@ -268,21 +268,4 @@ struct identity { return std::forward(t); } }; - -// port from https://en.cppreference.com/w/cpp/numeric/bit_cast -template -std::enable_if_t< - sizeof(To) == sizeof(From) && - std::is_trivially_copyable_v && - std::is_trivially_copyable_v, - To> -bit_cast(const From& src) noexcept { - static_assert(std::is_trivially_constructible_v, - "This implementation additionally requires " - "destination type to be trivially constructible"); - - To dst; - std::memcpy(&dst, &src, sizeof(To)); - return dst; -} } From bbfc2e4bfa4da4010215a42acaa6093b9d16d01c Mon Sep 17 00:00:00 2001 From: zhouhang95 <765229842@qq.com> Date: Thu, 29 Aug 2024 21:05:33 +0800 Subject: [PATCH 4/8] IKChains --- projects/FBX/FBXSDK.cpp | 65 +++++++++++++++++++++++++++++++++-------- 1 file changed, 53 insertions(+), 12 deletions(-) diff --git a/projects/FBX/FBXSDK.cpp b/projects/FBX/FBXSDK.cpp index 4736a559d2..a8b7dc1729 100644 --- a/projects/FBX/FBXSDK.cpp +++ b/projects/FBX/FBXSDK.cpp @@ -1530,9 +1530,9 @@ struct NewFBXRigPose : INode { if (Transformations.count(bi)) { auto trans = Transformations[bi]; glm::mat4 matTrans = glm::translate(vec_to_other(trans->translate)); - glm::mat4 matRotx = glm::rotate( (float)(trans->rotate[0] * M_PI / 180), glm::vec3(1,0,0) ); - glm::mat4 matRoty = glm::rotate( (float)(trans->rotate[1] * M_PI / 180), glm::vec3(0,1,0) ); - glm::mat4 matRotz = glm::rotate( (float)(trans->rotate[2] * M_PI / 180), glm::vec3(0,0,1) ); + glm::mat4 matRotx = glm::rotate(glm::radians(trans->rotate[0]), glm::vec3(1,0,0) ); + glm::mat4 matRoty = glm::rotate(glm::radians(trans->rotate[1]), glm::vec3(0,1,0) ); + glm::mat4 matRotz = glm::rotate(glm::radians(trans->rotate[2]), glm::vec3(0,0,1) ); transform = matTrans*matRoty*matRotx*matRotz; transform = transforms[bi] * transform * transformsInv[bi]; } @@ -2077,7 +2077,7 @@ std::pair twoBoneIK( vec3f to_pole = normalize(cross(cross(root_to_effect, root_to_jointTarget), root_to_effect)); float cos_theta = (sqr(upper_limb_length) + sqr(desired_length) - sqr(lower_limb_length)) / (2.0f * upper_limb_length * desired_length); float sin_theta = sqrt(1 - sqr(cos_theta)); - output_joint = root + normalize(root_to_effect) * cos_theta + to_pole * sin_theta; + output_joint = root + (normalize(root_to_effect) * cos_theta + to_pole * sin_theta) * upper_limb_length; output_end = effector; } @@ -2091,21 +2091,62 @@ struct IKChains : INode { auto items = get_input("items")->getRaw(); auto skeletonBoneNameMapping = getBoneNameMapping(skeleton.get()); auto ikDriversBoneNameMapping = getBoneNameMapping(ikDrivers.get()); + std::map bone_connects; + for (auto i = 0; i < skeleton->polys.size(); i++) { + bone_connects[skeleton->loops[i * 2 + 1]] = skeleton->loops[i * 2]; + } + auto ordering = TopologicalSorting(bone_connects, skeleton.get()); + + auto &verts = skeleton->verts; + auto &transform_r0 = skeleton->verts.attr("transform_r0"); + auto &transform_r1 = skeleton->verts.attr("transform_r1"); + auto &transform_r2 = skeleton->verts.attr("transform_r2"); for (auto item: items) { std::string TwistName = item->MatchByName? item->MidName: item->TwistName; std::string GoalName = item->MatchByName? item->TipName: item->GoalName; - vec3f root = skeleton->verts[skeletonBoneNameMapping[item->RootName]]; - vec3f &joint = skeleton->verts[skeletonBoneNameMapping[item->MidName]]; - vec3f &end = skeleton->verts[skeletonBoneNameMapping[item->TipName]]; + auto root_index = skeletonBoneNameMapping[item->RootName]; + vec3f root = skeleton->verts[root_index]; + auto joint_index = skeletonBoneNameMapping[item->MidName]; + vec3f joint = skeleton->verts[joint_index]; + auto end_index = skeletonBoneNameMapping[item->TipName]; + vec3f end = skeleton->verts[end_index]; vec3f jointTarget = ikDrivers->verts[ikDriversBoneNameMapping[TwistName]]; vec3f effector = ikDrivers->verts[ikDriversBoneNameMapping[GoalName]]; - zeno::log_info("{} {} {}", root, joint, end); - zeno::log_info("{} {}", jointTarget, effector); auto [midPos, tipPos] = twoBoneIK(root, joint, end, jointTarget, effector); - // set ... - joint = midPos; - end = tipPos; + auto parent = glm::rotation(bit_cast(normalize(joint - root)), bit_cast(normalize(midPos - root))); + auto from_ = parent * bit_cast(normalize(end - joint)); + auto child = glm::rotation(from_, bit_cast(normalize(tipPos - midPos))); + auto transforms = getBoneMatrix(skeleton.get()); + auto transformsInv = getInvertedBoneMatrix(skeleton.get()); + bool start = false; + std::map cache; + for (auto bi: ordering) { + if (bi == root_index) { + start = true; + } + if (start) { + glm::mat4 transform = glm::mat4(1.0f); + if (bi == root_index) { + transform = glm::translate(glm::vec3(transforms[bi][3])) * glm::toMat4(parent) * glm::translate(-glm::vec3(transforms[bi][3])); + } + else if (bi == joint_index) { + transform = glm::translate(glm::vec3(transforms[bi][3])) * glm::toMat4(child) * glm::translate(-glm::vec3(transforms[bi][3])); + } + if (bone_connects.count(bi) && cache.count(bone_connects[bi])) { + transform = cache[bone_connects[bi]] * transform; + } + if (bi == end_index && item->OrientTip) { + auto target_pos = transform_pos(transform, verts[bi]); + transform = glm::translate(bit_cast(target_pos - verts[bi])); + } + cache[bi] = transform; + verts[bi] = transform_pos(transform, verts[bi]); + transform_r0[bi] = transform_nrm(transform, transform_r0[bi]); + transform_r1[bi] = transform_nrm(transform, transform_r1[bi]); + transform_r2[bi] = transform_nrm(transform, transform_r2[bi]); + } + } } set_output("Skeleton", skeleton); From f58d96bc19af7445991f5c56fe1313892ce199ae Mon Sep 17 00:00:00 2001 From: zhouhang95 <765229842@qq.com> Date: Thu, 29 Aug 2024 21:14:05 +0800 Subject: [PATCH 5/8] remove unused code --- projects/FBX/FBXSDK.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/projects/FBX/FBXSDK.cpp b/projects/FBX/FBXSDK.cpp index a8b7dc1729..6325b6e631 100644 --- a/projects/FBX/FBXSDK.cpp +++ b/projects/FBX/FBXSDK.cpp @@ -2015,7 +2015,6 @@ struct IKChainsItem : INode { item->TipName = get_input2("TipName"); item->MatchByName = get_input2("MatchByName"); item->TwistName = get_input2("TwistName"); - item->Blend = get_input2("Blend"); item->OrientTip = get_input2("OrientTip"); set_output2("poseItem", std::move(item)); @@ -2030,8 +2029,7 @@ ZENDEFNODE(IKChainsItem, { {"bool", "MatchByName", "1"}, {"string", "TwistName", ""}, {"string", "GoalName", ""}, - {"float", "Blend", "1"}, - {"bool", "OrientTip", "1"}, + {"bool", "OrientTip", "0"}, }, { "poseItem", @@ -2117,8 +2115,6 @@ struct IKChains : INode { auto parent = glm::rotation(bit_cast(normalize(joint - root)), bit_cast(normalize(midPos - root))); auto from_ = parent * bit_cast(normalize(end - joint)); auto child = glm::rotation(from_, bit_cast(normalize(tipPos - midPos))); - auto transforms = getBoneMatrix(skeleton.get()); - auto transformsInv = getInvertedBoneMatrix(skeleton.get()); bool start = false; std::map cache; for (auto bi: ordering) { @@ -2128,10 +2124,10 @@ struct IKChains : INode { if (start) { glm::mat4 transform = glm::mat4(1.0f); if (bi == root_index) { - transform = glm::translate(glm::vec3(transforms[bi][3])) * glm::toMat4(parent) * glm::translate(-glm::vec3(transforms[bi][3])); + transform = glm::translate(bit_cast(verts[bi])) * glm::toMat4(parent) * glm::translate(-bit_cast(verts[bi])); } else if (bi == joint_index) { - transform = glm::translate(glm::vec3(transforms[bi][3])) * glm::toMat4(child) * glm::translate(-glm::vec3(transforms[bi][3])); + transform = glm::translate(bit_cast(verts[bi])) * glm::toMat4(child) * glm::translate(-bit_cast(verts[bi])); } if (bone_connects.count(bi) && cache.count(bone_connects[bi])) { transform = cache[bone_connects[bi]] * transform; From 0b7f52fc5aced503672ba9b7de68339f662bade4 Mon Sep 17 00:00:00 2001 From: zhouhang95 <765229842@qq.com> Date: Fri, 30 Aug 2024 15:46:02 +0800 Subject: [PATCH 6/8] improve GaussSeidelSolve --- projects/FBX/FBXSDK.cpp | 38 ++++++++++++++++---------------------- 1 file changed, 16 insertions(+), 22 deletions(-) diff --git a/projects/FBX/FBXSDK.cpp b/projects/FBX/FBXSDK.cpp index 6325b6e631..69448a8cbf 100644 --- a/projects/FBX/FBXSDK.cpp +++ b/projects/FBX/FBXSDK.cpp @@ -2035,7 +2035,7 @@ ZENDEFNODE(IKChainsItem, { "poseItem", }, {}, - {"Animation"}, + {"deprecated"}, }); float sqr(float v) { @@ -2159,7 +2159,7 @@ ZENDEFNODE(IKChains, { "Skeleton", }, {}, - {"Animation"}, + {"deprecated"}, }); #if 0 @@ -2177,34 +2177,28 @@ void GaussSeidelSolve(std::vector> &A, std::vector &b, int max_iter, float tol) { int iter=0; - float b_nrm = length(b); + float b_nrm = 0; + for(int i=0;i Date: Sat, 31 Aug 2024 21:31:46 +0800 Subject: [PATCH 7/8] temp --- projects/FBX/FBXSDK.cpp | 214 ++++++++++++++++++++++++++++++++-------- 1 file changed, 175 insertions(+), 39 deletions(-) diff --git a/projects/FBX/FBXSDK.cpp b/projects/FBX/FBXSDK.cpp index 69448a8cbf..a2486233cb 100644 --- a/projects/FBX/FBXSDK.cpp +++ b/projects/FBX/FBXSDK.cpp @@ -2162,7 +2162,6 @@ ZENDEFNODE(IKChains, { {"deprecated"}, }); -#if 0 float length(std::vector &b) { float l = 0; @@ -2203,52 +2202,56 @@ void GaussSeidelSolve(std::vector> &A, std::vector &b, iter++; } } - void computeJointJacobian(std::vector &index, - std::vector &J, - std::vector &r, - //skeleton * skel_ptr, - vec3f e_curr - ) +vec3f getJointPos(int id, PrimitiveObject * skel_ptr) { + return skel_ptr->verts[id]; +} +void computeJointJacobian(std::vector &index, + std::vector &J, + std::vector &r, + PrimitiveObject * skel_ptr, + vec3f e_curr + ) +{ + J.resize(index.size()); + for(int i=0;i &J, std::vector> &JTJ, float alpha) +} +void computeJTJ(std::vector &J, std::vector> &JTJ, float alpha) +{ + JTJ.resize(J.size()); + for(int i=0;i &index, std::vector &dtheta, std::vector &theta, float &dist) { dtheta.resize(theta.size()); - dtheta.assign(0); - //skeleton = FK(theta); - //e_curr = getJointPos(id, skeleton); - //de = e - tarPos; - dist = length(de); + dtheta.assign(dtheta.size(), 0); + PrimitiveObject * skeleton = nullptr; //skeleton = FK(theta); + glm::vec3 e_curr = getJointPos(id, skeleton); + glm::vec3 de = e_curr - tarPos; + dist = glm::length(de); if(dist<0.0001) return; //computeJointJacobian(); @@ -2265,8 +2268,11 @@ void solveJointUpdate(int id, dtheta[index[i]] = x[i]; } } +std::vector getIds(int startId, int endId, PrimitiveObject* skeletonPtr) { -void SolveIKConstrained(//skeletonPtr, + return {}; +} +void SolveIKConstrained(PrimitiveObject* skeletonPtr, std::vector & theta, std::vector & theta_constraints, std::vector &targets, @@ -2286,19 +2292,19 @@ void SolveIKConstrained(//skeletonPtr, for (int i = 0; i < endEffectorIDs.size(); i++) { auto endid = endEffectorIDs[i]; auto startid = startIDs[i]; - //index = getIds(startId, endId, skeletonPtr); + std::vector index = getIds(startid, endid, skeletonPtr); float e_i; - solveJointUpdate(endid, index, dtheta[i], theta, e_i); +// solveJointUpdate(endid, index, dtheta[i], theta, e_i); err += e_i; } if (err < 0.0001) break; std::vector w; w.resize(theta.size()); - w.assign(0); + w.assign(w.size(), 0); std::vector total_dtheta; total_dtheta.resize(theta.size()); - total_dtheta.assign(0); + total_dtheta.assign(total_dtheta.size(), 0); for(int j=0;j(); + item->startName = get_input2("startName"); + item->endEffectorName = get_input2("endEffectorName"); + item->targetPos = get_input2("targetPos"); + + set_output2("IkChain", std::move(item)); + } +}; + +ZENDEFNODE(IkChainsItem, { + { + {"string", "startName", ""}, + {"string", "endEffectorName", ""}, + {"vec3f", "targetPos", ""}, + }, + { + "IkChain", + }, + {}, + {"FBXSDK"}, +}); + +struct JointLimitObject : PrimitiveObject { + std::string boneName; + vec3i enableLimit; + vec2f xLimit; + vec2f yLimit; + vec2f zLimit; +}; + +struct JointLimitItem : INode { + virtual void apply() override { + auto item = std::make_shared(); + item->boneName = get_input2("boneName"); + item->enableLimit = { + get_input2("enableXLimit"), + get_input2("enableYLimit"), + get_input2("enableZLimit"), + }; + item->xLimit = get_input2("xLimit"); + item->yLimit = get_input2("yLimit"); + item->zLimit = get_input2("zLimit"); + + set_output2("JointLimit", std::move(item)); + } +}; + +ZENDEFNODE(JointLimitItem, { + { + {"string", "startName", ""}, + {"bool", "enableXLimit", "0"}, + {"vec2f", "xLimit", "0,0"}, + {"bool", "enableYLimit", "0"}, + {"vec2f", "yLimit", "0,0"}, + {"bool", "enableZLimit", "0"}, + {"vec2f", "zLimit", "0,0"}, + }, + { + "JointLimit", + }, + {}, + {"FBXSDK"}, +}); + +struct IkSolver : INode { + void apply() override { + auto skeleton = get_input2("Skeleton"); + PrimitiveObject* skeletonPtr = skeleton.get(); + auto boneNameMapping = getBoneNameMapping(skeletonPtr); + int iter_max = 50; + std::vector theta; + std::vector theta_constraints; + if (has_input("jointLimits")) { + auto items = get_input("jointLimits")->getRaw(); + for (auto &item: items) { + + + } + } + std::vector targets; + std::vector endEffectorIDs; + std::vector startIDs; + { + auto items = get_input("IkChains")->getRaw(); + for (auto &item: items) { + if (boneNameMapping.count(item->startName) == 0) { + log_warn("Not find ik startBone: {}", item->startName); + continue; + } + if (boneNameMapping.count(item->endEffectorName) == 0) { + log_warn("Not find ik endEffector: {}", item->endEffectorName); + continue; + } + endEffectorIDs.push_back(boneNameMapping[item->endEffectorName]); + startIDs.push_back(boneNameMapping[item->startName]); + targets.push_back(item->targetPos); + } + } + +// SolveIKConstrained( +// skeletonPtr, +// theta, +// theta_constraints, +// targets, +// endEffectorIDs, +// startIDs, +// iter_max +// ); + } +}; +ZENDEFNODE(IkSolver, { + { + "Skeleton", + {"list", "IkChains"}, + {"list", "jointLimits"}, + }, + { + "Skeleton", + }, + {}, + {"FBXSDK"}, +}); } \ No newline at end of file From 73cabb7968c2044cdc0bdf82585996991d239051 Mon Sep 17 00:00:00 2001 From: zhouhang95 <765229842@qq.com> Date: Tue, 3 Sep 2024 04:11:58 +0800 Subject: [PATCH 8/8] fbx-ik --- projects/FBX/FBXSDK.cpp | 437 +++++++++++++++++++++++++++++++++------- 1 file changed, 359 insertions(+), 78 deletions(-) diff --git a/projects/FBX/FBXSDK.cpp b/projects/FBX/FBXSDK.cpp index a2486233cb..9f3b68b301 100644 --- a/projects/FBX/FBXSDK.cpp +++ b/projects/FBX/FBXSDK.cpp @@ -364,7 +364,7 @@ ZENDEFNODE(ReadFBXFile, { "fbx_object", }, {}, - {"FBX"}, + {"FBXSDK"}, }); /** @@ -836,7 +836,7 @@ ZENDEFNODE(NewFBXImportSkin, { "prim", }, {}, - {"primitive"}, + {"FBXSDK"}, }); static int GetSkeletonFromBindPose(FbxManager* lSdkManager, FbxScene* lScene, std::shared_ptr& prim) { @@ -1068,7 +1068,7 @@ ZENDEFNODE(NewFBXImportSkeleton, { "prim", }, {}, - {"primitive"}, + {"FBXSDK"}, }); struct NewFBXImportAnimation : INode { @@ -1230,7 +1230,7 @@ ZENDEFNODE(NewFBXImportAnimation, { "prim", }, {}, - {"primitive"}, + {"FBXSDK"}, }); struct NewFBXImportCamera : INode { @@ -1375,7 +1375,7 @@ ZENDEFNODE(NewFBXImportCamera, { "far", }, {}, - {"primitive"}, + {"FBXSDK"}, }); } #endif @@ -1738,7 +1738,7 @@ ZENDEFNODE(NewFBXBoneDeform, { "prim", }, {}, - {"primitive"}, + {"FBXSDK"}, }); struct NewFBXExtractKeyframe : INode { @@ -1817,7 +1817,7 @@ ZENDEFNODE(NewFBXExtractKeyframe, { "keyframe", }, {}, - {"primitive"}, + {"FBXSDK"}, }); @@ -1876,7 +1876,7 @@ ZENDEFNODE(NewFBXGenerateAnimation, { "DeformPointTransforms", }, {}, - {"primitive"}, + {"FBXSDK"}, }); @@ -2185,20 +2185,23 @@ void GaussSeidelSolve(std::vector> &A, std::vector &b, return; while(iter &index, vec3f e_curr ) { - J.resize(index.size()); + J.resize(index.size() * 3); for(int i=0;i &J, std::vector> &JTJ, float alpha) @@ -2237,47 +2242,206 @@ void computeJTJ(std::vector &J, std::vector> &JTJ, flo } JTJ[i][i] += alpha; } + for(int i=0;i FK( + std::vector theta + , std::shared_ptr skel_ptr +) { + std::vector Transformations; + for (auto i = 0; i < skel_ptr->verts.size(); i++) { + auto mx = glm::rotate(theta[i * 3 + 0], glm::vec3(1, 0, 0)); + auto my = glm::rotate(theta[i * 3 + 1], glm::vec3(0, 1, 0)); + auto mz = glm::rotate(theta[i * 3 + 2], glm::vec3(0, 0, 1)); + auto Transformation = mx * my * mz; + Transformations.push_back(Transformation); + } + + auto skeleton = std::dynamic_pointer_cast(skel_ptr->clone()); + std::map bone_connects; + for (auto i = 0; i < skeleton->polys.size(); i++) { + bone_connects[skeleton->loops[i * 2 + 1]] = skeleton->loops[i * 2]; + } + auto ordering = TopologicalSorting(bone_connects, skeleton.get()); + auto &verts = skeleton->verts; + auto &transform_r0 = skeleton->verts.add_attr("transform_r0"); + auto &transform_r1 = skeleton->verts.add_attr("transform_r1"); + auto &transform_r2 = skeleton->verts.add_attr("transform_r2"); + auto transforms = getBoneMatrix(skeleton.get()); + auto transformsInv = getInvertedBoneMatrix(skeleton.get()); + auto boneNames = getBoneNames(skeleton.get()); + std::map cache; + for (auto bi: ordering) { + glm::mat4 transform = glm::mat4(1.0f); + auto trans = Transformations[bi]; + transform = transforms[bi] * trans * transformsInv[bi]; + if (bone_connects.count(bi)) { + transform = cache[bone_connects[bi]] * transform; + } + cache[bi] = transform; + verts[bi] = transform_pos(transform, verts[bi]); + transform_r0[bi] = transform_nrm(transform, transform_r0[bi]); + transform_r1[bi] = transform_nrm(transform, transform_r1[bi]); + transform_r2[bi] = transform_nrm(transform, transform_r2[bi]); + } + return skeleton; } -#if 0 void solveJointUpdate(int id, - glm::vec3 tarPos, + vec3f tarPos, + std::shared_ptr skel_ptr, std::vector &index, std::vector &dtheta, std::vector &theta, - float &dist) + float &dist, + float scale) { dtheta.resize(theta.size()); dtheta.assign(dtheta.size(), 0); - PrimitiveObject * skeleton = nullptr; //skeleton = FK(theta); - glm::vec3 e_curr = getJointPos(id, skeleton); - glm::vec3 de = e_curr - tarPos; - dist = glm::length(de); - if(dist<0.0001) +// zeno::log_error("{} FK.....", id); + std::shared_ptr skeleton = FK(theta, skel_ptr); +// zeno::log_error("{} FK----------", id); + vec3f e_curr = getJointPos(id, skeleton.get()); + vec3f de = tarPos - e_curr; + dist = glm::length(bit_cast(de)); + if(dist / scale <0.0001) return; - //computeJointJacobian(); - //computeJTJ(..,..,0.01); - auto b = std::vector(index.size()); + std::vector r; + { + auto &transform_r0 = skeleton->verts.attr("transform_r0"); + auto &transform_r1 = skeleton->verts.attr("transform_r1"); + auto &transform_r2 = skeleton->verts.attr("transform_r2"); + for (auto i = 0; i < skeleton->verts.size(); i++) { + r.push_back(zeno::normalize(transform_r0[i])); + r.push_back(zeno::normalize(transform_r1[i])); + r.push_back(zeno::normalize(transform_r2[i])); + } + } + std::vector J; + computeJointJacobian(index, J, r, skel_ptr.get(), e_curr); + if (0) { + // log + auto boneNames = getBoneNames(skel_ptr.get()); + for (auto i = 0; i < index.size(); i++) { + auto idx = index[i]; + std::cout << boneNames[idx] << " : "; + std::cout << J[i * 3 + 0][0] << ", " << J[i * 3 + 0][1] << ", " << J[i * 3 + 0][2] << "; "; + std::cout << J[i * 3 + 1][0] << ", " << J[i * 3 + 1][1] << ", " << J[i * 3 + 1][2] << "; "; + std::cout << J[i * 3 + 2][0] << ", " << J[i * 3 + 2][1] << ", " << J[i * 3 + 2][2] << "; "; + } + } + +// zeno::log_error("computeJointJacobian"); + std::vector> JTJ; + computeJTJ(J, JTJ, 0.001); + for (auto i = 0; i < JTJ.size(); i++) { +// std::cout << JTJ[i][i] << ' '; + } +// zeno::log_error("computeJTJ"); + auto b = std::vector(index.size() * 3); for(int i=0;i(0); + std::vector x(index.size() * 3); GaussSeidelSolve(JTJ, b, x, 100, 0.00001); +// zeno::log_error("GaussSeidelSolve"); + for (auto i = 0; i < x.size(); i++) { +// std::cout << x[i] << ' '; + } for(int i=0;i getIds(int endId, int depth, PrimitiveObject* skeletonPtr) { + std::map connects; + auto count = skeletonPtr->loops.size() / 2; + for (auto i = 0; i < count; i++) { + auto parentId = skeletonPtr->loops[2 * i + 0]; + auto childId = skeletonPtr->loops[2 * i + 1]; + connects[childId] = parentId; + } + std::vector result = {endId}; + auto cur_id = endId; + for (auto i = 0; i < depth; i++) { + if (connects.count(cur_id) == 0) { + break; + } + cur_id = connects[cur_id]; + result.push_back(cur_id); + } + + return result; +} + +float computeError(int id, std::shared_ptr skeletion, vec3f targetPos + , std::vector & theta) { + auto curPose = FK(theta, skeletion); + auto curJointPos = getJointPos(id, curPose.get()); + return zeno::distance(curJointPos, targetPos); +} + +float proposeTheta(std::vector &ids, std::shared_ptr skeletion, std::vector &targetPoss + , std::vector & new_theta, std::vector & theta, std::vector & dtheta, std::vector & total_theta + , std::vector &limit, float alpha, std::vector w) { + + for (int i = 0; i < theta.size(); ++i) { + new_theta[i] = theta[i] + alpha * dtheta[i] / (w[i] > 0 ? w[i] : 1); + auto tmp_theta = clamp(total_theta[i] + new_theta[i], limit[i][0], limit[i][1]); + new_theta[i] = tmp_theta - total_theta[i]; + } + float e = 0; + for (auto i = 0; i < ids.size(); i++) { + e += computeError(ids[i], skeletion, targetPoss[i], new_theta); } + return e; } -std::vector getIds(int startId, int endId, PrimitiveObject* skeletonPtr) { - return {}; + +void line_search(std::vector &ids, std::shared_ptr skeletion, std::vector &targetPoss + , std::vector & theta, std::vector & dtheta, std::vector & total_theta + , std::vector &limit, float damp, std::vector &w, float prev_err) { + std::vector new_theta = theta; + float alpha = 1; + + float e; + e = proposeTheta(ids, skeletion, targetPoss, new_theta, theta, dtheta, total_theta, limit, alpha, w); + + if (e < prev_err) { + theta = new_theta; + return; + } + while (alpha > 1e-7) { + alpha *= damp; + e = proposeTheta(ids, skeletion, targetPoss, new_theta, theta, dtheta, total_theta, limit, alpha, w); + if (e < prev_err) { + theta = new_theta; + return; + } + } } -void SolveIKConstrained(PrimitiveObject* skeletonPtr, + +void SolveIKConstrained(std::shared_ptr skeletonPtr, std::vector & theta, + std::vector & total_theta, std::vector & theta_constraints, std::vector &targets, std::vector endEffectorIDs, - std::vector startIDs, + std::vector depths, int iter_max ) { @@ -2286,18 +2450,36 @@ void SolveIKConstrained(PrimitiveObject* skeletonPtr, int iter = 0; std::vector old_theta; old_theta = theta; + float prev_err = INFINITY; + float scale = 1; + { + for (int i = 0; i < endEffectorIDs.size(); i++) { + auto endId = endEffectorIDs[i]; + auto depth = depths[i]; + std::vector index = getIds(endId, depth, skeletonPtr.get()); + auto tarPos = targets[i]; + float e_i; + solveJointUpdate(endId, tarPos, skeletonPtr, index, dtheta[i], theta, e_i, 1); + scale = max(scale, e_i); + } + } while(iter index = getIds(startid, endid, skeletonPtr); +// zeno::log_error("i: {}", i); + auto endId = endEffectorIDs[i]; + auto depth = depths[i]; + std::vector index = getIds(endId, depth, skeletonPtr.get()); + auto tarPos = targets[i]; float e_i; -// solveJointUpdate(endid, index, dtheta[i], theta, e_i); + solveJointUpdate(endId, tarPos, skeletonPtr, index, dtheta[i], theta, e_i, scale); err += e_i; } - if (err < 0.0001) + prev_err = err; +// std::cout<<"current err:"< w; w.resize(theta.size()); @@ -2309,29 +2491,36 @@ void SolveIKConstrained(PrimitiveObject* skeletonPtr, for(int j=0;j 0 ? 1 : 0; + w[i] += abs(dtheta[j][i]) > 0 ? 1 : 0; } - - for (int i = 0; i < theta.size(); ++i) { - theta[i] = theta[i] + total_dtheta[i] / (w[i] > 0 ? w[i] : 1); - theta[i] = clamp(theta[i], theta_constraints[i][0], theta_constraints[i][1]); - } - - float max_dtheta = 0; - for(int i=0;iverts.size(); i++) { + std::cout << boneNames[i] << " : "; + std::cout << total_dtheta[i * 3 + 0] << ", "; + std::cout << total_dtheta[i * 3 + 1] << ", "; + std::cout << total_dtheta[i * 3 + 2] << ", " << std::endl; + } } - if(max_dtheta<0.0001) break; - old_theta = theta; +// float max_dtheta = 0; +// for(int i=0;i(); - item->startName = get_input2("startName"); + item->depth = get_input2("depth"); item->endEffectorName = get_input2("endEffectorName"); item->targetPos = get_input2("targetPos"); @@ -2349,8 +2538,8 @@ struct IkChainsItem : INode { ZENDEFNODE(IkChainsItem, { { - {"string", "startName", ""}, {"string", "endEffectorName", ""}, + {"int", "depth", "2"}, {"vec3f", "targetPos", ""}, }, { @@ -2387,7 +2576,7 @@ struct JointLimitItem : INode { ZENDEFNODE(JointLimitItem, { { - {"string", "startName", ""}, + {"string", "boneName", ""}, {"bool", "enableXLimit", "0"}, {"vec2f", "xLimit", "0,0"}, {"bool", "enableYLimit", "0"}, @@ -2405,52 +2594,83 @@ ZENDEFNODE(JointLimitItem, { struct IkSolver : INode { void apply() override { auto skeleton = get_input2("Skeleton"); - PrimitiveObject* skeletonPtr = skeleton.get(); - auto boneNameMapping = getBoneNameMapping(skeletonPtr); - int iter_max = 50; - std::vector theta; - std::vector theta_constraints; + auto boneNameMapping = getBoneNameMapping(skeleton.get()); + int iter_max = get_input2("iterCount"); + auto &enableXYZLimit = skeleton->add_attr("enableXYZLimit"); + auto &xLimit = skeleton->add_attr("xLimit"); + auto &yLimit = skeleton->add_attr("yLimit"); + auto &zLimit = skeleton->add_attr("zLimit"); if (has_input("jointLimits")) { - auto items = get_input("jointLimits")->getRaw(); + auto items = get_input("jointLimits")->getRaw(); for (auto &item: items) { - - + if (boneNameMapping.count(item->boneName)) { + auto index = boneNameMapping[item->boneName]; + enableXYZLimit[index] = item->enableLimit; + xLimit[index] = item->xLimit; + yLimit[index] = item->yLimit; + zLimit[index] = item->zLimit; + } + } + } + std::vector theta; + std::vector theta_constraints; + { + theta.resize(skeleton->verts.size() * 3); + for (auto i = 0; i < skeleton->verts.size(); i++) { + theta_constraints.push_back(enableXYZLimit[i][0]? xLimit[i]: vec2f(-INFINITY, INFINITY)); + theta_constraints.push_back(enableXYZLimit[i][1]? yLimit[i]: vec2f(-INFINITY, INFINITY)); + theta_constraints.push_back(enableXYZLimit[i][2]? zLimit[i]: vec2f(-INFINITY, INFINITY)); } } std::vector targets; std::vector endEffectorIDs; - std::vector startIDs; + std::vector depths; { auto items = get_input("IkChains")->getRaw(); for (auto &item: items) { - if (boneNameMapping.count(item->startName) == 0) { - log_warn("Not find ik startBone: {}", item->startName); - continue; - } if (boneNameMapping.count(item->endEffectorName) == 0) { log_warn("Not find ik endEffector: {}", item->endEffectorName); continue; } endEffectorIDs.push_back(boneNameMapping[item->endEffectorName]); - startIDs.push_back(boneNameMapping[item->startName]); + depths.push_back(item->depth); targets.push_back(item->targetPos); } } -// SolveIKConstrained( -// skeletonPtr, -// theta, -// theta_constraints, -// targets, -// endEffectorIDs, -// startIDs, -// iter_max -// ); + auto &total_theta_3 = skeleton->verts.attr("TotalTheta"); + std::vector total_theta(skeleton->verts.size() * 3); + for (auto i = 0; i < skeleton->verts.size(); i++) { + total_theta[i*3 + 0] = total_theta_3[i][0]; + total_theta[i*3 + 1] = total_theta_3[i][1]; + total_theta[i*3 + 2] = total_theta_3[i][2]; + } + SolveIKConstrained( + skeleton, + theta, + total_theta, + theta_constraints, + targets, + endEffectorIDs, + depths, + iter_max + ); + std::shared_ptr out_skeleton = FK(theta, skeleton); + { + auto &total_theta = out_skeleton->verts.attr("TotalTheta"); + for (auto i = 0; i < out_skeleton->verts.size(); i++) { + total_theta[i][0] += theta[i * 3 + 0]; + total_theta[i][1] += theta[i * 3 + 1]; + total_theta[i][2] += theta[i * 3 + 2]; + } + } + set_output2("Skeleton", out_skeleton); } }; ZENDEFNODE(IkSolver, { { "Skeleton", + {"int", "iterCount", "50"}, {"list", "IkChains"}, {"list", "jointLimits"}, }, @@ -2460,4 +2680,65 @@ ZENDEFNODE(IkSolver, { {}, {"FBXSDK"}, }); + +struct IkJointConstraints : INode { + void apply() override { + auto skeleton = get_input2("Skeleton"); + auto boneNameMapping = getBoneNameMapping(skeleton.get()); + auto rest_skeleton = get_input2("RestSkeleton"); + std::vector enableXYZLimit(skeleton->verts.size()); + std::vector xLimit(skeleton->verts.size()); + std::vector yLimit(skeleton->verts.size()); + std::vector zLimit(skeleton->verts.size()); + if (has_input("jointLimits")) { + auto items = get_input("jointLimits")->getRaw(); + for (auto &item: items) { + if (boneNameMapping.count(item->boneName)) { + auto index = boneNameMapping[item->boneName]; + enableXYZLimit[index] = item->enableLimit; + xLimit[index] = item->xLimit; + yLimit[index] = item->yLimit; + zLimit[index] = item->zLimit; + } + } + } + auto &total_theta_3 = skeleton->verts.attr("TotalTheta"); + for (auto i = 0; i < skeleton->verts.size(); i++) { + if (enableXYZLimit[i][0]) { + total_theta_3[i][0] = clamp(total_theta_3[i][0], xLimit[i][0], xLimit[i][1]); + } + if (enableXYZLimit[i][1]) { + total_theta_3[i][1] = clamp(total_theta_3[i][1], yLimit[i][0], yLimit[i][1]); + } + if (enableXYZLimit[i][2]) { + total_theta_3[i][2] = clamp(total_theta_3[i][2], zLimit[i][0], zLimit[i][1]); + } + } + + std::vector total_theta(skeleton->verts.size() * 3); + for (auto i = 0; i < skeleton->verts.size(); i++) { + total_theta.push_back(total_theta_3[i][0]); + total_theta.push_back(total_theta_3[i][1]); + total_theta.push_back(total_theta_3[i][2]); + + } + std::shared_ptr out_skeleton = FK(total_theta, skeleton); + set_output2("Skeleton", out_skeleton); + } +}; +ZENDEFNODE(IkJointConstraints, { + { + "Skeleton", + "RestSkeleton", + {"list", "jointLimits"}, + }, + { + "Skeleton", + }, + {}, + {"FBXSDK"}, +}); + + + } \ No newline at end of file