/// <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; Vector3D pos = Goal.Position; MatrixD wm = reference.WorldMatrix; Vector3D direction = pos - wm.Translation; direction += (Goal.Velocity - linearV) * owner.elapsedTime.TotalSeconds; double distance = direction.Normalize(); linearV.X = linearV.Y = linearV.Z = 0; double diff = owner.RotationAid.Rotate(owner.elapsedTime, direction, Vector3D.Zero, wm.GetDirectionVector(ReferenceForward), wm.GetDirectionVector(ReferenceUp), ref angularV); if (diff < OrientationEpsilon) { angularV = Vector3D.Zero; return(true); } else { return(false); } }
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); }
private bool ScanForObstacles(BasePilot owner, double distance) { foreach (IMyCameraBlock cam in Cameras) { if (cam.EnableRaycast) { Vector3D scan = cam.GetPosition() + TargetVector * distance; cam.Enabled = true; if (cam.CanScan(scan)) { //owner.Log?.Invoke("Scanning via " + cam.CustomName); Obstacle = cam.Raycast(scan); return(!Obstacle.IsEmpty()); } } } //owner.Log?.Invoke("No cameras can scan at this time."); return(false); }
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(); //linear velocity linearV = direction * MaxLinearSpeed + Goal.Velocity; //angular velocity double diff = owner.RotationAid.Rotate(owner.elapsedTime, 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. Set it to desired linear velocity.</param> /// <param name="angularV">Initial value - current rotation. Set it to desired rotation.</param> /// <returns>True if goal is considered achieved.</returns> public abstract bool Update(BasePilot owner, ref Vector3D linearV, ref Vector3D angularV);
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); } }