public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (Goal == null)
{
return(false);
}
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Goal.UpdateTime(owner.elapsedTime);
Vector3D currentGoalPos = Goal.CurrentPosition;
Vector3D direction = currentGoalPos - wm.Translation;
double distance = direction.Normalize();
double target_distance = distance;
double diff;
if (!Vector3D.IsZero(Approach))
{
Vector3D minusApproach = -Approach;
diff = owner.RotateToMatch(Approach, Facing,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
PlaneD alignment = new PlaneD(wm.Translation, minusApproach);
Vector3D alignedPos = alignment.Intersection(ref currentGoalPos, ref minusApproach);
Vector3D correction = alignedPos - wm.Translation;
if (!Vector3D.IsZero(correction, PositionEpsilon)) //are we on approach vector?
{ //no - let's move there
direction = correction;
distance = direction.Normalize();
}
//otherwise, we can keep our current direction
}
else
{
diff = owner.RotateToMatch(direction, Facing,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
}
//rotate the ship to face it
if (diff > OrientationEpsilon) //we still need to rotate
{
linearV = Goal.Velocity; //match velocities with our target, then.
}
else //we are good
{
//how quickly can we go, assuming we still need to stop at the end?
double accel = owner.GetMaxAccelerationFor(-direction);
double braking_time = Math.Sqrt(2 * distance / accel);
double acceptable_velocity = Math.Min(VelocityUsage * accel * braking_time, MaxLinearSpeed);
//extra slowdown when close to the target
acceptable_velocity = Math.Min(acceptable_velocity, distance);
//moving relative to the target
linearV = direction * acceptable_velocity + Goal.Velocity;
angularV = Vector3D.Zero;
}
return(TryLockIn(distance) || (target_distance < PositionEpsilon));
}
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (Goal == null)
{
return(false);
}
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Goal.UpdateTime(owner.elapsedTime);
Vector3D direction = Goal.CurrentPosition - wm.Translation;
double distance = direction.Normalize();
//linear velocity
linearV = direction * MaxLinearSpeed + Goal.Velocity;
//angular velocity
double diff = owner.RotateToMatch(direction, Vector3D.Zero,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
if (diff < OrientationEpsilon)
{
angularV = Vector3D.Zero;
}
return((diff < OrientationEpsilon) && (distance < PositionEpsilon));
}
/// <summary>
/// Queries the strategy on which linear and angular velocities the ship should have.
/// </summary>
/// <param name="owner">AutoPilot instance that queries the strategy.</param>
/// <param name="linearV">Initial value - current linear velocity. Is set to desired linear velocity.</param>
/// <param name="angularV">Initial value - current rotation. Is set to desired rotation.</param>
/// <returns>True if goal is considered achieved.</returns>
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (Goal == null)
{
return(false);
}
IMyCubeBlock reference = Reference ?? owner.Controller;
Vector3D pos = Goal.CurrentPosition;
MatrixD wm = reference.WorldMatrix;
Vector3D direction = pos - wm.Translation;
double distance = direction.Normalize();
linearV.X = linearV.Y = linearV.Z = 0;
double diff = owner.RotateToMatch(direction, Vector3D.Zero,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
if (diff < OrientationEpsilon)
{
angularV = Vector3D.Zero;
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// Queries the strategy on which linear and angular velocities the ship should have.
/// </summary>
/// <param name="owner">AutoPilot instance that queries the strategy.</param>
/// <param name="linearV">Initial value - current linear velocity. Is set to desired linear velocity.</param>
/// <param name="angularV">Initial value - current rotation. Is set to desired rotation.</param>
/// <returns>True if goal is considered achieved.</returns>
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (Goal == null)
{
return(false);
}
bool distanceok = false;
bool orientationok = false;
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Goal.UpdateTime(owner.elapsedTime);
Vector3D direction = Goal.CurrentPosition - wm.Translation;
double distance = direction.Normalize();
Vector3D facingdirection = direction; //we should face our goal, still.
if (distance < Goal.TargetDistance) //Are we too close to the goal?
{ // yep! better back off.
direction *= -1;
distance = Goal.TargetDistance - distance;
}
else //nah, we aren't there yet - just cut the distance we need to travel.
{
distance -= Goal.TargetDistance;
}
if (distance > PositionEpsilon) //Are we too far from our desired position?
{
//rotate the ship to face it
double diff = owner.RotateToMatch(facingdirection, Vector3D.Zero,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
if (diff > OrientationEpsilon) //we still need to rotate
{
linearV = Goal.Velocity; //match velocities with our target, then.
}
else //we are good
{
orientationok = true;
//how quickly can we go, assuming we still need to stop at the end?
double accel = owner.GetMaxAccelerationFor(-direction);
double braking_time = Math.Sqrt(2 * distance / accel);
double acceptable_velocity = Math.Min(VelocityUsage * accel * braking_time, MaxLinearSpeed);
//extra slowdown when close to the target
acceptable_velocity = Math.Min(acceptable_velocity, distance);
//moving relative to the target
linearV = direction * acceptable_velocity + Goal.Velocity;
angularV = Vector3D.Zero;
}
}
else //we are close to our ideal position - attempting to rotate the ship is not a good idea.
{
distanceok = true;
orientationok = true;
linearV = Goal.Velocity;
angularV = Vector3D.Zero;
}
return(distanceok && orientationok);
}
/// <summary>
/// Queries the strategy on which linear and angular velocities the ship should have.
/// </summary>
/// <param name="owner">AutoPilot instance that queries the strategy.</param>
/// <param name="linearV">Initial value - current linear velocity. Is set to desired linear velocity.</param>
/// <param name="angularV">Initial value - current rotation. Is set to desired rotation.</param>
/// <returns>False, since orbiter has no goal to achieve.</returns>
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (Goal == null)
{
return(false);
}
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Vector3D radius = Goal.CurrentPosition - wm.Translation;
double R = radius.Normalize();
Vector3D vel = Normal.Cross(radius);
vel.Normalize();
linearV = Goal.Velocity + vel * MaxLinearSpeed + radius * (R - Goal.TargetDistance);
double diff = owner.RotateToMatch(radius, Normal,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
return(false);
}