QUYVN/I150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

243
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Transform/Rigid3.cs Normal file
View File

@@ -0,0 +1,243 @@
/*
* 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));
}
}