mtgodot-poc/libgr2/src/gr2_skeleton.cpp

// M0 T5 —— 骨架提取 + bind pose 自洽检查。见 docs/reference/steps/M0-gr2-reader.md
// 规格：granny_transform.cpp BuildCompositeTransform4x4 / granny_math.cpp
//       MatrixEqualsQuaternion3x3 + MatrixMultiply3x3 /
//       granny_matrix_operations.cpp ColumnMatrixMultiply4x3Impl /
//       granny_world_pose.cpp BuildWorldPoseNoCompositeLOD + granny_bone_operations.cpp
//       BuildFullWorldPoseOnly_Generic
#include "internal.h"
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>

namespace gr2 {

using detail::Ref;
using detail::TypeMember;

namespace {

// 行主序 4x4：R = A * B（ColumnMatrixMultiply4x3Impl 语义，平移在第 3 行 elem 12..14）
Mat4 mul4x3(const Mat4& A, const Mat4& B) {
    Mat4 R{};
    for (int i = 0; i < 3; ++i) {
        for (int k = 0; k < 3; ++k)
            R[i * 4 + k] = A[i * 4 + 0] * B[0 * 4 + k]
                         + A[i * 4 + 1] * B[1 * 4 + k]
                         + A[i * 4 + 2] * B[2 * 4 + k];
        R[i * 4 + 3] = 0.0f;
    }
    for (int k = 0; k < 3; ++k)
        R[12 + k] = A[12] * B[0 * 4 + k]
                  + A[13] * B[1 * 4 + k]
                  + A[14] * B[2 * 4 + k]
                  + B[12 + k];
    R[15] = 1.0f;
    return R;
}

// granny_math.cpp MatrixEqualsQuaternion3x3（行主序 3x3）
void quat_to_m3(const float q[4], float d[9]) {
    const float xx = q[0] * q[0], yy = q[1] * q[1], zz = q[2] * q[2];
    const float xy = q[0] * q[1], xz = q[0] * q[2], yz = q[1] * q[2];
    const float wx = q[3] * q[0], wy = q[3] * q[1], wz = q[3] * q[2];
    d[0] = 1 - 2 * (yy + zz); d[1] = 2 * (xy - wz);     d[2] = 2 * (xz + wy);
    d[3] = 2 * (xy + wz);     d[4] = 1 - 2 * (xx + zz); d[5] = 2 * (yz - wx);
    d[6] = 2 * (xz - wy);     d[7] = 2 * (yz + wx);     d[8] = 1 - 2 * (xx + yy);
}

void m3_mul(const float a[9], const float b[9], float r[9]) {
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            r[i * 3 + j] = a[i * 3 + 0] * b[0 * 3 + j]
                         + a[i * 3 + 1] * b[1 * 3 + j]
                         + a[i * 3 + 2] * b[2 * 3 + j];
}

} // namespace

// granny_transform (68B: u32 Flags; f32 Position[3]; f32 Orientation[4]; f32 ScaleShear[3][3])
void File::Impl::read_transform(Ref tf, Transform& out) const {
    out = Transform{};
    const uint8_t* p = at(tf, 68);
    if (!p) return;
    std::memcpy(&out.flags, p, 4);
    std::memcpy(out.position, p + 4, 12);
    std::memcpy(out.orientation, p + 16, 16);
    std::memcpy(out.scale_shear, p + 32, 36);
}

// → 行主序 4x4，上 3x3 = transpose(U3)，平移在 elem 12..14。BuildCompositeTransform4x4 语义。
Mat4 compose(const Transform& t) {
    float q3[9];
    quat_to_m3(t.orientation, q3);
    float u3[9];
    if (t.flags & 0x4 /* HasScaleShear */) m3_mul(q3, t.scale_shear, u3);
    else std::memcpy(u3, q3, sizeof u3);

    Mat4 out{};
    // C 的上 3x3 = transpose(U3)
    out[0] = u3[0]; out[1] = u3[3]; out[2] = u3[6]; out[3] = 0;
    out[4] = u3[1]; out[5] = u3[4]; out[6] = u3[7]; out[7] = 0;
    out[8] = u3[2]; out[9] = u3[5]; out[10] = u3[8]; out[11] = 0;
    out[12] = t.position[0]; out[13] = t.position[1]; out[14] = t.position[2]; out[15] = 1;
    return out;
}

void File::Impl::transform_to_composite(Ref tf, Mat4& out, uint32_t& flags) const {
    Transform t;
    read_transform(tf, t);
    if (!at(tf, 68)) {
        out = Mat4{1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};
        flags = 0;
        return;
    }
    out = compose(t);
    flags = t.flags;
}

// skeleton: {String Name; RefToArray Bones(bone[]); int32 LODType; VariantRef ExtendedData}
// bone:     {String Name; int32 ParentIndex; Transform LocalTransform;
//            Real32[16] InverseWorldTransform; Real32 LODError; VariantRef ExtendedData}
// 全程按类型树取偏移，不硬编码。
bool File::Impl::extract_skeleton(Ref skel_obj, Ref skel_type, Skeleton& out) {
    out.bones.clear();
    if (!skel_obj.valid() || !skel_type.valid()) return false;

    const auto& sms = parse_type(skel_type);
    const TypeMember* mBones = find_member(sms, "Bones");
    if (!mBones) return false;

    Ref   bones_base;
    uint32_t bone_count = 0, stride = 0;
    if (!read_ref_to_array(skel_obj, *mBones, bones_base, bone_count, stride)) return false;
    if (bone_count == 0) return false;
    if (bone_count > 8192 || stride == 0) return false;

    const auto& bms = parse_type(mBones->ref_type);
    const TypeMember* mName   = find_member(bms, "Name");
    const TypeMember* mParent = find_member(bms, "ParentIndex");
    const TypeMember* mLocal  = find_member(bms, "LocalTransform");
    if (!mLocal) mLocal = find_member(bms, "Transform");
    const TypeMember* mInvW   = find_member(bms, "InverseWorldTransform");
    if (!mInvW) mInvW = find_member(bms, "InverseWorld4x4");
    if (!mLocal || !mInvW) return false;

    out.bones.resize(bone_count);
    for (uint32_t i = 0; i < bone_count; ++i) {
        Ref b = { bones_base.section, bones_base.offset + i * stride };
        Bone& bo = out.bones[i];
        if (mName)   bo.name   = rd_str(member_slot(b, *mName));
        if (mParent) bo.parent = rd_i32(member_slot(b, *mParent));
        read_transform(member_slot(b, *mLocal), bo.local);
        bo.local_transform = compose(bo.local);
        bo.lt_flags = bo.local.flags;
        const uint8_t* iw = at(member_slot(b, *mInvW), 64);
        if (iw) for (int k = 0; k < 16; ++k) std::memcpy(&bo.inverse_world[k], iw + k * 4, 4);
        else bo.inverse_world = Mat4{};
    }
    return true;
}

// ── M2：姿势采样（自由函数 = 主 primitive；File::* 是便捷封装）──────────
namespace {
void accumulate_world(const Skeleton& sk, const std::vector<Mat4>& local,
                      std::vector<Mat4>& world, std::vector<Mat4>& skin,
                      const Mat4& root_offset) {
    const size_t n = sk.bones.size();
    const Mat4 I{1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};
    world.assign(n, I);
    skin.assign(n, I);
    for (size_t i = 0; i < n; ++i) {
        int32_t par = sk.bones[i].parent;
        const Mat4& parent_w = (par < 0 || (size_t)par >= i) ? root_offset : world[par];
        world[i] = mul4x3((i < local.size()) ? local[i] : sk.bones[i].local_transform, parent_w);
        skin[i]  = mul4x3(sk.bones[i].inverse_world, world[i]);
    }
}
} // namespace

void bind_pose(const Skeleton& sk, std::vector<Mat4>& world, std::vector<Mat4>& skin) {
    std::vector<Mat4> local(sk.bones.size());
    for (size_t i = 0; i < sk.bones.size(); ++i) local[i] = sk.bones[i].local_transform;
    accumulate_world(sk, local, world, skin, sk.initial_placement);
}

void sample_pose(const Skeleton& sk, const Animation& an, float t,
                 std::vector<Mat4>& world, std::vector<Mat4>& skin,
                 const Mat4* root_offset) {
    std::vector<Mat4> track_local;
    an.sample_local(t, track_local);

    std::vector<Mat4> local(sk.bones.size());
    for (size_t i = 0; i < sk.bones.size(); ++i) local[i] = sk.bones[i].local_transform;  // 默认 = bind

    // track 按骨骼名匹配（bone_index 若已在 anim 提取时对某个 skeleton 映射过，可能不是这个 sk）
    for (size_t ti = 0; ti < an.tracks.size() && ti < track_local.size(); ++ti) {
        const BoneTrack& tr = an.tracks[ti];
        int bi = -1;
        for (size_t k = 0; k < sk.bones.size(); ++k)
            if (sk.bones[k].name == tr.bone_name) { bi = int(k); break; }
        if (bi >= 0) local[bi] = track_local[ti];
    }
    accumulate_world(sk, local, world, skin, root_offset ? *root_offset : sk.initial_placement);
}

bool File::bind_pose(int skeleton, std::vector<Mat4>& world, std::vector<Mat4>& skin) const {
    if (!impl_ || skeleton < 0 || (size_t)skeleton >= impl_->skels_cache.size()) return false;
    gr2::bind_pose(impl_->skels_cache[skeleton], world, skin);
    return true;
}

bool File::sample_pose(int anim, float t, int skeleton,
                       std::vector<Mat4>& world, std::vector<Mat4>& skin) const {
    if (!impl_ || skeleton < 0 || (size_t)skeleton >= impl_->skels_cache.size()) return false;
    if (anim < 0 || (size_t)anim >= info_.animations.size())
        return bind_pose(skeleton, world, skin);
    gr2::sample_pose(impl_->skels_cache[skeleton], info_.animations[anim], t, world, skin);
    return true;
}

double File::Impl::skeleton_self_check(int skeleton) const {
    if (skeleton < 0 || (size_t)skeleton >= skels_cache.size()) return 1e9;
    const auto& sk = skels_cache[skeleton];
    const size_t n = sk.bones.size();
    if (n == 0) return 1e9;

    std::vector<Mat4> world(n);
    double dev = 0.0;
    size_t worst = 0;
    for (size_t i = 0; i < n; ++i) {
        int32_t par = sk.bones[i].parent;
        if (par < 0 || (size_t)par >= i) {
            // 根：world = local * InitialPlacement（BuildWorldPose 的 Offset4x4）
            world[i] = mul4x3(sk.bones[i].local_transform, sk.initial_placement);
        } else {
            world[i] = mul4x3(sk.bones[i].local_transform, world[par]);
        }
        // E = InverseWorld * world  ≈ I
        Mat4 E = mul4x3(sk.bones[i].inverse_world, world[i]);
        double bd = 0.0;
        for (int k = 0; k < 16; ++k) {
            float want = (k % 5 == 0) ? 1.0f : 0.0f;
            bd = std::max(bd, (double)std::fabs(E[k] - want));
        }
        if (bd > dev) { dev = bd; worst = i; }
    }
    if (getenv("GR2_DEBUG")) {
        fprintf(stderr, "[skel] self-check dev=%.5f worst bone %zu '%s' parent=%d\n",
                dev, worst, sk.bones[worst].name.c_str(), sk.bones[worst].parent);
        const Mat4& E = mul4x3(sk.bones[worst].inverse_world, world[worst]);
        fprintf(stderr, "[skel] E[worst]:");
        for (float x : E) fprintf(stderr, " %.3f", x);
        fprintf(stderr, "\n");
    }
    return dev;
}

} // namespace gr2