77 lines
2.6 KiB
C#
77 lines
2.6 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
|
|
namespace MyParking.Shared
|
|
{
|
|
// 纯数学地把车队参考点速度分解成各成员车体系中的刚体速度。
|
|
public static class FleetKinematics
|
|
{
|
|
public static IReadOnlyList<FleetMemberCommand> Decompose(
|
|
FleetLayout layout,
|
|
FleetMotionCommand command)
|
|
{
|
|
if (layout == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(layout));
|
|
}
|
|
|
|
EnsureCommandIsFinite(command);
|
|
|
|
var commands =
|
|
new FleetMemberCommand[layout.VehicleCount];
|
|
var referencePoint = command.ReferencePointInFleet;
|
|
var referenceTwist = command.TwistAtReferencePoint;
|
|
|
|
for (var index = 0;
|
|
index < layout.VehicleCount;
|
|
index++)
|
|
{
|
|
var vehicle = layout.Vehicles[index];
|
|
var offsetXMeters =
|
|
vehicle.PoseInFleet.XMeters -
|
|
referencePoint.XMeters;
|
|
var offsetYMeters =
|
|
vehicle.PoseInFleet.YMeters -
|
|
referencePoint.YMeters;
|
|
|
|
var twistAtVehicleInFleet = new Twist2D(
|
|
referenceTwist.VxMetersPerSecond -
|
|
referenceTwist.OmegaRadiansPerSecond *
|
|
offsetYMeters,
|
|
referenceTwist.VyMetersPerSecond +
|
|
referenceTwist.OmegaRadiansPerSecond *
|
|
offsetXMeters,
|
|
referenceTwist.OmegaRadiansPerSecond);
|
|
|
|
var fleetPoseInVehicle =
|
|
FrameTransform2D.Inverse(
|
|
vehicle.PoseInFleet);
|
|
var twistInVehicleBody =
|
|
FrameTransform2D.TransformTwistAtSamePoint(
|
|
fleetPoseInVehicle,
|
|
twistAtVehicleInFleet);
|
|
|
|
commands[index] = new FleetMemberCommand(
|
|
vehicle.VehicleId,
|
|
twistInVehicleBody);
|
|
}
|
|
|
|
return Array.AsReadOnly(commands);
|
|
}
|
|
|
|
private static void EnsureCommandIsFinite(
|
|
FleetMotionCommand command)
|
|
{
|
|
NumericGuard.EnsureFinite(
|
|
command.ReferencePointInFleet.XMeters,
|
|
nameof(command.ReferencePointInFleet));
|
|
NumericGuard.EnsureFinite(
|
|
command.ReferencePointInFleet.YMeters,
|
|
nameof(command.ReferencePointInFleet));
|
|
NumericGuard.EnsureFinite(
|
|
command.TwistAtReferencePoint,
|
|
nameof(command.TwistAtReferencePoint));
|
|
}
|
|
}
|
|
}
|