Correctly follow rails according to wheelbase

4 files changed, 43 insertions, 9 deletions

diff --git a/game/vehicles/railloco.cpp b/game/vehicles/railloco.cpp
index 146d2e0..a1add29 100644
--- a/game/vehicles/railloco.cpp
+++ b/game/vehicles/railloco.cpp
@@ -12,7 +12,7 @@ void
 RailLoco::tick(TickDuration dur)
 {
 	linkDist += dur.count() * speed;
-	auto curLink {linkHist.getAt(0.F)};
+	auto curLink {linkHist.getCurrent()};
 	while (linkDist > curLink.first->length) {
 		location = curLink.first->positionAt(curLink.first->length, curLink.second);
 		const auto & nexts {curLink.first->nexts[1 - curLink.second]};
@@ -28,7 +28,21 @@ RailLoco::tick(TickDuration dur)
 			speed = 0;
 		}
 	}
-	location = curLink.first->positionAt(linkDist, curLink.second);
+	const auto b1pos = curLink.first->positionAt(linkDist, curLink.second);
+	const auto b2pos
+			= (linkDist >= wheelBase) ? curLink.first->positionAt(linkDist - wheelBase, curLink.second) : [&]() {
+				  float b2linkDist {};
+				  const auto b2Link = linkHist.getAt(wheelBase - linkDist, &b2linkDist);
+				  return b2Link.first->positionAt(b2linkDist, b2Link.second);
+			  }();
+	location.GetPos() = (b1pos.GetPos() + b2pos.GetPos()) / 2.F;
+	const auto diff = glm::normalize(b2pos.GetPos() - b1pos.GetPos());
+	location.GetRot().x = -vector_pitch(diff);
+	location.GetRot().y = vector_yaw(diff);
 }
 
-Brush47::Brush47(const LinkPtr & l) : RailLoco(l, "brush47.obj", "brush47.png") { }
+Brush47::Brush47(const LinkPtr & l) : RailLoco(l, "brush47.obj", "brush47.png")
+{
+	wheelBase = 15.7F;
+	linkDist = wheelBase;
+}
diff --git a/game/vehicles/railloco.h b/game/vehicles/railloco.h
index 833b661..ff8871b 100644
--- a/game/vehicles/railloco.h
+++ b/game/vehicles/railloco.h
@@ -1,11 +1,17 @@
 #include "game/network/link.h"
+#include "game/vehicles/vehicle.h"
 #include "game/worldobject.h"
-#include "vehicle.h"
 #include <string>
 
-class RailLoco : public Vehicle {
+class RailVehicle : public Vehicle {
 public:
 	using Vehicle::Vehicle;
+	float wheelBase;
+};
+
+class RailLoco : public RailVehicle {
+public:
+	using RailVehicle::RailVehicle;
 	void tick(TickDuration elapsed) override;
 };
 
diff --git a/game/vehicles/vehicle.cpp b/game/vehicles/vehicle.cpp
index 569063d..f722cff 100644
--- a/game/vehicles/vehicle.cpp
+++ b/game/vehicles/vehicle.cpp
@@ -24,15 +24,26 @@ LinkHistory::add(const LinkWPtr & l, unsigned char d)
 }
 
 LinkHistory::Return
-LinkHistory::getAt(float len) const
+LinkHistory::getCurrent() const
+{
+	return {links.front().first.lock(), links.front().second};
+}
+
+LinkHistory::Return
+LinkHistory::getAt(float len, float * rem) const
 {
 	auto litr = links.begin();
 	while (len > 0.F && litr != links.end()) {
-		len -= litr->first.lock()->length;
 		litr++;
+		if (litr != links.end()) {
+			len -= litr->first.lock()->length;
+		}
 	}
 	if (litr == links.end()) {
 		litr--;
 	}
+	if (rem) {
+		*rem = -len;
+	}
 	return {litr->first.lock(), litr->second};
 }
diff --git a/game/vehicles/vehicle.h b/game/vehicles/vehicle.h
index f7defb3..8a85a28 100644
--- a/game/vehicles/vehicle.h
+++ b/game/vehicles/vehicle.h
@@ -5,17 +5,20 @@
 #include <game/network/link.h>
 #include <game/worldobject.h>
 #include <string>
+#include <utility>
+#include <vector>
 
 class LinkHistory {
 public:
 	using Entry = std::pair<LinkWPtr, unsigned char /*dir*/>;
 	using Return = std::pair<LinkCPtr, unsigned char /*dir*/>;
 	Return add(const LinkWPtr &, unsigned char);
-	Return getAt(float) const;
+	[[nodiscard]] Return getCurrent() const;
+	[[nodiscard]] Return getAt(float, float *) const;
 
 private:
 	std::vector<Entry> links;
-	float totalLen;
+	float totalLen {0.F};
 };
 
 class Vehicle : public WorldObject, public Physical {