using System;

namespace MultiWheelC.TrajectoryPlanning.Utils;

/// <summary>Two-dimensional rigid-body coordinate transforms. Units are preserved.</summary>
public static class CoordinateTransform
{
    public static void LocalToWorld(
        double originX, double originY, double headingRadians,
        double localX, double localY,
        out double worldX, out double worldY)
    {
        double cosine = Math.Cos(headingRadians);
        double sine = Math.Sin(headingRadians);
        worldX = originX + cosine * localX - sine * localY;
        worldY = originY + sine * localX + cosine * localY;
    }
}
