Correctly follow rails according to wheelbase

1 files changed, 17 insertions, 3 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;
+}