Initial commit
This commit is contained in:
@@ -0,0 +1,247 @@
|
||||
/*
|
||||
* 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}] }}";
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user