다관절 절차적 기구학: FABRIK 솔버, 지형 레이캐스팅 및 거미/4족 보행 상태 머신

--!strict
local RunService = game:GetService("RunService")
local Workspace = game:GetService("Workspace")

export type LegChain = {
    RootAttachment: Attachment,
    Bones: { BasePart },
    Lengths: { number },
    CurrentFootPos: Vector3,
    TargetFootPos: Vector3,
    LastGroundedPos: Vector3,
    StepProgress: number, // 0.0 to 1.0
    IsStepping: boolean,
    GaitPhaseOffset: number, // 0.0 to 1.0
}

local ProceduralIK = {}
ProceduralIK.__index = ProceduralIK

function ProceduralIK.solveFABRIK(points: { Vector3 }, lengths: { number }, target: Vector3, maxIterations: number, tolerance: number): { Vector3 }
    local n = #points
    local totalLength = 0
    for _, l in ipairs(lengths) do totalLength += l end

    local origin = points[1]
    local distToTarget = (target - origin).Magnitude

    if distToTarget >= totalLength then
        -- Target unreachable: stretch in a straight line
        local dir = (target - origin).Unit
        for i = 2, n do
            points[i] = points[i - 1] + dir * lengths[i - 1]
        end
        return points
    end

    -- Iterative relaxation
    for _ = 1, maxIterations do
        -- Backward pass: set tip to target
        points[n] = target
        for i = n - 1, 1, -1 do
            local dir = (points[i] - points[i + 1]).Unit
            points[i] = points[i + 1] + dir * lengths[i]
        end

        -- Forward pass: anchor base to origin
        points[1] = origin
        for i = 1, n - 1 do
            local dir = (points[i + 1] - points[i]).Unit
            points[i + 1] = points[i] + dir * lengths[i]
        end

        if (points[n] - target).Magnitude <= tolerance then
            break
        end
    end

    return points
end

function ProceduralIK.sampleCubicBezier(p0: Vector3, p1: Vector3, p2: Vector3, p3: Vector3, t: number): Vector3
    local u = 1 - t
    return (u^3 * p0) + (3 * u^2 * t * p1) + (3 * u * t^2 * p2) + (t^3 * p3)
end

function ProceduralIK:UpdateLeg(leg: LegChain, chassisCF: CFrame, velocity: Vector3, dt: number, globalGaitTime: number)
    local hipWorld = chassisCF:PointToWorldSpace(leg.RootAttachment.Position)
    local maxReach = leg.Lengths[1] + leg.Lengths[2] + leg.Lengths[3]

    -- Predict future foot target based on velocity lead
    local leadOffset = velocity * 0.25
    local rayOrigin = hipWorld + leadOffset + Vector3.new(0, 4, 0)
    local rayParams = RaycastParams.new()
    rayParams.FilterType = RaycastFilterType.Exclude

    local rayResult = Workspace:Raycast(rayOrigin, Vector3.new(0, -maxReach * 1.5, 0), rayParams)
    local idealTarget = rayResult and rayResult.Position or (hipWorld + Vector3.new(0, -maxReach * 0.8, 0))

    local distToCurrent = (idealTarget - leg.CurrentFootPos).Magnitude
    local gaitPhase = (globalGaitTime + leg.GaitPhaseOffset) % 1.0

    if not leg.IsStepping and distToCurrent > (maxReach * 0.45) and gaitPhase < 0.5 then
        leg.IsStepping = true
        leg.LastGroundedPos = leg.CurrentFootPos
        leg.TargetFootPos = idealTarget
        leg.StepProgress = 0
    end

    if leg.IsStepping then
        leg.StepProgress = math.min(1.0, leg.StepProgress + dt * 4.5)
        local t = leg.StepProgress
        local p0 = leg.LastGroundedPos
        local p3 = leg.TargetFootPos
        local stepHeight = Vector3.new(0, maxReach * 0.35, 0)
        local p1 = p0 + stepHeight
        local p2 = p3 + stepHeight

        leg.CurrentFootPos = ProceduralIK.sampleCubicBezier(p0, p1, p2, p3, t)

        if leg.StepProgress >= 1.0 then
            leg.IsStepping = false
            leg.CurrentFootPos = leg.TargetFootPos
        end
    end

    -- Solve FABRIK chain
    local initialPoints = {
        hipWorld,
        hipWorld + Vector3.new(0, -leg.Lengths[1], 0),
        hipWorld + Vector3.new(0, -(leg.Lengths[1] + leg.Lengths[2]), 0),
        leg.CurrentFootPos
    }

    local solved = ProceduralIK.solveFABRIK(initialPoints, leg.Lengths, leg.CurrentFootPos, 4, 0.05)

    -- Apply CFrame to bone parts
    for i = 1, #leg.Bones do
        local pA = solved[i]
        local pB = solved[i + 1]
        local center = (pA + pB) * 0.5
        leg.Bones[i].CFrame = CFrame.lookAt(center, pB)
    end
end

return ProceduralIK