RailInterpolator.cc

#include "RailInterpolator.hh"
 
#include "game/field/RailManager.hh"
 
namespace Kinoko::Field {

RailInterpolator::RailInterpolator(f32 speed, u32 idx)
    : m_points(RailManager::Instance()->rail(idx)->points()), m_currVel(0.0f) {
    m_railIdx = idx;
    auto *rail = RailManager::Instance()->rail(idx);
    m_pointCount = rail->pointCount();
    m_isOscillating = rail->isOscillating();
    m_speed = speed;
}

RailInterpolator::~RailInterpolator() = default;

const EGG::Vector3f &RailInterpolator::floorNrm(size_t idx) const {
    return RailManager::Instance()->rail(m_railIdx)->floorNrm(idx);
}

f32 RailInterpolator::railLength() const {
    return RailManager::Instance()->rail(m_railIdx)->getPathLength();
}

void RailInterpolator::updateVel() {
    f32 t = m_segmentT;
    setCurrVel((1.0f - t) * m_prevPointVel + t * m_nextPointVel);
}

void RailInterpolator::calcVelocities() {
    m_prevPointVel = static_cast<f32>(m_points[m_currPointIdx].setting[0]);
    m_nextPointVel =
            shouldChangeDirection() ? 0.0f : static_cast<f32>(m_points[m_nextPointIdx].setting[0]);
 
    if (m_prevPointVel == 0.0f) {
        m_prevPointVel = m_speed;
    }
 
    if (m_nextPointVel == 0.0f) {
        m_nextPointVel = m_speed;
    }
 
    // @bug This is callable with an invalid m_nextPointIdx, so we need a safeguard
    // This invalid state is resolved very shortly after, but the vel propagates to the next frame
    // If t is ever not 0, this leads to undefined behavior, so we guard against it here
    if (shouldChangeDirection()) {
        ASSERT(m_segmentT == 0.0f);
    }
}

bool RailInterpolator::shouldChangeDirection() const {
    if (!m_isOscillating) {
        return m_pointCount == m_nextPointIdx;
    }
 
    return m_movementDirectionForward ? m_nextPointIdx == m_pointCount : m_nextPointIdx == -1;
}

void RailInterpolator::calcDirectionChange() {
    if (!m_isOscillating) {
        return;
    }
 
    if (m_movementDirectionForward) {
        m_nextPointIdx -= 2;
    } else {
        m_nextPointIdx += 2;
    }
 
    m_movementDirectionForward = !m_movementDirectionForward;
}
 
void RailInterpolator::calcNextIndices() {
    if (m_movementDirectionForward) {
        ++m_currPointIdx;
        ++m_nextPointIdx;
    } else {
        --m_currPointIdx;
        --m_nextPointIdx;
    }
 
    if (!m_isOscillating) {
        if (m_nextPointIdx == m_pointCount) {
            m_nextPointIdx = 0;
        }
 
        if (m_currPointIdx == m_pointCount) {
            m_currPointIdx = 0;
        }
    }
}

RailLinearInterpolator::RailLinearInterpolator(f32 speed, u32 idx) : RailInterpolator(speed, idx) {
    m_transitions = RailManager::Instance()->rail(m_railIdx)->getLinearTransitions();
    init(0.0f, 0);
}

RailLinearInterpolator::~RailLinearInterpolator() = default;

void RailLinearInterpolator::init(f32 t, u32 idx) {
    m_segmentT = t;
    m_currPointIdx = idx;
 
    bool isLastPoint = (static_cast<u16>(idx) == m_pointCount - 1);
    m_nextPointIdx = isLastPoint ? idx - 1 : idx + 1;
    m_movementDirectionForward = isLastPoint ? !m_isOscillating : true;
 
    m_curPos = m_points[m_currPointIdx].pos;
    m_currentDirection = m_points[m_nextPointIdx].pos - m_curPos;
    m_curTangentDir = m_currentDirection;
    m_curTangentDir.normalise2();
    m_currVel = m_speed;
    m_prevPointVel = m_speed;
    m_nextPointVel = m_speed;
    m_4a = false;
    m_usePerPointVelocities = false;
    m_segmentLength = m_currentDirection.length();
    m_currSegmentVel = m_speed / m_segmentLength;
}

RailInterpolator::Status RailLinearInterpolator::calc() {
    if (m_4a) {
        m_curPos = m_points[m_pointCount - 1].pos;
 
        return Status::ChangingDirection;
    }
 
    if (m_usePerPointVelocities) {
        updateVel();
    }
 
    m_segmentT += m_currSegmentVel;
    m_curPos = lerp(m_segmentT, m_currPointIdx, m_nextPointIdx);
 
    if (m_segmentT <= 1.0f) {
        return Status::InProgress;
    }
 
    Status status = Status::SegmentEnd;
 
    calcNextSegment();
 
    if (shouldChangeDirection()) {
        status = Status::ChangingDirection;
 
        calcDirectionChange();
    }
 
    m_currentDirection = m_points[m_nextPointIdx].pos - m_points[m_currPointIdx].pos;
    m_segmentLength = m_currentDirection.length();
    m_curTangentDir = m_currentDirection;
    m_currSegmentVel = m_currVel / m_segmentLength;
    m_curTangentDir.normalise2();
 
    return status;
}

void RailLinearInterpolator::setCurrVel(f32 speed) {
    m_currVel = speed;
    m_currSegmentVel = m_currVel / m_segmentLength;
}

void RailLinearInterpolator::evalCubicBezierOnPath(f32 t, EGG::Vector3f &currDir,
        EGG::Vector3f &curTangentDir) {
    s16 currIdx = 0;
    f32 len = 0.0f;
 
    getPathLocation(t, currIdx, len);
 
    s16 nextIdx = currIdx + 1;
    if (nextIdx == m_pointCount) {
        nextIdx = 0;
    }
 
    currDir = m_points[currIdx].pos * (1.0f - len) + m_points[nextIdx].pos * len;
    curTangentDir = m_transitions[currIdx].m_dir;
}

void RailLinearInterpolator::getPathLocation(f32 t, s16 &idx, f32 &len) {
    if (!m_movementDirectionForward) {
        return;
    }
 
    f32 dist = m_segmentT * m_transitions[m_currPointIdx].m_length;
    if (t <= dist) {
        idx = m_currPointIdx;
        len = (dist - t) * m_transitions[m_currPointIdx].m_lengthInv;
        return;
    }
 
    for (s32 i = 0; i < m_pointCount; ++i) {
        s32 currIdx = m_currPointIdx - 1;
        if (currIdx == -1) {
            currIdx = m_pointCount - 1;
        }
 
        currIdx -= i;
        if (currIdx < 0) {
            currIdx += m_pointCount;
        }
 
        dist += m_transitions[currIdx].m_length;
        if (t <= dist) {
            idx = currIdx;
            len = (dist - t) * m_transitions[currIdx].m_lengthInv;
            return;
        }
    }
}

void RailLinearInterpolator::calcNextSegment() {
    calcNextIndices();
 
    // @bug The game accesses POTI points out-of-bounds, but then course-corrects by
    // setting m_segmentT to 0.0f once it detects an invalid nextPointIdx, but after
    // it already read from undefined memory.
    if (shouldChangeDirection()) {
        m_segmentT = 0.0f;
    } else {
        f32 prevDirLength = m_segmentLength;
        m_currentDirection = m_points[m_nextPointIdx].pos - m_points[m_currPointIdx].pos;
        m_segmentLength = m_currentDirection.length();
        m_segmentT = ((m_segmentT - 1.0f) * prevDirLength) / m_segmentLength;
 
        if (m_segmentT > 1.0f) {
            m_segmentT = 0.99f;
        }
    }
 
    if (m_usePerPointVelocities) {
        calcVelocities();
    }
}

EGG::Vector3f RailLinearInterpolator::lerp(f32 t, u32 currIdx, u32 nextIdx) const {
    return m_points[currIdx].pos * (1.0f - t) + m_points[nextIdx].pos * t;
}

RailSmoothInterpolator::RailSmoothInterpolator(f32 speed, u32 idx) : RailInterpolator(speed, idx) {
    auto *rail = RailManager::Instance()->rail(m_railIdx);
    m_transitions = rail->getSplineTransitions();
    m_pathPercentages = rail->getPathPercentages();
 
    init(0.0f, 0);
}

RailSmoothInterpolator::~RailSmoothInterpolator() = default;

void RailSmoothInterpolator::init(f32 t, u32 idx) {
    m_segmentT = t;
 
    m_currPointIdx = idx;
 
    if (idx == static_cast<u32>(m_pointCount - 1) && m_isOscillating) {
        m_nextPointIdx = m_currPointIdx - 1;
        m_movementDirectionForward = false;
        m_curTangentDir = calcCubicBezierTangentDir(m_segmentT, m_transitions[m_currPointIdx]);
        m_currSegmentVel = m_currVel * m_transitions[m_nextPointIdx].m_lengthInv;
    } else {
        m_nextPointIdx = idx + 1 == m_pointCount ? 0 : idx + 1;
        m_movementDirectionForward = true;
        m_curTangentDir = calcCubicBezierTangentDir(m_segmentT, m_transitions[m_currPointIdx]);
        m_currSegmentVel = m_currVel * m_transitions[m_currPointIdx].m_lengthInv;
    }
 
    m_curPos = m_points[m_currPointIdx].pos;
    m_prevPos = m_points[m_currPointIdx].pos;
    m_currVel = m_speed;
    m_prevPointVel = m_speed;
    m_nextPointVel = m_speed;
    m_velocity = 0.0f;
    m_4a = false;
    m_usePerPointVelocities = false;
    m_velocityPending = false;
}

RailInterpolator::Status RailSmoothInterpolator::calc() {
    if (m_4a) {
        m_curPos = m_transitions[m_pointCount - 2].m_p3;
 
        return Status::ChangingDirection;
    }
 
    if (m_usePerPointVelocities) {
        updateVel();
    }
 
    m_prevPos = m_curPos;
 
    f32 t = m_movementDirectionForward ? calcT(m_segmentT) : calcT(1.0f - m_segmentT);
 
    calcCubicBezier(t, m_currPointIdx, m_nextPointIdx, m_curPos, m_curTangentDir);
 
    m_velocityPending = true;
    m_segmentT += m_currSegmentVel;
 
    if (m_segmentT <= 1.0f) {
        return Status::InProgress;
    }
 
    Status status = Status::SegmentEnd;
 
    calcNextSegment();
 
    if (shouldChangeDirection()) {
        status = Status::ChangingDirection;
 
        calcDirectionChange();
    }
 
    if (m_movementDirectionForward) {
        m_currSegmentVel = m_currVel * m_transitions[m_currPointIdx].m_lengthInv;
    } else {
        m_currSegmentVel = m_currVel * m_transitions[m_nextPointIdx].m_lengthInv;
    }
 
    return status;
}

void RailSmoothInterpolator::setCurrVel(f32 speed) {
    m_currVel = speed;
 
    if (m_movementDirectionForward) {
        m_currSegmentVel = speed * m_transitions[m_currPointIdx].m_lengthInv;
    } else {
        m_currSegmentVel = speed * m_transitions[m_nextPointIdx].m_lengthInv;
    }
}

void RailSmoothInterpolator::evalCubicBezierOnPath(f32 t, EGG::Vector3f &currDir,
        EGG::Vector3f &curTangentDir) {
    s16 currIdx = 0;
    f32 len = 0.0f;
 
    getPathLocation(t, currIdx, len);
 
    s16 nextIdx = currIdx + 1;
    if (nextIdx == m_pointCount) {
        nextIdx = 0;
    }
 
    len = m_movementDirectionForward ? calcT(len) : calcT(1.0f - len);
 
    calcCubicBezier(len, currIdx, nextIdx, currDir, curTangentDir);
}

void RailSmoothInterpolator::getPathLocation(f32 t, s16 &idx, f32 &len) {
    if (!m_movementDirectionForward) {
        return;
    }
 
    f32 dist = m_segmentT * m_transitions[m_currPointIdx].m_length;
    if (t <= dist) {
        idx = m_currPointIdx;
        len = (dist - t) * m_transitions[m_currPointIdx].m_lengthInv;
        return;
    }
 
    for (s32 i = 0; i < m_pointCount; ++i) {
        s32 currIdx = m_currPointIdx - 1;
        if (currIdx == -1) {
            currIdx = m_pointCount - 1;
        }
 
        currIdx -= i;
        if (currIdx < 0) {
            currIdx += m_pointCount;
        }
 
        dist += m_transitions[currIdx].m_length;
        if (t <= dist) {
            idx = currIdx;
            len = (dist - t) * m_transitions[currIdx].m_lengthInv;
            return;
        }
    }
}

void RailSmoothInterpolator::calcCubicBezier(f32 t, u32 currIdx, u32 nextIdx, EGG::Vector3f &pos,
        EGG::Vector3f &dir) const {
    const auto &transition =
            m_movementDirectionForward ? m_transitions[currIdx] : m_transitions[nextIdx];
 
    pos = calcCubicBezierPos(t, transition);
    dir = calcCubicBezierTangentDir(t, transition);
}

EGG::Vector3f RailSmoothInterpolator::calcCubicBezierPos(f32 t,
        const RailSplineTransition &trans) const {
    f32 dt = 1.0f - t;
 
    EGG::Vector3f res = trans.m_p0 * (dt * dt * dt);
    res += trans.m_p1 * (3.0f * t * (dt * dt));
    res += trans.m_p2 * (3.0f * (t * t) * dt);
    res += trans.m_p3 * (t * t * t);
 
    return res;
}

EGG::Vector3f RailSmoothInterpolator::calcCubicBezierTangentDir(f32 t,
        const RailSplineTransition &trans) const {
    EGG::Vector3f c1 = trans.m_p0 * -1.0f + trans.m_p1 * 3.0f - (trans.m_p2 * 3.0f) + trans.m_p3;
    EGG::Vector3f c2 = trans.m_p0 * 3.0f - trans.m_p1 * 6.0f + trans.m_p2 * 3.0f;
    EGG::Vector3f c3 = trans.m_p0 * -3.0f + trans.m_p1 * 3.0f;
    EGG::Vector3f ret = c1 * 3.0f * (t * t) + c2 * 2.0f * t + c3;
 
    ret.normalise2();
 
    if (!m_movementDirectionForward) {
        ret *= -1.0f;
    }
 
    return ret;
}

f32 RailSmoothInterpolator::calcT(f32 t) const {
    constexpr u32 ESTIMATOR_SAMPLE_COUNT = RailSpline::getEstimatorSampleCount();
    constexpr f32 ESTIMATOR_STEP = RailSpline::getEstimatorStep();
 
    u16 sampleIdx = m_movementDirectionForward ? m_currPointIdx * ESTIMATOR_SAMPLE_COUNT :
                                                 m_nextPointIdx * ESTIMATOR_SAMPLE_COUNT;
 
    for (u16 i = 0; i < ESTIMATOR_SAMPLE_COUNT - 1; ++i) {
        f32 currPercent = m_pathPercentages[sampleIdx + i];
        f32 nextPercent = m_pathPercentages[sampleIdx + i + 1];
 
        if (currPercent <= t && nextPercent > t) {
            f32 delta = (t - currPercent) / (nextPercent - currPercent);
            return ESTIMATOR_STEP * static_cast<f32>(i) + ESTIMATOR_STEP * delta;
        }
    }
 
    f32 lastPercent = m_pathPercentages[sampleIdx + ESTIMATOR_SAMPLE_COUNT - 1];
 
    if (lastPercent <= t && 1.0f >= t) {
        u16 idx = ESTIMATOR_SAMPLE_COUNT - 1;
        f32 delta = (t - lastPercent) / (1.0f - lastPercent);
        return ESTIMATOR_STEP * static_cast<f32>(idx) + ESTIMATOR_STEP * delta;
    }
 
    return 0.0f;
}

void RailSmoothInterpolator::calcNextSegment() {
    f32 nextT = m_segmentT - 1.0f;
 
    if (m_isOscillating) {
        if (m_nextPointIdx == 0 || m_nextPointIdx == m_pointCount - 1) {
            m_segmentT = 0.0f;
        } else if (m_movementDirectionForward) {
            m_segmentT = nextT * m_transitions[m_currPointIdx].m_length *
                    m_transitions[m_nextPointIdx].m_lengthInv;
        } else {
            m_segmentT = nextT * m_transitions[m_currPointIdx - 1].m_length *
                    m_transitions[m_nextPointIdx - 1].m_lengthInv;
        }
    } else {
        m_segmentT = nextT * m_transitions[m_currPointIdx].m_length *
                m_transitions[m_nextPointIdx].m_lengthInv;
    }
 
    if (m_segmentT > 1.0f) {
        m_segmentT = 0.99f;
    }
 
    calcNextIndices();
 
    if (m_usePerPointVelocities) {
        calcVelocities();
    }
}
 
} // namespace Kinoko::Field