SF 차량 동역학: 폐루프 PID 고도 제어, 4점 레이캐스트 서스펜션 및 지면 효과 공기역학

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

export type PIDConfig = {
    Kp: number,
    Ki: number,
    Kd: number,
    MaxForce: number,
    MinForce: number,
}

export type ThrusterState = {
    Offset: Vector3,
    PrevError: number,
    Integral: number,
}

export type Hovercraft = {
    RootPart: BasePart,
    TargetHeight: number,
    Thrusters: { ThrusterState },
    PID: PIDConfig,
    LinearDrag: number,
    AngularDrag: number,
}

local HoverEngine = {}
HoverEngine.__index = HoverEngine

local GRAVITY = 196.2

function HoverEngine.new(root: BasePart, targetHeight: number): Hovercraft
    local halfX = root.Size.X * 0.45
    local halfZ = root.Size.Z * 0.45

    local thrusters: { ThrusterState } = {
        { Offset = Vector3.new(halfX, 0, halfZ), PrevError = 0, Integral = 0 },   -- Front Right
        { Offset = Vector3.new(-halfX, 0, halfZ), PrevError = 0, Integral = 0 },  -- Front Left
        { Offset = Vector3.new(halfX, 0, -halfZ), PrevError = 0, Integral = 0 },  -- Rear Right
        { Offset = Vector3.new(-halfX, 0, -halfZ), PrevError = 0, Integral = 0 }, -- Rear Left
    }

    local self: Hovercraft = setmetatable({
        RootPart = root,
        TargetHeight = targetHeight,
        Thrusters = thrusters,
        PID = {
            Kp = 850,
            Ki = 45,
            Kd = 120,
            MaxForce = root.AssemblyMass * GRAVITY * 2.5,
            MinForce = 0,
        },
        LinearDrag = 0.985,
        AngularDrag = 0.92,
    }, HoverEngine)

    return self
end

function HoverEngine:Update(dt: number)
    local root = self.RootPart
    local rootCF = root.CFrame
    local rootVel = root.AssemblyLinearVelocity
    local mass = root.AssemblyMass

    local rayParams = RaycastParams.new()
    rayParams.FilterType = RaycastFilterType.Exclude

    local totalThrustForce = Vector3.zero
    local totalThrustTorque = Vector3.zero

    for _, thruster in ipairs(self.Thrusters) do
        local worldPoint = rootCF:PointToWorldSpace(thruster.Offset)
        local rayDir = -rootCF.UpVector * (self.TargetHeight * 2.2)

        local result = Workspace:Raycast(worldPoint, rayDir, rayParams)
        local currentDist = result and (worldPoint - result.Position).Magnitude or (self.TargetHeight * 2.2)

        local errorVal = self.TargetHeight - currentDist

        -- P Term
        local pOut = self.PID.Kp * errorVal

        -- I Term with anti-windup clamping
        thruster.Integral = math.clamp(thruster.Integral + errorVal * dt, -50, 50)
        local iOut = self.PID.Ki * thruster.Integral

        -- D Term
        local dError = (errorVal - thruster.PrevError) / math.max(dt, 0.001)
        local dOut = self.PID.Kd * dError
        thruster.PrevError = errorVal

        -- Calculate net vertical thruster force
        local rawForce = pOut + iOut + dOut
        -- Counter gravity baseline
        local baseGravityShare = (mass * GRAVITY) / #self.Thrusters
        local finalThrust = math.clamp(rawForce + baseGravityShare, self.PID.MinForce, self.PID.MaxForce)

        if result then
            local thrustVector = rootCF.UpVector * finalThrust
            totalThrustForce += thrustVector

            local leverArm = worldPoint - root.AssemblyCenterOfMass
            totalThrustTorque += leverArm:Cross(thrustVector)
        end
    end

    -- Apply aerodynamic drag damping
    local horizontalVel = rootVel - (rootCF.UpVector * rootVel:Dot(rootCF.UpVector))
    local lateralDrift = horizontalVel:Dot(rootCF.RightVector)
    local lateralCounterForce = -rootCF.RightVector * (lateralDrift * mass * 4.0)

    root:ApplyAssemblyForce(totalThrustForce + lateralCounterForce)
    root:ApplyAssemblyTorque(totalThrustTorque)
end

return HoverEngine