Damped Springs

--******************************************************************************
-- CalcDampedSimpleHarmonicMotion
-- This function will update the supplied position and velocity values over
-- the given time according to the spring parameters.
-- - An angular frequency is given to control how fast the spring oscillates.
-- - A damping ratio is given to control how fast the motion decays.
--     damping ratio > 1: over damped
--     damping ratio = 1: critically damped
--     damping ratio < 1: under damped
--******************************************************************************
function CalcDampedSimpleHarmonicMotion()
    local pPos              = -- position value to update
    local pVel              = -- velocity value to update
    local equilibriumPos    = -- position to approach
    local deltaTime         = -- time to update over
    local angularFrequency  = -- angular frequency of motion
    local dampingRatio      = -- damping ratio of motion
    local initialPos        = 0
    local initialVel        = 0

    local epsilon = 0.0001

    -- if there is no angular frequency, the spring will not move
    if angularFrequency < epsilon then
        return
    end

    -- clamp the damping ratio in legal range
    if dampingRatio < 0.0 then
        dampingRatio = 0.0
        -- calculate initial state in equilibrium relative space
        initialPos = pPos - equilibriumPos
        initialVel = pVel
    end

    -- if over-damped
    if dampingRatio > (1.0 + epsilon) then
        -- calculate constants based on motion parameters
        -- Note: These could be cached off between multiple calls using the same
        -- parameters for deltaTime, angularFrequency and dampingRatio.
        local za        = -angularFrequency * dampingRatio
        local zb        = angularFrequency * math.sqrt(dampingRatio*dampingRatio - 1.0)
        local z1        = za - zb
        local z2        = za + zb
        local expTerm1  = math.exp(z1 * deltaTime)
        local expTerm2  = math.exp(z2 * deltaTime)

        --update motion
        local c1    = (initialVel - initialPos*z2) / (-2.0*zb) -- z1 - z2 = -2*zb
        local c2    = initialPos - c1
        pPos        = equilibriumPos + c1*expTerm1 + c2*expTerm2
        pVel        = c1*z1*expTerm1 + c2*z2*expTerm2

    -- else if critically damped
    elseif dampingRatio > (1.0 - epsilon) then
        -- calculate constants based on motion parameters
        -- Note: These could be cached off between multiple calls using the same
        -- parameters for deltaTime, angularFrequency and dampingRatio.
        local expTerm = math.exp(-angularFrequency * deltaTime)

        --update motion
        local c1    = initialVel + angularFrequency * initialPos
        local c2    = initialPos
        local c3    = (c1*deltaTime + c2) * expTerm
        pPos        = equilibriumPos + c3
        pVel        = (c1*expTerm) - (c3*angularFrequency)

    --else under-damped
    else
        -- calculate constants based on motion parameters
        -- Note: These could be cached off between multiple calls using the same
        -- parameters for deltaTime, angularFrequency and dampingRatio.
        local omegaZeta = angularFrequency * dampingRatio
        local alpha     = angularFrequency * math.sqrt(1.0 - (dampingRatio*dampingRatio))
        local expTerm   = math.exp(-omegaZeta * deltaTime)
        local cosTerm   = math.cos(alpha * deltaTime)
        local sinTerm   = math.sin(alpha * deltaTime)

        -- update motion
        local c1    = initialPos
        local c2    = (initialVel + (omegaZeta*initialPos)) / alpha
        pPos        = equilibriumPos + (expTerm*((c1*cosTerm) + (c2*sinTerm)))
        pVel        = -expTerm*(((c1*omegaZeta) - (c2*alpha))*cosTerm + ((c1*alpha) + (c2*omegaZeta))*sinTerm)
    end
end