public void Update(Vector3D target)
{
MatrixD transpose = MatrixD.Transpose(rc.WorldMatrix);
Vector3D meToTarget = rc.WorldMatrix.Translation - target;
Vector3D localError = Vector3D.TransformNormal(meToTarget, transpose);
localError.Y = 0;
if (localError.X > -0.5 && localError.X < 0.5)
{
localError.X = 0;
}
if (localError.Z > -0.5 && localError.Z < 0.5)
{
localError.Z = 0;
}
float correction = (float)forwardPID.Control(localError.Z);
float force = correction * rc.CalculateShipMass().TotalMass;
float rightLeft = (float)anglePID.Control(-localError.X);
Vector3D localVelocity = Vector3D.TransformNormal(rc.GetShipVelocities().LinearVelocity, transpose);
float angle = -rightLeft;
if (localVelocity.Z < 0)
{
angle *= -1;
}
foreach (Wheel w in wheels)
{
IMyMotorSuspension wheel = w.block;
if (first)
{
Vector3D center = Vector3D.TransformNormal(rc.CenterOfMass - rc.GetPosition(), transpose);
Vector3D local = Vector3D.TransformNormal(wheel.GetPosition() - rc.GetPosition(), transpose);
wheel.InvertSteer = (local.Z > center.Z);
wheel.InvertPropulsion = (wheel.Orientation.Left != rc.Orientation.Forward);
wheel.Brake = false;
}
if (wheel.Steering)
{
wheel.SetValueFloat("Steer override", angle);
}
if (wheel.Propulsion)
{
float maxForce = w.maxForce;
if (maxForce <= 0)
{
continue;
}
float percent = MathHelper.Clamp(force / maxForce, -1, 1);
force -= percent * maxForce;
wheel.SetValueFloat("Propulsion override", percent);
}
}
first = false;
}
/// <summary>Creates a new power wheel</summary>
/// <param name="wheel">The actual wheel it wraps</param>
/// <param name="wb">The wheel base, to which the power wheel will be added</param>
/// <param name="tform">Coordinates transformer, should be one that makes the Z axis parallel the wheel direction and Y axis the up axis</param>
public PowerWheel(IMyMotorSuspension wheel, WheelBase wb, CoordinatesTransformer tform)
{
this.wheelBase = wb;
this.CubeSize = wheel.CubeGrid.GridSizeEnum;
this.Position = tform.Pos(wheel.GetPosition());
this.IsRight = RIGHT.Dot(tform.Dir(wheel.WorldMatrix.Up)) > 0;
this.reverseY = UP.Dot(tform.Dir(wheel.WorldMatrix.Backward)) > 0;
this.transformer = tform;
this.wheel = wheel;
wheel.Height = 10;
this.maxY = wheel.Height;
wheel.Height = -10;
this.minY = wheel.Height;
if (this.reverseY)
{
float tmp = this.minY;
this.minY = -this.maxY;
this.maxY = -tmp;
}
this.WheelSize = (wheel.Top.Max - wheel.Top.Min).X + 1;
this.Mass = this.CubeSize == MyCubeSize.Small
? this.WheelSize == 1 ? 105 : (this.WheelSize == 3 ? 205 : 310)
: this.WheelSize == 1 ? 420 : (this.WheelSize == 3 ? 590 : 760);
// real center of rotation of the wheel
this.Position += tform.Dir(wheel.WorldMatrix.Up) * wheel.CubeGrid.GridSize;
this.wheelBase.AddWheel(this);
}
public void AddWheel(IMyMotorSuspension wheel)
{
Base6Directions.Direction blockOrientation = Controller.Orientation.TransformDirectionInverse(wheel.Orientation.Up);
if (blockOrientation == Base6Directions.Direction.Left || blockOrientation == Base6Directions.Direction.Right)
{
double steeringAngle = Math.Abs(GetAngleFromCoM(wheel.GetPosition()));
if (steeringAngle > Math.PI / 2)
{
steeringAngle -= Math.PI / 2;
}
if (!IsFrontSide(wheel.GetPosition()))
{
steeringAngle = Math.PI / 2 - steeringAngle;
}
Wheels.Add(new VehicleWheel(new SuspensionWrapper(wheel, blockOrientation), IsFrontSide(wheel.GetPosition()), steeringAngle / 90));
}
}
public void Drive(float propulsionOverride, float steeringOverride)
{
Vector3D forward = Controller.WorldMatrix.Forward;
Vector3D com = Controller.CenterOfMass;
for (int i = 0; i < Wheels.Length; i++)
{
var wheel = Wheels[i];
IMyMotorSuspension suspension = wheel.Suspension;
float propulsionFactor = (float)suspension.WorldMatrix.Left.Dot(forward);
suspension.SetValueFloat("Propulsion override", propulsionFactor * propulsionOverride);
Vector3D offset = com - suspension.GetPosition();
float steeringFactor = (float)offset.Dot(forward);
suspension.SetValueFloat("Steer override", steeringFactor * steeringOverride);
}
}
public SuspensionWrapper(IMyMotorSuspension newSuspension, ref IMyShipController controller)
{
Suspension = newSuspension;
Controller = controller;
ToGridCenterDistance = (float)(Controller.CubeGrid.GetPosition() - Suspension.GetPosition()).Length();
Suspension.MaxSteerAngle = (float)(Math.PI / 4.0);
}