145 lines
4.9 KiB
C#
145 lines
4.9 KiB
C#
// 车体、运动、车队坐标系之间的转换
|
|
using System;
|
|
|
|
namespace MyParking.Shared
|
|
{
|
|
/// <summary>
|
|
/// 提供二维刚体坐标系之间的点、向量、位姿和速度变换。
|
|
/// 坐标系采用X向前、Y向左、逆时针为正的右手系。
|
|
/// </summary>
|
|
public static class FrameTransform2D
|
|
{
|
|
/// <summary>
|
|
/// 将源坐标系中的点变换到目标坐标系。
|
|
/// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
|
|
/// </summary>
|
|
public static Point2D TransformPoint(
|
|
Pose2D sourcePoseInTarget,
|
|
Point2D pointInSource)
|
|
{
|
|
var cos = Math.Cos(sourcePoseInTarget.YawRadians);
|
|
var sin = Math.Sin(sourcePoseInTarget.YawRadians);
|
|
|
|
return new Point2D(
|
|
sourcePoseInTarget.XMeters +
|
|
cos * pointInSource.XMeters -
|
|
sin * pointInSource.YMeters,
|
|
|
|
sourcePoseInTarget.YMeters +
|
|
sin * pointInSource.XMeters +
|
|
cos * pointInSource.YMeters);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将目标坐标系中的点反向变换到源坐标系。
|
|
/// </summary>
|
|
public static Point2D InverseTransformPoint(
|
|
Pose2D sourcePoseInTarget,
|
|
Point2D pointInTarget)
|
|
{
|
|
var dx = pointInTarget.XMeters - sourcePoseInTarget.XMeters;
|
|
|
|
var dy = pointInTarget.YMeters - sourcePoseInTarget.YMeters;
|
|
|
|
var cos = Math.Cos(sourcePoseInTarget.YawRadians);
|
|
var sin = Math.Sin(sourcePoseInTarget.YawRadians);
|
|
|
|
return new Point2D(
|
|
cos * dx + sin * dy,
|
|
-sin * dx + cos * dy);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将源坐标系中的向量旋转到目标坐标系。
|
|
/// 向量没有位置，因此不叠加平移量。
|
|
/// </summary>
|
|
public static Point2D TransformVector(
|
|
Pose2D sourcePoseInTarget,
|
|
Point2D vectorInSource)
|
|
{
|
|
var cos = Math.Cos(sourcePoseInTarget.YawRadians);
|
|
var sin = Math.Sin(sourcePoseInTarget.YawRadians);
|
|
|
|
return new Point2D(
|
|
cos * vectorInSource.XMeters -
|
|
sin * vectorInSource.YMeters,
|
|
|
|
sin * vectorInSource.XMeters +
|
|
cos * vectorInSource.YMeters);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 组合两级坐标变换。
|
|
/// parentFromMiddle表示middle在parent中的位姿；
|
|
/// middleFromChild表示child在middle中的位姿；
|
|
/// 返回child在parent中的位姿。
|
|
/// </summary>
|
|
public static Pose2D Compose(
|
|
Pose2D parentFromMiddle,
|
|
Pose2D middleFromChild)
|
|
{
|
|
var childPositionInParent = TransformPoint(
|
|
parentFromMiddle,
|
|
middleFromChild.Position);
|
|
|
|
return new Pose2D(
|
|
childPositionInParent.XMeters,
|
|
childPositionInParent.YMeters,
|
|
AngleMath.NormalizeRadians(
|
|
parentFromMiddle.YawRadians +
|
|
middleFromChild.YawRadians));
|
|
}
|
|
|
|
/// <summary>
|
|
/// 对坐标变换求逆。
|
|
/// 输入child在parent中的位姿，返回parent在child中的位姿。
|
|
/// </summary>
|
|
public static Pose2D Inverse(Pose2D childPoseInParent)
|
|
{
|
|
var cos = Math.Cos(childPoseInParent.YawRadians);
|
|
var sin = Math.Sin(childPoseInParent.YawRadians);
|
|
|
|
return new Pose2D(
|
|
-cos * childPoseInParent.XMeters -
|
|
sin * childPoseInParent.YMeters,
|
|
|
|
sin * childPoseInParent.XMeters -
|
|
cos * childPoseInParent.YMeters,
|
|
|
|
AngleMath.NormalizeRadians(
|
|
-childPoseInParent.YawRadians));
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// 将源坐标系中的位姿变换到目标坐标系。
|
|
/// </summary>
|
|
public static Pose2D TransformPose(
|
|
Pose2D sourcePoseInTarget,
|
|
Pose2D poseInSource)
|
|
{
|
|
return Compose(sourcePoseInTarget, poseInSource);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 转换同一物理参考点处的速度表达坐标系。
|
|
/// 只旋转线速度，角速度保持不变。
|
|
/// </summary>
|
|
public static Twist2D TransformTwistAtSamePoint(
|
|
Pose2D sourcePoseInTarget,
|
|
Twist2D twistInSource)
|
|
{
|
|
var linearVelocityInTarget = TransformVector(
|
|
sourcePoseInTarget,
|
|
new Point2D(
|
|
twistInSource.VxMetersPerSecond,
|
|
twistInSource.VyMetersPerSecond));
|
|
|
|
return new Twist2D(
|
|
linearVelocityInTarget.XMeters,
|
|
linearVelocityInTarget.YMeters,
|
|
twistInSource.OmegaRadiansPerSecond);
|
|
}
|
|
}
|
|
}
|