로블록스 절차적 발 착지 IK: 2-Bone 역기구학 코사인 법칙, 골반 높이 보정 및 표면 법선 정렬

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

local FootIK = {}
FootIK.__index = FootIK

export type LegRig = {
    HipMotor: Motor6D,
    KneeMotor: Motor6D,
    AnkleMotor: Motor6D,
    HipAttachment: Attachment,
    ThighLength: number,
    CalfLength: number,
    CurrentFootOffset: number,
}

export type CharacterIK = {
    Character: Model,
    RootJoint: Motor6D,
    LeftLeg: LegRig,
    RightLeg: LegRig,
    RayParams: RaycastParams,
    HipDropOffset: number,
}

local function SolveTwoBone(origin: CFrame, targetPos: Vector3, l1: number, l2: number): (CFrame, CFrame)
    local localized = origin:PointToObjectSpace(targetPos)
    local dist = localized.Magnitude
    dist = math.clamp(dist, 0.05, (l1 + l2) - 0.001)

    local cosAlpha = (l1 * l1 + dist * dist - l2 * l2) / (2 * l1 * dist)
    local alpha = math.acos(math.clamp(cosAlpha, -1, 1))

    local cosBeta = (l1 * l1 + l2 * l2 - dist * dist) / (2 * l1 * l2)
    local beta = math.acos(math.clamp(cosBeta, -1, 1))

    local planeRot = CFrame.lookAt(Vector3.zero, localized)
    local hipRot = planeRot * CFrame.Angles(alpha, 0, 0)
    local kneeRot = CFrame.Angles(-(math.pi - beta), 0, 0)

    return hipRot, kneeRot
end

function FootIK.Step(ik: CharacterIK, dt: number)
    local char = ik.Character
    local rootPart = char.PrimaryPart
    if not rootPart then return end

    local rayDown = Vector3.new(0, -5, 0)
    local leftOrigin = ik.LeftLeg.HipAttachment.WorldPosition
    local rightOrigin = ik.RightLeg.HipAttachment.WorldPosition

    local leftHit = Workspace:Raycast(leftOrigin, rayDown, ik.RayParams)
    local rightHit = Workspace:Raycast(rightOrigin, rayDown, ik.RayParams)

    local leftTarget = leftHit and leftHit.Position or (leftOrigin + Vector3.new(0, -3.2, 0))
    local rightTarget = rightHit and rightHit.Position or (rightOrigin + Vector3.new(0, -3.2, 0))

    -- 골반 하강(Hip Drop) 계산
    local leftDeficit = leftOrigin.Y - leftTarget.Y
    local rightDeficit = rightOrigin.Y - rightTarget.Y
    local maxDeficit = math.max(leftDeficit, rightDeficit)
    local targetHipDrop = math.clamp(maxDeficit - 3.2, 0, 1.8)

    ik.HipDropOffset = ik.HipDropOffset + (targetHipDrop - ik.HipDropOffset) * math.clamp(dt * 15, 0, 1)
    ik.RootJoint.Transform = CFrame.new(0, -ik.HipDropOffset, 0)

    -- 좌측 다리 연산
    local leftHipCF = ik.LeftLeg.HipMotor.Part0.CFrame * ik.LeftLeg.HipMotor.C0
    local lHipRot, lKneeRot = SolveTwoBone(leftHipCF, leftTarget, ik.LeftLeg.ThighLength, ik.LeftLeg.CalfLength)
    ik.LeftLeg.HipMotor.Transform = lHipRot
    ik.LeftLeg.KneeMotor.Transform = lKneeRot

    -- 우측 다리 연산
    local rightHipCF = ik.RightLeg.HipMotor.Part0.CFrame * ik.RightLeg.HipMotor.C0
    local rHipRot, rKneeRot = SolveTwoBone(rightHipCF, rightTarget, ik.RightLeg.ThighLength, ik.RightLeg.CalfLength)
    ik.RightLeg.HipMotor.Transform = rHipRot
    ik.RightLeg.KneeMotor.Transform = rKneeRot
end

return FootIK