From 558c020cca675de4eef3129bccef7ca6f2cecc4f Mon Sep 17 00:00:00 2001
From: Prateek Machiraju <prateek.machiraju@gmail.com>
Date: Sat, 25 Jan 2020 02:10:28 -0500
Subject: [PATCH] Fix duplicated state when using quintic splines (#2307)

Generating a trajectory using quintic splines caused a duplicated state at all knot points.
---
 wpilibc/src/main/native/cpp/spline/SplineHelper.cpp    |  9 +++++++--
 .../edu/wpi/first/wpilibj/spline/SplineHelper.java     | 10 ++++++++--
 2 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
index cbfc8c1e75..e5d5b118f8 100644
--- a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
+++ b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -166,7 +166,12 @@ SplineHelper::QuinticControlVectorsFromWaypoints(
     const auto scalar =
         1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
 
-    vectors.push_back(QuinticControlVector(scalar, p0));
+    // Only add the first control vector if this is the first iteration. The
+    // last control vector of this iteration is the first control vector of
+    // the next iteration.
+    if (i == 0) {
+      vectors.push_back(QuinticControlVector(scalar, p0));
+    }
     vectors.push_back(QuinticControlVector(scalar, p1));
   }
   return vectors;
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
index a419bf9d9e..d41048a0bf 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -69,7 +69,13 @@ public final class SplineHelper {
       // This just makes the splines look better.
       final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
 
-      vectors.add(getQuinticControlVector(scalar, p0));
+      // Only add the first control vector if this is the first iteration. The
+      // last control vector of this iteration is the first control vector of
+      // the next iteration.
+      if (i == 0) {
+        vectors.add(getQuinticControlVector(scalar, p0));
+      }
+
       vectors.add(getQuinticControlVector(scalar, p1));
     }
     return vectors;