BQP/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Transform/Rigid3.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  RobotNet10.Shared.Numbers;

namespace CartographerSharp.Transform;

/// <summary>
/// Represents a rigid 3D transformation (translation + rotation).
/// </summary>
/// <remarks>
/// Creates a new rigid 3D transformation.
/// </remarks>
public struct Rigid3d(Vector3 translation, Quaternion rotation)
{
    private Vector3 _translation = translation;
    private Quaternion _rotation = Quaternion.Normalize(rotation);

    /// <summary>
    /// Creates an identity transformation.
    /// </summary>
    public static Rigid3d Identity => new(Vector3.Zero, Quaternion.Identity);

    /// <summary>
    /// Creates a rotation-only transformation.
    /// </summary>
    public static Rigid3d FromRotation(Quaternion rotation)
    {
        return new Rigid3d(Vector3.Zero, rotation);
    }

    /// <summary>
    /// Creates a translation-only transformation.
    /// </summary>
    public static Rigid3d FromTranslation(Vector3 translation)
    {
        return new Rigid3d(translation, Quaternion.Identity);
    }

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

    /// <summary>
    /// Gets the rotation quaternion.
    /// </summary>
    public readonly Quaternion Rotation => _rotation;

    /// <summary>
    /// Computes the inverse transformation.
    /// </summary>
    public readonly Rigid3d Inverse()
    {
        var invRotation = Quaternion.Conjugate(_rotation);
        var invTranslation = Vector3.Transform(-_translation, invRotation);
        return new Rigid3d(invTranslation, invRotation);
    }

    /// <summary>
    /// Transforms a point by this transformation.
    /// </summary>
    public readonly Vector3 TransformPoint(Vector3 point)
    {
        return Vector3.Transform(point, _rotation) + _translation;
    }

    /// <summary>
    /// Composes two transformations: this * other.
    /// </summary>
    public static Rigid3d operator *(Rigid3d lhs, Rigid3d rhs)
    {
        var composedTranslation = Vector3.Transform(rhs._translation, lhs._rotation) + lhs._translation;
        var composedRotation = Quaternion.Normalize(lhs._rotation * rhs._rotation);
        return new Rigid3d(composedTranslation, composedRotation);
    }

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

    /// <summary>
    /// Checks if the transformation is valid (no NaN values, quaternion is normalized).
    /// </summary>
    public readonly bool IsValid()
    {
        const double tolerance = 1e-3;
        var norm = _rotation.Length();
        var normDiff = Math.Abs(1.0 - norm);
        return !double.IsNaN(_translation.X) && !double.IsNaN(_translation.Y) && !double.IsNaN(_translation.Z) && normDiff < tolerance;
    }

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

/// <summary>
/// Float version of Rigid3.
/// </summary>
/// <remarks>
/// Creates a new rigid 3D transformation.
/// </remarks>
public struct Rigid3f(Vector3 translation, Quaternion rotation)
{
    private Vector3 _translation = translation;
    private Quaternion _rotation = Quaternion.Normalize(rotation);

    /// <summary>
    /// Creates an identity transformation.
    /// </summary>
    public static Rigid3f Identity => new(Vector3.Zero, Quaternion.Identity);

    /// <summary>
    /// Creates a rotation-only transformation.
    /// </summary>
    public static Rigid3f FromRotation(Quaternion rotation)
    {
        return new Rigid3f(Vector3.Zero, rotation);
    }

    /// <summary>
    /// Creates a translation-only transformation.
    /// </summary>
    public static Rigid3f FromTranslation(Vector3 translation)
    {
        return new Rigid3f(translation, Quaternion.Identity);
    }

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

    /// <summary>
    /// Gets the rotation quaternion.
    /// </summary>
    public readonly Quaternion Rotation => _rotation;

    /// <summary>
    /// Computes the inverse transformation.
    /// </summary>
    public readonly Rigid3f Inverse()
    {
        var invRotation = Quaternion.Conjugate(_rotation);
        var invTranslation = Vector3.Transform(-_translation, invRotation);
        return new Rigid3f(invTranslation, invRotation);
    }

    /// <summary>
    /// Transforms a point by this transformation.
    /// </summary>
    public readonly Vector3 TransformPoint(Vector3 point)
    {
        return Vector3.Transform(point, _rotation) + _translation;
    }

    /// <summary>
    /// Composes two transformations: this * other.
    /// </summary>
    public static Rigid3f operator *(Rigid3f lhs, Rigid3f rhs)
    {
        var composedTranslation = Vector3.Transform(rhs._translation, lhs._rotation) + lhs._translation;
        var composedRotation = Quaternion.Normalize(lhs._rotation * rhs._rotation);
        return new Rigid3f(composedTranslation, composedRotation);
    }

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

    /// <summary>
    /// Checks if the transformation is valid (no NaN values, quaternion is normalized).
    /// </summary>
    public readonly bool IsValid()
    {
        const double tolerance = 1e-3;
        var norm = _rotation.Length();
        var normDiff = Math.Abs(1.0 - norm);
        return !double.IsNaN(_translation.X) && !double.IsNaN(_translation.Y) && !double.IsNaN(_translation.Z) &&
               normDiff < tolerance;
    }

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

/// <summary>
/// Converts (roll, pitch, yaw) to a unit length quaternion.
/// Based on the URDF specification http://wiki.ros.org/urdf/XML/joint.
/// </summary>
public static class QuaternionUtils
{
    /// <summary>
    /// Creates a quaternion from roll, pitch, yaw angles (in radians).
    /// Rotation order: roll (X), pitch (Y), yaw (Z).
    /// </summary>
    public static Quaternion RollPitchYaw(double roll, double pitch, double yaw)
    {
        var halfRoll = roll / 2.0;
        var halfPitch = pitch / 2.0;
        var halfYaw = yaw / 2.0;

        var cr = Math.Cos(halfRoll);
        var sr = Math.Sin(halfRoll);
        var cp = Math.Cos(halfPitch);
        var sp = Math.Sin(halfPitch);
        var cy = Math.Cos(halfYaw);
        var sy = Math.Sin(halfYaw);

        // Quaternion multiplication: yaw * pitch * roll
        var w = cr * cp * cy + sr * sp * sy;
        var x = sr * cp * cy - cr * sp * sy;
        var y = cr * sp * cy + sr * cp * sy;
        var z = cr * cp * sy - sr * sp * cy;

        return Quaternion.Normalize(new Quaternion(x, y, z, w));
    }
}