public MoveAndGripRules(double linearVelocityLimit, Angle angularVelocityLimit, double grippingTime, double releasingTime)
{
LinearVelocityLimit = linearVelocityLimit;
AngularVelocityLimit = angularVelocityLimit;
GrippingTime = grippingTime;
ReleasingTime = releasingTime;
}
/// <summary>
/// Создает камеру с видом от первого лица
/// </summary>
/// <param name="source">Тело, к которому привязана камера</param>
/// <param name="offset">Смещение камеры относительно Location тела</param>
/// <param name="viewAngle"> </param>
/// <param name="aspectRatio"> </param>
public FirstPersonCamera(Body source, Frame3D offset,
Angle viewAngle, double aspectRatio) : base(aspectRatio)
{
_source = source;
_offset = offset;
ViewAngle = viewAngle;
_defaultWorldMatrix = Matrix.LookAtLH(Vector3.Zero, new Vector3(-1, 0, 0), UpVector);
}
/// <summary>
///robot pivots around itself
/// </summary>
/// <typeparam name="TCommand"></typeparam>
/// <param name="rules"></param>
/// <param name="angle"></param>
/// <returns></returns>
public static TCommand DWMRotate <TCommand>(this IDWMRules <TCommand> rules, AIRLab.Mathematics.Angle angle)
where TCommand : IDWMCommand, new()
{
var botRadius = rules.DistanceBetweenWheels / 2;
var arcLength = botRadius * angle.Radian;
var routes = Math.Abs(arcLength) / (2 * Math.PI * rules.WheelRadius);
var period = 2 * Math.PI / rules.RotationSpeedLimit.Radian;
var duration = routes * period;
return(new TCommand
{
DifWheelMovement = new DifWheelMovement
{
Duration = duration,
LeftRotatingVelocity = Math.Sign(angle.Radian) * rules.RotationSpeedLimit,
RightRotatingVelocity = -Math.Sign(angle.Radian) * rules.RotationSpeedLimit
}
});
}
/// <summary>
/// robot moves along an arc
/// </summary>
/// <typeparam name="TCommand"></typeparam>
/// <param name="rules"></param>
/// <param name="arcRadius"></param>
/// <param name="angle"></param>
/// <param name="direction">true for moving right, and false to moving left</param>
/// <returns></returns>
public static TCommand DWMMoveArc <TCommand>(this IDWMRules <TCommand> rules,
double arcRadius,
AIRLab.Mathematics.Angle angle,
bool direction)
where TCommand : IDWMCommand, new()
{
double innerArcLength = arcRadius * angle.Radian;
double outerArcLength = (arcRadius + rules.DistanceBetweenWheels) * angle.Radian;
var arcDiff = outerArcLength / innerArcLength;
var routes = Math.Abs(outerArcLength) / (2 * Math.PI * rules.WheelRadius);
var period = 2 * Math.PI / rules.RotationSpeedLimit.Radian;
var duration = routes * period;
var rightRotatingVelocity = Angle.Zero;
var leftRotatingVelocity = Angle.Zero;
if (direction)
{
leftRotatingVelocity = rules.RotationSpeedLimit;
rightRotatingVelocity = Angle.FromRad(leftRotatingVelocity.Radian / arcDiff);
}
else
{
rightRotatingVelocity = rules.RotationSpeedLimit;
leftRotatingVelocity = Angle.FromRad(rightRotatingVelocity.Radian / arcDiff);
}
return(new TCommand
{
DifWheelMovement = new DifWheelMovement
{
Duration = duration,
LeftRotatingVelocity = leftRotatingVelocity,
RightRotatingVelocity = rightRotatingVelocity
}
});
}
public static Frame3D DoPitch(Angle angle)
{
return(new Frame3D(0, 0, 0, angle, new Angle(), new Angle()));
}
public static Frame3D DoRoll(Angle angle)
{
return(new Frame3D(0, 0, 0, new Angle(), new Angle(), angle));
}
