QUYVN/BQP
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-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

247
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Transform/Rigid2.cs Normal file
View File

@@ -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}] }}";
}
}

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));
}
}

201
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Transform/TransformOperations.cs Normal file
View File

@@ -0,0 +1,201 @@
/*
* 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>
/// Transform operations for Cartographer.
/// </summary>
public static class TransformOperations
{
/// <summary>
/// Returns the non-negative rotation angle in radians of the 3D transformation.
/// </summary>
public static double GetAngle(Rigid3d transform)
{
var vec = new Vector3(transform.Rotation.X, transform.Rotation.Y, transform.Rotation.Z);
var vecNorm = vec.Length();
return 2.0 * Math.Atan2(vecNorm, Math.Abs(transform.Rotation.W));
}
/// <summary>
/// Returns the yaw component in radians of the given 3D rotation.
/// Assuming rotation is composed of three rotations around X, then Y, then Z,
/// returns the angle of the Z rotation.
/// </summary>
public static double GetYaw(Quaternion rotation)
{
var direction = Vector3.Transform(Vector3.UnitX, rotation);
return Math.Atan2(direction.Y, direction.X);
}
/// <summary>
/// Returns the yaw component in radians of the given 3D transformation.
/// </summary>
public static double GetYaw(Rigid3d transform)
{
return GetYaw(transform.Rotation);
}
/// <summary>
/// Returns an angle-axis vector (a vector with the length of the rotation angle
/// pointing to the direction of the rotation axis) representing the same
/// rotation as the given quaternion.
/// </summary>
public static Vector3 RotationQuaternionToAngleAxisVector(Quaternion quaternion)
{
var normalized = Quaternion.Normalize(quaternion);
// We choose the quaternion with positive 'w', i.e., the one with a smaller
// angle that represents this orientation.
if (normalized.W < 0.0)
{
normalized = new Quaternion(-normalized.X, -normalized.Y, -normalized.Z, -normalized.W);
}
// We convert the normalized_quaternion into a vector along the rotation axis
// with length of the rotation angle.
var vec = new Vector3(normalized.X, normalized.Y, normalized.Z);
var vecNorm = vec.Length();
const double kCutoffAngle = 1e-7; // We linearize below this angle.
var angle = 2.0 * Math.Atan2(vecNorm, normalized.W);
var scale = angle < kCutoffAngle ? 2.0 : angle / Math.Sin(angle / 2.0);
return new Vector3(
(scale * normalized.X),
(scale * normalized.Y),
(scale * normalized.Z)
);
}
/// <summary>
/// Returns a quaternion representing the same rotation as the given angle-axis vector.
/// </summary>
public static Quaternion AngleAxisVectorToRotationQuaternion(Vector3 angleAxis)
{
const double kCutoffAngle = 1e-8; // We linearize below this angle.
var norm = angleAxis.Length();
double scale = 0.5;
double w = 1.0;
if (norm * norm > kCutoffAngle)
{
scale = Math.Sin(norm / 2.0) / norm;
w = Math.Cos(norm / 2.0);
}
return Quaternion.Normalize(new Quaternion(
(scale * angleAxis.X),
(scale * angleAxis.Y),
(scale * angleAxis.Z),
w
));
}
/// <summary>
/// Projects 'transform' onto the XY plane.
/// </summary>
public static Rigid2d Project2D(Rigid3d transform)
{
var translation2D = new Vector2(transform.Translation.X, transform.Translation.Y);
var yaw = GetYaw(transform);
return new Rigid2d(translation2D, yaw);
}
/// <summary>
/// Embeds 'transform' into 3D space in the XY plane.
/// </summary>
public static Rigid3d Embed3D(Rigid2d transform)
{
var translation3D = new Vector3(transform.Translation.X, transform.Translation.Y, 0.0);
var rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, transform.Rotation);
return new Rigid3d(translation3D, rotation);
}
/// <summary>
/// Interpolates between two transforms at different times.
/// </summary>
public static Rigid3d Interpolate(
Rigid3d startTransform,
long startTime,
Rigid3d endTransform,
long endTime,
long targetTime)
{
if (startTime == endTime)
{
return startTransform;
}
var factor = (double)(targetTime - startTime) / (endTime - startTime);
factor = Math.Max(0.0, Math.Min(1.0, factor)); // Clamp to [0, 1]
// Interpolate translation linearly
var interpolatedTranslation = new Vector3(
(startTransform.Translation.X + factor * (endTransform.Translation.X - startTransform.Translation.X)),
(startTransform.Translation.Y + factor * (endTransform.Translation.Y - startTransform.Translation.Y)),
(startTransform.Translation.Z + factor * (endTransform.Translation.Z - startTransform.Translation.Z))
);
// Interpolate rotation using SLERP
var interpolatedRotation = SlerpQuaternions(
startTransform.Rotation,
endTransform.Rotation,
factor
);
return new Rigid3d(interpolatedTranslation, interpolatedRotation);
}
/// <summary>
/// Spherical linear interpolation of quaternions.
/// </summary>
private static Quaternion SlerpQuaternions(Quaternion start, Quaternion end, double factor)
{
start = Quaternion.Normalize(start);
end = Quaternion.Normalize(end);
var cosTheta = start.W * end.W + start.X * end.X + start.Y * end.Y + start.Z * end.Z;
var absCosTheta = Math.Abs(cosTheta);
double prevScale = 1.0 - factor;
double nextScale = factor;
if (absCosTheta < 1.0 - 1e-5)
{
var theta = Math.Acos(absCosTheta);
var sinTheta = Math.Sin(theta);
prevScale = Math.Sin((1.0 - factor) * theta) / sinTheta;
nextScale = Math.Sin(factor * theta) / sinTheta;
}
if (cosTheta < 0.0)
{
nextScale = -nextScale;
}
return new Quaternion(
(prevScale * start.X + nextScale * end.X),
(prevScale * start.Y + nextScale * end.Y),
(prevScale * start.Z + nextScale * end.Z),
(prevScale * start.W + nextScale * end.W)
);
}
}