From ba3f5124f15a4085cc4e5aa0d48493eac2a729f2 Mon Sep 17 00:00:00 2001
From: Emma Broman <emma95.broman@hotmail.com>
Date: Fri, 30 Oct 2020 09:57:07 +0100
Subject: [PATCH] Use Simpson's rule for speed integration computation

---
 modules/autonavigation/speedfunction.cpp | 17 ++++++++---------
 1 file changed, 8 insertions(+), 9 deletions(-)

diff --git a/modules/autonavigation/speedfunction.cpp b/modules/autonavigation/speedfunction.cpp
index d725470a01..da50fc3d7c 100644
--- a/modules/autonavigation/speedfunction.cpp
+++ b/modules/autonavigation/speedfunction.cpp
@@ -24,6 +24,7 @@
 
 #include <modules/autonavigation/speedfunction.h>
 
+#include <modules/autonavigation/helperfunctions.h>
 #include <ghoul/logging/logmanager.h>
 #include <ghoul/misc/easing.h>
 #include <algorithm>
@@ -49,15 +50,13 @@ double SpeedFunction::scaledValue(double time, double duration, double pathLengt
 }
 
 void SpeedFunction::initIntegratedSum() {
-    // apply duration constraint (eq. 14 in Eberly)
-    double speedSum = 0.0;
-    int steps = 100;
-    double h = 1.0 / steps;
-    for (double t = 0.0; t <= 1.0; t += h) {
-        double midpointSpeed = 0.5 * (value(t + 0.5*h) + value(t - 0.5*h));
-        speedSum += h * midpointSpeed;
-    }
-    _integratedSum = speedSum;
+    const int steps = 100;
+    _integratedSum = helpers::simpsonsRule(
+        0.0,
+        1.0,
+        steps,
+        [this](double t) { return value(t); }
+    );
 }
 
 SexticDampenedSpeed::SexticDampenedSpeed() {