public override bool Update(BasePilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Vector3D forward = wm.GetDirectionVector(ReferenceForward);
Vector3D up = wm.GetDirectionVector(ReferenceUp);
Vector3D right = forward.Cross(up);
double max_up_accel = owner.GetMaxAccelerationFor(up);
Vector3D currentV = linearV;
Goal.Update(owner.elapsedTime);
//determine radial velocity component
Vector3D my_projection = wm.Translation;
ProjectOntoSphere(ref my_projection);
owner.Log?.Invoke($"Me: {my_projection}\n");
Vector3D radius = my_projection - Sphere.Center;
radius.Normalize();
Vector3D radial_direction = my_projection - wm.Translation;
double radial_distance = radial_direction.Normalize();
owner.Log?.Invoke($"Radial error: {radial_distance:F1}\n");
//determine vector pointing towards the projection of the target
Vector3D goal_projection = Goal.Position;
ProjectOntoSphere(ref goal_projection);
owner.Log?.Invoke($"Goal: {goal_projection}\n");
//determine tangential velocity component
Vector3D goal_direction = goal_projection - wm.Translation;
double goal_distance = goal_direction.Normalize();
owner.Log?.Invoke($"Distance: {goal_distance:F1}\n");
Vector3D tangent_direction = Vector3D.ProjectOnPlane(ref goal_direction, ref radius);
if (tangent_direction.Normalize() < OrientationEpsilon)
{ //if our goal is directly opposite of us, just go in the direction we are facing
tangent_direction = wm.GetDirectionVector(ReferenceForward);
tangent_direction = Vector3D.ProjectOnPlane(ref tangent_direction, ref radius);
if (tangent_direction.Normalize() < OrientationEpsilon)
{ //if we are facing straight down or up, use "down" direction instead
tangent_direction = -wm.GetDirectionVector(ReferenceUp);
tangent_direction = Vector3D.ProjectOnPlane(ref tangent_direction, ref radius);
tangent_direction.Normalize();
}
}
double goal_speed = MaxSpeedFor(owner.GetMaxAccelerationFor(-tangent_direction), goal_distance);
linearV = tangent_direction * goal_speed;
linearV += radial_direction * MaxSpeedFor(owner.GetMaxAccelerationFor(-radial_direction), radial_distance);
double diff = owner.RotationAid.Rotate(owner.elapsedTime, tangent_direction, radius, forward, up, ref angularV);
angularV.X += tangent_direction.Dot(forward) * currentV.Dot(tangent_direction) / (Sphere.Radius + radial_distance);
angularV.Z += tangent_direction.Dot(right) * currentV.Dot(tangent_direction) / (Sphere.Radius + radial_distance);
if (goal_distance < PositionEpsilon || diff < OrientationEpsilon)
{
angularV = Vector3D.Zero;
}
return(goal_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(BasePilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Goal.Update(owner.elapsedTime);
Vector3D direction = Goal.Position - wm.Translation;
double distance = direction.Normalize();
if (distance < Goal.Distance)
{
direction *= -1;
distance = Goal.Distance - distance;
}
if (distance > PositionEpsilon)
{
//linear velocity
double accel = owner.GetMaxAccelerationFor(-direction);
Vector3D targetv = direction * MaxSpeedFor(accel, distance);
linearV = targetv + Goal.Velocity;
}
else
{
linearV = Vector3D.Zero;
}
angularV = Vector3D.Zero;
return(Vector3D.IsZero(linearV));
}
public override bool Update(BasePilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Goal.Update(owner.elapsedTime);
Vector3D currentGoalPos = Goal.Position;
Vector3D direction = currentGoalPos - wm.Translation;
double distance = direction.Normalize();
double target_distance = distance;
double diff;
if (!Vector3D.IsZero(Approach))
{
Vector3D minusApproach = -Approach;
diff = owner.RotationAid.Rotate(owner.elapsedTime,
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.RotationAid.Rotate(owner.elapsedTime,
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(Lock.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(BasePilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
bool distanceok = false;
bool orientationok = false;
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Goal.Update(owner.elapsedTime);
Vector3D direction = Goal.Position - wm.Translation;
double distance = direction.Normalize();
Vector3D facingdirection = direction; //we should face our goal, still.
if (distance < Goal.Distance) //Are we too close to the goal?
{ // yep! better back off.
direction *= -1;
distance = Goal.Distance - distance;
}
else //nah, we aren't there yet - just cut the distance we need to travel.
{
distance -= Goal.Distance;
}
if (distance > PositionEpsilon) //Are we too far from our desired position?
{
//rotate the ship to face it
double diff = owner.RotationAid.Rotate(owner.elapsedTime, 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);
//moving relative to the target
linearV = direction * MaxSpeedFor(accel, distance) + 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(BasePilot 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.TotalSeconds;
//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.TotalSeconds);
//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);
}
}