feat(std): add function operator

Author: rcarcasses

libs/std/include/rtbot/std/Function.h

@@ -0,0 +1,167 @@
+#ifndef FUNCTION_H
+#define FUNCTION_H
+
+#include <algorithm>
+#include <cmath>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "rtbot/Operator.h"
+
+namespace rtbot {
+
+template <class T, class V>
+struct Function : public Operator<T, V> {
+  enum class InterpolationType { LINEAR, HERMITE };
+
+  Function() = default;
+
+  Function(string const& id, vector<pair<V, V>> points, string type = "linear") : Operator<T, V>(id) {
+    if (points.size() < 2) {
+      throw runtime_error(typeName() + ": at least 2 points are required for interpolation");
+    }
+
+    sort(points.begin(), points.end());
+    this->points = points;
+
+    if (type == "linear") {
+      this->type = InterpolationType::LINEAR;
+    } else if (type == "hermite") {
+      this->type = InterpolationType::HERMITE;
+    } else {
+      throw runtime_error(typeName() + ": invalid interpolation type. Use 'linear' or 'hermite'");
+    }
+
+    this->addDataInput("i1", 1);
+    this->addOutput("o1");
+  }
+
+  virtual Bytes collect() override {
+    Bytes bytes = Operator<T, V>::collect();
+
+    // Serialize points
+    size_t pointsSize = points.size();
+    bytes.insert(bytes.end(), reinterpret_cast<const unsigned char*>(&pointsSize),
+                 reinterpret_cast<const unsigned char*>(&pointsSize) + sizeof(pointsSize));
+
+    for (const auto& point : points) {
+      bytes.insert(bytes.end(), reinterpret_cast<const unsigned char*>(&point.first),
+                   reinterpret_cast<const unsigned char*>(&point.first) + sizeof(point.first));
+      bytes.insert(bytes.end(), reinterpret_cast<const unsigned char*>(&point.second),
+                   reinterpret_cast<const unsigned char*>(&point.second) + sizeof(point.second));
+    }
+
+    // Serialize interpolation type
+    auto typeVal = static_cast<int>(type);
+    bytes.insert(bytes.end(), reinterpret_cast<const unsigned char*>(&typeVal),
+                 reinterpret_cast<const unsigned char*>(&typeVal) + sizeof(typeVal));
+
+    return bytes;
+  }
+
+  virtual void restore(Bytes::const_iterator& it) override {
+    Operator<T, V>::restore(it);
+
+    // Deserialize points
+    size_t pointsSize = *reinterpret_cast<const size_t*>(&(*it));
+    it += sizeof(pointsSize);
+
+    points.clear();
+    for (size_t i = 0; i < pointsSize; i++) {
+      V x = *reinterpret_cast<const V*>(&(*it));
+      it += sizeof(V);
+      V y = *reinterpret_cast<const V*>(&(*it));
+      it += sizeof(V);
+      points.push_back({x, y});
+    }
+
+    // Deserialize interpolation type
+    int typeVal = *reinterpret_cast<const int*>(&(*it));
+    it += sizeof(typeVal);
+    type = static_cast<InterpolationType>(typeVal);
+  }
+
+  string typeName() const override { return "Function"; }
+
+  OperatorMessage<T, V> processData() override {
+    string inputPort;
+    auto in = this->getDataInputs();
+    if (in.size() == 1)
+      inputPort = in.at(0);
+    else
+      throw runtime_error(typeName() + " : more than 1 input port found");
+
+    OperatorMessage<T, V> outputMsgs;
+    Message<T, V> msg = this->getDataInputLastMessage(inputPort);
+    V x = msg.value;
+
+    size_t i = 0;
+    while (i < points.size() - 1 && points[i + 1].first <= x) {
+      i++;
+    }
+
+    if (type == InterpolationType::LINEAR) {
+      if (i == 0 && x < points[0].first) {
+        msg.value = linearInterpolate(points[0].first, points[0].second, points[1].first, points[1].second, x);
+      } else if (i == points.size() - 1) {
+        msg.value = linearInterpolate(points[i - 1].first, points[i - 1].second, points[i].first, points[i].second, x);
+      } else {
+        msg.value = linearInterpolate(points[i].first, points[i].second, points[i + 1].first, points[i + 1].second, x);
+      }
+    } else {
+      if (i == 0 && x < points[0].first) {
+        msg.value = hermiteInterpolate(points[0].second, points[1].second, getTangent(0), getTangent(1),
+                                       (x - points[0].first) / (points[1].first - points[0].first));
+      } else if (i >= points.size() - 2) {
+        size_t last = points.size() - 1;
+        msg.value =
+            hermiteInterpolate(points[last - 1].second, points[last].second, getTangent(last - 1), getTangent(last),
+                               (x - points[last - 1].first) / (points[last].first - points[last - 1].first));
+      } else {
+        msg.value = hermiteInterpolate(points[i].second, points[i + 1].second, getTangent(i), getTangent(i + 1),
+                                       (x - points[i].first) / (points[i + 1].first - points[i].first));
+      }
+    }
+
+    PortMessage<T, V> v;
+    v.push_back(msg);
+    outputMsgs.emplace("o1", v);
+    return outputMsgs;
+  }
+
+  vector<pair<V, V>> getPoints() const { return points; }
+  string getInterpolationType() const { return type == InterpolationType::LINEAR ? "linear" : "hermite"; }
+
+ private:
+  vector<pair<V, V>> points;
+  InterpolationType type;
+
+  static V linearInterpolate(V x1, V y1, V x2, V y2, V x) { return y1 + (y2 - y1) * (x - x1) / (x2 - x1); }
+
+  static V hermiteInterpolate(V y0, V y1, V m0, V m1, V mu) {
+    V mu2 = mu * mu;
+    V mu3 = mu2 * mu;
+    V h00 = 2 * mu3 - 3 * mu2 + 1;
+    V h10 = mu3 - 2 * mu2 + mu;
+    V h01 = -2 * mu3 + 3 * mu2;
+    V h11 = mu3 - mu2;
+
+    return h00 * y0 + h10 * m0 + h01 * y1 + h11 * m1;
+  }
+
+  V getTangent(size_t i) const {
+    if (i == 0) {
+      return (points[1].second - points[0].second) / (points[1].first - points[0].first);
+    } else if (i == points.size() - 1) {
+      return (points[i].second - points[i - 1].second) / (points[i].first - points[i - 1].first);
+    } else {
+      return (points[i + 1].second - points[i - 1].second) / (points[i + 1].first - points[i - 1].first);
+    }
+  }
+};
+
+}  // namespace rtbot
+
+#endif  // FUNCTION_H

libs/std/include/rtbot/std/Function.md

@@ -0,0 +1,55 @@
+---
+behavior:
+  buffered: false
+  throughput: constant
+view:
+  shape: circle
+  latex:
+    template: |
+      f(x)
+jsonschema:
+  type: object
+  properties:
+    id:
+      type: string
+      description: The id of the operator
+    points:
+      type: array
+      description: Array of (x,y) coordinates defining the function
+      minItems: 2
+      items:
+        type: array
+        minItems: 2
+        maxItems: 2
+        items:
+          type: number
+    type:
+      type: string
+      enum: ["linear", "hermite"]
+      default: "linear"
+      description: Interpolation method to use
+  required: ["id", "points"]
+---
+
+# Function
+
+Inputs: `i1`  
+Outputs: `o1`
+
+The `Function` operator transforms input values through a user-defined function specified by a set of points. It supports both linear and Hermite interpolation methods.
+
+For input values between defined points, the operator performs interpolation according to the specified method:
+
+- `linear`: Simple linear interpolation between adjacent points
+- `hermite`: Smooth cubic Hermite interpolation using estimated tangents at each point
+
+For input values outside the defined range, the operator extrapolates using the same method.
+
+The `Function` operator does not hold a message buffer on `i1`. It emits transformed values through `o1` immediately after receiving input.
+
+Example usage for linear interpolation between points (0,0) and (1,1):
+
+```cpp
+vector<pair<double, double>> points = {{0.0, 0.0}, {1.0, 1.0}};
+auto func = Function<uint64_t, double>("func", points, "linear");
+```

libs/std/test/test_function.cpp

@@ -0,0 +1,78 @@
+#include <catch2/catch.hpp>
+#include <cmath>
+
+#include "rtbot/std/Function.h"
+
+using namespace rtbot;
+using namespace std;
+
+TEST_CASE("Function operator - Linear interpolation") {
+  vector<pair<double, double>> points = {{0.0, 0.0}, {1.0, 2.0}, {2.0, 4.0}, {3.0, 6.0}};
+  auto func = Function<uint64_t, double>("func", points, "linear");
+
+  SECTION("Interpolation between points") {
+    func.receiveData(Message<uint64_t, double>(1, 0.5));
+    auto output = func.executeData();
+    REQUIRE(output.find("func")->second.find("o1")->second.at(0).value == Approx(1.0));
+
+    func.receiveData(Message<uint64_t, double>(2, 1.5));
+    output = func.executeData();
+    REQUIRE(output.find("func")->second.find("o1")->second.at(0).value == Approx(3.0));
+  }
+
+  SECTION("Extrapolation before first point") {
+    func.receiveData(Message<uint64_t, double>(1, -1.0));
+    auto output = func.executeData();
+    REQUIRE(output.find("func")->second.find("o1")->second.at(0).value == Approx(-2.0));
+  }
+
+  SECTION("Extrapolation after last point") {
+    func.receiveData(Message<uint64_t, double>(1, 4.0));
+    auto output = func.executeData();
+    REQUIRE(output.find("func")->second.find("o1")->second.at(0).value == Approx(8.0));
+  }
+}
+
+TEST_CASE("Function operator - Hermite interpolation") {
+  vector<pair<double, double>> points = {{0.0, 0.0}, {1.0, 1.0}, {2.0, 0.0}, {3.0, 1.0}};
+  auto func = Function<uint64_t, double>("func", points, "hermite");
+
+  SECTION("Interpolation between points") {
+    func.receiveData(Message<uint64_t, double>(1, 0.5));
+    auto output = func.executeData();
+    REQUIRE(output.find("func")->second.find("o1")->second.at(0).value > 0.5);
+
+    func.receiveData(Message<uint64_t, double>(2, 1.5));
+    output = func.executeData();
+    double y = output.find("func")->second.find("o1")->second.at(0).value;
+    REQUIRE(y >= 0.0);
+    REQUIRE(y <= 1.0);
+  }
+}
+
+TEST_CASE("Function operator - Serialization") {
+  vector<pair<double, double>> points = {{0.0, 0.0}, {1.0, 2.0}, {2.0, 4.0}, {3.0, 6.0}};
+  auto func1 = Function<uint64_t, double>("func", points, "linear");
+
+  // Add some data and process it
+  func1.receiveData(Message<uint64_t, double>(1, 1.5));
+  auto output1 = func1.executeData();
+
+  // Serialize
+  Bytes bytes = func1.collect();
+
+  // Create new operator and restore state
+  auto func2 = Function<uint64_t, double>("func", {{0.0, 0.0}, {1.0, 1.0}}, "linear");  // Different initial state
+  Bytes::const_iterator it = bytes.begin();
+  func2.restore(it);
+
+  // Verify state was properly restored
+  REQUIRE(func2.getPoints() == points);
+  REQUIRE(func2.getInterpolationType() == "linear");
+
+  // Verify behavior is identical
+  func2.receiveData(Message<uint64_t, double>(1, 1.5));
+  auto output2 = func2.executeData();
+  REQUIRE(output2.find("func")->second.find("o1")->second.at(0).value ==
+          output1.find("func")->second.find("o1")->second.at(0).value);
+}