3 files changed, 80 insertions, 2 deletions

diff --git a/src/control/AutoPilot.cpp b/src/control/AutoPilot.cpp
index 89284a96..e3d5c9e9 100644
--- a/src/control/AutoPilot.cpp
+++ b/src/control/AutoPilot.cpp
@@ -2,5 +2,50 @@
 #include "patcher.h"
 #include "AutoPilot.h"
 
-WRAPPER void CAutoPilot::RemoveOnePathNode() { EAXJMP(0x413A00); }
+#include "CarCtrl.h"
+#include "Curves.h"
+#include "PathFind.h"
+
+#if 0
 WRAPPER void CAutoPilot::ModifySpeed(float) { EAXJMP(0x4137B0); }
+#else
+void CAutoPilot::ModifySpeed(float speed)
+{
+	m_fMaxTrafficSpeed = max(0.01f, speed);
+	float positionBetweenNodes = (float)(CTimer::GetTimeInMilliseconds() - m_nTimeEnteredCurve) / m_nTimeToSpendOnCurrentCurve;
+	CCarPathLink* pCurrentLink = &ThePaths.m_carPathLinks[m_nCurrentPathNodeInfo];
+	CCarPathLink* pNextLink = &ThePaths.m_carPathLinks[m_nNextPathNodeInfo];
+	float currentPathLinkForwardX = m_nCurrentDirection * ThePaths.m_carPathLinks[m_nCurrentPathNodeInfo].dirX;
+	float currentPathLinkForwardY = m_nCurrentDirection * ThePaths.m_carPathLinks[m_nCurrentPathNodeInfo].dirY;
+	float nextPathLinkForwardX = m_nNextDirection * ThePaths.m_carPathLinks[m_nNextPathNodeInfo].dirX;
+	float nextPathLinkForwardY = m_nNextDirection * ThePaths.m_carPathLinks[m_nNextPathNodeInfo].dirY;
+	CVector positionOnCurrentLinkIncludingLane(
+		pCurrentLink->posX + ((m_nCurrentLane + 0.5f) * LANE_WIDTH) * currentPathLinkForwardY,
+		pCurrentLink->posY - ((m_nCurrentLane + 0.5f) * LANE_WIDTH) * currentPathLinkForwardX,
+		0.0f);
+	CVector positionOnNextLinkIncludingLane(
+		pNextLink->posX + ((m_nNextLane + 0.5f) * LANE_WIDTH) * nextPathLinkForwardY,
+		pNextLink->posY - ((m_nNextLane + 0.5f) * LANE_WIDTH) * nextPathLinkForwardX,
+		0.0f);
+	m_nTimeToSpendOnCurrentCurve = CCurves::CalcSpeedScaleFactor(
+		&positionOnCurrentLinkIncludingLane,
+		&positionOnNextLinkIncludingLane,
+		currentPathLinkForwardX, currentPathLinkForwardY,
+		nextPathLinkForwardX, nextPathLinkForwardY
+	) * (1000.0f / m_fMaxTrafficSpeed);
+#ifdef FIX_BUGS
+	/* Casting timer to float is very unwanted, and in this case even causes crashes. */
+	m_nTimeEnteredCurve = CTimer::GetTimeInMilliseconds() -
+		(uint32)(positionBetweenNodes * m_nTimeToSpendOnCurrentCurve);
+#else
+	m_nTimeEnteredCurve = CTimer::GetTimeInMilliseconds() - positionBetweenNodes * m_nSpeedScaleFactor;
+#endif
+}
+#endif
+
+void CAutoPilot::RemoveOnePathNode()
+{
+	--m_nPathFindNodesCount;
+	for (int i = 0; i < m_nPathFindNodesCount; i++)
+		m_aPathFindNodesInfo[i] = m_aPathFindNodesInfo[i + 1];
+}
\ No newline at end of file
diff --git a/src/control/CarCtrl.cpp b/src/control/CarCtrl.cpp
index cae010d2..1d59c6c8 100644
--- a/src/control/CarCtrl.cpp
+++ b/src/control/CarCtrl.cpp
@@ -428,7 +428,7 @@ CCarCtrl::GenerateOneRandomCar()
 	pCar->AutoPilot.m_nTimeEnteredCurve = CTimer::GetTimeInMilliseconds() -
 		(uint32)((0.5f + positionBetweenNodes) * pCar->AutoPilot.m_nTimeToSpendOnCurrentCurve);
 #else
-	pCar->AutoPilot.m_nTotalSpeedScaleFactor = CTimer::GetTimeInMilliseconds() -
+	pCar->AutoPilot.m_nTimeEnteredCurve = CTimer::GetTimeInMilliseconds() -
 		(0.5f + positionBetweenNodes) * pCar->AutoPilot.m_nSpeedScaleFactor;
 #endif
 	CVector directionCurrentLink(directionCurrentLinkX, directionCurrentLinkY, 0.0f);
diff --git a/src/control/Curves.cpp b/src/control/Curves.cpp
index 84d4af5a..c5f64bf7 100644
--- a/src/control/Curves.cpp
+++ b/src/control/Curves.cpp
@@ -2,5 +2,38 @@
 #include "patcher.h"
 #include "Curves.h"
 
+#if 0
 WRAPPER float CCurves::CalcSpeedScaleFactor(CVector*, CVector*, float, float, float, float) { EAXJMP(0x420410); }
+#else
+float CCurves::CalcSpeedScaleFactor(CVector* pPoint1, CVector* pPoint2, float dir1X, float dir1Y, float dir2X, float dir2Y)
+{
+	CVector2D dir1(dir1X, dir1Y);
+	CVector2D dir2(dir2X, dir2Y);
+	float distance = (*pPoint1 - *pPoint2).Magnitude2D();
+	float dp = DotProduct2D(dir1, dir2);
+	if (dp > 0.9f)
+		return distance + Abs((pPoint1->x * dir1Y - pPoint1->y * dir1X) - (pPoint2->x * dir1Y - pPoint2->y * dir1X));
+	else
+		return ((1.0f - dp) * 0.2f + 1.0f) * distance;
+}
+#endif
+
+#if 0
 WRAPPER void CCurves::CalcCurvePoint(CVector*, CVector*, CVector*, CVector*, float, int32, CVector*, CVector*) { EAXJMP(0x4204D0); }
+#else
+void CCurves::CalcCurvePoint(CVector* pPos1, CVector* pPos2, CVector* pDir1, CVector* pDir2, float between, int32 timeOnCurve, CVector* pOutPos, CVector* pOutDir)
+{
+	float actualFactor = CalcSpeedScaleFactor(pPos1, pPos2, pDir1->x, pDir1->y, pDir2->x, pDir2->y);
+	CVector2D dir1 = *pDir1 * actualFactor;
+	CVector2D dir2 = *pDir2 * actualFactor;
+	float curveCoef = 0.5f - 0.5f * cos(3.1415f * between);
+	*pOutPos = CVector(
+		(pPos1->x + between * dir1.x) * (1.0f - curveCoef) + (pPos2->x - (1 - between) * dir2.x) * curveCoef,
+		(pPos1->y + between * dir1.y) * (1.0f - curveCoef) + (pPos2->y - (1 - between) * dir2.y) * curveCoef,
+		0.0f);
+	*pOutDir = CVector(
+		(dir1.x * (1.0f - curveCoef) + dir2.x * curveCoef) / (timeOnCurve * 0.001f),
+		(dir1.y * (1.0f - curveCoef) + dir2.y * curveCoef) / (timeOnCurve * 0.001f),
+		0.0f);
+}
+#endif
\ No newline at end of file