BQP/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Transform/Rigid2.cs

/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using CartographerSharp.Common.Math;
using  RobotNet10.Shared.Numbers;

namespace CartographerSharp.Transform;

/// <summary>
/// Represents a rigid 2D transformation (translation + rotation).
/// </summary>
/// <remarks>
/// Creates a new rigid 2D transformation.
/// </remarks>
public struct Rigid2d(Vector2 translation, double rotation)
{
    private Vector2 _translation = translation;
    private readonly double _rotation = rotation; // Rotation angle in radians

    /// <summary>
    /// Creates an identity transformation.
    /// </summary>
    public static Rigid2d Identity => new(Vector2.Zero, 0.0);

    /// <summary>
    /// Creates a rotation-only transformation.
    /// </summary>
    public static Rigid2d FromRotation(double rotation)
    {
        return new Rigid2d(Vector2.Zero, rotation);
    }

    /// <summary>
    /// Creates a translation-only transformation.
    /// </summary>
    public static Rigid2d FromTranslation(Vector2 translation)
    {
        return new Rigid2d(translation, 0.0);
    }

    /// <summary>
    /// Gets the translation component.
    /// </summary>
    public readonly Vector2 Translation => _translation;

    /// <summary>
    /// Gets the rotation angle in radians.
    /// </summary>
    public readonly double Rotation => _rotation;

    /// <summary>
    /// Gets the normalized angle in the range [-pi, pi].
    /// </summary>
    public readonly double NormalizedAngle()
    {
        return MathUtils.NormalizeAngleDifference(_rotation);
    }

    /// <summary>
    /// Computes the inverse transformation.
    /// </summary>
    public readonly Rigid2d Inverse()
    {
        var cos = Math.Cos(_rotation);
        var sin = Math.Sin(_rotation);
        
        // Inverse rotation matrix R(-θ) = [cos(θ), sin(θ); -sin(θ), cos(θ)]
        // Inverse translation = -R(-θ) * translation
        var invTranslation = new Vector2(
            (-cos * _translation.X - sin * _translation.Y),
            (sin * _translation.X - cos * _translation.Y)
        );
        
        return new Rigid2d(invTranslation, -_rotation);
    }

    /// <summary>
    /// Transforms a point by this transformation.
    /// </summary>
    public readonly Vector2 TransformPoint(Vector2 point)
    {
        var cos = Math.Cos(_rotation);
        var sin = Math.Sin(_rotation);
        
        return new Vector2(
            (cos * point.X - sin * point.Y + _translation.X),
            (sin * point.X + cos * point.Y + _translation.Y)
        );
    }

    /// <summary>
    /// Composes two transformations: this * other.
    /// </summary>
    public static Rigid2d operator *(Rigid2d lhs, Rigid2d rhs)
    {
        var lhsCos = Math.Cos(lhs._rotation);
        var lhsSin = Math.Sin(lhs._rotation);
        
        // Rotate rhs translation by lhs rotation, then add lhs translation
        var composedTranslation = new Vector2(
            (lhsCos * rhs._translation.X - lhsSin * rhs._translation.Y + lhs._translation.X),
            (lhsSin * rhs._translation.X + lhsCos * rhs._translation.Y + lhs._translation.Y)
        );
        
        var composedRotation = lhs._rotation + rhs._rotation;
        
        return new Rigid2d(composedTranslation, composedRotation);
    }

    /// <summary>
    /// Transforms a point by a transformation.
    /// </summary>
    public static Vector2 operator *(Rigid2d rigid, Vector2 point)
    {
        return rigid.TransformPoint(point);
    }

    public override readonly string ToString()
    {
        return $"{{ t: [{_translation.X}, {_translation.Y}], r: [{_rotation}] }}";
    }
}

/// <summary>
/// Float version of Rigid2.
/// </summary>
/// <remarks>
/// Creates a new rigid 2D transformation.
/// </remarks>
public struct Rigid2f(Vector2 translation, double rotation)
{
    private Vector2 _translation = translation;
    private readonly double _rotation = rotation; // Rotation angle in radians

    /// <summary>
    /// Creates an identity transformation.
    /// </summary>
    public static Rigid2f Identity => new(Vector2.Zero, 0.0);

    /// <summary>
    /// Creates a rotation-only transformation.
    /// </summary>
    public static Rigid2f FromRotation(double rotation)
    {
        return new Rigid2f(Vector2.Zero, rotation);
    }

    /// <summary>
    /// Creates a translation-only transformation.
    /// </summary>
    public static Rigid2f FromTranslation(Vector2 translation)
    {
        return new Rigid2f(translation, 0.0);
    }

    /// <summary>
    /// Gets the translation component.
    /// </summary>
    public readonly Vector2 Translation => _translation;

    /// <summary>
    /// Gets the rotation angle in radians.
    /// </summary>
    public readonly double Rotation => _rotation;

    /// <summary>
    /// Gets the normalized angle in the range [-pi, pi].
    /// </summary>
    public readonly double NormalizedAngle()
    {
        return MathUtils.NormalizeAngleDifference(_rotation);
    }

    /// <summary>
    /// Computes the inverse transformation.
    /// </summary>
    public readonly Rigid2f Inverse()
    {
        var cos = Math.Cos(_rotation);
        var sin = Math.Sin(_rotation);
        
        var invTranslation = new Vector2(
            -cos * _translation.X - sin * _translation.Y,
            sin * _translation.X - cos * _translation.Y
        );
        
        return new Rigid2f(invTranslation, -_rotation);
    }

    /// <summary>
    /// Transforms a point by this transformation.
    /// </summary>
    public readonly Vector2 TransformPoint(Vector2 point)
    {
        var cos = Math.Cos(_rotation);
        var sin = Math.Sin(_rotation);
        
        return new Vector2(
            cos * point.X - sin * point.Y + _translation.X,
            sin * point.X + cos * point.Y + _translation.Y
        );
    }

    /// <summary>
    /// Composes two transformations: this * other.
    /// </summary>
    public static Rigid2f operator *(Rigid2f lhs, Rigid2f rhs)
    {
        var lhsCos = Math.Cos(lhs._rotation);
        var lhsSin = Math.Sin(lhs._rotation);
        
        var composedTranslation = new Vector2(
            lhsCos * rhs._translation.X - lhsSin * rhs._translation.Y + lhs._translation.X,
            lhsSin * rhs._translation.X + lhsCos * rhs._translation.Y + lhs._translation.Y
        );
        
        var composedRotation = lhs._rotation + rhs._rotation;
        
        return new Rigid2f(composedTranslation, composedRotation);
    }

    /// <summary>
    /// Transforms a point by a transformation.
    /// </summary>
    public static Vector2 operator *(Rigid2f rigid, Vector2 point)
    {
        return rigid.TransformPoint(point);
    }

    public override readonly string ToString()
    {
        return $"{{ t: [{_translation.X}, {_translation.Y}], r: [{_rotation}] }}";
    }
}