Split and organize first pass.

CMakeLists.txt

@@ -8,14 +8,17 @@ find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_ros REQUIRED)
 find_package(rclcpp REQUIRED)
 
-add_library(optimizer src/optimizer.cpp)
+add_library(simplex_solver
-target_compile_features(optimizer PUBLIC cxx_std_17)  # Require C++17
+  src/CostFunction.cpp
-target_include_directories(optimizer PUBLIC
+  src/SimplexSolver.cpp)
+target_compile_features(simplex_solver PUBLIC cxx_std_17)  # Require C++17
+target_include_directories(simplex_solver PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 
 add_executable(main src/main.cpp)
 target_compile_features(main PUBLIC cxx_std_17)  # Require C++17
+target_link_libraries(main simplex_solver)
 target_include_directories(main PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
@@ -27,7 +30,7 @@ install(
 )
 install(
   TARGETS
-    optimizer
+    simplex_solver
   EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib/${PROJECT_NAME}
   LIBRARY DESTINATION lib/${PROJECT_NAME}

include/j7s-optimization/CostFunction.hpp

@@ -7,20 +7,25 @@
 // Unless required by applicable law or agreed to in writing, software
 // distributed under the License is distributed on an "AS IS" BASIS,
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-#ifndef J7S__OPTIMIZER_HPP_
+#ifndef J7S__COSTFUNCTION_HPP_
-#define J7S__OPTIMIZER_HPP_
+#define J7S__COSTFUNCTION_HPP_
 
 namespace j7s
 {
-
+class CostFunction
-class Optimizer
 {
 public:
-  Optimizer();
+    CostFunction(double a, double b, double c);
+    double eval(double input) const;
 
-  virtual ~Optimizer();
+    double actualBest() const;
+
+private:
+    double m_a;
+    double m_b;
+    double m_c;
 };
 
 }  // namespace j7s
 
-#endif  // J7S__OPTIMIZER_HPP_
+#endif  // J7S__COSTFUNCTION_HPP_

include/j7s-optimization/SimplexSolver.hpp

@@ -0,0 +1,42 @@
+// Copyright 2022 James Pace
+// All Rights Reserved.
+//
+// For a license to this software contact
+// James Pace at jpace121@gmail.com.
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#ifndef J7S__SIMPLEXSOLVER_HPP_
+#define J7S__SIMPLEXSOLVER_HPP_
+
+#include "j7s-optimization/CostFunction.hpp"
+#include "j7s-optimization/common.hpp"
+
+#include <vector>
+
+namespace j7s
+{
+
+class SimplexSolver
+{
+public:
+    SimplexSolver(const CostFunction & costFunction, const std::vector<double> initSimplex);
+
+    IterationState update();
+
+    Coordinate bestCoord() const;
+
+private:
+    const CostFunction m_costFunction;
+    std::vector<Coordinate> m_currentSimplex;
+
+    // Helper functions.
+    double newPoint();
+    std::vector<Coordinate> contract();
+    double calcVolume();
+};
+
+}  // namespace j7s
+
+#endif  // J7S__SIMPLEXSOLVER_HPP_

include/j7s-optimization/common.hpp

@@ -0,0 +1,41 @@
+// Copyright 2022 James Pace
+// All Rights Reserved.
+//
+// For a license to this software contact
+// James Pace at jpace121@gmail.com.
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#ifndef J7S__COMMON_HPP_
+#define J7S__COMMON_HPP_
+
+namespace j7s
+{
+
+// The state after a single iteration of the solver.
+enum class IterationState
+{
+    OK,
+    CONVERGED
+};
+
+// A coordinate is pair of a cost input and the resulting the cost.
+// We save them together to minimize cost function evaluations.
+// This isn't the cleanest thing in the world, but it's worth saving
+// evaluations.
+struct Coordinate
+{
+    double input;
+    double cost;
+
+    Coordinate() : input{0.0}, cost{0.0} {};
+    Coordinate(double input, double cost) : input{input}, cost{cost} {};
+
+    // Sort by cost.
+    bool operator<(const Coordinate & other) const { return (cost < other.cost); }
+};
+
+}  // namespace j7s
+
+#endif  // J7S__COMMON_HPP_

src/CostFunction.cpp

@@ -0,0 +1,33 @@
+// Copyright 2022 James Pace
+// All Rights Reserved.
+//
+// For a license to this software contact
+// James Pace at jpace121@gmail.com.
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#include "j7s-optimization/CostFunction.hpp"
+
+#include <cmath>
+
+namespace j7s
+{
+CostFunction::CostFunction(double a, double b, double c) : m_a{a}, m_b{b}, m_c{c} {}
+
+double CostFunction::eval(double input) const
+{
+    return m_a * std::pow(input, 2) + m_b * input + m_c;
+}
+
+double CostFunction::actualBest() const
+{
+    // y = a*x**2 + b*x + c
+    // dy/dx = 2*a*x + b
+    // 0 = 2*a*x + b
+    // -b/2*a = x
+
+    return (-m_b / (2 * m_a));
+}
+
+}  // namespace j7s

src/SimplexSolver.cpp

@@ -0,0 +1,138 @@
+// Copyright 2022 James Pace
+// All Rights Reserved.
+//
+// For a license to this software contact
+// James Pace at jpace121@gmail.com.
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#include "j7s-optimization/SimplexSolver.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <stdexcept>
+
+namespace j7s
+{
+SimplexSolver::SimplexSolver(
+    const CostFunction & costFunction, const std::vector<double> initSimplex) :
+    m_costFunction{costFunction}
+{
+    m_currentSimplex.reserve(initSimplex.size());
+
+    for (const auto val : initSimplex)
+    {
+        m_currentSimplex.emplace_back(val, m_costFunction.eval(val));
+    }
+    std::sort(m_currentSimplex.begin(), m_currentSimplex.end());
+}
+
+Coordinate SimplexSolver::bestCoord() const
+{
+    return m_currentSimplex.front();
+}
+
+double SimplexSolver::newPoint()
+{
+    if (m_currentSimplex.size() == 0)
+    {
+        throw std::runtime_error("Simplex can't be missing.");
+    }
+
+    // Calculate sum.
+    double biggest = m_currentSimplex.back().input;
+    // Calculate volume.
+    double sum = 0.0;
+    // All but the biggest, (would be adding 0...)
+    for (unsigned int index = 0; index < m_currentSimplex.size() - 1; index++)
+    {
+        const double diff = m_currentSimplex[index].input - biggest;
+        sum += diff;
+    }
+
+    const double newPoint = sum * (2.0 / m_currentSimplex.size());
+
+    return newPoint;
+}
+
+std::vector<Coordinate> SimplexSolver::contract()
+{
+    const auto smallest = m_currentSimplex.front();
+
+    std::vector<Coordinate> newVector;
+    newVector.reserve(m_currentSimplex.size());
+    newVector.emplace_back(smallest);
+
+    // TODO: Really check size before I get here...
+    for (auto it = m_currentSimplex.begin() + 1; it != m_currentSimplex.end(); it++)
+    {
+        const auto oldInput = it->input;
+        const auto newInput = 0.5 * (oldInput + smallest.input);
+        const auto newCost = m_costFunction.eval(newInput);
+        newVector.emplace_back(newInput, newCost);
+    }
+    std::sort(newVector.begin(), newVector.end());
+
+    return newVector;
+}
+
+double SimplexSolver::calcVolume()
+{
+    // TODO: For reals do something like:
+    // https://math.stackexchange.com/questions/337197/finding-the-volume-of-a-tetrahedron-by-given-vertices
+    // For now:
+    // Sort by input and find the difference squared between the first and last.
+
+    const auto inputLess = [](const Coordinate & first, const Coordinate & second)
+    { return first.input < second.input; };
+
+    // Copy the vector so we don't sort the original.
+    std::vector<Coordinate> simplexCopy = m_currentSimplex;
+    std::sort(simplexCopy.begin(), simplexCopy.end(), inputLess);
+    const auto smallest = simplexCopy.front();
+    const auto biggest = simplexCopy.back();
+    return std::pow(biggest.input - smallest.input, 2.0);
+}
+
+IterationState SimplexSolver::update()
+{
+    if (m_currentSimplex.size() < 3)
+    {
+        throw std::runtime_error("Simplex can't be a line.");
+    }
+    // Check for convergence and potentially early return.
+    // TODO: Make configurable.
+    const auto volume = calcVolume();
+    if (volume < 1e-4)
+    {
+        return IterationState::CONVERGED;
+    }
+
+    // Do update.
+    Coordinate potential;
+    potential.input = newPoint();
+    potential.cost = m_costFunction.eval(potential.input);
+
+    // TODO: Understand why looking at second biggest, not biggest.
+    // Explanation from paper is that it is in order to make
+    // the new guess of the next iteration different the current
+    // biggest.
+    const auto secondBiggest = *(m_currentSimplex.end() - 2);
+    if (potential.cost < secondBiggest.cost)
+    {
+        // Replace the last simplex value with the better one.
+        *(m_currentSimplex.end() - 1) = potential;
+        // TODO: DO I need to sort or is it sorted already?
+        std::sort(m_currentSimplex.begin(), m_currentSimplex.end());
+    }
+    else
+    {
+        // Do a contraction.
+        m_currentSimplex = contract();
+    }
+
+    return IterationState::OK;
+}
+
+}  // namespace j7s

src/main.cpp

@@ -7,224 +7,33 @@
 // Unless required by applicable law or agreed to in writing, software
 // distributed under the License is distributed on an "AS IS" BASIS,
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-#include <cmath>
 #include <iostream>
-#include <vector>
-#include <algorithm>
-#include <stdexcept>
 
-#include "j7s-optimization/optimizer.hpp"
+#include "j7s-optimization/CostFunction.hpp"
-
+#include "j7s-optimization/SimplexSolver.hpp"
-class CostFunction
+#include "j7s-optimization/common.hpp"
-{
-public:
-    CostFunction(double a, double b, double c);
-    double eval(double input) const;
-
-    double actualBest() const;
-
-private:
-    double m_a;
-    double m_b;
-    double m_c;
-};
-
-CostFunction::CostFunction(double a, double b, double c) : m_a{a}, m_b{b}, m_c{c} {}
-
-double CostFunction::eval(double input) const
-{
-    return m_a * std::pow(input, 2) + m_b * input + m_c;
-}
-
-double CostFunction::actualBest() const
-{
-    // y = a*x**2 + b*x + c
-    // dy/dx = 2*a*x + b
-    // 0 = 2*a*x + b
-    // -b/2*a = x
-
-    return (-m_b / (2 * m_a));
-}
-
-// A coordinate is pair of a cost input and the resulting the cost.
-// We save them together to minimize cost function evaluations.
-// This isn't the cleanest thing in the world, but it's worth saving
-// evaluations.
-struct Coordinate
-{
-    double input;
-    double cost;
-
-    Coordinate() : input{0.0}, cost{0.0} {};
-    Coordinate(double input, double cost) : input{input}, cost{cost} {};
-
-    // Sort by cost.
-    bool operator<(const Coordinate & other) const { return (cost < other.cost); }
-};
-
-enum class IterationState {
-    OK,
-    CONVERGED
-};
-
-class SimplexSolver
-{
-public:
-    SimplexSolver(const CostFunction & costFunction, const std::vector<double> initSimplex);
-
-    IterationState update();
-
-    Coordinate bestCoord() const;
-
-private:
-    const CostFunction m_costFunction;
-    std::vector<Coordinate> m_currentSimplex;
-
-    // Helper functions.
-    double newPoint();
-    std::vector<Coordinate> contract();
-    double calcVolume();
-};
-
-SimplexSolver::SimplexSolver(const CostFunction & costFunction, const std::vector<double> initSimplex):
-    m_costFunction{costFunction}
-{
-    m_currentSimplex.reserve(initSimplex.size());
-
-    for(const auto val : initSimplex)
-    {
-        m_currentSimplex.emplace_back(val, m_costFunction.eval(val));
-    }
-    std::sort(m_currentSimplex.begin(), m_currentSimplex.end());
-}
-
-Coordinate SimplexSolver::bestCoord() const
-{
-    return m_currentSimplex.front();
-}
-
-double SimplexSolver::newPoint()
-{
-    if (m_currentSimplex.size() == 0)
-    {
-        throw std::runtime_error("Simplex can't be missing.");
-    }
-
-    // Calculate sum.
-    double biggest = m_currentSimplex.back().input;
-    // Calculate volume.
-    double sum = 0.0;
-    // All but the biggest, (would be adding 0...)
-    for (unsigned int index = 0; index < m_currentSimplex.size() - 1; index++)
-    {
-        const double diff = m_currentSimplex[index].input - biggest;
-        sum += diff;
-    }
-
-    const double newPoint = sum * (2.0 / m_currentSimplex.size());
-
-    return newPoint;
-}
-
-std::vector<Coordinate> SimplexSolver::contract()
-{
-    const auto smallest = m_currentSimplex.front();
-
-    std::vector<Coordinate> newVector;
-    newVector.reserve(m_currentSimplex.size());
-    newVector.emplace_back(smallest);
-
-    // TODO: Really check size before I get here...
-    for(auto it = m_currentSimplex.begin() + 1; it != m_currentSimplex.end(); it++)
-    {
-        const auto oldInput = it->input;
-        const auto newInput = 0.5*(oldInput + smallest.input);
-        const auto newCost = m_costFunction.eval(newInput);
-        newVector.emplace_back(newInput, newCost);
-    }
-    std::sort(newVector.begin(), newVector.end());
-
-    return newVector;
-}
-
-double SimplexSolver::calcVolume()
-{
-    // TODO: For reals do something like:
-    // https://math.stackexchange.com/questions/337197/finding-the-volume-of-a-tetrahedron-by-given-vertices
-    // For now:
-    // Sort by input and find the difference squared between the first and last.
-
-    const auto inputLess = [](const Coordinate & first, const Coordinate & second)
-    { return first.input < second.input; };
-
-    // Copy the vector so we don't sort the original.
-    std::vector<Coordinate> simplexCopy = m_currentSimplex;
-    std::sort(simplexCopy.begin(), simplexCopy.end(), inputLess);
-    const auto smallest = simplexCopy.front();
-    const auto biggest = simplexCopy.back();
-    return std::pow(biggest.input - smallest.input, 2.0);
-}
-
-IterationState SimplexSolver::update()
-{
-    if (m_currentSimplex.size() < 3)
-    {
-        throw std::runtime_error("Simplex can't be a line.");
-    }
-    // Check for convergence and potentially early return.
-    // TODO: Make configurable.
-    const auto volume = calcVolume();
-    if(volume < 1e-4)
-    {
-        return IterationState::CONVERGED;
-    }
-
-    // Do update.
-    Coordinate potential;
-    potential.input = newPoint();
-    potential.cost = m_costFunction.eval(potential.input);
-
-    // TODO: Understand why looking at second biggest, not biggest.
-    // Explanation from paper is that it is in order to make
-    // the new guess of the next iteration different the current
-    // biggest.
-    const auto secondBiggest = *(m_currentSimplex.end() - 2);
-    if(potential.cost < secondBiggest.cost)
-    {
-        // Replace the last simplex value with the better one.
-        *(m_currentSimplex.end() - 1) = potential;
-        // TODO: DO I need to sort or is it sorted already?
-        std::sort(m_currentSimplex.begin(), m_currentSimplex.end());
-    }
-    else
-    {
-        // Do a contraction.
-        m_currentSimplex = contract();
-    }
-
-    return IterationState::OK;
-}
 
 int main(int, char **)
 {
-    const CostFunction cost(2.0, 3.0, 4.0);
+    const j7s::CostFunction cost(2.0, 3.0, 4.0);
     const std::vector<double> init_simplex = {-10, 0, 10};
-    SimplexSolver solver(cost, init_simplex);
+    j7s::SimplexSolver solver(cost, init_simplex);
 
-    IterationState state = IterationState::OK;
+    j7s::IterationState state = j7s::IterationState::OK;
-    for(int cnt = 0; cnt < 1000; cnt++)
+    for (int cnt = 0; cnt < 1000; cnt++)
     {
         state = solver.update();
-        if(state == IterationState::CONVERGED)
+        if (state == j7s::IterationState::CONVERGED)
         {
             break;
         }
     }
-    if(state == IterationState::CONVERGED)
+    if (state == j7s::IterationState::CONVERGED)
     {
         const auto best = solver.bestCoord();
         std::cout << "Converged! Best Input: " << best.input << " Cost: " << best.cost << std::endl;
-        std::cout << "Actual Best: " << cost.actualBest() << " Cost: " << cost.eval(cost.actualBest()) << std::endl;
+        std::cout << "Actual Best: " << cost.actualBest()
+                  << " Cost: " << cost.eval(cost.actualBest()) << std::endl;
     }
     else
     {

src/optimizer.cpp

@@ -1,23 +0,0 @@
-// Copyright 2022 James Pace
-// All Rights Reserved.
-//
-// For a license to this software contact
-// James Pace at jpace121@gmail.com.
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-#include "j7s-optimization/optimizer.hpp"
-
-namespace j7s
-{
-
-Optimizer::Optimizer()
-{
-}
-
-Optimizer::~Optimizer()
-{
-}
-
-}  // namespace j7s