/// <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);
}
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));
}
/// <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;
MatrixD wm = reference.WorldMatrix;
Goal.UpdateTime(owner.elapsedTime);
Vector3D direction = Goal.CurrentPosition - wm.Translation;
double distance = direction.Normalize();
if (distance < Goal.TargetDistance)
{
direction *= -1;
distance = Goal.TargetDistance - distance;
}
if (distance > PositionEpsilon)
{
//linear velocity
double accel = owner.GetMaxAccelerationFor(-direction);
double braking_time = Math.Sqrt(2 * distance / accel);
double acceptable_velocity = Math.Min(VelocityUsage * accel * braking_time, MaxLinearSpeed);
acceptable_velocity = Math.Min(acceptable_velocity, distance);//slow down when close
Vector3D targetv = direction * acceptable_velocity;
linearV = targetv + Goal.Velocity;
}
else
{
linearV = Vector3D.Zero;
}
angularV = Vector3D.Zero;
if (distance < PositionEpsilon)
{
owner.Log?.Invoke("Target position reached.");
}
return(Vector3D.IsZero(linearV));
}
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (CurrentState == State.Destination)
{
return(true);
}
angularV = Vector3D.Zero;
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
TargetVector = wm.GetDirectionVector(ReferenceForward);
BoundingSphereD bs = reference.CubeGrid.WorldVolume;
Vector3D refpoint = bs.Center + TargetVector * (bs.Radius + Distance);
RaycastChargeSpeed = 0;
foreach (IMyCameraBlock cam in Cameras)
{
if (cam.IsWorking && (cam.WorldMatrix.Forward.Dot(TargetVector) > Math.Cos(cam.RaycastConeLimit)))
{
RaycastChargeSpeed += 2000.0;
cam.Enabled = true;
cam.EnableRaycast = true;
}
}
if (RaycastChargeSpeed == 0) //we can't scan in that orientation (or at all)!
{
throw new Exception("Can't scan!");
}
//owner.Log?.Invoke($"Direction: {TargetVector.X:F1}:{TargetVector.Y:F1}:{TargetVector.Z:F1}");
double accel = owner.GetMaxAccelerationFor(TargetVector);
double decel = owner.GetMaxAccelerationFor(-TargetVector);
//owner.Log?.Invoke($"Accel: {accel:F1}m/s^2");
//owner.Log?.Invoke($"Decel: {decel:F1}m/s^2");
if (decel == 0) //we can't decelerate afterwards!
{
throw new Exception("Can't decelerate!");
}
double speed = linearV.Length();
//account for possible acceleration
speed += accel * owner.elapsedTime;
//account for obstacle velocity relative to us
double obstacle_speed = Obstacle.IsEmpty() ? 0.0
: Math.Max(0, Obstacle.Velocity.Dot(-TargetVector));
speed += obstacle_speed;
//time required to charge the camera enough to scan
double scan_charge_time = (speed * speed) / (2 * decel * (RaycastChargeSpeed - speed));
//this is reaction time - how much time will pass before we can re-scan
scan_charge_time = Math.Max(scan_charge_time, owner.elapsedTime);
//distance requried to decelerate + distance we will pass in time till the next scan
double scan_dist = scan_charge_time * RaycastChargeSpeed;
scan_dist = Math.Max(scan_dist, 100);
if (ScanForObstacles(owner, scan_dist))
{ //obstacle detected - we are done
linearV = Vector3D.Zero;
return(true);
}
else
{ //no obstacles detected - fly at top speed
//owner.Log?.Invoke("Miss");
linearV = TargetVector * MaxLinearSpeed;
return(false);
}
}