Add autopilot functionality

mccore/src/main/java/minecrafttransportsimulator/baseclasses/NavBeacon.java

@@ -87,6 +87,11 @@ public double getBearingDelta(AEntityB_Existing entity) {
         return delta;
     }
 
+    public double getGlideSlopeDelta(AEntityB_Existing entity) {
+        double delta = glideSlope - Math.toDegrees(Math.asin((entity.position.y - position.y) / entity.position.distanceTo(position)));
+        return delta;
+    }
+
     public IWrapperNBT save(IWrapperNBT data) {
         data.setString("name", name);
         data.setDouble("glideSlope", glideSlope);

mccore/src/main/java/minecrafttransportsimulator/baseclasses/NavWaypoint.java

@@ -0,0 +1,214 @@
+package minecrafttransportsimulator.baseclasses;
+import java.util.*;
+
+import minecrafttransportsimulator.entities.components.AEntityB_Existing;
+import minecrafttransportsimulator.entities.components.AEntityD_Definable;
+import minecrafttransportsimulator.mcinterface.AWrapperWorld;
+import minecrafttransportsimulator.mcinterface.IWrapperNBT;
+import minecrafttransportsimulator.mcinterface.InterfaceManager;
+
+/**
+ * waypoint class
+ */
+
+//structure:{waypoints:{index:value,name:value,targetSpeed:value,bearing:value,position:value}}
+
+public class NavWaypoint {
+    public static final String WAYPOINT_LISTING_KEY = "waypoints";
+    private static final Map<AWrapperWorld, Map<String, NavWaypoint>> cachedWaypointMaps = new HashMap<>();
+    private static final Map<AEntityD_Definable, Map<String, NavWaypoint>> v_cachedWaypointMaps = new HashMap<>();
+
+    //use an editable value as key
+    public final String index;
+    //information
+    public final String name;
+    public final double targetSpeed;
+    public final double bearing;
+    public final Point3D position;
+
+
+
+    //Global Operation
+    public static Map<String, NavWaypoint> getAllWaypointsFromWorld(AWrapperWorld world) {
+        if (!cachedWaypointMaps.containsKey(world)) {
+            IWrapperNBT waypointListing = world.getData(WAYPOINT_LISTING_KEY);
+            if (waypointListing != null) {
+                Map<String, NavWaypoint> waypointMap = new HashMap<>();
+                for (String waypointIndex : waypointListing.getAllNames()) {
+                    waypointMap.put(waypointIndex, new NavWaypoint(waypointListing.getData(waypointIndex)));
+                }
+                cachedWaypointMaps.put(world, waypointMap);
+            } else {
+                return Collections.emptyMap();
+            }
+        }
+
+        return cachedWaypointMaps.get(world);
+    }
+
+
+    public static NavWaypoint getByIndexFromWorld(AWrapperWorld world, String index) {
+        if (!cachedWaypointMaps.containsKey(world)) {
+            IWrapperNBT waypointListing = world.getData(WAYPOINT_LISTING_KEY);
+            if (waypointListing != null) {
+                Map<String, NavWaypoint> waypointMap = new HashMap<>();
+                for (String waypointIndex : waypointListing.getAllNames()) {
+                    waypointMap.put(waypointIndex, new NavWaypoint(waypointListing.getData(waypointIndex)));
+                }
+                cachedWaypointMaps.put(world, waypointMap);
+            } else {
+                return null;
+            }
+        }
+
+        return cachedWaypointMaps.get(world).get(index);
+    }
+
+    public static void removeFromWorld(AWrapperWorld world, String index) {
+        if (index != null) {
+            if (cachedWaypointMaps.containsKey(world)) {
+                cachedWaypointMaps.get(world).remove(index);
+            }
+
+            IWrapperNBT waypointListing = world.getData(WAYPOINT_LISTING_KEY);
+            if (waypointListing != null) {
+                waypointListing.deleteEntry(index);
+                world.setData(WAYPOINT_LISTING_KEY, waypointListing);
+            }
+        }
+    }
+
+
+    public NavWaypoint(AWrapperWorld world, String index, String name, double targetspeed, double bearing, Point3D position) {
+        this.index = index;
+        this.name = name;
+        this.targetSpeed = targetspeed;
+        this.bearing = bearing;
+        this.position = position;
+        IWrapperNBT waypointListing = world.getData(WAYPOINT_LISTING_KEY);
+        if (waypointListing == null) {
+            waypointListing = InterfaceManager.coreInterface.getNewNBTWrapper();
+        }
+        waypointListing.setData(index, save(InterfaceManager.coreInterface.getNewNBTWrapper()));
+        world.setData(WAYPOINT_LISTING_KEY, waypointListing);
+        cachedWaypointMaps.remove(world);
+    }
+
+
+    //Vehicle Operation
+
+    public NavWaypoint(AEntityD_Definable<?> entity, String index, String name, double targetspeed, double bearing, Point3D position, IWrapperNBT data) {
+        this.index = index;
+        this.name = name;
+        this.targetSpeed = targetspeed;
+        this.bearing = bearing;
+        this.position = position;
+        IWrapperNBT waypointListing = data.getData(WAYPOINT_LISTING_KEY);
+        if (waypointListing == null) {
+            waypointListing = InterfaceManager.coreInterface.getNewNBTWrapper();
+        }
+        waypointListing.setData(index, save(InterfaceManager.coreInterface.getNewNBTWrapper()));
+        data.setData(WAYPOINT_LISTING_KEY, waypointListing);
+        v_cachedWaypointMaps.remove(entity);
+    }
+
+    public static NavWaypoint getByIndexFromVehicle(AEntityD_Definable entity, String index, IWrapperNBT data) {
+        //check data
+        if(data == null)return null;
+
+        if (!v_cachedWaypointMaps.containsKey(entity)) {
+            IWrapperNBT waypointListing = data.getData(WAYPOINT_LISTING_KEY);
+            if (waypointListing != null) {
+                Map<String, NavWaypoint> waypointMap = new HashMap<>();
+                for (String waypointIndex : waypointListing.getAllNames()) {
+                    waypointMap.put(waypointIndex, new NavWaypoint(waypointListing.getData(waypointIndex)));
+                }
+                v_cachedWaypointMaps.put(entity, waypointMap);
+            } else {
+                return null;
+            }
+        }
+
+        //check if this index is existed
+        if(v_cachedWaypointMaps.get(entity)!=null)return v_cachedWaypointMaps.get(entity).get(index);
+        else return null;
+    }
+    public static Map<String, NavWaypoint> getAllWaypointsFromVehicle(AEntityD_Definable entity, IWrapperNBT data) {
+        //check data
+        if(data == null)return null;
+
+        if (!v_cachedWaypointMaps.containsKey(entity)) {
+            IWrapperNBT waypointListing = data.getData(WAYPOINT_LISTING_KEY);
+            if (waypointListing != null) {
+                Map<String, NavWaypoint> waypointMap = new HashMap<>();
+                for (String waypointIndex : waypointListing.getAllNames()) {
+                    waypointMap.put(waypointIndex, new NavWaypoint(waypointListing.getData(waypointIndex)));
+                }
+                v_cachedWaypointMaps.put(entity, waypointMap);
+            } else {
+                return null;
+            }
+        }
+
+        return v_cachedWaypointMaps.get(entity);
+    }
+    public static void removeFromVehicle(AEntityD_Definable entity, String index, IWrapperNBT data) {
+        if (index != null) {
+            if (v_cachedWaypointMaps.containsKey(entity)) {
+                v_cachedWaypointMaps.get(entity).remove(index);
+            }
+
+            IWrapperNBT waypointListing = data.getData(WAYPOINT_LISTING_KEY);
+            if (waypointListing != null) {
+                waypointListing.deleteEntry(index);
+                data.setData(WAYPOINT_LISTING_KEY, waypointListing);
+            }
+        }
+    }
+
+
+
+    //Universal Operation
+
+    //internal init
+    private NavWaypoint(IWrapperNBT data) {
+        this.index = data.getString("index");
+        this.name = data.getString("name");
+        this.targetSpeed = data.getDouble("targetSpeed");
+        this.bearing = data.getDouble("bearing");
+        this.position = data.getPoint3dCompact("location");
+    }
+
+    public double getBearingDelta(AEntityB_Existing entity) {
+        double delta = Math.toDegrees(Math.atan2(entity.position.x - position.x, entity.position.z - position.z)) + bearing + 180;
+        while (delta < -180)
+            delta += 360;
+        while (delta > 180)
+            delta -= 360;
+        return delta;
+    }
+
+    public IWrapperNBT save(IWrapperNBT data) {
+        data.setString("index",index);
+        data.setString("name", name);
+        data.setDouble("targetSpeed", targetSpeed);
+        data.setDouble("bearing", bearing);
+        data.setPoint3dCompact("location", position);
+        return data;
+    }
+
+    public static void sortWaypointListByIndex(List<NavWaypoint> globalWaypointList) {
+        Collections.sort(globalWaypointList, new Comparator<NavWaypoint>() {
+            @Override
+            public int compare(NavWaypoint wp1, NavWaypoint wp2) {
+                try {
+                    int index1 = Integer.parseInt(wp1.index);
+                    int index2 = Integer.parseInt(wp2.index);
+                    return Integer.compare(index1, index2);
+                } catch (NumberFormatException e) {
+                    return 0;
+                }
+            }
+        });
+    }
+}

mccore/src/main/java/minecrafttransportsimulator/baseclasses/NavWaypointUpdater.java

@@ -0,0 +1,102 @@
+package minecrafttransportsimulator.baseclasses;
+
+import minecrafttransportsimulator.entities.components.AEntityD_Definable;
+import minecrafttransportsimulator.mcinterface.AWrapperWorld;
+import minecrafttransportsimulator.mcinterface.IWrapperNBT;
+
+/**
+ * used for updating waypoint data from client.
+ */
+public class NavWaypointUpdater {
+    public NavWaypoint currentWaypoint;
+
+    public NavWaypointUpdater(NavWaypoint currentWaypoint) {
+        this.currentWaypoint = currentWaypoint;
+    }
+
+    private void destroyFromWorld(AWrapperWorld world) {
+        if (currentWaypoint != null) {
+            NavWaypoint.removeFromWorld(world, currentWaypoint.index);
+        }
+    }
+    public void updateStateFromWorld(AWrapperWorld world, String waypointIndex, String waypointName, String targetSpeedStr, String bearingStr, String xStr, String yStr, String zStr, String isDeleted ) {
+        boolean Deleted = Boolean.parseBoolean(isDeleted);
+
+        if(Deleted == true){
+            destroyFromWorld(world);
+        }else{
+            try {
+                if (waypointIndex == null || waypointIndex.trim().isEmpty()) {
+                    throw new IllegalArgumentException("Waypoint index cannot be empty.");
+                }
+
+                String name = waypointName;
+
+                double targetSpeed = Double.parseDouble(targetSpeedStr);
+
+                double bearing = Double.parseDouble(bearingStr);
+
+                double x = Double.parseDouble(xStr);
+                double y = Double.parseDouble(yStr);
+                double z = Double.parseDouble(zStr);
+
+                if (currentWaypoint != null) {
+                    NavWaypoint.removeFromWorld(world, currentWaypoint.index);
+                    currentWaypoint = null;
+                }
+                currentWaypoint = new NavWaypoint(world,waypointIndex,waypointName,targetSpeed,bearing,new Point3D(x, y, z));
+
+            } catch (NumberFormatException e) {
+//            System.err.println("Invalid input: Expected numerical values for targetSpeed, bearing, and position.");
+            } catch (IllegalArgumentException e) {
+//            System.err.println("Invalid input: " + e.getMessage());
+            } catch (Exception e) {
+//            System.err.println("An unexpected error occurred: " + e.getMessage());
+            }
+        }
+
+    }
+
+    private void destroyFromVehicle(AEntityD_Definable entity, IWrapperNBT data) {
+        if (currentWaypoint != null) {
+            NavWaypoint.removeFromVehicle(entity, currentWaypoint.index,data);
+        }
+    }
+    public void updateStateFromVehicle(AEntityD_Definable entity,IWrapperNBT data ,String waypointIndex, String waypointName, String targetSpeedStr, String bearingStr, String xStr, String yStr, String zStr, String isDeleted ) {
+        boolean Deleted = Boolean.parseBoolean(isDeleted);
+
+        if(Deleted == true){
+            destroyFromVehicle(entity, data);
+        }else{
+            try {
+                if (waypointIndex == null || waypointIndex.trim().isEmpty()) {
+                    throw new IllegalArgumentException("Waypoint index cannot be empty.");
+                }
+
+                String name = waypointName;
+
+                double targetSpeed = Double.parseDouble(targetSpeedStr);
+
+                double bearing = Double.parseDouble(bearingStr);
+
+                double x = Double.parseDouble(xStr);
+                double y = Double.parseDouble(yStr);
+                double z = Double.parseDouble(zStr);
+
+                if (currentWaypoint != null) {
+                    NavWaypoint.removeFromVehicle(entity, currentWaypoint.index, data);
+                    currentWaypoint = null;
+                }
+                currentWaypoint = new NavWaypoint(entity,waypointIndex,waypointName,targetSpeed,bearing,new Point3D(x, y, z),data);
+
+            } catch (NumberFormatException e) {
+//            System.err.println("Invalid input: Expected numerical values for targetSpeed, bearing, and position.");
+            } catch (IllegalArgumentException e) {
+//            System.err.println("Invalid input: " + e.getMessage());
+            } catch (Exception e) {
+//            System.err.println("An unexpected error occurred: " + e.getMessage());
+            }
+        }
+
+    }
+}

mccore/src/main/java/minecrafttransportsimulator/baseclasses/PIDController.java

@@ -0,0 +1,27 @@
+package minecrafttransportsimulator.baseclasses;
+
+public class PIDController {
+    private double kp;
+    private double ki;
+    private double kd;
+    private double prevError;
+    private double integral;
+
+    public PIDController(double kp, double ki, double kd) {
+        this.kp = kp;
+        this.ki = ki;
+        this.kd = kd;
+    }
+
+    public double loop(double error, double step) {
+        integral += error * step;
+        double derivative = (error - prevError) / step;
+        prevError = error;
+        return kp * error + ki * integral + kd * derivative;
+    }
+
+    public void clear() {
+        integral = 0;
+        prevError = 0;
+    }
+}