diff --git a/src/AnimationMerger.cpp b/src/AnimationMerger.cpp index 921e2656..9013fc36 100644 --- a/src/AnimationMerger.cpp +++ b/src/AnimationMerger.cpp @@ -1643,6 +1643,98 @@ AnimationMerger::ApplyMotionResult AnimationMerger::applyMotionClip( const auto& q = clipQuats[frame][joint]; return Ogre::Quaternion(q[3], q[0], q[1], q[2]); // (w,x,y,z) }; + // #857: STABILIZED TWIST TRANSPORT. The aim above transports the + // bone's DIRECTION but inherits the target bind's roll — gesture- + // heavy clips (dance, wave) lose forearm/spine roll and read + // flat. Decompose the source's frame rotation into swing (the + // direction, already transported) + twist about the bone axis: + // Δ(f) = Ws(f) · Ws_bind⁻¹ (canonical axes) + // aim(f) = arc(d_bind → d(f)) + // θ(f) = signed angle of aim(f)⁻¹·Δ(f) about d_bind + // θ is wrapped to (−π,π] then UNWRAPPED across frames (a ±180° + // pop near the shortest-arc degeneracy would otherwise flip the + // roll direction mid-clip once a gain ≠ 1 scales it) and composed + // on the target about the SAME pole the source decomposed about: + // Qbase = arc(dt_bind → d_ref) · Wt_bind (once per bone) + // Wt(f) = twist(θ·gain, ds) · arc(d_ref → ds) · Qbase + // Recomposing per-frame from the TARGET BIND direction instead + // (the pre-#857 form) decomposes about a different pole than the + // source did — the roll component "between" the two poles leaks + // into swing and inflates amplitude (measured: Mixamo self-parity + // arm amp +38%, elbow error 3.3°→6.3°). With matched poles the + // per-frame relative motion is the source's world delta exactly, + // conjugated into target axes — direction stays absolute, roll + // transports losslessly, self-parity drops below the aim-only + // baseline. Per-role gains: collars damped (they share the + // shoulder line's roll), everything else transports fully. + static const float kTwistGain[22] = { + 1.f, 1.f, 1.f, 1.f, 1.f, 1.f, // spine chain + head + 0.5f, 1.f, 1.f, 1.f, // rcollar damped, right arm + 0.5f, 1.f, 1.f, 1.f, // lcollar damped, left arm + 1.f, 1.f, 1.f, 1.f, // right leg + foot + 1.f, 1.f, 1.f, 1.f }; // left leg + foot + constexpr float kTwistCap = 2.618f; // 150° — runaway-unwrap guard + std::vector> twistTheta( + static_cast(frames), + std::vector(static_cast(Jc), 0.0f)); + { + constexpr float kTau = 2.0f * static_cast(M_PI); + std::vector prev(static_cast(Jc), 0.0f); + std::vector has(static_cast(Jc), 0); + for (int f = 0; f < frames; ++f) + for (int c = 0; c < Jc; ++c) { + const Ogre::Vector3& as = + srcLocalAxis[static_cast(c)]; + if (as.squaredLength() <= 1e-8f) continue; + const auto& rq = cmuRestWorld[static_cast(c)]; + const Ogre::Quaternion restQ(rq[3], rq[0], rq[1], rq[2]); + const auto& sd = clipRestDir[static_cast(c)]; + Ogre::Vector3 dbind(sd[0], sd[1], sd[2]); + dbind.normalise(); + const Ogre::Quaternion Wf = clipQ(f, c); + const Ogre::Vector3 d = Wf * as; + const Ogre::Quaternion delta = Wf * restQ.Inverse(); + const Ogre::Quaternion aim = dbind.getRotationTo(d); + const Ogre::Quaternion tw = aim.Inverse() * delta; + float th = 2.0f * std::atan2( + tw.x * dbind.x + tw.y * dbind.y + tw.z * dbind.z, + tw.w); + th = std::remainder(th, kTau); + if (has[static_cast(c)]) + th += kTau * std::round( + (prev[static_cast(c)] - th) / kTau); + prev[static_cast(c)] = th; + has[static_cast(c)] = 1; + twistTheta[static_cast(f)] + [static_cast(c)] = + std::clamp(th, -kTwistCap, kTwistCap); + } + } + // Reference-aligned roll baseline per bone: aim the target bind + // at the source's REFERENCE direction once, so every per-frame + // swing below decomposes about the source's own pole. + std::vector dref(static_cast(Jc), + Ogre::Vector3::ZERO); + for (int c = 0; c < Jc; ++c) { + const auto& sd = clipRestDir[static_cast(c)]; + Ogre::Vector3 v(sd[0], sd[1], sd[2]); + if (v.squaredLength() > 1e-8f) { + v.normalise(); + dref[static_cast(c)] = CtInv * v; + } + } + std::vector Qbase(static_cast(nBones), + Ogre::Quaternion::IDENTITY); + for (int i = 0; i < nBones; ++i) { + const int c = boneToCanon[i]; + if (c >= 0 && c < Jc + && srcLocalAxis[static_cast(c)].squaredLength() + > 1e-8f) + Qbase[static_cast(i)] = + tb.tgtBindDir[static_cast(c)] + .getRotationTo(dref[static_cast(c)]) + * tb.bindWorld[static_cast(i)]; + } std::vector tracks( static_cast(nBones), nullptr); for (int i = 0; i < nBones; ++i) @@ -1675,10 +1767,19 @@ AnimationMerger::ApplyMotionResult AnimationMerger::applyMotionClip( (clipQ(f, c) * srcLocalAxis[static_cast(c)]); const Ogre::Quaternion R = - tb.tgtBindDir[static_cast(c)] - .getRotationTo(ds); - const Ogre::Quaternion Wt = - R * tb.bindWorld[static_cast(i)]; + dref[static_cast(c)].getRotationTo(ds); + Ogre::Quaternion Wt = + R * Qbase[static_cast(i)]; + // #857: re-apply the source's roll about the aimed + // direction (the swing above deliberately dropped it). + const float th = twistTheta[static_cast(f)] + [static_cast(c)] + * kTwistGain[c]; + if (std::abs(th) > 1e-5f) { + Ogre::Vector3 axis = ds; + axis.normalise(); + Wt = Ogre::Quaternion(Ogre::Radian(th), axis) * Wt; + } local = Wp.Inverse() * Wt; W[static_cast(i)] = Wt; } else { diff --git a/src/AnimationMerger_test.cpp b/src/AnimationMerger_test.cpp index e4f8a2ac..b451f1e3 100644 --- a/src/AnimationMerger_test.cpp +++ b/src/AnimationMerger_test.cpp @@ -1139,3 +1139,201 @@ TEST_F(AnimationMergerTest, ArmSpaceFollowsAnimationRename) "walk_wide"); EXPECT_GT(degBetween(wide, neutral), 15.0f); // it actually moved back } + +// ── #857: twist transport in the bind-referenced direction retarget ───────── + +namespace { +// Canonical-clip inputs for a virtual SOURCE rig whose bind is rotated 90° +// about Z (a deliberately foreign convention — exercises the restWorld +// conjugation): restDir = clean canonical T-pose directions, restWorld = the +// same non-identity quat everywhere, and identity motion W(f) = restQ. +constexpr int kJc = 22; +const Ogre::Quaternion kSrcRest(Ogre::Degree(90), Ogre::Vector3::UNIT_Z); + +std::vector> canonRestDirs() +{ + static const float d[kJc][3] = { + {0,1,0},{0,1,0},{0,1,0},{0,1,0},{0,1,0},{0,1,0}, + {-1,0,0},{-1,0,0},{-1,0,0},{-1,0,0}, + {1,0,0},{1,0,0},{1,0,0},{1,0,0}, + {0,-1,0},{0,-1,0},{0,-1,0},{0,0,1}, + {0,-1,0},{0,-1,0},{0,-1,0},{0,0,1}}; + std::vector> out(kJc); + for (int c = 0; c < kJc; ++c) out[c] = {d[c][0], d[c][1], d[c][2]}; + return out; +} + +std::vector> srcRestWorld() +{ + return std::vector>( + kJc, {kSrcRest.x, kSrcRest.y, kSrcRest.z, kSrcRest.w}); +} + +// frames of identity motion (every joint sits at the source bind) +std::vector>> identityClip(int frames) +{ + return std::vector>>( + frames, std::vector>( + kJc, {kSrcRest.x, kSrcRest.y, kSrcRest.z, kSrcRest.w})); +} + +// signed rotation angle of world-orientation delta `q` about unit axis `ax` +float twistDegAbout(const Ogre::Quaternion& q, const Ogre::Vector3& ax) +{ + const float s = q.x * ax.x + q.y * ax.y + q.z * ax.z; + return 2.0f * std::atan2(s, q.w) * 180.0f / static_cast(M_PI); +} +} // namespace + +TEST_F(AnimationMergerTest, TwistTransportCarriesBoneRoll) +{ + // makeArmRigEntity resolves only 9/22 roles — below applyMotionClip's + // humanoid gate (>= 11) — so build a fuller rig (13 roles: + hands/feet). + auto skelRes = Ogre::SkeletonManager::getSingleton().create( + "twist_roll_skel", + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + unsigned short h = 0; + auto bone = [&](const char* n, const Ogre::Vector3& p, Ogre::Bone* par) { + auto* b = skelRes->createBone(n, h++); + b->setPosition(p); + if (par) par->addChild(b); + return b; + }; + auto* hips = bone("Hips", {0, 1.0f, 0}, nullptr); + auto* spine = bone("Spine", {0, 0.3f, 0}, hips); + bone("Head", {0, 0.4f, 0}, spine); + auto* lleg = bone("LeftUpLeg", {0.15f, -0.1f, 0}, hips); + bone("LeftFoot", {0, -0.8f, 0}, lleg); + auto* rleg = bone("RightUpLeg", {-0.15f, -0.1f, 0}, hips); + bone("RightFoot", {0, -0.8f, 0}, rleg); + auto* rsh = bone("RightArm", {-0.2f, 0.1f, 0}, spine); + auto* rfa = bone("RightForeArm", {-0.3f, 0, 0}, rsh); + bone("RightHand", {-0.25f, 0, 0}, rfa); + auto* lsh = bone("LeftArm", {0.2f, 0.1f, 0}, spine); + auto* lfa = bone("LeftForeArm", {0.3f, 0, 0}, lsh); + bone("LeftHand", {0.25f, 0, 0}, lfa); + skelRes->setBindingPose(); + auto mesh = createInMemoryMesh("twist_roll_mesh", skelRes); + auto* sm = Manager::getSingleton()->getSceneMgr(); + Ogre::Entity* ent = sm->createEntity("twist_roll_ent", mesh); + ASSERT_NE(ent, nullptr); + Ogre::SkeletonInstance* skel = ent->getSkeleton(); + + // Source: left upper arm (role 11, +X) ROLLS 60° about its own axis over + // the clip while its direction stays put. Pre-#857 the retarget dropped + // this entirely (aim-only) — the target bone never moved. + const int frames = 31; + auto quats = identityClip(frames); + for (int f = 0; f < frames; ++f) { + const float a = 60.0f * static_cast(f) / (frames - 1); + const Ogre::Quaternion w = + Ogre::Quaternion(Ogre::Degree(a), Ogre::Vector3::UNIT_X) + * kSrcRest; + quats[f][11] = {w.x, w.y, w.z, w.w}; + } + const auto res = AnimationMerger::applyMotionClip( + skel, "twistclip", quats, 30, /*worldFrame=*/true, srcRestWorld(), + false, 8, false, canonRestDirs()); + ASSERT_TRUE(res.ok) << res.error.toStdString(); + + // Direction is invariant under a pure roll… + const Ogre::Vector3 dir0(1, 0, 0); + skel->reset(true); + skel->getAnimation("twistclip")->apply(skel, 1.0f); + skel->_updateTransforms(); + const Ogre::Vector3 a = + skel->getBone("LeftArm")->_getDerivedPosition(); + const Ogre::Vector3 b = + skel->getBone("LeftForeArm")->_getDerivedPosition(); + EXPECT_GT((b - a).normalisedCopy().dotProduct(dir0), 0.99f); + + // …but the bone's world orientation now carries the 60° roll about it. + const Ogre::Quaternion w = + skel->getBone("LeftArm")->_getDerivedOrientation(); + EXPECT_NEAR(twistDegAbout(w, dir0), 60.0f, 4.0f); + + // Legs saw identity source motion — they must not pick up any rotation. + const Ogre::Quaternion leg = + skel->getBone("LeftUpLeg")->_getDerivedOrientation(); + EXPECT_NEAR(std::abs(leg.w), 1.0f, 1e-3f); + + sm->destroyEntity(ent); +} + +TEST_F(AnimationMergerTest, TwistUnwrapKeepsDampedCollarContinuous) +{ + // Rig WITH clavicles (Mixamo "Shoulder" → collar roles 6/10). + auto skelRes = Ogre::SkeletonManager::getSingleton().create( + "twist_collar_skel", + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + unsigned short h = 0; + auto bone = [&](const char* n, const Ogre::Vector3& p, Ogre::Bone* par) { + auto* b = skelRes->createBone(n, h++); + b->setPosition(p); + if (par) par->addChild(b); + return b; + }; + auto* hips = bone("Hips", {0, 1.0f, 0}, nullptr); + auto* spine = bone("Spine", {0, 0.3f, 0}, hips); + bone("Head", {0, 0.4f, 0}, spine); + bone("LeftUpLeg", {0.15f, -0.1f, 0}, hips); + bone("RightUpLeg", {-0.15f, -0.1f, 0}, hips); + auto* lcol = bone("LeftShoulder", {0.05f, 0.25f, 0}, spine); + auto* larm = bone("LeftArm", {0.15f, 0, 0}, lcol); + bone("LeftForeArm", {0.3f, 0, 0}, larm); + auto* rcol = bone("RightShoulder", {-0.05f, 0.25f, 0}, spine); + auto* rarm = bone("RightArm", {-0.15f, 0, 0}, rcol); + bone("RightForeArm", {-0.3f, 0, 0}, rarm); + skelRes->setBindingPose(); + auto mesh = createInMemoryMesh("twist_collar_mesh", skelRes); + auto* sm = Manager::getSingleton()->getSceneMgr(); + Ogre::Entity* ent = sm->createEntity("twist_collar_ent", mesh); + Ogre::SkeletonInstance* skel = ent->getSkeleton(); + + // Source: left collar (role 10, +X) rolls 0 → 240° — past the ±180° wrap. + // The gain table damps collars to 0.5×, which is exactly where a missing + // unwrap explodes: wrapped −120° would scale to −60° instead of the + // capped +150°'s half — a mid-clip snap. + const int frames = 61; + auto quats = identityClip(frames); + for (int f = 0; f < frames; ++f) { + const float a = 240.0f * static_cast(f) / (frames - 1); + const Ogre::Quaternion w = + Ogre::Quaternion(Ogre::Degree(a), Ogre::Vector3::UNIT_X) + * kSrcRest; + quats[f][10] = {w.x, w.y, w.z, w.w}; + } + const auto res = AnimationMerger::applyMotionClip( + skel, "collarclip", quats, 30, true, srcRestWorld(), + false, 8, false, canonRestDirs()); + ASSERT_TRUE(res.ok) << res.error.toStdString(); + + // Sample densely: the collar's world orientation must move CONTINUOUSLY + // (no wrap snap) and end near 240° × 0.5 = 120° about +X. + Ogre::Quaternion prev = Ogre::Quaternion::IDENTITY; + float maxStepDeg = 0.0f; + Ogre::Quaternion last; + auto* anim = skel->getAnimation("collarclip"); + const float len = anim->getLength(); + for (int s = 0; s <= 60; ++s) { + skel->reset(true); + anim->apply(skel, len * static_cast(s) / 60.0f); + skel->_updateTransforms(); + const Ogre::Quaternion w = + skel->getBone("LeftShoulder")->_getDerivedOrientation(); + if (s > 0) { + const Ogre::Quaternion d = w * prev.Inverse(); + const float step = 2.0f * std::acos(std::min( + 1.0f, std::abs(d.w))) * 180.0f / static_cast(M_PI); + maxStepDeg = std::max(maxStepDeg, step); + } + prev = w; + last = w; + } + EXPECT_LT(maxStepDeg, 15.0f) << "collar roll snapped mid-clip (unwrap)"; + // 240° source twist hits the 150° runaway-unwrap cap FIRST, then the + // 0.5× collar gain: 150 × 0.5 = 75°. + EXPECT_NEAR(twistDegAbout(last, Ogre::Vector3::UNIT_X), 75.0f, 8.0f); + + sm->destroyEntity(ent); +}