Rest carriages on two bogies that follow the rail

src/main/java/net/tfminecraft/vehicleframework/vehicles/handlers/TrainHandler.java

@@ -55,6 +55,7 @@
 import net.tfminecraft.vehicleframework.vehicles.ActiveVehicle;
 import net.tfminecraft.vehicleframework.vehicles.component.fuel.FuelTank;
 import net.tfminecraft.vehicleframework.vehicles.handlers.container.Container;
+import net.tfminecraft.vehicleframework.vehicles.handlers.train.Bogies;
 import net.tfminecraft.vehicleframework.vehicles.handlers.train.Connector;
 import net.tfminecraft.vehicleframework.vehicles.handlers.train.LocomotiveOverdrive;
 
@@ -91,10 +92,15 @@ public class TrainHandler {
 	private final ThrottleTape.DwellState tapeDwell = new ThrottleTape.DwellState();
 	// Blocks across the wheels. With it, the move animations turn at the train's speed.
 	private double wheelDiameter;
+	// Two-bogie carriages rest on the rail under each bogie instead of their centre.
+	private Bogies bogies;
 
 	public TrainHandler(ConfigurationSection config) {
 		locomotive = config.getBoolean("locomotive", false);
 		wheelDiameter = Math.max(0, config.getDouble("wheel-diameter", 0));
+		if (config.contains("bogies")) {
+			bogies = new Bogies(config.getStringList("bogies"));
+		}
 		if(config.contains("front-connector")) {
 			front = new Connector(config.getString("front-connector"));
 		}
@@ -114,6 +120,9 @@ public TrainHandler(ActiveVehicle v, TrainHandler another) {
 		locomotive = another.locomotive;
 		wheelDiameter = another.wheelDiameter;
 		this.v = v;
+		if (another.bogies != null) {
+			bogies = new Bogies(v, another.bogies);
+		}
 		if(another.isAttachable()) {
 			front = new Connector(v, another.getFront());
 		}
@@ -140,6 +149,9 @@ public LocomotiveOverdrive getOverdrive() {
 	}
 
 	public void updateModel(ActiveModel m) {
+		if (bogies != null) {
+			bogies.updateModel(m);
+		}
 		if(isAttachable()) {
 			front.updateModel(m);
 		}
@@ -765,7 +777,48 @@ private void followTrackUnderEntity() {
 	}
 
 	private record CarPlacement(ActiveVehicle vehicle, TrackSpline spline, double s, int sign,
-			double missingSpacing, TrackPose pose) {
+			double missingSpacing, TrackPose pose, TrackPose[] bogieRails) {
+		CarPlacement(ActiveVehicle vehicle, TrackSpline spline, double s, int sign,
+				double missingSpacing, TrackPose pose) {
+			this(vehicle, spline, s, sign, missingSpacing, pose, null);
+		}
+	}
+
+	// The junction route the consist is placed along, so bogies can straddle a junction.
+	private record Route(TrackRegistry registry, boolean takeBranch, UUID stemId, UUID branchId,
+			double junctionS, int facingSign, double stemLength, boolean stemLoop, double branchLength) {
+		/** The rail at {@code offset} along the track from {@code s}, following the route. */
+		TrackPose rail(UUID splineId, double s, double offset) {
+			TrackJunctionTravel.Pose at = TrackJunctionTravel.rewind(splineId, s, offset > 0 ? -1 : 1,
+					Math.abs(offset), takeBranch, stemId == null ? splineId : stemId, branchId, junctionS,
+					facingSign, stemLength, stemLoop, branchLength);
+			TrackSpline spline = at.splineId == null ? null : registry.get(at.splineId).orElse(null);
+			if (spline == null) {
+				return null;
+			}
+			if (at.missingSpacing <= 1e-9) {
+				return spline.sampleAt(at.s);
+			}
+			// Past the end of the route: carry on straight off that end.
+			double outwards = at.s <= 1e-9 ? -1 : 1;
+			return Bogies.rail(spline, at.s + outwards * at.missingSpacing);
+		}
+	}
+
+	// A bogie car's two rails along the route, or null to place it as a rigid car.
+	private TrackPose[] bogieRails(Route route, UUID onSpline, double at) {
+		if (!onBogies() || route == null) {
+			return null;
+		}
+		double[] offsets = bogies.offsets();
+		TrackPose[] rails = new TrackPose[offsets.length];
+		for (int i = 0; i < offsets.length; i++) {
+			rails[i] = route.rail(onSpline, at, offsets[i]);
+			if (rails[i] == null) {
+				return null;
+			}
+		}
+		return rails;
 	}
 
 	private void applyPlacements(List<CarPlacement> placements) {
@@ -775,30 +828,41 @@ private void applyPlacements(List<CarPlacement> placements) {
 			train.s = placement.s;
 			train.travelSign = placement.sign;
 			applyPose(placement.vehicle, placement.pose());
+			if (placement.bogieRails() != null) {
+				train.bogies.follow(placement.bogieRails(), placement.pose());
+			}
 		}
 	}
 
+	private boolean onBogies() {
+		return bogies != null && bogies.isReady();
+	}
+
 	private List<CarPlacement> planCars() {
 		List<CarPlacement> placements = new ArrayList<>();
 		TrackSpline spline = boundSpline();
 		if (spline == null) {
 			return placements;
 		}
-		placements.add(new CarPlacement(v, spline, s, travelSign, 0, spline.sampleAt(s)));
 		TrackRegistry registry = VehicleFramework.getTrackRegistry();
-		if (registry == null) {
-			return placements;
-		}
-		TrackJunction route = routeJunction();
+		TrackJunction route = registry == null ? null : routeJunction();
 		UUID stemId = route == null ? null : route.stemSplineId;
 		UUID branchId = route == null ? null : route.branchSplineId;
 		double junctionS = route == null ? 0 : route.s;
 		int facingSign = route == null ? 1 : route.facingSign;
-		TrackSpline stem = stemId == null ? null : registry.get(stemId).orElse(null);
-		TrackSpline branch = branchId == null ? null : registry.get(branchId).orElse(null);
+		TrackSpline stem = stemId == null || registry == null ? null : registry.get(stemId).orElse(null);
+		TrackSpline branch = branchId == null || registry == null ? null : registry.get(branchId).orElse(null);
 		double stemLength = stem == null ? spline.length() : stem.length();
 		boolean stemLoop = stem != null ? stem.isLoop() : spline.isLoop();
 		double branchLength = branch == null ? 0 : branch.length();
+		Route along = registry == null ? null : new Route(registry, takeBranch && route != null,
+				stemId, branchId, junctionS, facingSign, stemLength, stemLoop, branchLength);
+		TrackPose[] locoRails = bogieRails(along, splineId, s);
+		placements.add(new CarPlacement(v, spline, s, travelSign, 0,
+				locoRails == null ? spline.sampleAt(s) : bogies.bodyPose(locoRails), locoRails));
+		if (registry == null) {
+			return placements;
+		}
 		UUID parentSpline = splineId;
 		double parentS = s;
 		// Models face the +s tangent even in reverse. Couplers stay on that
@@ -836,8 +900,11 @@ private List<CarPlacement> planCars() {
 			}
 			TrackSpline carSpline = pose.splineId == null ? null : registry.get(pose.splineId).orElse(null);
 			if (carSpline != null) {
-				TrackPose carPose = carSpline.sampleAt(pose.s);
-				if (parentTrain.canHaveAttached() && carTrain.isAttachable() && pose.missingSpacing <= 1e-9) {
+				TrackPose[] rails = carTrain.bogieRails(along, pose.splineId, pose.s);
+				TrackPose carPose = rails == null ? carSpline.sampleAt(pose.s) : carTrain.bogies.bodyPose(rails);
+				// A car on bogies follows its own rails; its couplers swing to meet, as real ones do.
+				if (rails == null && parentTrain.canHaveAttached() && carTrain.isAttachable()
+						&& pose.missingSpacing <= 1e-9) {
 					// Arc spacing locates the car on the route; rigid couplers must meet
 					// in world space. A separate tangent at each centre opens a gap on bends.
 					TrackPose parentPose = placements.get(placements.size() - 1).pose();
@@ -847,7 +914,7 @@ private List<CarPlacement> planCars() {
 						// Keep the sampled pose until connector blueprints and model transforms load.
 					}
 				}
-				placements.add(new CarPlacement(car, carSpline, pose.s, carTravelSign, pose.missingSpacing, carPose));
+				placements.add(new CarPlacement(car, carSpline, pose.s, carTravelSign, pose.missingSpacing, carPose, rails));
 			} else {
 				return List.of();
 			}

src/main/java/net/tfminecraft/vehicleframework/vehicles/handlers/train/Bogies.java

@@ -0,0 +1,160 @@
+package net.tfminecraft.vehicleframework.vehicles.handlers.train;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.bukkit.util.Vector;
+
+import com.ticxo.modelengine.api.model.ActiveModel;
+import com.ticxo.modelengine.api.model.bone.ModelBone;
+
+import net.tfminecraft.vehicleframework.bones.BoneRotator;
+import net.tfminecraft.vehicleframework.bones.ConvertedAngle;
+import net.tfminecraft.vehicleframework.bones.RotationLimits;
+import net.tfminecraft.vehicleframework.tracks.TrackPose;
+import net.tfminecraft.vehicleframework.tracks.TrackSpline;
+import net.tfminecraft.vehicleframework.vehicles.ActiveVehicle;
+
+/**
+ * A carriage on two bogies, as real coaches are built. The body rests on the rail under
+ * each bogie, so it lies along the line between them, and each bogie turns and tilts to
+ * follow the rail under it. Bogie bones must pivot at the bogie's centre, and the body
+ * rotator must pivot at the model's origin.
+ */
+public final class Bogies {
+	// Bogies closer than this along the car cannot set the body's angle.
+	static final double MIN_SPAN = 0.25;
+	private final List<String> bones;
+	private ActiveVehicle v;
+	private final List<BoneRotator> rotators = new ArrayList<>();
+	// Along the car from the model's origin, in blocks; +z faces +s.
+	private double[] offsets;
+
+	public Bogies(List<String> bones) {
+		this.bones = List.copyOf(bones);
+	}
+
+	public Bogies(ActiveVehicle v, Bogies another) {
+		this.bones = another.bones;
+		this.v = v;
+	}
+
+	/** Whether this car has two bogies and its model is loaded. */
+	public boolean isReady() {
+		if (bones.size() != 2 || v == null) {
+			return false;
+		}
+		if (offsets != null) {
+			return true;
+		}
+		try {
+			ActiveModel model = v.getModel();
+			double[] found = new double[2];
+			List<BoneRotator> made = new ArrayList<>();
+			ModelBone[] foundBones = new ModelBone[2];
+			for (int i = 0; i < 2; i++) {
+				foundBones[i] = model.getBone(bones.get(i)).orElseThrow();
+				found[i] = foundBones[i].getBlueprintBone().getRotatedGlobalPosition().z() * model.getScale().z();
+			}
+			if (Math.abs(found[0] - found[1]) < MIN_SPAN) {
+				// Both bogies at one point along the car: nothing to rest the body between.
+				return false;
+			}
+			for (ModelBone bone : foundBones) {
+				made.add(new BoneRotator(v, v.getEntity(), bone, new RotationLimits()));
+			}
+			rotators.addAll(made);
+			offsets = found;
+			return true;
+		} catch (RuntimeException notLoaded) {
+			// The model or its bones are not there yet; place the car as a rigid one.
+			return false;
+		}
+	}
+
+	public void updateModel(ActiveModel model) {
+		for (BoneRotator rotator : rotators) {
+			rotator.updateModel(model);
+		}
+	}
+
+	/** Along the car from the model's origin to each bogie, in blocks, +z facing +s. */
+	public double[] offsets() {
+		return offsets.clone();
+	}
+
+	/** Where the car's origin goes so each bogie sits on its rail, in {@link #offsets()} order. */
+	public TrackPose bodyPose(TrackPose[] rails) {
+		return bodyPose(rails[0], rails[1], offsets[0], offsets[1]);
+	}
+
+	/** Turns each bogie to the rail under it, relative to the body. */
+	public void follow(TrackPose[] rails, TrackPose body) {
+		for (int i = 0; i < rotators.size(); i++) {
+			float[] turn = turn(body, rails[i]);
+			rotators.get(i).rotateToTarget(turn[0], turn[1], 0f, 1f, true, true, false);
+		}
+	}
+
+	static TrackPose bodyPose(TrackSpline spline, double s, double first, double second) {
+		return bodyPose(rail(spline, s + first), rail(spline, s + second), first, second);
+	}
+
+	/** The body resting on two rails, each under the bogie at its offset along the car. */
+	static TrackPose bodyPose(TrackPose firstRail, TrackPose secondRail, double first, double second) {
+		if (Math.abs(first - second) < 1e-6) {
+			// One point to rest on: keep the origin back along the rail from it.
+			return shifted(firstRail, -first);
+		}
+		boolean firstAhead = first > second;
+		TrackPose a = firstAhead ? firstRail : secondRail;
+		TrackPose b = firstAhead ? secondRail : firstRail;
+		double front = Math.max(first, second);
+		double back = Math.min(first, second);
+		Vector along = new Vector(a.x - b.x, a.y - b.y, a.z - b.z);
+		double length = along.length();
+		if (length < 1e-6) {
+			return a;
+		}
+		along.multiply(1 / length);
+		// The body is the straight line between the two bogie centres; the origin sits
+		// where it would along that line.
+		double middle = (front + back) / 2;
+		double x = (a.x + b.x) / 2 - along.getX() * middle;
+		double y = (a.y + b.y) / 2 - along.getY() * middle;
+		double z = (a.z + b.z) / 2 - along.getZ() * middle;
+		float yaw = (float) Math.toDegrees(Math.atan2(-along.getX(), along.getZ()));
+		float pitch = (float) Math.toDegrees(Math.atan2(-along.getY(), Math.hypot(along.getX(), along.getZ())));
+		return new TrackPose(x, y, z, yaw, pitch);
+	}
+
+	/** Yaw and pitch that turn a bogie on this body to the rail under it, as bone angles. */
+	static float[] turn(TrackPose body, TrackPose rail) {
+		float railYaw = rail.yaw;
+		float railPitch = rail.pitch;
+		// A bogie on a track laid the other way sees its rail heading backwards.
+		if (Math.abs(ConvertedAngle.wrapDegrees(railYaw - body.yaw)) > 90) {
+			railYaw += 180;
+			railPitch = -railPitch;
+		}
+		float yaw = ConvertedAngle.wrapDegrees(-(railYaw - body.yaw));
+		return new float[] {yaw, railPitch - body.pitch};
+	}
+
+	private static TrackPose shifted(TrackPose pose, double along) {
+		double yaw = Math.toRadians(pose.yaw);
+		double pitch = Math.toRadians(pose.pitch);
+		double horizontal = Math.cos(pitch) * along;
+		return new TrackPose(pose.x - Math.sin(yaw) * horizontal, pose.y - Math.sin(pitch) * along,
+				pose.z + Math.cos(yaw) * horizontal, pose.yaw, pose.pitch);
+	}
+
+	// The rail at a distance along the track, carrying on straight past its ends.
+	public static TrackPose rail(TrackSpline spline, double at) {
+		double end = spline.length();
+		double clamped = spline.isLoop() ? at : Math.max(0, Math.min(end, at));
+		TrackPose pose = spline.sampleAt(clamped);
+		double past = at - clamped;
+		return Math.abs(past) < 1e-9 ? pose : shifted(pose, past);
+	}
+}

src/main/resources/vehicles/passenger_car.yml

@@ -15,6 +15,10 @@ passenger_car:
         train:
             # Blocks across the wheels, so they turn at the train's speed.
             wheel-diameter: 1.0
+            # Rest the body on its two bogies, each following its own rail.
+            bogies:
+                - bogey
+                - bogey2
             front-connector: connector_front
             back-connector: connector_back
     states: