From d7b8d6463bcba9b433e8fa61abe7dd1558a6c4c3 Mon Sep 17 00:00:00 2001
From: Christian Semmler <mail@csemmler.com>
Date: Sat, 1 Jul 2023 01:24:46 +0200
Subject: [PATCH] lego: Implement/match CalculateNewVel (#66)

* lego: Implement/match CalculateNewVel

* remove braces

* consistency
---
 LEGO1/legonavcontroller.cpp | 36 +++++++++++++++++++++++++-----------
 LEGO1/legonavcontroller.h   |  1 +
 LEGO1/legoutil.h            | 12 ++++++++++++
 LEGO1/mxdsobject.cpp        | 10 +++++-----
 4 files changed, 43 insertions(+), 16 deletions(-)

diff --git a/LEGO1/legonavcontroller.cpp b/LEGO1/legonavcontroller.cpp
index af618de9..90c0a485 100644
--- a/LEGO1/legonavcontroller.cpp
+++ b/LEGO1/legonavcontroller.cpp
@@ -106,8 +106,7 @@ void LegoNavController::SetControlMax(int p_hMax, int p_vMax)
   this->m_hMax = p_hMax;
   this->m_vMax = p_vMax;
 
-  if (VideoManager()->GetVideoParam().flags().GetFullScreen())
-  {
+  if (VideoManager()->GetVideoParam().flags().GetFullScreen()) {
     this->m_hMax = 640;
     this->m_vMax = 480;
   }
@@ -134,19 +133,15 @@ void LegoNavController::ResetToDefault()
 void LegoNavController::SetTargets(int p_hPos, int p_vPos, MxBool p_accel)
 {
   if (this->m_trackDefault != FALSE)
-  {
     ResetToDefault();
-  }
 
-  if (p_accel != FALSE)
-  {
+  if (p_accel != FALSE) {
     this->m_targetTurnSpeed = CalculateNewTargetSpeed(p_hPos, this->m_hMax / 2, this->m_turnMaxSpeed);
     this->m_targetMovementSpeed = CalculateNewTargetSpeed(this->m_vMax - p_vPos, this->m_vMax / 2, this->m_movementMaxSpeed);
     this->m_turnAccel = CalculateNewAccel(p_hPos, this->m_hMax / 2, this->m_turnMaxAccel, (int)this->m_turnMinAccel);
     this->m_movementAccel = CalculateNewAccel(this->m_vMax - p_vPos, this->m_vMax / 2, this->m_movementMaxAccel, (int)this->m_movementMinAccel);
   }
-  else
-  {
+  else {
     this->m_targetTurnSpeed = 0.0f;
     this->m_targetMovementSpeed = 0.0f;
     this->m_movementAccel = this->m_movementDecel;
@@ -179,9 +174,28 @@ float LegoNavController::CalculateNewAccel(int p_pos, int p_center, float p_maxA
   result = Abs(diff) * p_maxAccel / p_center;
 
   if (result < p_minAccel)
-  {
-    result = (float)p_minAccel;
-  }
+    result = (float) p_minAccel;
 
   return result;
 }
+
+// OFFSET: LEGO1 0x10054fe0
+float LegoNavController::CalculateNewVel(float p_targetVel, float p_currentVel, float p_accel, float p_time)
+{
+  float newVel = p_currentVel;
+
+  float velDiff = p_targetVel - p_currentVel;
+  int vSign = velDiff > 0 ? 1 : -1;
+
+  if (Abs(velDiff) > this->m_zeroThreshold) {
+    float deltaVel = p_accel * p_time;
+    newVel = p_currentVel + (deltaVel * vSign);
+
+    if (vSign > 0)
+      newVel = Min(newVel, p_targetVel);
+    else
+      newVel = Max(newVel, p_targetVel);
+  }
+
+  return newVel;
+}
\ No newline at end of file
diff --git a/LEGO1/legonavcontroller.h b/LEGO1/legonavcontroller.h
index 9bffd7dd..89af9f12 100644
--- a/LEGO1/legonavcontroller.h
+++ b/LEGO1/legonavcontroller.h
@@ -40,6 +40,7 @@ class LegoNavController : public MxCore
   void SetTargets(int p_hPos, int p_vPos, MxBool p_accel);
   float CalculateNewTargetSpeed(int p_pos, int p_center, float p_maxSpeed);
   float CalculateNewAccel(int p_pos, int p_center, float p_maxAccel, int p_minAccel);
+  float CalculateNewVel(float p_targetVel, float p_currentVel, float p_accel, float p_time);
 
 private:
   int m_hMax;
diff --git a/LEGO1/legoutil.h b/LEGO1/legoutil.h
index e94a6440..f0e97ac5 100644
--- a/LEGO1/legoutil.h
+++ b/LEGO1/legoutil.h
@@ -7,4 +7,16 @@ inline T Abs(T p_t)
   return p_t < 0 ? -p_t : p_t;
 }
 
+template <class T>
+inline T Min(T p_t1, T p_t2)
+{
+  return p_t1 < p_t2 ? p_t1 : p_t2;
+}
+
+template <class T>
+inline T Max(T p_t1, T p_t2)
+{
+  return p_t1 > p_t2 ? p_t1 : p_t2;
+}
+
 #endif // LEGOUTIL_H
diff --git a/LEGO1/mxdsobject.cpp b/LEGO1/mxdsobject.cpp
index fd4b7434..66b1ed6c 100644
--- a/LEGO1/mxdsobject.cpp
+++ b/LEGO1/mxdsobject.cpp
@@ -45,16 +45,16 @@ MxDSObject &MxDSObject::operator=(MxDSObject &p_dsObject)
 }
 
 // OFFSET: LEGO1 0x100bf8e0
-void MxDSObject::SetObjectName(const char *p_name)
+void MxDSObject::SetObjectName(const char *p_objectName)
 {
-  if (p_name != this->m_objectName) {
+  if (p_objectName != this->m_objectName) {
     delete[] this->m_objectName;
 
-    if (p_name) {
-      this->m_objectName = new char[strlen(p_name) + 1];
+    if (p_objectName) {
+      this->m_objectName = new char[strlen(p_objectName) + 1];
 
       if (this->m_objectName) {
-        strcpy(this->m_objectName, p_name);
+        strcpy(this->m_objectName, p_objectName);
       }
     }
     else {