From 902930bee477273f2750f6025f989d3faa8244ec Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Thu, 10 Aug 2023 15:38:33 +0800
Subject: [PATCH 01/50] fix: add default category to show in node list

---
 zeno/include/zeno/utils/PropertyVisitor.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/zeno/include/zeno/utils/PropertyVisitor.h b/zeno/include/zeno/utils/PropertyVisitor.h
index 2a5d74c0e0..4aa258be42 100644
--- a/zeno/include/zeno/utils/PropertyVisitor.h
+++ b/zeno/include/zeno/utils/PropertyVisitor.h
@@ -176,6 +176,7 @@ namespace zeno {
                 if (!bHasInitializedDefaultObject) {
                     GetDescriptor().inputs.emplace_back("SRC");
                     GetDescriptor().outputs.emplace_back("DST");
+                    GetDescriptor().categories.emplace_back("AutoNode");
                     bHasInitializedDefaultObject = true;
                 }
 

From 3ff4ed0bbce68366038d3733e88fd76a17456acb Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Thu, 10 Aug 2023 15:38:44 +0800
Subject: [PATCH 02/50] feat: add calc slope node

---
 projects/Roads/nodes/src/cost.cpp             | 86 +++++++++++++++----
 projects/Roads/utilities/include/roads/data.h | 26 ++++--
 projects/Roads/utilities/include/roads/grid.h | 32 +++++++
 projects/Roads/utilities/include/roads/pch.h  |  6 ++
 .../Roads/utilities/include/roads/roads.h     |  3 +-
 projects/Roads/utilities/src/grid.cpp         | 55 +++++++++++-
 6 files changed, 185 insertions(+), 23 deletions(-)
 create mode 100644 projects/Roads/utilities/include/roads/grid.h
 create mode 100644 projects/Roads/utilities/include/roads/pch.h

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index c98c239e2b..ea1398e575 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -1,24 +1,26 @@
-#include "zeno/zeno.h"
-#include "zeno/utils/logger.h"
+#include "roads/roads.h"
 #include "zeno/PrimitiveObject.h"
 #include "zeno/types/UserData.h"
 #include "zeno/utils/PropertyVisitor.h"
-#include "roads/roads.h"
+#include "zeno/utils/logger.h"
+#include "zeno/zeno.h"
 
-template <typename ...Args>
-inline void RoadsAssert(const bool Expr, const std::string& InMsg = "[Roads] Assert Failed", Args... args) {
+template<typename... Args>
+inline void RoadsAssert(const bool Expr, const std::string &InMsg = "[Roads] Assert Failed", Args... args) {
     if (!Expr) {
         zeno::log_error(InMsg, args...);
+        std::quick_exit(-1);
     }
 }
 
-template <typename GridType = roads::Point>
-roads::DynamicGrid<GridType> BuildGridFromPrimitive(const zeno::AttrVector<zeno::vec3f>& DataSource, int32_t Nx, int32_t Ny) {
-    RoadsAssert(Nx * Ny <= DataSource.size());
+roads::DynamicGrid<roads::AdvancePoint> BuildGridFromPrimitive(const zeno::AttrVector<zeno::vec3f> &PositionSource, const zeno::AttrVector<float> &CurvatureSource, int32_t Nx, int32_t Ny) {
+    RoadsAssert(Nx * Ny <= PositionSource.size());
+    RoadsAssert(Nx * Ny <= CurvatureSource.size());
 
-    roads::DynamicGrid<GridType> Grid(Nx, Ny);
+    roads::DynamicGrid<roads::AdvancePoint> Grid(Nx, Ny);
     for (size_t i = 0; i < Nx * Ny; ++i) {
-        Grid[i] = DataSource[i];
+        Grid[i].Position = PositionSource[i];
+        Grid[i].Curvature = CurvatureSource[i];
     }
 
     return Grid;
@@ -26,6 +28,52 @@ roads::DynamicGrid<GridType> BuildGridFromPrimitive(const zeno::AttrVector<zeno:
 
 namespace {
     using namespace zeno;
+    using namespace roads;
+
+    struct PrimCalcSlope : public zeno::reflect::IParameterAutoNode<PrimCalcSlope> {
+        ZENO_GENERATE_NODE_BODY(PrimCalcSlope);
+
+        std::shared_ptr<zeno::PrimitiveObject> Primitive;
+        ZENO_DECLARE_INPUT_FIELD(Primitive, "Prim");
+        ZENO_DECLARE_OUTPUT_FIELD(Primitive, "Prim");
+
+        std::string SizeXChannel;
+        ZENO_DECLARE_INPUT_FIELD(SizeXChannel, "UserData_NxChannel", false, "", "nx");
+
+        std::string SizeYChannel;
+        ZENO_DECLARE_INPUT_FIELD(SizeYChannel, "UserData_NyChannel", false, "", "ny");
+
+        int Nx = 0;
+        ZENO_BINDING_PRIMITIVE_USERDATA(Primitive, Nx, SizeXChannel, false);
+
+        int Ny = 0;
+        ZENO_BINDING_PRIMITIVE_USERDATA(Primitive, Ny, SizeYChannel, false);
+
+        std::string HeightChannel;
+        ZENO_DECLARE_INPUT_FIELD(HeightChannel, "Vert_PositionChannel", false, "", "pos");
+
+        std::string OutputChannel;
+        ZENO_DECLARE_INPUT_FIELD(OutputChannel, "Vert_OutputChannel", false, "", "gradient");
+
+        zeno::AttrVector<float> HeightList{};
+        ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, HeightList, HeightChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
+
+        void apply() override {
+            RoadsAssert(AutoParameter->Nx * AutoParameter->Ny <= AutoParameter->HeightList.size(), "Bad size in userdata! Check your nx ny.");
+
+            DynamicGrid<HeightPoint> HeightField(AutoParameter->Nx, AutoParameter->Ny);
+            for (size_t i = 0; i < HeightField.size(); ++i) {
+                HeightField[i] = AutoParameter->HeightList[i];
+            }
+
+            DynamicGrid<SlopePoint> SlopeField = CalculateSlope(HeightField);
+            if (!AutoParameter->Primitive->verts.has_attr(AutoParameter->OutputChannel)) {
+                AutoParameter->Primitive->verts.add_attr<float>(AutoParameter->OutputChannel);
+            }
+            std::vector<float>& SlopeAttr = AutoParameter->Primitive->verts.attr<float>(AutoParameter->OutputChannel);
+            SlopeAttr.insert(SlopeAttr.begin(), SlopeField.begin(), SlopeField.end());
+        }
+    };
 
     struct CalcPathCost_Simple : public zeno::reflect::IParameterAutoNode<CalcPathCost_Simple> {
         ZENO_GENERATE_NODE_BODY(CalcPathCost_Simple);
@@ -43,18 +91,26 @@ namespace {
         std::string SizeYChannel;
         ZENO_DECLARE_INPUT_FIELD(SizeYChannel, "UserData_NyChannel", false, "", "ny");
 
+        std::string PositionChannel;
+        ZENO_DECLARE_INPUT_FIELD(PositionChannel, "Vert_PositionChannel", false, "", "pos");
+
+        std::string CurvatureChannel;
+        ZENO_DECLARE_INPUT_FIELD(CurvatureChannel, "Vert_CurvatureChannel", false, "", "curvature");
+
         int Nx = 0;
         ZENO_BINDING_PRIMITIVE_USERDATA(Primitive, Nx, SizeXChannel, false);
 
         int Ny = 0;
         ZENO_BINDING_PRIMITIVE_USERDATA(Primitive, Ny, SizeYChannel, false);
 
-        zeno::AttrVector<vec3f> PositionList {};
-        ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, PositionList, "pos", zeno::reflect::EZenoPrimitiveAttr::VERT);
+        zeno::AttrVector<vec3f> PositionList{};
+        ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, PositionList, PositionChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
+
+        zeno::AttrVector<float> CurvatureList{};
+        ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, CurvatureList, CurvatureChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
 
         void apply() override {
-            BuildGridFromPrimitive(PositionList, Nx, Ny);
-            zeno::log_info("aaa: {} {}", AutoParameter->Nx, AutoParameter->Ny);
+            auto Grid = BuildGridFromPrimitive(AutoParameter->PositionList, AutoParameter->CurvatureList, AutoParameter->Nx, AutoParameter->Ny);
         }
     };
-}
+}// namespace
diff --git a/projects/Roads/utilities/include/roads/data.h b/projects/Roads/utilities/include/roads/data.h
index ebbbc80888..c6eed4a456 100644
--- a/projects/Roads/utilities/include/roads/data.h
+++ b/projects/Roads/utilities/include/roads/data.h
@@ -17,9 +17,16 @@ namespace roads {
 
     struct Point : public Eigen::Vector3d {
         Point(const std::array<float, 3>& InArray) : Eigen::Vector3d(InArray[0], InArray[1], InArray[2]) {}
-        Point() = default;
+
+        using Eigen::Vector3d::Vector3d;
+    };
+
+    struct Point2D : public Eigen::Vector2d {
+        using Eigen::Vector2d::Vector2d;
     };
 
+    struct IntPoint : public std::array<size_t, 3> {};
+
     struct Triangle : public std::array<Point, 3> {
         bool AnyAngleLargerThan(const double Degree) {
             assert(Degree <= 180.0f);
@@ -34,20 +41,27 @@ namespace roads {
     };
 
     template <typename T>
-    struct ArrayList : public std::vector<T> {};
+    struct ArrayList : public std::vector<T> {
+        using std::vector<T>::vector;
+    };
 
     template <size_t X, size_t Y, typename GridPointType = Point>
     struct Grid : public std::array<GridPointType, X * Y> {};
 
-    template <typename GridPointType = Point>
-    struct DynamicGrid : public std::vector<GridPointType> {
+    template<typename GridPointType>
+    struct CustomGridBase : public ArrayList<GridPointType> {
         size_t Nx, Ny;
 
-        DynamicGrid(const size_t InNx, const size_t InNy) : std::vector<GridPointType>(InNx * InNy), Nx(InNx), Ny(InNy) {}
-        DynamicGrid(DynamicGrid&& OtherGridToMove)  noexcept : std::vector<GridPointType>(std::forward<std::vector<GridPointType>>(OtherGridToMove)) {
+        CustomGridBase(const size_t InNx, const size_t InNy) : ArrayList<GridPointType>(InNx * InNy), Nx(InNx), Ny(InNy) { }
+        CustomGridBase(CustomGridBase&& OtherGridToMove)  noexcept : ArrayList<GridPointType>(std::forward<ArrayList<GridPointType>>(OtherGridToMove)) {
             Nx = OtherGridToMove.Nx;
             Ny = OtherGridToMove.Ny;
         }
     };
 
+    template <typename GridPointType = Point>
+    struct DynamicGrid : public CustomGridBase<GridPointType> {
+        using CustomGridBase<GridPointType>::CustomGridBase;
+    };
+
 }// namespace roads
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
new file mode 100644
index 0000000000..ec53f75777
--- /dev/null
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include "pch.h"
+
+namespace roads {
+
+    struct CostPoint {
+        size_t Cost = 0;
+        Point Position;
+    };
+
+    struct AdvancePoint {
+        Point Position;
+        double Curvature = 0.0;
+    };
+
+    using HeightPoint = double;
+    using SlopePoint = double;
+
+    ROADS_API DynamicGrid<CostPoint> CalcCost_Simple(const DynamicGrid<AdvancePoint> &BaseGrid);
+
+    ROADS_API ROADS_INLINE double EuclideanDistance(const Point &Point1, const Point &Point2);
+
+    ROADS_API ROADS_INLINE double EuclideanDistance(const Point2D &Point1, const Point2D &Point2);
+
+    ROADS_API DynamicGrid<SlopePoint> CalculateSlope(const DynamicGrid<HeightPoint>& InHeightField);
+
+    namespace energy {
+        ROADS_API double CalculateStepSize(const Point2D &Pt, const Point2D &P, const Point2D &PrevX, const Point2D &CurrX);
+    }// namespace energy
+
+}// namespace roads
diff --git a/projects/Roads/utilities/include/roads/pch.h b/projects/Roads/utilities/include/roads/pch.h
new file mode 100644
index 0000000000..062c4b1cb6
--- /dev/null
+++ b/projects/Roads/utilities/include/roads/pch.h
@@ -0,0 +1,6 @@
+#pragma once
+
+#define ROADS_API __declspec(dllexport)
+#define ROADS_INLINE inline
+
+#include "data.h"
diff --git a/projects/Roads/utilities/include/roads/roads.h b/projects/Roads/utilities/include/roads/roads.h
index b622de0f51..5f1d2ab3ee 100644
--- a/projects/Roads/utilities/include/roads/roads.h
+++ b/projects/Roads/utilities/include/roads/roads.h
@@ -1,3 +1,4 @@
 #pragma once
 
-#include "data.h"
+#include "pch.h"
+#include "grid.h"
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index 1da5bc1778..2c32e523eb 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -1,6 +1,59 @@
-#include "roads/roads.h"
+#include "roads/grid.h"
 #include "Eigen/Eigen"
+#include "roads/roads.h"
 
 using namespace roads;
 
+DynamicGrid<CostPoint> roads::CalcCost_Simple(const DynamicGrid<AdvancePoint> &BaseGrid) {
+    return {0, 0};
+}
+
+double roads::EuclideanDistance(const Point &Point1, const Point &Point2) {
+    return std::sqrt(std::pow(Point2.x() - Point1.x(), 2) + std::pow(Point2.y() - Point1.y(), 2) + std::pow(Point2.z() - Point1.z(), 2));
+}
+
+double roads::EuclideanDistance(const Point2D &Point1, const Point2D &Point2) {
+    return std::sqrt(std::pow(Point2.x() - Point1.x(), 2) + std::pow(Point2.y() - Point1.y(), 2));
+}
+
+DynamicGrid<SlopePoint> roads::CalculateSlope(const DynamicGrid<HeightPoint> &InHeightField) {
+    constexpr static std::array<int32_t, 4> XDirection4 = { -1, 0, 1, 0 };
+    constexpr static std::array<int32_t, 4> YDirection4 = { 0, 1, 0, -1 };
+    constexpr static size_t DirectionSize = std::max(XDirection4.size(), YDirection4.size());
+
+    const size_t SizeX = InHeightField.Nx;
+    const size_t SizeY = InHeightField.Ny;
+
+    DynamicGrid<SlopePoint> Result(SizeX, SizeY);
+
+    for (int32_t y = 0; y < SizeY; ++y) {
+        for (int32_t x = 0; x < SizeX; ++x) {
+            const size_t OriginIdx = x + y * SizeX;
+            double MaxSlope = 0.0;
+            for (int32_t Direction = 0; Direction < DirectionSize; ++Direction) {
+                const int32_t nx = x + XDirection4[Direction];
+                const int32_t ny = y + YDirection4[Direction];
+                if (nx < 0 || ny < 0) continue;
+
+                const size_t idx = nx + ny * SizeX;
+                if (idx >= InHeightField.size()) continue;
+
+                const double HeightDiff = InHeightField[OriginIdx] - InHeightField[idx];
+                const double Distance = std::sqrt(1 + HeightDiff * HeightDiff);
+                MaxSlope = std::max(MaxSlope, HeightDiff / Distance);
+            }
+            Result[OriginIdx] = MaxSlope;
+        }
+    }
+
+    return Result;
+}
+
+double roads::energy::CalculateStepSize(const Point2D &Pt, const Point2D &P, const Point2D &PrevX, const Point2D &CurrX) {
+    const double LengthPtToXc = EuclideanDistance(Pt, CurrX);
+    const double LengthPtToP = EuclideanDistance(Pt, P);
+    const double LengthPrevXToP = EuclideanDistance(PrevX, P);
+    const double LengthPrevXToCurrX = EuclideanDistance(PrevX, CurrX);
 
+    return 0.0;
+}

From 481eff23a39629ae20a6a621de4780bc130c167b Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Thu, 10 Aug 2023 16:33:53 +0800
Subject: [PATCH 03/50] feat: add openmp support

---
 projects/Roads/nodes/src/cost.cpp             | 19 +++++++++++++------
 projects/Roads/utilities/CMakeLists.txt       | 19 +++++++++++++++++++
 projects/Roads/utilities/include/roads/grid.h |  2 +-
 projects/Roads/utilities/src/grid.cpp         |  1 +
 4 files changed, 34 insertions(+), 7 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index ea1398e575..61d4be2e14 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -1,4 +1,5 @@
 #include "roads/roads.h"
+#include <limits>
 #include "zeno/PrimitiveObject.h"
 #include "zeno/types/UserData.h"
 #include "zeno/utils/PropertyVisitor.h"
@@ -20,7 +21,7 @@ roads::DynamicGrid<roads::AdvancePoint> BuildGridFromPrimitive(const zeno::AttrV
     roads::DynamicGrid<roads::AdvancePoint> Grid(Nx, Ny);
     for (size_t i = 0; i < Nx * Ny; ++i) {
         Grid[i].Position = PositionSource[i];
-        Grid[i].Curvature = CurvatureSource[i];
+        Grid[i].Gradient = CurvatureSource[i];
     }
 
     return Grid;
@@ -94,8 +95,8 @@ namespace {
         std::string PositionChannel;
         ZENO_DECLARE_INPUT_FIELD(PositionChannel, "Vert_PositionChannel", false, "", "pos");
 
-        std::string CurvatureChannel;
-        ZENO_DECLARE_INPUT_FIELD(CurvatureChannel, "Vert_CurvatureChannel", false, "", "curvature");
+        std::string GradientChannel;
+        ZENO_DECLARE_INPUT_FIELD(GradientChannel, "Vert_GradientChannel", false, "", "gradient");
 
         int Nx = 0;
         ZENO_BINDING_PRIMITIVE_USERDATA(Primitive, Nx, SizeXChannel, false);
@@ -106,11 +107,17 @@ namespace {
         zeno::AttrVector<vec3f> PositionList{};
         ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, PositionList, PositionChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
 
-        zeno::AttrVector<float> CurvatureList{};
-        ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, CurvatureList, CurvatureChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
+        zeno::AttrVector<float> GradientList{};
+        ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, GradientList, GradientChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
 
         void apply() override {
-            auto Grid = BuildGridFromPrimitive(AutoParameter->PositionList, AutoParameter->CurvatureList, AutoParameter->Nx, AutoParameter->Ny);
+            //auto Grid = BuildGridFromPrimitive(AutoParameter->PositionList, AutoParameter->GradientList, AutoParameter->Nx, AutoParameter->Ny);
+            // TODO [darc] : Change cost function, now just simply use gradient value :
+            if (!AutoParameter->Primitive->verts.has_attr(AutoParameter->OutputChannel)) {
+                AutoParameter->Primitive->verts.add_attr<float>(AutoParameter->OutputChannel);
+            }
+            auto Output = AutoParameter->Primitive->verts.attr<float>(AutoParameter->OutputChannel);
+            Output.insert(Output.begin(), AutoParameter->GradientList.begin(), AutoParameter->GradientList.end());
         }
     };
 }// namespace
diff --git a/projects/Roads/utilities/CMakeLists.txt b/projects/Roads/utilities/CMakeLists.txt
index 77743de130..134de64ec5 100644
--- a/projects/Roads/utilities/CMakeLists.txt
+++ b/projects/Roads/utilities/CMakeLists.txt
@@ -1,4 +1,5 @@
 FIND_PACKAGE(Eigen3 REQUIRED NO_MODULE)
+FIND_PACKAGE(OpenMP REQUIRED)
 
 FILE(GLOB_RECURSE SOURCE_FILES src/*.cpp src/*.h)
 
@@ -9,3 +10,21 @@ TARGET_LINK_LIBRARIES(Roads PRIVATE Eigen3::Eigen)
 
 ADD_EXECUTABLE(RoadsTest test/main.cpp)
 TARGET_LINK_LIBRARIES(RoadsTest PUBLIC Roads)
+
+IF (OPENMP_FOUND)
+    SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+    SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS}" ${OpenMP_EXE_LINKER_FLAGS})
+    TARGET_LINK_LIBRARIES(Roads PRIVATE OpenMP::OpenMP_CXX)
+    if (MSVC)
+        include(CheckCXXCompilerFlag)
+        check_cxx_compiler_flag("/openmp:experimental" MSVC_SUPPORT_OPENMP_EXPERIMENTAL)
+        if (MSVC_SUPPORT_OPENMP_EXPERIMENTAL)
+            target_compile_options(zeno PRIVATE $<IF:$<COMPILE_LANGUAGE:CXX>,/openmp:experimental,>)
+        endif()
+        check_cxx_compiler_flag("/openmp:llvm" MSVC_SUPPORT_OPENMP_LLVM)
+        if (MSVC_SUPPORT_OPENMP_LLVM)
+            target_compile_options(zeno PRIVATE $<IF:$<COMPILE_LANGUAGE:CXX>,/openmp:llvm,>)
+        endif()
+    endif()
+ENDIF()
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index ec53f75777..aa6e082345 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -11,7 +11,7 @@ namespace roads {
 
     struct AdvancePoint {
         Point Position;
-        double Curvature = 0.0;
+        double Gradient = 0.0;
     };
 
     using HeightPoint = double;
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index 2c32e523eb..5de053efa6 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -26,6 +26,7 @@ DynamicGrid<SlopePoint> roads::CalculateSlope(const DynamicGrid<HeightPoint> &In
 
     DynamicGrid<SlopePoint> Result(SizeX, SizeY);
 
+#pragma omp parallel for
     for (int32_t y = 0; y < SizeY; ++y) {
         for (int32_t x = 0; x < SizeX; ++x) {
             const size_t OriginIdx = x + y * SizeX;

From a15ed74ade82081f7a51aa6ac5716e5f04d63729 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Fri, 11 Aug 2023 13:05:05 +0800
Subject: [PATCH 04/50] fix: gcc compile error (s-word MSVC

---
 zeno/include/zeno/core/INode.h            | 7 ++++---
 zeno/include/zeno/utils/PropertyVisitor.h | 2 +-
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/zeno/include/zeno/core/INode.h b/zeno/include/zeno/core/INode.h
index eadda81683..0708aa903f 100644
--- a/zeno/include/zeno/core/INode.h
+++ b/zeno/include/zeno/core/INode.h
@@ -43,11 +43,13 @@ struct INode {
     ZENO_API void doOnlyApply();
 
 protected:
+    ZENO_API virtual void complete();
+    ZENO_API virtual void apply() = 0;
+
+public:
     ZENO_API bool requireInput(std::string const &ds);
 
     ZENO_API virtual void preApply();
-    ZENO_API virtual void complete();
-    ZENO_API virtual void apply() = 0;
 
     ZENO_API Graph *getThisGraph() const;
     ZENO_API Session *getThisSession() const;
@@ -107,7 +109,6 @@ struct INode {
 
     ZENO_API TempNodeCaller temp_node(std::string const &id);
 
-    friend struct reflect;
 };
 
 }
diff --git a/zeno/include/zeno/utils/PropertyVisitor.h b/zeno/include/zeno/utils/PropertyVisitor.h
index 4aa258be42..de9f87e02a 100644
--- a/zeno/include/zeno/utils/PropertyVisitor.h
+++ b/zeno/include/zeno/utils/PropertyVisitor.h
@@ -23,7 +23,7 @@ namespace zeno {
     using namespace zeno;
 
     // using friend struct to break INode package!
-    struct reflect {
+    namespace reflect {
         static constexpr unsigned int crc_table[256] = {
             0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f,
             0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,

From 035eee167b798a09ba906ccaf4ab74dca2e3b322 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Fri, 11 Aug 2023 14:03:57 +0800
Subject: [PATCH 05/50] fix: gcc build

---
 zeno/include/zeno/utils/PropertyVisitor.h | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/zeno/include/zeno/utils/PropertyVisitor.h b/zeno/include/zeno/utils/PropertyVisitor.h
index de9f87e02a..2ebd8e7657 100644
--- a/zeno/include/zeno/utils/PropertyVisitor.h
+++ b/zeno/include/zeno/utils/PropertyVisitor.h
@@ -214,6 +214,14 @@ namespace zeno {
 
         template<typename ParentType, typename InputType, size_t Hash>
         struct InputField : public Field<ParentType, InputType, Hash> {
+            using Field<ParentType, InputType, Hash>::Parent;
+            using Field<ParentType, InputType, Hash>::Type;
+            using Field<ParentType, InputType, Hash>::KeyName;
+            using Field<ParentType, InputType, Hash>::ValueRef;
+            using Field<ParentType, InputType, Hash>::DefaultValue;
+            using Field<ParentType, InputType, Hash>::DisplayName;
+            using Field<ParentType, InputType, Hash>::IsOptional;
+
             InputField(Parent &ParentRef, Type &InValueRef, std::string InKeyName, bool InIsOptional = false, const std::optional<std::string> &InDisplayName = std::nullopt, const std::optional<std::string> &InDefaultValue = std::nullopt)
                 : Field<ParentType, InputType, Hash>(ParentRef, InValueRef, InKeyName, InIsOptional, InDisplayName, InDefaultValue) {
                 ParentRef.HookList.InputHook.push_back(ToCaptured());
@@ -263,6 +271,14 @@ namespace zeno {
 
         template<typename ParentType, typename InputType, size_t Hash>
         struct OutputField : public Field<ParentType, InputType, Hash> {
+            using Field<ParentType, InputType, Hash>::Parent;
+            using Field<ParentType, InputType, Hash>::Type;
+            using Field<ParentType, InputType, Hash>::KeyName;
+            using Field<ParentType, InputType, Hash>::ValueRef;
+            using Field<ParentType, InputType, Hash>::DefaultValue;
+            using Field<ParentType, InputType, Hash>::DisplayName;
+            using Field<ParentType, InputType, Hash>::IsOptional;
+
             OutputField(Parent &ParentRef, Type &InValueRef, std::string InKeyName, bool InIsOptional = false, const std::optional<std::string> &InDisplayName = std::nullopt, const std::optional<std::string> &InDefaultValue = std::nullopt)
                 : Field<ParentType, InputType, Hash>(ParentRef, InValueRef, InKeyName, InIsOptional, InDisplayName) {
                 ParentRef.HookList.OutputHook.push_back(ToCaptured());

From 40f7f502e46e7618763f6cb0f9bde52a19305b72 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Fri, 11 Aug 2023 14:16:45 +0800
Subject: [PATCH 06/50] fix: gcc compile error again

---
 zeno/include/zeno/utils/PropertyVisitor.h | 20 +++++++++-----------
 1 file changed, 9 insertions(+), 11 deletions(-)

diff --git a/zeno/include/zeno/utils/PropertyVisitor.h b/zeno/include/zeno/utils/PropertyVisitor.h
index 2ebd8e7657..4025b5f9ed 100644
--- a/zeno/include/zeno/utils/PropertyVisitor.h
+++ b/zeno/include/zeno/utils/PropertyVisitor.h
@@ -214,20 +214,18 @@ namespace zeno {
 
         template<typename ParentType, typename InputType, size_t Hash>
         struct InputField : public Field<ParentType, InputType, Hash> {
-            using Field<ParentType, InputType, Hash>::Parent;
-            using Field<ParentType, InputType, Hash>::Type;
             using Field<ParentType, InputType, Hash>::KeyName;
             using Field<ParentType, InputType, Hash>::ValueRef;
             using Field<ParentType, InputType, Hash>::DefaultValue;
             using Field<ParentType, InputType, Hash>::DisplayName;
             using Field<ParentType, InputType, Hash>::IsOptional;
 
-            InputField(Parent &ParentRef, Type &InValueRef, std::string InKeyName, bool InIsOptional = false, const std::optional<std::string> &InDisplayName = std::nullopt, const std::optional<std::string> &InDefaultValue = std::nullopt)
+            InputField(ParentType &ParentRef, InputType &InValueRef, std::string InKeyName, bool InIsOptional = false, const std::optional<std::string> &InDisplayName = std::nullopt, const std::optional<std::string> &InDefaultValue = std::nullopt)
                 : Field<ParentType, InputType, Hash>(ParentRef, InValueRef, InKeyName, InIsOptional, InDisplayName, InDefaultValue) {
                 ParentRef.HookList.InputHook.push_back(ToCaptured());
 
                 static bool bHasInitialized = false;
-                static SocketDescriptor SDescriptor = SocketDescriptor{ValueTypeToString<Type>::TypeName, KeyName, DefaultValue, DisplayName};
+                static SocketDescriptor SDescriptor = SocketDescriptor{ValueTypeToString<InputType>::TypeName, KeyName, DefaultValue, DisplayName};
                 if (!bHasInitialized) {
                     bHasInitialized = true;
                     ParentType::GetDescriptor().inputs.push_back(SDescriptor);
@@ -237,14 +235,14 @@ namespace zeno {
             inline void ReadObject(INode *Node) {
                 zeno::log_debug("[AutoNode] Reading zany '{}'", KeyName);
                 if (!IsOptional || Node->has_input(KeyName)) {
-                    ValueRef = Node->get_input<RawType_t<Type>>(KeyName);
+                    ValueRef = Node->get_input<RawType_t<InputType>>(KeyName);
                 }
             }
 
             inline void ReadPrimitiveValue(INode *Node) {
                 zeno::log_debug("[AutoNode] Reading primitive value '{}'", KeyName);
                 if (!IsOptional || Node->has_input(KeyName)) {
-                    ValueRef = Node->get_input2<RawType_t<Type>>(KeyName);
+                    ValueRef = Node->get_input2<RawType_t<InputType>>(KeyName);
                 }
             }
 
@@ -253,7 +251,7 @@ namespace zeno {
                     zeno::log_error("Trying to read value from a nullptr Node.");
                     return;
                 }
-                if constexpr (IsSharedPtr<Type>()) {
+                if constexpr (IsSharedPtr<InputType>()) {
                     ReadObject(Node);
                 } else {
                     ReadPrimitiveValue(Node);
@@ -279,12 +277,12 @@ namespace zeno {
             using Field<ParentType, InputType, Hash>::DisplayName;
             using Field<ParentType, InputType, Hash>::IsOptional;
 
-            OutputField(Parent &ParentRef, Type &InValueRef, std::string InKeyName, bool InIsOptional = false, const std::optional<std::string> &InDisplayName = std::nullopt, const std::optional<std::string> &InDefaultValue = std::nullopt)
+            OutputField(ParentType &ParentRef, InputType &InValueRef, std::string InKeyName, bool InIsOptional = false, const std::optional<std::string> &InDisplayName = std::nullopt, const std::optional<std::string> &InDefaultValue = std::nullopt)
                 : Field<ParentType, InputType, Hash>(ParentRef, InValueRef, InKeyName, InIsOptional, InDisplayName) {
                 ParentRef.HookList.OutputHook.push_back(ToCaptured());
 
                 static bool bHasInitialized = false;
-                static SocketDescriptor SDescriptor = SocketDescriptor{ValueTypeToString<Type>::TypeName, KeyName, DefaultValue, DisplayName};
+                static SocketDescriptor SDescriptor = SocketDescriptor{ValueTypeToString<InputType>::TypeName, KeyName, DefaultValue, DisplayName};
 
                 if (!bHasInitialized) {
                     bHasInitialized = true;
@@ -309,7 +307,7 @@ namespace zeno {
                     zeno::log_error("Trying to read value from a nullptr Node.");
                     return;
                 }
-                if constexpr (IsSharedPtr<Type>()) {
+                if constexpr (IsSharedPtr<InputType>()) {
                     WriteObject(Node);
                 } else {
                     WritePrimitiveValue(Node);
@@ -474,7 +472,7 @@ namespace zeno {
                 };
             }
         };
-    };
+    };// namespace reflect
 
     template<typename T>
     reflect::INodeParameterObject<T>::INodeParameterObject(INode *Node) : NodeParameterBase(Node) {

From 62d1956587b19fc1be9f42fd86bca73a1cfb5bda Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Fri, 11 Aug 2023 18:42:33 +0800
Subject: [PATCH 07/50] feat: simple shortest path

---
 projects/Roads/nodes/src/cost.cpp             | 56 +++++++++++++++----
 projects/Roads/utilities/CMakeLists.txt       | 11 ++++
 projects/Roads/utilities/include/roads/data.h | 14 ++++-
 projects/Roads/utilities/include/roads/grid.h | 20 +++++--
 projects/Roads/utilities/src/grid.cpp         | 45 +++++++++++++--
 5 files changed, 124 insertions(+), 22 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 61d4be2e14..d0ebc14b36 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -5,6 +5,8 @@
 #include "zeno/utils/PropertyVisitor.h"
 #include "zeno/utils/logger.h"
 #include "zeno/zeno.h"
+#include "boost/graph/dijkstra_shortest_paths.hpp"
+#include <iostream>
 
 template<typename... Args>
 inline void RoadsAssert(const bool Expr, const std::string &InMsg = "[Roads] Assert Failed", Args... args) {
@@ -76,7 +78,8 @@ namespace {
         }
     };
 
-    struct CalcPathCost_Simple : public zeno::reflect::IParameterAutoNode<CalcPathCost_Simple> {
+    struct ZENO_CRTP(CalcPathCost_Simple, zeno::reflect::IParameterAutoNode) {
+    //struct CalcPathCost_Simple : public zeno::reflect::IParameterAutoNode<CalcPathCost_Simple> {
         ZENO_GENERATE_NODE_BODY(CalcPathCost_Simple);
 
         std::shared_ptr<zeno::PrimitiveObject> Primitive;
@@ -84,19 +87,22 @@ namespace {
         ZENO_DECLARE_OUTPUT_FIELD(Primitive, "Prim");
 
         std::string OutputChannel;
-        ZENO_DECLARE_INPUT_FIELD(OutputChannel, "OutputChannel", false, "", "path_cost");
+        ZENO_DECLARE_INPUT_FIELD(OutputChannel, "Output Channel (Vertex Attr)", false, "", "path_cost");
 
         std::string SizeXChannel;
-        ZENO_DECLARE_INPUT_FIELD(SizeXChannel, "UserData_NxChannel", false, "", "nx");
+        ZENO_DECLARE_INPUT_FIELD(SizeXChannel, "Nx Channel (UserData)", false, "", "nx");
 
         std::string SizeYChannel;
-        ZENO_DECLARE_INPUT_FIELD(SizeYChannel, "UserData_NyChannel", false, "", "ny");
+        ZENO_DECLARE_INPUT_FIELD(SizeYChannel, "Ny Channel (UserData)", false, "", "ny");
 
         std::string PositionChannel;
-        ZENO_DECLARE_INPUT_FIELD(PositionChannel, "Vert_PositionChannel", false, "", "pos");
+        ZENO_DECLARE_INPUT_FIELD(PositionChannel, "Position Channel (Vertex Attr)", false, "", "pos");
 
         std::string GradientChannel;
-        ZENO_DECLARE_INPUT_FIELD(GradientChannel, "Vert_GradientChannel", false, "", "gradient");
+        ZENO_DECLARE_INPUT_FIELD(GradientChannel, "Gradient Channel (Vertex Attr)", false, "", "gradient");
+
+        bool bRemoveTriangles;
+        ZENO_DECLARE_INPUT_FIELD(bRemoveTriangles, "Remove Triangles", false, "", "true");
 
         int Nx = 0;
         ZENO_BINDING_PRIMITIVE_USERDATA(Primitive, Nx, SizeXChannel, false);
@@ -113,11 +119,41 @@ namespace {
         void apply() override {
             //auto Grid = BuildGridFromPrimitive(AutoParameter->PositionList, AutoParameter->GradientList, AutoParameter->Nx, AutoParameter->Ny);
             // TODO [darc] : Change cost function, now just simply use gradient value :
-            if (!AutoParameter->Primitive->verts.has_attr(AutoParameter->OutputChannel)) {
-                AutoParameter->Primitive->verts.add_attr<float>(AutoParameter->OutputChannel);
+            AutoParameter->Nx;
+            RoadsAssert(AutoParameter->Nx * AutoParameter->Ny <= AutoParameter->GradientList.size(), "Bad nx ny.");
+
+            DynamicGrid<CostPoint> CostGrid(AutoParameter->Nx, AutoParameter->Ny);
+            CostGrid.insert(CostGrid.begin(), AutoParameter->GradientList.begin(), AutoParameter->GradientList.begin() + (AutoParameter->Nx * AutoParameter->Ny));
+            auto Graph = CreateWeightGraphFromCostGrid(CostGrid, ConnectiveType::FOUR);
+            zeno::log_info("cccc: {}", boost::num_vertices(Graph));
+
+            using VertexDescriptor = boost::graph_traits<WeightedGridUndirectedGraph>::vertex_descriptor;
+
+            std::vector<VertexDescriptor> p(boost::num_vertices(Graph));
+            std::vector<double> d(boost::num_vertices(Graph));
+            VertexDescriptor Start { 1 };
+            VertexDescriptor Goal { 999999 };
+
+            boost::dijkstra_shortest_paths(Graph, Start, boost::predecessor_map(&p[0]).distance_map(&d[0]));
+
+            std::vector<boost::graph_traits<WeightedGridUndirectedGraph>::vertex_descriptor > path;
+            boost::graph_traits<WeightedGridUndirectedGraph>::vertex_descriptor current = Goal;
+
+            while(current!=Start)
+            {
+                path.push_back(current);
+                current = p[current];
+            }
+            path.push_back(Start);
+
+//            AutoParameter->Primitive = std::make_shared<zeno::PrimitiveObject>();
+            if (AutoParameter->bRemoveTriangles) {
+                AutoParameter->Primitive->tris.clear();
+            }
+            AutoParameter->Primitive->lines.resize(path.size() - 1);
+            for (size_t i = 0; i < path.size() - 1; ++i) {
+                AutoParameter->Primitive->lines[i] = zeno::vec2i(path[i], path[i+1]);
             }
-            auto Output = AutoParameter->Primitive->verts.attr<float>(AutoParameter->OutputChannel);
-            Output.insert(Output.begin(), AutoParameter->GradientList.begin(), AutoParameter->GradientList.end());
         }
     };
 }// namespace
diff --git a/projects/Roads/utilities/CMakeLists.txt b/projects/Roads/utilities/CMakeLists.txt
index 134de64ec5..61792d150f 100644
--- a/projects/Roads/utilities/CMakeLists.txt
+++ b/projects/Roads/utilities/CMakeLists.txt
@@ -1,5 +1,8 @@
+SET(Boost_ADDITIONAL_VERSIONS "1.82.0" "1.82")
+
 FIND_PACKAGE(Eigen3 REQUIRED NO_MODULE)
 FIND_PACKAGE(OpenMP REQUIRED)
+FIND_PACKAGE(Boost COMPONENTS graph)
 
 FILE(GLOB_RECURSE SOURCE_FILES src/*.cpp src/*.h)
 
@@ -27,4 +30,12 @@ IF (OPENMP_FOUND)
             target_compile_options(zeno PRIVATE $<IF:$<COMPILE_LANGUAGE:CXX>,/openmp:llvm,>)
         endif()
     endif()
+ELSE()
+    MESSAGE(FATAL_ERROR "Roads Module is depending on OpenMP. You have to install it first.")
+ENDIF()
+
+IF (TARGET Boost::graph)
+    TARGET_LINK_LIBRARIES(Roads PRIVATE Boost::graph)
+ELSE()
+    MESSAGE(FATAL_ERROR "Roads Module is depending on Boost::graph. You have to install it first.")
 ENDIF()
diff --git a/projects/Roads/utilities/include/roads/data.h b/projects/Roads/utilities/include/roads/data.h
index c6eed4a456..e56121e5b2 100644
--- a/projects/Roads/utilities/include/roads/data.h
+++ b/projects/Roads/utilities/include/roads/data.h
@@ -15,6 +15,11 @@ namespace roads {
         OBTUSE = 2,
     };
 
+    enum class ConnectiveType {
+        FOUR = 4,
+        EIGHT = 8,
+    };
+
     struct Point : public Eigen::Vector3d {
         Point(const std::array<float, 3>& InArray) : Eigen::Vector3d(InArray[0], InArray[1], InArray[2]) {}
 
@@ -25,7 +30,9 @@ namespace roads {
         using Eigen::Vector2d::Vector2d;
     };
 
-    struct IntPoint : public std::array<size_t, 3> {};
+    struct IntPoint2D : public std::array<long, 2> {};
+
+    struct IntPoint : public std::array<long, 3> {};
 
     struct Triangle : public std::array<Point, 3> {
         bool AnyAngleLargerThan(const double Degree) {
@@ -43,6 +50,11 @@ namespace roads {
     template <typename T>
     struct ArrayList : public std::vector<T> {
         using std::vector<T>::vector;
+        using std::vector<T>::size;
+
+        bool IsValidIndex(size_t Index) const {
+            return Index < size();
+        }
     };
 
     template <size_t X, size_t Y, typename GridPointType = Point>
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index aa6e082345..b237b09d75 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -2,12 +2,12 @@
 
 #include "pch.h"
 
-namespace roads {
+//#include "boost/graph/use_mpi.hpp"
+#include "boost/graph/graph_traits.hpp"
+#include "boost/graph/graph_concepts.hpp"
+#include "boost/graph/adjacency_list.hpp"
 
-    struct CostPoint {
-        size_t Cost = 0;
-        Point Position;
-    };
+namespace roads {
 
     struct AdvancePoint {
         Point Position;
@@ -16,8 +16,12 @@ namespace roads {
 
     using HeightPoint = double;
     using SlopePoint = double;
+    using CostPoint = double;
 
-    ROADS_API DynamicGrid<CostPoint> CalcCost_Simple(const DynamicGrid<AdvancePoint> &BaseGrid);
+    using EdgeWeightProperty = boost::property<boost::edge_weight_t, double>;
+    using WeightedGridUndirectedGraph = boost::adjacency_list<boost::listS,boost::vecS,boost::directedS,boost::no_property,EdgeWeightProperty>;
+    using WeightedGridUndirectedGraphIterator = boost::graph_traits<WeightedGridUndirectedGraph>::edge_iterator;
+    using Edge = std::pair<size_t, size_t>;
 
     ROADS_API ROADS_INLINE double EuclideanDistance(const Point &Point1, const Point &Point2);
 
@@ -25,6 +29,10 @@ namespace roads {
 
     ROADS_API DynamicGrid<SlopePoint> CalculateSlope(const DynamicGrid<HeightPoint>& InHeightField);
 
+    ROADS_API WeightedGridUndirectedGraph CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint >& InCostGrid, const ConnectiveType Type);
+
+    ROADS_API ArrayList<ArrayList<double>> FloydWarshallShortestPath(WeightedGridUndirectedGraph &InGraph);
+
     namespace energy {
         ROADS_API double CalculateStepSize(const Point2D &Pt, const Point2D &P, const Point2D &PrevX, const Point2D &CurrX);
     }// namespace energy
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index 5de053efa6..260b0a1007 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -1,13 +1,9 @@
 #include "roads/grid.h"
 #include "Eigen/Eigen"
-#include "roads/roads.h"
+#include "boost/graph/floyd_warshall_shortest.hpp"
 
 using namespace roads;
 
-DynamicGrid<CostPoint> roads::CalcCost_Simple(const DynamicGrid<AdvancePoint> &BaseGrid) {
-    return {0, 0};
-}
-
 double roads::EuclideanDistance(const Point &Point1, const Point &Point2) {
     return std::sqrt(std::pow(Point2.x() - Point1.x(), 2) + std::pow(Point2.y() - Point1.y(), 2) + std::pow(Point2.z() - Point1.z(), 2));
 }
@@ -50,6 +46,45 @@ DynamicGrid<SlopePoint> roads::CalculateSlope(const DynamicGrid<HeightPoint> &In
     return Result;
 }
 
+WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint> &InCostGrid, const ConnectiveType Type) {
+    ArrayList<IntPoint2D> Directions = { { 0, -1 }, { 0, 1 }, { -1, 0 }, { 1, 0 } };
+    if (Type == ConnectiveType::EIGHT) {
+        Directions.insert(Directions.end(), { { -1, -1 }, { 1, -1 }, { -1, 1 }, { 1, 1 } });
+    }
+
+    WeightedGridUndirectedGraph NewGraph { InCostGrid.size() };
+
+    // boost graph library seem not provide thread safe
+#pragma omp parallel for
+    for (int32_t y = 0; y < InCostGrid.Ny; ++y) {
+        for (int32_t x = 0; x < InCostGrid.Nx; ++x) {
+            const size_t OriginIdx = y * InCostGrid.Nx + x;
+            boost::property_map<WeightedGridUndirectedGraph, boost::edge_weight_t>::type WeightMap = boost::get(boost::edge_weight, NewGraph);
+            for (auto & Direction : Directions) {
+                const size_t ix = x + Direction[0];
+                const size_t iy = y + Direction[1];
+                if (ix >= InCostGrid.Nx || iy >= InCostGrid.Ny) continue;
+                const size_t TargetIdx = iy * InCostGrid.Nx + ix;
+                using EdgeDescriptor = boost::graph_traits<WeightedGridUndirectedGraph>::edge_descriptor;
+                auto [edge1, _] = boost::add_edge(OriginIdx, TargetIdx, NewGraph);
+                auto [edge2, _2] = boost::add_edge(TargetIdx, OriginIdx,  NewGraph);
+                WeightMap[edge1] = InCostGrid[TargetIdx] - InCostGrid[OriginIdx] + 30.0;
+                WeightMap[edge2] = InCostGrid[OriginIdx] - InCostGrid[TargetIdx] + 30.0;
+            }
+        }
+    }
+
+    return NewGraph;
+}
+
+ArrayList<ArrayList<double>> roads::FloydWarshallShortestPath(WeightedGridUndirectedGraph &InGraph) {
+    ArrayList<ArrayList<double>> D { InGraph.m_vertices.size() };
+    ArrayList<double> d (InGraph.m_vertices.size(), (std::numeric_limits<double>::max)());
+    printf("%llu", InGraph.m_vertices.size());
+    boost::floyd_warshall_all_pairs_shortest_paths(InGraph, D, boost::distance_map(&d[0]));
+    return D;
+}
+
 double roads::energy::CalculateStepSize(const Point2D &Pt, const Point2D &P, const Point2D &PrevX, const Point2D &CurrX) {
     const double LengthPtToXc = EuclideanDistance(Pt, CurrX);
     const double LengthPtToP = EuclideanDistance(Pt, P);

From 0121bdb9f0097768af2dab4f20301777b54c7e95 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Wed, 16 Aug 2023 18:22:20 +0800
Subject: [PATCH 08/50] feat: add simple flow path

---
 projects/Roads/nodes/src/cost.cpp             | 107 +++++++++++++++++-
 projects/Roads/utilities/include/roads/grid.h |  26 +++++
 2 files changed, 130 insertions(+), 3 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index d0ebc14b36..e5d4a82328 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -1,12 +1,15 @@
+#include "boost/graph/dijkstra_shortest_paths.hpp"
 #include "roads/roads.h"
-#include <limits>
 #include "zeno/PrimitiveObject.h"
 #include "zeno/types/UserData.h"
 #include "zeno/utils/PropertyVisitor.h"
 #include "zeno/utils/logger.h"
 #include "zeno/zeno.h"
-#include "boost/graph/dijkstra_shortest_paths.hpp"
 #include <iostream>
+#include <limits>
+#include <numeric>
+#include <queue>
+#include <stack>
 
 template<typename... Args>
 inline void RoadsAssert(const bool Expr, const std::string &InMsg = "[Roads] Assert Failed", Args... args) {
@@ -33,7 +36,7 @@ namespace {
     using namespace zeno;
     using namespace roads;
 
-    struct PrimCalcSlope : public zeno::reflect::IParameterAutoNode<PrimCalcSlope> {
+    struct ZENO_CRTP(PrimCalcSlope, zeno::reflect::IParameterAutoNode) {
         ZENO_GENERATE_NODE_BODY(PrimCalcSlope);
 
         std::shared_ptr<zeno::PrimitiveObject> Primitive;
@@ -156,4 +159,102 @@ namespace {
             }
         }
     };
+
+    struct ZENO_CRTP(HeightFieldFlowPath_Simple, zeno::reflect::IParameterAutoNode) {
+        ZENO_GENERATE_NODE_BODY(HeightFieldFlowPath_Simple);
+
+        std::shared_ptr<zeno::PrimitiveObject> Primitive;
+        ZENO_DECLARE_INPUT_FIELD(Primitive, "Prim");
+        ZENO_DECLARE_OUTPUT_FIELD(Primitive, "Prim");
+
+        bool bShouldSmooth;
+        ZENO_DECLARE_INPUT_FIELD(bShouldSmooth, "Enable Smooth", false, "", "false");
+
+        float DeltaAltitudeThreshold;
+        ZENO_DECLARE_INPUT_FIELD(DeltaAltitudeThreshold, "Delta Altitude Threshold", false, "", "1e-06");
+
+        float HeuristicRatio;
+        ZENO_DECLARE_INPUT_FIELD(HeuristicRatio, "Heuristic Ratio (0 - 1)", false, "", "0.3");
+
+        std::string SizeXChannel;
+        ZENO_DECLARE_INPUT_FIELD(SizeXChannel, "Nx Channel (UserData)", false, "", "nx");
+
+        std::string SizeYChannel;
+        ZENO_DECLARE_INPUT_FIELD(SizeYChannel, "Ny Channel (UserData)", false, "", "ny");
+
+        int Nx = 0;
+        ZENO_BINDING_PRIMITIVE_USERDATA(Primitive, Nx, SizeXChannel, false);
+
+        int Ny = 0;
+        ZENO_BINDING_PRIMITIVE_USERDATA(Primitive, Ny, SizeYChannel, false);
+
+        std::string RiverChannel;
+        ZENO_DECLARE_INPUT_FIELD(RiverChannel, "Output River Channel (Vertex Attr)", false, "", "is_river");
+
+        std::string LakeChannel;
+        ZENO_DECLARE_INPUT_FIELD(LakeChannel, "Output Lake Channel (Vertex Attr)", false, "", "is_lake");
+
+        std::string HeightChannel;
+        ZENO_DECLARE_INPUT_FIELD(HeightChannel, "Height Channel (Vertex Attr)", false, "", "height");
+
+        std::string WaterChannel;
+        ZENO_DECLARE_INPUT_FIELD(WaterChannel, "Water Channel (Vertex Attr)", false, "", "water");
+
+        zeno::AttrVector<float> Heightmap{};
+        ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, Heightmap, HeightChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
+
+        zeno::AttrVector<float> WaterMask{};
+        ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, WaterMask, WaterChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
+
+        void apply() override {
+            auto& Prim = AutoParameter->Primitive;
+            auto& HeightField = AutoParameter->Heightmap;
+            auto& Water = AutoParameter->WaterMask;
+            const auto SizeX = AutoParameter->Nx;
+            const auto SizeY = AutoParameter->Ny;
+            const size_t NumVert = SizeX * SizeY;
+
+            auto& River = Prim->add_attr<float>(AutoParameter->RiverChannel);
+
+            static const std::array<IntPoint2D, 8> SDirection {
+                IntPoint2D { 0, -1 }, { 0, 1 }, { -1, 0 }, { 1, 0 },
+                { -1, -1 }, { 1, -1 }, { -1, 1 }, { 1, 1 }
+            };
+
+            std::stack<size_t> Stack;
+
+            const float MaxHeight = *std::max_element(std::begin(HeightField), std::end(HeightField));
+            const float MinHeight = *std::min_element(std::begin(HeightField), std::end(HeightField));
+            const float RiverHeightMax = (MaxHeight - MinHeight) * AutoParameter->HeuristicRatio;
+
+            std::set<size_t> Visited;
+            for (size_t i = 0; i < NumVert; ++i) {
+                if (Water[i] < 1e-7) {
+                    continue;
+                }
+
+                Stack.push(i);
+                while (!Stack.empty()) {
+                    long idx = long(Stack.top());
+                    Visited.insert(idx);
+                    Stack.pop();
+                    if (HeightField[idx] > RiverHeightMax) continue;
+                    River[idx] = 1;
+                    long y = idx / SizeX;
+                    long x = idx % SizeX;
+                    for (const IntPoint2D& Dir : SDirection) {
+                        long ix = x + Dir[0];
+                        long iy = y + Dir[1];
+                        if (ix > 0 && iy > 0 && ix < SizeX && iy < SizeX) {
+                            size_t nidx = iy * SizeX + ix;
+                            if (Visited.find(nidx) == std::end(Visited) && std::abs(HeightField[nidx] - HeightField[idx]) < AutoParameter->DeltaAltitudeThreshold) {
+                                Stack.push(nidx);
+                            }
+                        }
+                    }
+                }
+            }
+
+        }
+    };
 }// namespace
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index b237b09d75..100068994c 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include "pch.h"
+#include <numeric>
 
 //#include "boost/graph/use_mpi.hpp"
 #include "boost/graph/graph_traits.hpp"
@@ -33,6 +34,31 @@ namespace roads {
 
     ROADS_API ArrayList<ArrayList<double>> FloydWarshallShortestPath(WeightedGridUndirectedGraph &InGraph);
 
+    template <typename T>
+    ROADS_API void AverageSmooth(ArrayList<T>& InOutContainer, size_t Iteration = 1) {
+        auto num_vertices = InOutContainer.size();
+        ArrayList<float> NewHeights(num_vertices);
+
+        for (unsigned int iteration = 0; iteration < 4; ++iteration) {
+#pragma omp parallel for
+            for (size_t i = 0; i < num_vertices; ++i) {
+                // Include the vertex itself and its neighbors.
+                ArrayList<T*> Points = InOutContainer[i].Neighbors;
+                Points.push_back(&InOutContainer[i]);
+
+                // Calculate the average height.
+                NewHeights[i] = std::accumulate(begin(Points), end(Points), 0.0f,
+                                                [](float sum, const T* v) { return sum + v->Height; }) / Points.size();
+            }
+
+            // Update the heights for the next iteration or the final result.
+#pragma omp parallel for
+            for (size_t i = 0; i < num_vertices; ++i) {
+                InOutContainer[i].Height = NewHeights[i];
+            }
+        }
+    }
+
     namespace energy {
         ROADS_API double CalculateStepSize(const Point2D &Pt, const Point2D &P, const Point2D &PrevX, const Point2D &CurrX);
     }// namespace energy

From 4e50ef436549b5aa33c55027f01ef8b8a684117d Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Tue, 22 Aug 2023 12:56:02 +0800
Subject: [PATCH 09/50] feat: add more connective to shortest path algorithm

---
 projects/Roads/nodes/src/cost.cpp             |  4 ++--
 projects/Roads/utilities/include/roads/data.h |  2 ++
 projects/Roads/utilities/include/roads/grid.h |  2 +-
 projects/Roads/utilities/src/grid.cpp         | 15 +++++++++++----
 4 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index e5d4a82328..b79b029c58 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -127,7 +127,7 @@ namespace {
 
             DynamicGrid<CostPoint> CostGrid(AutoParameter->Nx, AutoParameter->Ny);
             CostGrid.insert(CostGrid.begin(), AutoParameter->GradientList.begin(), AutoParameter->GradientList.begin() + (AutoParameter->Nx * AutoParameter->Ny));
-            auto Graph = CreateWeightGraphFromCostGrid(CostGrid, ConnectiveType::FOUR);
+            auto Graph = CreateWeightGraphFromCostGrid(CostGrid, ConnectiveType::SIXTEEN);
             zeno::log_info("cccc: {}", boost::num_vertices(Graph));
 
             using VertexDescriptor = boost::graph_traits<WeightedGridUndirectedGraph>::vertex_descriptor;
@@ -135,7 +135,7 @@ namespace {
             std::vector<VertexDescriptor> p(boost::num_vertices(Graph));
             std::vector<double> d(boost::num_vertices(Graph));
             VertexDescriptor Start { 1 };
-            VertexDescriptor Goal { 999999 };
+            VertexDescriptor Goal { 933333 };
 
             boost::dijkstra_shortest_paths(Graph, Start, boost::predecessor_map(&p[0]).distance_map(&d[0]));
 
diff --git a/projects/Roads/utilities/include/roads/data.h b/projects/Roads/utilities/include/roads/data.h
index e56121e5b2..0d4d07037d 100644
--- a/projects/Roads/utilities/include/roads/data.h
+++ b/projects/Roads/utilities/include/roads/data.h
@@ -18,6 +18,8 @@ namespace roads {
     enum class ConnectiveType {
         FOUR = 4,
         EIGHT = 8,
+        SIXTEEN = 16,
+        FOURTY = 40,
     };
 
     struct Point : public Eigen::Vector3d {
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index 100068994c..d37fa334d0 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -30,7 +30,7 @@ namespace roads {
 
     ROADS_API DynamicGrid<SlopePoint> CalculateSlope(const DynamicGrid<HeightPoint>& InHeightField);
 
-    ROADS_API WeightedGridUndirectedGraph CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint >& InCostGrid, const ConnectiveType Type);
+    ROADS_API WeightedGridUndirectedGraph CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint >& InCostGrid, ConnectiveType Type, float PowParam = 1.5f);
 
     ROADS_API ArrayList<ArrayList<double>> FloydWarshallShortestPath(WeightedGridUndirectedGraph &InGraph);
 
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index 260b0a1007..85f57fe7e7 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -46,11 +46,16 @@ DynamicGrid<SlopePoint> roads::CalculateSlope(const DynamicGrid<HeightPoint> &In
     return Result;
 }
 
-WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint> &InCostGrid, const ConnectiveType Type) {
+WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint> &InCostGrid, const ConnectiveType Type, float PowParam/*=1.5f*/) {
     ArrayList<IntPoint2D> Directions = { { 0, -1 }, { 0, 1 }, { -1, 0 }, { 1, 0 } };
-    if (Type == ConnectiveType::EIGHT) {
+    if (Type >= ConnectiveType::EIGHT) {
         Directions.insert(Directions.end(), { { -1, -1 }, { 1, -1 }, { -1, 1 }, { 1, 1 } });
     }
+    if (Type >= ConnectiveType::SIXTEEN) {
+        Directions.insert(Directions.end(), { { -1, -2 }, { 1, -2 }, { -2, -1 }, { 2, -1 }, { -2, 1 }, { 2, 1 }, { -1, 2 }, { 1, 2 } });
+    }
+    if (Type >= ConnectiveType::FOURTY) {
+    }
 
     WeightedGridUndirectedGraph NewGraph { InCostGrid.size() };
 
@@ -68,8 +73,10 @@ WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGr
                 using EdgeDescriptor = boost::graph_traits<WeightedGridUndirectedGraph>::edge_descriptor;
                 auto [edge1, _] = boost::add_edge(OriginIdx, TargetIdx, NewGraph);
                 auto [edge2, _2] = boost::add_edge(TargetIdx, OriginIdx,  NewGraph);
-                WeightMap[edge1] = InCostGrid[TargetIdx] - InCostGrid[OriginIdx] + 30.0;
-                WeightMap[edge2] = InCostGrid[OriginIdx] - InCostGrid[TargetIdx] + 30.0;
+//                WeightMap[edge1] = std::pow(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] > 0 ? std::min(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 20.0, InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 10.0) : InCostGrid[TargetIdx] - InCostGrid[OriginIdx], 2);
+//                WeightMap[edge2] = std::pow(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] > 0 ? std::min(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 20.0, InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 10.0) : InCostGrid[OriginIdx] - InCostGrid[TargetIdx], 2);
+                WeightMap[edge1] = std::pow(InCostGrid[TargetIdx] - InCostGrid[OriginIdx], PowParam);
+                WeightMap[edge2] = std::pow(InCostGrid[OriginIdx] - InCostGrid[TargetIdx], PowParam);
             }
         }
     }

From d6cea9487c5d9a9727ce2e16bd4c0f921ac3f7ba Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Tue, 22 Aug 2023 14:41:49 +0800
Subject: [PATCH 10/50] feat: support class derived from std::string

---
 zeno/include/zeno/utils/PropertyVisitor.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/zeno/include/zeno/utils/PropertyVisitor.h b/zeno/include/zeno/utils/PropertyVisitor.h
index 4025b5f9ed..c029b24681 100644
--- a/zeno/include/zeno/utils/PropertyVisitor.h
+++ b/zeno/include/zeno/utils/PropertyVisitor.h
@@ -253,6 +253,8 @@ namespace zeno {
                 }
                 if constexpr (IsSharedPtr<InputType>()) {
                     ReadObject(Node);
+                } else if constexpr(std::is_base_of_v<std::string, InputType>) {
+                    ValueRef = InputType(Node->get_input2<std::string>(KeyName));
                 } else {
                     ReadPrimitiveValue(Node);
                 }

From 0dba061f753cfca63ac4901c53571d8886098a4e Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Tue, 22 Aug 2023 14:52:52 +0800
Subject: [PATCH 11/50] feat: update path finding node

---
 projects/Roads/nodes/src/cost.cpp     | 66 +++++++++++++++++++++++++--
 projects/Roads/utilities/src/grid.cpp | 18 +++++++-
 2 files changed, 78 insertions(+), 6 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index b79b029c58..6dc594d3e5 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -1,4 +1,5 @@
 #include "boost/graph/dijkstra_shortest_paths.hpp"
+#include "boost/graph/astar_search.hpp"
 #include "roads/roads.h"
 #include "zeno/PrimitiveObject.h"
 #include "zeno/types/UserData.h"
@@ -32,6 +33,28 @@ roads::DynamicGrid<roads::AdvancePoint> BuildGridFromPrimitive(const zeno::AttrV
     return Grid;
 }
 
+namespace zeno::reflect {
+
+    struct ConnectiveTypeInput : std::string {
+        using std::string::string;
+    };
+
+    struct PathAlgorithmTypeInput : std::string {
+        using std::string::string;
+    };
+
+    template<>
+    struct ValueTypeToString<ConnectiveTypeInput> {
+        inline static std::string TypeName = "enum 4 8 16 40";
+    };
+
+    template<>
+    struct ValueTypeToString<PathAlgorithmTypeInput> {
+        inline static std::string TypeName = "enum Dijkstra A*";
+    };
+
+}
+
 namespace {
     using namespace zeno;
     using namespace roads;
@@ -81,6 +104,21 @@ namespace {
         }
     };
 
+    template <typename Graph, typename CostType>
+    class PathDistanceHeuristic : public boost::astar_heuristic<Graph, CostType> {
+        typedef typename boost::graph_traits< Graph >::vertex_descriptor Vertex;
+
+    public:
+        PathDistanceHeuristic(Vertex Goal) : m_Goal(Goal) {}
+
+        CostType operator()(Vertex u) {
+            return 0;
+        }
+
+    private:
+        Vertex m_Goal;
+    };
+
     struct ZENO_CRTP(CalcPathCost_Simple, zeno::reflect::IParameterAutoNode) {
     //struct CalcPathCost_Simple : public zeno::reflect::IParameterAutoNode<CalcPathCost_Simple> {
         ZENO_GENERATE_NODE_BODY(CalcPathCost_Simple);
@@ -104,6 +142,12 @@ namespace {
         std::string GradientChannel;
         ZENO_DECLARE_INPUT_FIELD(GradientChannel, "Gradient Channel (Vertex Attr)", false, "", "gradient");
 
+        zeno::reflect::ConnectiveTypeInput PathConnective;
+        ZENO_DECLARE_INPUT_FIELD(PathConnective, "Path Connective", false, "", "16");
+
+        zeno::reflect::PathAlgorithmTypeInput Algorithm;
+        ZENO_DECLARE_INPUT_FIELD(Algorithm, "Path Finding Algorithm", false, "", "Dijkstra");
+
         bool bRemoveTriangles;
         ZENO_DECLARE_INPUT_FIELD(bRemoveTriangles, "Remove Triangles", false, "", "true");
 
@@ -125,9 +169,20 @@ namespace {
             AutoParameter->Nx;
             RoadsAssert(AutoParameter->Nx * AutoParameter->Ny <= AutoParameter->GradientList.size(), "Bad nx ny.");
 
+            ConnectiveType Connective = ConnectiveType::SIXTEEN;
+            if (AutoParameter->PathConnective == "4") {
+                Connective = ConnectiveType::FOUR;
+            } else if (AutoParameter->PathConnective == "8") {
+                Connective = ConnectiveType::EIGHT;
+            } else if (AutoParameter->PathConnective == "16") {
+                Connective = ConnectiveType::SIXTEEN;
+            } else if (AutoParameter->PathConnective == "40") {
+                Connective = ConnectiveType::FOURTY;
+            }
+
             DynamicGrid<CostPoint> CostGrid(AutoParameter->Nx, AutoParameter->Ny);
             CostGrid.insert(CostGrid.begin(), AutoParameter->GradientList.begin(), AutoParameter->GradientList.begin() + (AutoParameter->Nx * AutoParameter->Ny));
-            auto Graph = CreateWeightGraphFromCostGrid(CostGrid, ConnectiveType::SIXTEEN);
+            auto Graph = CreateWeightGraphFromCostGrid(CostGrid, Connective, 2);
             zeno::log_info("cccc: {}", boost::num_vertices(Graph));
 
             using VertexDescriptor = boost::graph_traits<WeightedGridUndirectedGraph>::vertex_descriptor;
@@ -137,7 +192,11 @@ namespace {
             VertexDescriptor Start { 1 };
             VertexDescriptor Goal { 933333 };
 
-            boost::dijkstra_shortest_paths(Graph, Start, boost::predecessor_map(&p[0]).distance_map(&d[0]));
+            if (AutoParameter->Algorithm == "Dijkstra") {
+                boost::dijkstra_shortest_paths(Graph, Start, boost::predecessor_map(&p[0]).distance_map(&d[0]));
+            } else if (AutoParameter->Algorithm == "A*") {
+                boost::astar_search_tree(Graph, Start, PathDistanceHeuristic<WeightedGridUndirectedGraph, double>(Goal), boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(boost::astar_visitor()));
+            }
 
             std::vector<boost::graph_traits<WeightedGridUndirectedGraph>::vertex_descriptor > path;
             boost::graph_traits<WeightedGridUndirectedGraph>::vertex_descriptor current = Goal;
@@ -149,13 +208,12 @@ namespace {
             }
             path.push_back(Start);
 
-//            AutoParameter->Primitive = std::make_shared<zeno::PrimitiveObject>();
             if (AutoParameter->bRemoveTriangles) {
                 AutoParameter->Primitive->tris.clear();
             }
             AutoParameter->Primitive->lines.resize(path.size() - 1);
             for (size_t i = 0; i < path.size() - 1; ++i) {
-                AutoParameter->Primitive->lines[i] = zeno::vec2i(path[i], path[i+1]);
+                AutoParameter->Primitive->lines[i] = zeno::vec2i(int(path[i]), int(path[i+1]));
             }
         }
     };
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index 85f57fe7e7..45d98d9ac0 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -46,6 +46,14 @@ DynamicGrid<SlopePoint> roads::CalculateSlope(const DynamicGrid<HeightPoint> &In
     return Result;
 }
 
+IntPoint2D InterpolatePoints(const IntPoint2D& A, const IntPoint2D& B, float t)
+{
+    IntPoint2D result;
+    result[0] = long(std::round(float(A[0]) + t * float(B[0] - A[0])));
+    result[1] = long(std::round(float(A[1]) + t * float(B[1] - A[1])));
+    return result;
+}
+
 WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint> &InCostGrid, const ConnectiveType Type, float PowParam/*=1.5f*/) {
     ArrayList<IntPoint2D> Directions = { { 0, -1 }, { 0, 1 }, { -1, 0 }, { 1, 0 } };
     if (Type >= ConnectiveType::EIGHT) {
@@ -55,6 +63,12 @@ WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGr
         Directions.insert(Directions.end(), { { -1, -2 }, { 1, -2 }, { -2, -1 }, { 2, -1 }, { -2, 1 }, { 2, 1 }, { -1, 2 }, { 1, 2 } });
     }
     if (Type >= ConnectiveType::FOURTY) {
+        // Directions.insert(Directions.end(), { { -2, -2 }, { -2, -1 }, { -2, 0 }, { -2, 1 }, { -2, 2 }, { -1, -2 }, { -1, 2 }, { 0, -2 }, { 0, 2 }, { 1, -2 }, { 1, 2 }, { 2, -2 }, { 2, -1 }, { 2, 0 }, { 2, 1 }, { 2, 2 } });
+        size_t original_size = Directions.size();
+        for(size_t i = 0; i + 1 < original_size; ++i) {
+            Directions.push_back(InterpolatePoints(Directions[i], Directions[i + 1], 1.0 / 3.0));
+            Directions.push_back(InterpolatePoints(Directions[i], Directions[i + 1], 2.0 / 3.0));
+        }
     }
 
     WeightedGridUndirectedGraph NewGraph { InCostGrid.size() };
@@ -75,8 +89,8 @@ WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGr
                 auto [edge2, _2] = boost::add_edge(TargetIdx, OriginIdx,  NewGraph);
 //                WeightMap[edge1] = std::pow(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] > 0 ? std::min(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 20.0, InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 10.0) : InCostGrid[TargetIdx] - InCostGrid[OriginIdx], 2);
 //                WeightMap[edge2] = std::pow(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] > 0 ? std::min(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 20.0, InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 10.0) : InCostGrid[OriginIdx] - InCostGrid[TargetIdx], 2);
-                WeightMap[edge1] = std::pow(InCostGrid[TargetIdx] - InCostGrid[OriginIdx], PowParam);
-                WeightMap[edge2] = std::pow(InCostGrid[OriginIdx] - InCostGrid[TargetIdx], PowParam);
+                WeightMap[edge1] = InCostGrid[TargetIdx] - InCostGrid[OriginIdx] + 30.0f;
+                WeightMap[edge2] = InCostGrid[OriginIdx] - InCostGrid[TargetIdx] + 30.0f;
             }
         }
     }

From 7ec4e0f0125d83555f46e04f91da04f9b83acf55 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Tue, 22 Aug 2023 15:21:48 +0800
Subject: [PATCH 12/50] feat: support curve type

---
 zeno/include/zeno/utils/PropertyVisitor.h | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/zeno/include/zeno/utils/PropertyVisitor.h b/zeno/include/zeno/utils/PropertyVisitor.h
index c029b24681..7328e1d952 100644
--- a/zeno/include/zeno/utils/PropertyVisitor.h
+++ b/zeno/include/zeno/utils/PropertyVisitor.h
@@ -13,6 +13,7 @@
 #include <zeno/core/Descriptor.h>
 #include <zeno/types/AttrVector.h>
 #include <zeno/types/UserData.h>
+#include <zeno/types/CUrveObject.h>
 #include <zeno/utils/logger.h>
 #include <zeno/zeno.h>
 
@@ -133,6 +134,11 @@ namespace zeno {
             inline static std::string TypeName = "bool";
         };
 
+        template<>
+        struct ValueTypeToString<std::shared_ptr<zeno::CurveObject>> {
+            inline static std::string TypeName = "curve";
+        };
+
         struct TypeAutoCallbackList {
             std::vector<std::function<void(INode *)>> InputHook;
             std::vector<std::function<void(INode *)>> OutputHook;

From b7deff965ef901f706b1908aaa21689d70969400 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Tue, 22 Aug 2023 15:52:38 +0800
Subject: [PATCH 13/50] feat: path cost support control by curve

---
 projects/Roads/nodes/src/cost.cpp     | 40 +++++++++++++++++++++++----
 projects/Roads/utilities/src/grid.cpp |  6 ++--
 2 files changed, 37 insertions(+), 9 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 6dc594d3e5..b021238053 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -3,6 +3,7 @@
 #include "roads/roads.h"
 #include "zeno/PrimitiveObject.h"
 #include "zeno/types/UserData.h"
+#include "zeno/types/CurveObject.h"
 #include "zeno/utils/PropertyVisitor.h"
 #include "zeno/utils/logger.h"
 #include "zeno/zeno.h"
@@ -112,13 +113,30 @@ namespace {
         PathDistanceHeuristic(Vertex Goal) : m_Goal(Goal) {}
 
         CostType operator()(Vertex u) {
-            return 0;
+            return std::abs<CostType>(m_Goal - u);
         }
 
     private:
         Vertex m_Goal;
     };
 
+    struct FoundGoal {};
+
+    template < class Vertex >
+    class PathDistanceVisitor : public boost::default_astar_visitor
+    {
+    public:
+        PathDistanceVisitor(Vertex goal) : m_goal(goal) {}
+        template < class Graph > void examine_vertex(Vertex u, Graph& g)
+        {
+            if (u == m_goal)
+                throw FoundGoal();
+        }
+
+    private:
+        Vertex m_goal;
+    };
+
     struct ZENO_CRTP(CalcPathCost_Simple, zeno::reflect::IParameterAutoNode) {
     //struct CalcPathCost_Simple : public zeno::reflect::IParameterAutoNode<CalcPathCost_Simple> {
         ZENO_GENERATE_NODE_BODY(CalcPathCost_Simple);
@@ -127,9 +145,6 @@ namespace {
         ZENO_DECLARE_INPUT_FIELD(Primitive, "Prim");
         ZENO_DECLARE_OUTPUT_FIELD(Primitive, "Prim");
 
-        std::string OutputChannel;
-        ZENO_DECLARE_INPUT_FIELD(OutputChannel, "Output Channel (Vertex Attr)", false, "", "path_cost");
-
         std::string SizeXChannel;
         ZENO_DECLARE_INPUT_FIELD(SizeXChannel, "Nx Channel (UserData)", false, "", "nx");
 
@@ -148,6 +163,9 @@ namespace {
         zeno::reflect::PathAlgorithmTypeInput Algorithm;
         ZENO_DECLARE_INPUT_FIELD(Algorithm, "Path Finding Algorithm", false, "", "Dijkstra");
 
+        std::shared_ptr<zeno::CurveObject> GradientCurve = nullptr;
+        ZENO_DECLARE_INPUT_FIELD(GradientCurve, "Gradient Cost Control", true);
+
         bool bRemoveTriangles;
         ZENO_DECLARE_INPUT_FIELD(bRemoveTriangles, "Remove Triangles", false, "", "true");
 
@@ -180,9 +198,16 @@ namespace {
                 Connective = ConnectiveType::FOURTY;
             }
 
+            auto GradientMapFunc = [Curve = AutoParameter->GradientCurve] (double In) -> double {
+                if (Curve) {
+                    return Curve->eval(float(In));
+                }
+                return In;
+            };
+
             DynamicGrid<CostPoint> CostGrid(AutoParameter->Nx, AutoParameter->Ny);
             CostGrid.insert(CostGrid.begin(), AutoParameter->GradientList.begin(), AutoParameter->GradientList.begin() + (AutoParameter->Nx * AutoParameter->Ny));
-            auto Graph = CreateWeightGraphFromCostGrid(CostGrid, Connective, 2);
+            auto Graph = CreateWeightGraphFromCostGrid(CostGrid, Connective, GradientMapFunc);
             zeno::log_info("cccc: {}", boost::num_vertices(Graph));
 
             using VertexDescriptor = boost::graph_traits<WeightedGridUndirectedGraph>::vertex_descriptor;
@@ -195,7 +220,10 @@ namespace {
             if (AutoParameter->Algorithm == "Dijkstra") {
                 boost::dijkstra_shortest_paths(Graph, Start, boost::predecessor_map(&p[0]).distance_map(&d[0]));
             } else if (AutoParameter->Algorithm == "A*") {
-                boost::astar_search_tree(Graph, Start, PathDistanceHeuristic<WeightedGridUndirectedGraph, double>(Goal), boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(boost::astar_visitor()));
+                try {
+                    boost::astar_search_tree(Graph, Start, PathDistanceHeuristic<WeightedGridUndirectedGraph, double>(Goal), boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(PathDistanceVisitor(Goal)));
+                } catch (FoundGoal) {
+                }
             }
 
             std::vector<boost::graph_traits<WeightedGridUndirectedGraph>::vertex_descriptor > path;
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index 45d98d9ac0..4edfc99140 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -54,7 +54,7 @@ IntPoint2D InterpolatePoints(const IntPoint2D& A, const IntPoint2D& B, float t)
     return result;
 }
 
-WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint> &InCostGrid, const ConnectiveType Type, float PowParam/*=1.5f*/) {
+WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint> &InCostGrid, const ConnectiveType Type, std::function<double(double)> GradientMappingFunc) {
     ArrayList<IntPoint2D> Directions = { { 0, -1 }, { 0, 1 }, { -1, 0 }, { 1, 0 } };
     if (Type >= ConnectiveType::EIGHT) {
         Directions.insert(Directions.end(), { { -1, -1 }, { 1, -1 }, { -1, 1 }, { 1, 1 } });
@@ -89,8 +89,8 @@ WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGr
                 auto [edge2, _2] = boost::add_edge(TargetIdx, OriginIdx,  NewGraph);
 //                WeightMap[edge1] = std::pow(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] > 0 ? std::min(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 20.0, InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 10.0) : InCostGrid[TargetIdx] - InCostGrid[OriginIdx], 2);
 //                WeightMap[edge2] = std::pow(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] > 0 ? std::min(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 20.0, InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 10.0) : InCostGrid[OriginIdx] - InCostGrid[TargetIdx], 2);
-                WeightMap[edge1] = InCostGrid[TargetIdx] - InCostGrid[OriginIdx] + 30.0f;
-                WeightMap[edge2] = InCostGrid[OriginIdx] - InCostGrid[TargetIdx] + 30.0f;
+                WeightMap[edge1] = GradientMappingFunc(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] + 1.0);
+                WeightMap[edge2] = GradientMappingFunc(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] + 1.0);
             }
         }
     }

From 4ad8c5b2a92e1a4774c1ddc3ee223c60b1630311 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Tue, 22 Aug 2023 15:52:55 +0800
Subject: [PATCH 14/50] fix: add header file

---
 projects/Roads/utilities/include/roads/grid.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index d37fa334d0..8fb6acaf33 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -2,6 +2,7 @@
 
 #include "pch.h"
 #include <numeric>
+#include <functional>
 
 //#include "boost/graph/use_mpi.hpp"
 #include "boost/graph/graph_traits.hpp"
@@ -30,7 +31,7 @@ namespace roads {
 
     ROADS_API DynamicGrid<SlopePoint> CalculateSlope(const DynamicGrid<HeightPoint>& InHeightField);
 
-    ROADS_API WeightedGridUndirectedGraph CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint >& InCostGrid, ConnectiveType Type, float PowParam = 1.5f);
+    ROADS_API WeightedGridUndirectedGraph CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint >& InCostGrid, ConnectiveType Type, std::function<double(double)> GradientMappingFunc = [] (double v) { return v; });
 
     ROADS_API ArrayList<ArrayList<double>> FloydWarshallShortestPath(WeightedGridUndirectedGraph &InGraph);
 

From fd76828ac09c95ae79a1c5d8d7aa3470d96420e0 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Tue, 22 Aug 2023 16:49:55 +0800
Subject: [PATCH 15/50] feat: control road path by curve

---
 projects/Roads/nodes/src/cost.cpp             | 33 ++++++++++++++++---
 projects/Roads/utilities/include/roads/grid.h |  8 +++--
 projects/Roads/utilities/src/grid.cpp         | 14 ++++++--
 3 files changed, 45 insertions(+), 10 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index b021238053..0fc39af85d 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -157,15 +157,24 @@ namespace {
         std::string GradientChannel;
         ZENO_DECLARE_INPUT_FIELD(GradientChannel, "Gradient Channel (Vertex Attr)", false, "", "gradient");
 
+        std::string CurvatureChannel;
+        ZENO_DECLARE_INPUT_FIELD(CurvatureChannel, "Curvature Channel (Vertex Attr)", false, "", "curvature");
+
         zeno::reflect::ConnectiveTypeInput PathConnective;
         ZENO_DECLARE_INPUT_FIELD(PathConnective, "Path Connective", false, "", "16");
 
         zeno::reflect::PathAlgorithmTypeInput Algorithm;
         ZENO_DECLARE_INPUT_FIELD(Algorithm, "Path Finding Algorithm", false, "", "Dijkstra");
 
+        std::shared_ptr<zeno::CurveObject> HeightCurve = nullptr;
+        ZENO_DECLARE_INPUT_FIELD(HeightCurve, "Height Cost Control", true);
+
         std::shared_ptr<zeno::CurveObject> GradientCurve = nullptr;
         ZENO_DECLARE_INPUT_FIELD(GradientCurve, "Gradient Cost Control", true);
 
+        std::shared_ptr<zeno::CurveObject> CurvatureCurve = nullptr;
+        ZENO_DECLARE_INPUT_FIELD(CurvatureCurve, "Curvature Cost Control", true);
+
         bool bRemoveTriangles;
         ZENO_DECLARE_INPUT_FIELD(bRemoveTriangles, "Remove Triangles", false, "", "true");
 
@@ -181,6 +190,9 @@ namespace {
         zeno::AttrVector<float> GradientList{};
         ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, GradientList, GradientChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
 
+        zeno::AttrVector<float> CurvatureList{};
+        ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, CurvatureList, CurvatureChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
+
         void apply() override {
             //auto Grid = BuildGridFromPrimitive(AutoParameter->PositionList, AutoParameter->GradientList, AutoParameter->Nx, AutoParameter->Ny);
             // TODO [darc] : Change cost function, now just simply use gradient value :
@@ -198,16 +210,27 @@ namespace {
                 Connective = ConnectiveType::FOURTY;
             }
 
-            auto GradientMapFunc = [Curve = AutoParameter->GradientCurve] (double In) -> double {
+            auto MapFuncGen = [] (const std::shared_ptr<zeno::CurveObject>& Curve) -> std::function<double(double)> {
                 if (Curve) {
-                    return Curve->eval(float(In));
+                    return [Curve] (double In) -> double {
+                        return Curve->eval(float(In));
+                    };
+                } else {
+                    zeno::log_warn("[Roads] Invalid Curve !");
+                    return [] (double In) -> double {
+                        return In;
+                    };
                 }
-                return In;
             };
 
             DynamicGrid<CostPoint> CostGrid(AutoParameter->Nx, AutoParameter->Ny);
-            CostGrid.insert(CostGrid.begin(), AutoParameter->GradientList.begin(), AutoParameter->GradientList.begin() + (AutoParameter->Nx * AutoParameter->Ny));
-            auto Graph = CreateWeightGraphFromCostGrid(CostGrid, Connective, GradientMapFunc);
+            CostGrid.resize(AutoParameter->Nx*AutoParameter->Ny);
+#pragma omp parallel for
+            for (size_t i = 0; i < AutoParameter->Nx * AutoParameter->Ny; ++i) {
+                CostGrid[i] = (CostPoint { AutoParameter->PositionList[i].at(1), AutoParameter->GradientList[i], AutoParameter->CurvatureList[i] });
+            }
+            // CostGrid.insert(CostGrid.begin(), AutoParameter->GradientList.begin(), AutoParameter->GradientList.begin() + (AutoParameter->Nx * AutoParameter->Ny));
+            auto Graph = CreateWeightGraphFromCostGrid(CostGrid, Connective, MapFuncGen(AutoParameter->HeightCurve), MapFuncGen(AutoParameter->GradientCurve), MapFuncGen(AutoParameter->CurvatureCurve));
             zeno::log_info("cccc: {}", boost::num_vertices(Graph));
 
             using VertexDescriptor = boost::graph_traits<WeightedGridUndirectedGraph>::vertex_descriptor;
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index 8fb6acaf33..4e34f697bb 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -18,7 +18,11 @@ namespace roads {
 
     using HeightPoint = double;
     using SlopePoint = double;
-    using CostPoint = double;
+    struct CostPoint {
+        double Height;
+        double Gradient; // slope
+        double Curvature;
+    };
 
     using EdgeWeightProperty = boost::property<boost::edge_weight_t, double>;
     using WeightedGridUndirectedGraph = boost::adjacency_list<boost::listS,boost::vecS,boost::directedS,boost::no_property,EdgeWeightProperty>;
@@ -31,7 +35,7 @@ namespace roads {
 
     ROADS_API DynamicGrid<SlopePoint> CalculateSlope(const DynamicGrid<HeightPoint>& InHeightField);
 
-    ROADS_API WeightedGridUndirectedGraph CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint >& InCostGrid, ConnectiveType Type, std::function<double(double)> GradientMappingFunc = [] (double v) { return v; });
+    ROADS_API WeightedGridUndirectedGraph CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint >& InCostGrid, ConnectiveType Type, const std::function<double(double)>& HeightMappingFunc = [] (double v) { return v; }, const std::function<double(double)>& GradientMappingFunc = [] (double v) { return v; }, const std::function<double(double)>& CurvatureMappingFunc = [] (double v) { return v; });
 
     ROADS_API ArrayList<ArrayList<double>> FloydWarshallShortestPath(WeightedGridUndirectedGraph &InGraph);
 
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index 4edfc99140..3724824130 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -54,7 +54,7 @@ IntPoint2D InterpolatePoints(const IntPoint2D& A, const IntPoint2D& B, float t)
     return result;
 }
 
-WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint> &InCostGrid, const ConnectiveType Type, std::function<double(double)> GradientMappingFunc) {
+WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint> &InCostGrid, const ConnectiveType Type, const std::function<double(double)>& HeightMappingFunc, const std::function<double(double)>& GradientMappingFunc, const std::function<double(double)>& CurvatureMappingFunc) {
     ArrayList<IntPoint2D> Directions = { { 0, -1 }, { 0, 1 }, { -1, 0 }, { 1, 0 } };
     if (Type >= ConnectiveType::EIGHT) {
         Directions.insert(Directions.end(), { { -1, -1 }, { 1, -1 }, { -1, 1 }, { 1, 1 } });
@@ -89,8 +89,16 @@ WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGr
                 auto [edge2, _2] = boost::add_edge(TargetIdx, OriginIdx,  NewGraph);
 //                WeightMap[edge1] = std::pow(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] > 0 ? std::min(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 20.0, InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 10.0) : InCostGrid[TargetIdx] - InCostGrid[OriginIdx], 2);
 //                WeightMap[edge2] = std::pow(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] > 0 ? std::min(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 20.0, InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 10.0) : InCostGrid[OriginIdx] - InCostGrid[TargetIdx], 2);
-                WeightMap[edge1] = GradientMappingFunc(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] + 1.0);
-                WeightMap[edge2] = GradientMappingFunc(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] + 1.0);
+                WeightMap[edge1] =
+                    (HeightMappingFunc(InCostGrid[TargetIdx].Height - InCostGrid[OriginIdx].Height + 1.0)
+                    + GradientMappingFunc(InCostGrid[TargetIdx].Gradient - InCostGrid[OriginIdx].Gradient + 1.0)
+                    + CurvatureMappingFunc(InCostGrid[TargetIdx].Curvature - InCostGrid[OriginIdx].Curvature + 1.0)
+                    );
+                WeightMap[edge2] =
+                    (HeightMappingFunc(InCostGrid[OriginIdx].Height - InCostGrid[TargetIdx].Height + 1.0)
+                    + GradientMappingFunc(InCostGrid[OriginIdx].Gradient - InCostGrid[TargetIdx].Gradient + 1.0)
+                    + CurvatureMappingFunc(InCostGrid[OriginIdx].Curvature - InCostGrid[TargetIdx].Curvature + 1.0)
+                    );
             }
         }
     }

From 6e143847cb326331c88b540978ec4d0e41148517 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Wed, 23 Aug 2023 13:32:59 +0800
Subject: [PATCH 16/50] feat: add a road astar algorithm

---
 projects/Roads/utilities/include/roads/grid.h | 50 +++++++++++++++++++
 1 file changed, 50 insertions(+)

diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index 4e34f697bb..884286e4b6 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -3,6 +3,7 @@
 #include "pch.h"
 #include <numeric>
 #include <functional>
+#include <queue>
 
 //#include "boost/graph/use_mpi.hpp"
 #include "boost/graph/graph_traits.hpp"
@@ -66,6 +67,55 @@ namespace roads {
 
     namespace energy {
         ROADS_API double CalculateStepSize(const Point2D &Pt, const Point2D &P, const Point2D &PrevX, const Point2D &CurrX);
+
+        template <typename IntLikeT>
+        IntLikeT GreatestCommonDivisor(IntLikeT i, IntLikeT j) {
+            if (0 == j)
+                return i;
+            return GreatestCommonDivisor(j, i % j);
+        }
+
+        template <typename PointType = IntPoint2D>
+        void RoadsShortestPath(const PointType& StartPoint, const PointType& GoalPoint, size_t MaskK, std::map<const PointType&, const PointType&>& PredecessorList, std::map<PointType, float>& CostTo, const std::function<float(PointType, IntPoint2D)>& CostFunction) {
+            // Adding Chebyshev distance as heuristic function
+            auto NewCostFunc = [CostFunction] (const PointType& a, const PointType& b) -> float {
+                return CostFunction(a, b) + std::max(std::abs(a[0] - b[0]), std::abs(a[1] - b[1]));
+            };
+
+            // initialize a priority queue Q with the initial point StartPoint
+            auto Comparator = [&CostTo](const PointType& Point1, const PointType& Point2) { return CostTo[Point1] < CostTo[Point2]; };
+
+            std::priority_queue<PointType, ArrayList<PointType>, decltype(Comparator)> Q(Comparator);
+            Q.push(StartPoint);
+
+            // while Q is not empty
+            while (!Q.empty()) {
+                // select the point p_ij from the priority queue with the smallest cost value c(p_ij)
+                PointType Point = Q.top();
+                Q.pop();
+
+                // if destination has been found p_ij = b, stop the algorithm
+                if (GoalPoint == Point) {
+                    break;
+                }
+
+                // for all points q ∈ M_k(p_ij)
+                for (int32_t dx = -int32_t(MaskK); dx <= MaskK; ++dx) {
+                    for (int32_t dy = -int32_t(MaskK); dy <= MaskK; ++dy) {
+                        if (GreatestCommonDivisor(std::abs(dx), std::abs(dy)) == 1) {
+                            PointType NeighbourPoint { Point[0] + dx, Point[1] + dy };
+                            float NewCost = CostTo[Point] + NewCostFunc(Point, NeighbourPoint);
+
+                            if (CostTo.find(NeighbourPoint) == CostTo.end() || NewCost < CostTo[NeighbourPoint]) {
+                                PredecessorList[NeighbourPoint] = Point;
+                                CostTo[NeighbourPoint] = NewCost;
+                                Q.push(NeighbourPoint);
+                            }
+                        }
+                    }
+                }
+            }
+        }
     }// namespace energy
 
 }// namespace roads

From 8046642d9c36347c13dd2d8c97cadc0d030c6fbd Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Wed, 23 Aug 2023 18:15:33 +0800
Subject: [PATCH 17/50] feat: support vec2f

---
 zeno/include/zeno/utils/PropertyVisitor.h | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/zeno/include/zeno/utils/PropertyVisitor.h b/zeno/include/zeno/utils/PropertyVisitor.h
index 7328e1d952..ec83ed2d08 100644
--- a/zeno/include/zeno/utils/PropertyVisitor.h
+++ b/zeno/include/zeno/utils/PropertyVisitor.h
@@ -109,6 +109,11 @@ namespace zeno {
             inline static std::string TypeName;
         };
 
+        template<>
+        struct ValueTypeToString<zeno::vec2f> {
+            inline static std::string TypeName = "vec2f";
+        };
+
         template<>
         struct ValueTypeToString<zeno::vec3f> {
             inline static std::string TypeName = "vec3f";

From cb42b2cce7e480022a06deae30491e1d130bb881 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Thu, 24 Aug 2023 11:43:57 +0800
Subject: [PATCH 18/50] feat: using Manhattan Distance for now

---
 projects/Roads/nodes/src/cost.cpp             | 143 +++++++++---------
 projects/Roads/utilities/CMakeLists.txt       |   3 +
 projects/Roads/utilities/include/roads/data.h |   3 +
 projects/Roads/utilities/include/roads/grid.h | 101 ++++++++++---
 4 files changed, 152 insertions(+), 98 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 0fc39af85d..7e6150f828 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -1,9 +1,9 @@
-#include "boost/graph/dijkstra_shortest_paths.hpp"
 #include "boost/graph/astar_search.hpp"
+#include "boost/graph/dijkstra_shortest_paths.hpp"
 #include "roads/roads.h"
 #include "zeno/PrimitiveObject.h"
-#include "zeno/types/UserData.h"
 #include "zeno/types/CurveObject.h"
+#include "zeno/types/UserData.h"
 #include "zeno/utils/PropertyVisitor.h"
 #include "zeno/utils/logger.h"
 #include "zeno/zeno.h"
@@ -54,7 +54,7 @@ namespace zeno::reflect {
         inline static std::string TypeName = "enum Dijkstra A*";
     };
 
-}
+}// namespace zeno::reflect
 
 namespace {
     using namespace zeno;
@@ -100,14 +100,14 @@ namespace {
             if (!AutoParameter->Primitive->verts.has_attr(AutoParameter->OutputChannel)) {
                 AutoParameter->Primitive->verts.add_attr<float>(AutoParameter->OutputChannel);
             }
-            std::vector<float>& SlopeAttr = AutoParameter->Primitive->verts.attr<float>(AutoParameter->OutputChannel);
+            std::vector<float> &SlopeAttr = AutoParameter->Primitive->verts.attr<float>(AutoParameter->OutputChannel);
             SlopeAttr.insert(SlopeAttr.begin(), SlopeField.begin(), SlopeField.end());
         }
     };
 
-    template <typename Graph, typename CostType>
+    template<typename Graph, typename CostType>
     class PathDistanceHeuristic : public boost::astar_heuristic<Graph, CostType> {
-        typedef typename boost::graph_traits< Graph >::vertex_descriptor Vertex;
+        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
 
     public:
         PathDistanceHeuristic(Vertex Goal) : m_Goal(Goal) {}
@@ -122,13 +122,12 @@ namespace {
 
     struct FoundGoal {};
 
-    template < class Vertex >
-    class PathDistanceVisitor : public boost::default_astar_visitor
-    {
+    template<class Vertex>
+    class PathDistanceVisitor : public boost::default_astar_visitor {
     public:
         PathDistanceVisitor(Vertex goal) : m_goal(goal) {}
-        template < class Graph > void examine_vertex(Vertex u, Graph& g)
-        {
+        template<class Graph>
+        void examine_vertex(Vertex u, Graph &g) {
             if (u == m_goal)
                 throw FoundGoal();
         }
@@ -138,7 +137,7 @@ namespace {
     };
 
     struct ZENO_CRTP(CalcPathCost_Simple, zeno::reflect::IParameterAutoNode) {
-    //struct CalcPathCost_Simple : public zeno::reflect::IParameterAutoNode<CalcPathCost_Simple> {
+        //struct CalcPathCost_Simple : public zeno::reflect::IParameterAutoNode<CalcPathCost_Simple> {
         ZENO_GENERATE_NODE_BODY(CalcPathCost_Simple);
 
         std::shared_ptr<zeno::PrimitiveObject> Primitive;
@@ -160,11 +159,14 @@ namespace {
         std::string CurvatureChannel;
         ZENO_DECLARE_INPUT_FIELD(CurvatureChannel, "Curvature Channel (Vertex Attr)", false, "", "curvature");
 
-        zeno::reflect::ConnectiveTypeInput PathConnective;
-        ZENO_DECLARE_INPUT_FIELD(PathConnective, "Path Connective", false, "", "16");
+        int ConnectiveMask;
+        ZENO_DECLARE_INPUT_FIELD(ConnectiveMask, "Connective Mask", false, "", "3");
 
-        zeno::reflect::PathAlgorithmTypeInput Algorithm;
-        ZENO_DECLARE_INPUT_FIELD(Algorithm, "Path Finding Algorithm", false, "", "Dijkstra");
+        zeno::vec2f Start;
+        ZENO_DECLARE_INPUT_FIELD(Start, "Start Point");
+
+        zeno::vec2f Goal;
+        ZENO_DECLARE_INPUT_FIELD(Goal, "Goal Point");
 
         std::shared_ptr<zeno::CurveObject> HeightCurve = nullptr;
         ZENO_DECLARE_INPUT_FIELD(HeightCurve, "Height Cost Control", true);
@@ -194,77 +196,66 @@ namespace {
         ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, CurvatureList, CurvatureChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
 
         void apply() override {
-            //auto Grid = BuildGridFromPrimitive(AutoParameter->PositionList, AutoParameter->GradientList, AutoParameter->Nx, AutoParameter->Ny);
-            // TODO [darc] : Change cost function, now just simply use gradient value :
-            AutoParameter->Nx;
             RoadsAssert(AutoParameter->Nx * AutoParameter->Ny <= AutoParameter->GradientList.size(), "Bad nx ny.");
 
-            ConnectiveType Connective = ConnectiveType::SIXTEEN;
-            if (AutoParameter->PathConnective == "4") {
-                Connective = ConnectiveType::FOUR;
-            } else if (AutoParameter->PathConnective == "8") {
-                Connective = ConnectiveType::EIGHT;
-            } else if (AutoParameter->PathConnective == "16") {
-                Connective = ConnectiveType::SIXTEEN;
-            } else if (AutoParameter->PathConnective == "40") {
-                Connective = ConnectiveType::FOURTY;
-            }
+            CostPoint GoalPoint { static_cast<size_t>(AutoParameter->Goal[0]), static_cast<size_t>(AutoParameter->Goal[1]) };
+            CostPoint StartPoint {static_cast<size_t>(AutoParameter->Start[0]), static_cast<size_t>(AutoParameter->Start[1])};
 
-            auto MapFuncGen = [] (const std::shared_ptr<zeno::CurveObject>& Curve) -> std::function<double(double)> {
+            auto MapFuncGen = [](const std::shared_ptr<zeno::CurveObject> &Curve) -> std::function<float(float)> {
                 if (Curve) {
-                    return [Curve] (double In) -> double {
+                    return [Curve](float In) -> float {
                         return Curve->eval(float(In));
                     };
                 } else {
                     zeno::log_warn("[Roads] Invalid Curve !");
-                    return [] (double In) -> double {
+                    return [](float In) -> float {
                         return In;
                     };
                 }
             };
 
+            auto HeightCostFunc = MapFuncGen(AutoParameter->HeightCurve);
+            auto GradientCostFunc = MapFuncGen(AutoParameter->GradientCurve);
+            auto CurvatureCostFunc = MapFuncGen(AutoParameter->CurvatureCurve);
+
             DynamicGrid<CostPoint> CostGrid(AutoParameter->Nx, AutoParameter->Ny);
-            CostGrid.resize(AutoParameter->Nx*AutoParameter->Ny);
-#pragma omp parallel for
+            CostGrid.resize(AutoParameter->Nx * AutoParameter->Ny);
+//#pragma omp parallel for
             for (size_t i = 0; i < AutoParameter->Nx * AutoParameter->Ny; ++i) {
-                CostGrid[i] = (CostPoint { AutoParameter->PositionList[i].at(1), AutoParameter->GradientList[i], AutoParameter->CurvatureList[i] });
-            }
-            // CostGrid.insert(CostGrid.begin(), AutoParameter->GradientList.begin(), AutoParameter->GradientList.begin() + (AutoParameter->Nx * AutoParameter->Ny));
-            auto Graph = CreateWeightGraphFromCostGrid(CostGrid, Connective, MapFuncGen(AutoParameter->HeightCurve), MapFuncGen(AutoParameter->GradientCurve), MapFuncGen(AutoParameter->CurvatureCurve));
-            zeno::log_info("cccc: {}", boost::num_vertices(Graph));
-
-            using VertexDescriptor = boost::graph_traits<WeightedGridUndirectedGraph>::vertex_descriptor;
-
-            std::vector<VertexDescriptor> p(boost::num_vertices(Graph));
-            std::vector<double> d(boost::num_vertices(Graph));
-            VertexDescriptor Start { 1 };
-            VertexDescriptor Goal { 933333 };
-
-            if (AutoParameter->Algorithm == "Dijkstra") {
-                boost::dijkstra_shortest_paths(Graph, Start, boost::predecessor_map(&p[0]).distance_map(&d[0]));
-            } else if (AutoParameter->Algorithm == "A*") {
-                try {
-                    boost::astar_search_tree(Graph, Start, PathDistanceHeuristic<WeightedGridUndirectedGraph, double>(Goal), boost::predecessor_map(&p[0]).distance_map(&d[0]).visitor(PathDistanceVisitor(Goal)));
-                } catch (FoundGoal) {
-                }
+                size_t x = i % AutoParameter->Nx;
+                size_t y = i / AutoParameter->Ny;
+                CostGrid[i] = (CostPoint{x, y, AutoParameter->PositionList[i].at(1), AutoParameter->GradientList[i], AutoParameter->CurvatureList[i]});
             }
 
-            std::vector<boost::graph_traits<WeightedGridUndirectedGraph>::vertex_descriptor > path;
-            boost::graph_traits<WeightedGridUndirectedGraph>::vertex_descriptor current = Goal;
+            std::function<float(const CostPoint&, const CostPoint&)> CostFunc = [HeightCostFunc, GradientCostFunc, CurvatureCostFunc, &CostGrid, Nx = AutoParameter->Nx] (const CostPoint& A, const CostPoint& B) -> float {
+                size_t ia = A[0] + A[1] * Nx;
+                size_t ib = B[0] + B[1] * Nx;
+                float Cost = HeightCostFunc(float(std::abs(CostGrid[ia].Height - CostGrid[ib].Height))) + GradientCostFunc(float(std::abs(CostGrid[ia].Gradient - CostGrid[ib].Gradient)) + CurvatureCostFunc(float(std::abs(CostGrid[ia].Curvature - CostGrid[ib].Curvature))));
+                return Cost;
+            };
+
+            std::unordered_map<CostPoint, CostPoint> Predecessor;
+            std::unordered_map<CostPoint, float> CostMap;
+            roads::energy::RoadsShortestPath(StartPoint, GoalPoint, CostPoint { static_cast<size_t>(AutoParameter->Nx), static_cast<size_t>(AutoParameter->Ny) }, AutoParameter->ConnectiveMask, Predecessor, CostMap, CostFunc);
+
+            zeno::log_info("qwq: {}, {}", Predecessor.size(), CostMap.size());
 
-            while(current!=Start)
-            {
-                path.push_back(current);
-                current = p[current];
+            CostPoint Current = GoalPoint;
+
+            ArrayList<size_t> Path;
+            while (Current != StartPoint) {
+                Path.push_back( Current[0] + Current[1] * AutoParameter->Nx );
+                Current = Predecessor[Current];
             }
-            path.push_back(Start);
+            Path.push_back(StartPoint[0] + StartPoint[1] * AutoParameter->Nx);
 
             if (AutoParameter->bRemoveTriangles) {
                 AutoParameter->Primitive->tris.clear();
             }
-            AutoParameter->Primitive->lines.resize(path.size() - 1);
-            for (size_t i = 0; i < path.size() - 1; ++i) {
-                AutoParameter->Primitive->lines[i] = zeno::vec2i(int(path[i]), int(path[i+1]));
+
+            AutoParameter->Primitive->lines.resize(Path.size() - 1);
+            for (size_t i = 0; i < Path.size() - 1; ++i) {
+                AutoParameter->Primitive->lines[i] = zeno::vec2i(int(Path[i]), int(Path[i+1]));
             }
         }
     };
@@ -316,19 +307,24 @@ namespace {
         ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, WaterMask, WaterChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
 
         void apply() override {
-            auto& Prim = AutoParameter->Primitive;
-            auto& HeightField = AutoParameter->Heightmap;
-            auto& Water = AutoParameter->WaterMask;
+            auto &Prim = AutoParameter->Primitive;
+            auto &HeightField = AutoParameter->Heightmap;
+            auto &Water = AutoParameter->WaterMask;
             const auto SizeX = AutoParameter->Nx;
             const auto SizeY = AutoParameter->Ny;
             const size_t NumVert = SizeX * SizeY;
 
-            auto& River = Prim->add_attr<float>(AutoParameter->RiverChannel);
+            auto &River = Prim->add_attr<float>(AutoParameter->RiverChannel);
 
-            static const std::array<IntPoint2D, 8> SDirection {
-                IntPoint2D { 0, -1 }, { 0, 1 }, { -1, 0 }, { 1, 0 },
-                { -1, -1 }, { 1, -1 }, { -1, 1 }, { 1, 1 }
-            };
+            static const std::array<IntPoint2D, 8> SDirection{
+                IntPoint2D{0, -1},
+                {0, 1},
+                {-1, 0},
+                {1, 0},
+                {-1, -1},
+                {1, -1},
+                {-1, 1},
+                {1, 1}};
 
             std::stack<size_t> Stack;
 
@@ -351,7 +347,7 @@ namespace {
                     River[idx] = 1;
                     long y = idx / SizeX;
                     long x = idx % SizeX;
-                    for (const IntPoint2D& Dir : SDirection) {
+                    for (const IntPoint2D &Dir: SDirection) {
                         long ix = x + Dir[0];
                         long iy = y + Dir[1];
                         if (ix > 0 && iy > 0 && ix < SizeX && iy < SizeX) {
@@ -363,7 +359,6 @@ namespace {
                     }
                 }
             }
-
         }
     };
 }// namespace
diff --git a/projects/Roads/utilities/CMakeLists.txt b/projects/Roads/utilities/CMakeLists.txt
index 61792d150f..b46fef4552 100644
--- a/projects/Roads/utilities/CMakeLists.txt
+++ b/projects/Roads/utilities/CMakeLists.txt
@@ -3,6 +3,7 @@ SET(Boost_ADDITIONAL_VERSIONS "1.82.0" "1.82")
 FIND_PACKAGE(Eigen3 REQUIRED NO_MODULE)
 FIND_PACKAGE(OpenMP REQUIRED)
 FIND_PACKAGE(Boost COMPONENTS graph)
+find_package(TBB CONFIG REQUIRED COMPONENTS tbb tbbmalloc)
 
 FILE(GLOB_RECURSE SOURCE_FILES src/*.cpp src/*.h)
 
@@ -39,3 +40,5 @@ IF (TARGET Boost::graph)
 ELSE()
     MESSAGE(FATAL_ERROR "Roads Module is depending on Boost::graph. You have to install it first.")
 ENDIF()
+
+TARGET_LINK_LIBRARIES(Roads PRIVATE TBB::tbb TBB::tbbmalloc)
diff --git a/projects/Roads/utilities/include/roads/data.h b/projects/Roads/utilities/include/roads/data.h
index 0d4d07037d..c62133ada1 100644
--- a/projects/Roads/utilities/include/roads/data.h
+++ b/projects/Roads/utilities/include/roads/data.h
@@ -19,6 +19,7 @@ namespace roads {
         FOUR = 4,
         EIGHT = 8,
         SIXTEEN = 16,
+        THIRDTY_TWO = 32,
         FOURTY = 40,
     };
 
@@ -53,6 +54,8 @@ namespace roads {
     struct ArrayList : public std::vector<T> {
         using std::vector<T>::vector;
         using std::vector<T>::size;
+        using std::vector<T>::begin;
+        using std::vector<T>::end;
 
         bool IsValidIndex(size_t Index) const {
             return Index < size();
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index 884286e4b6..197c417fed 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -1,14 +1,15 @@
 #pragma once
 
 #include "pch.h"
-#include <numeric>
 #include <functional>
+#include <numeric>
 #include <queue>
+#include <unordered_map>
 
 //#include "boost/graph/use_mpi.hpp"
-#include "boost/graph/graph_traits.hpp"
-#include "boost/graph/graph_concepts.hpp"
 #include "boost/graph/adjacency_list.hpp"
+#include "boost/graph/graph_concepts.hpp"
+#include "boost/graph/graph_traits.hpp"
 
 namespace roads {
 
@@ -19,14 +20,26 @@ namespace roads {
 
     using HeightPoint = double;
     using SlopePoint = double;
-    struct CostPoint {
+    struct CostPoint : std::array<size_t, 2> {
+        CostPoint() = default;
+
+        CostPoint(size_t x, size_t y, double InHeight = 0.0, double InGradient = 0.0, double InCurvature = 0.0)
+            : std::array<size_t, 2>(), Height(InHeight), Gradient(InGradient), Curvature(InCurvature) {
+            at(0) = x;
+            at(1) = y;
+        }
+
+        bool operator==(const CostPoint &Rhs) {
+            return at(0) == Rhs.at(0) && at(1) == Rhs.at(1);
+        }
+
         double Height;
-        double Gradient; // slope
+        double Gradient;// slope
         double Curvature;
     };
 
     using EdgeWeightProperty = boost::property<boost::edge_weight_t, double>;
-    using WeightedGridUndirectedGraph = boost::adjacency_list<boost::listS,boost::vecS,boost::directedS,boost::no_property,EdgeWeightProperty>;
+    using WeightedGridUndirectedGraph = boost::adjacency_list<boost::listS, boost::vecS, boost::directedS, boost::no_property, EdgeWeightProperty>;
     using WeightedGridUndirectedGraphIterator = boost::graph_traits<WeightedGridUndirectedGraph>::edge_iterator;
     using Edge = std::pair<size_t, size_t>;
 
@@ -34,14 +47,15 @@ namespace roads {
 
     ROADS_API ROADS_INLINE double EuclideanDistance(const Point2D &Point1, const Point2D &Point2);
 
-    ROADS_API DynamicGrid<SlopePoint> CalculateSlope(const DynamicGrid<HeightPoint>& InHeightField);
+    ROADS_API DynamicGrid<SlopePoint> CalculateSlope(const DynamicGrid<HeightPoint> &InHeightField);
 
-    ROADS_API WeightedGridUndirectedGraph CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint >& InCostGrid, ConnectiveType Type, const std::function<double(double)>& HeightMappingFunc = [] (double v) { return v; }, const std::function<double(double)>& GradientMappingFunc = [] (double v) { return v; }, const std::function<double(double)>& CurvatureMappingFunc = [] (double v) { return v; });
+    ROADS_API WeightedGridUndirectedGraph CreateWeightGraphFromCostGrid(
+        const DynamicGrid<CostPoint> &InCostGrid, ConnectiveType Type, const std::function<double(double)> &HeightMappingFunc = [](double v) { return v; }, const std::function<double(double)> &GradientMappingFunc = [](double v) { return v; }, const std::function<double(double)> &CurvatureMappingFunc = [](double v) { return v; });
 
     ROADS_API ArrayList<ArrayList<double>> FloydWarshallShortestPath(WeightedGridUndirectedGraph &InGraph);
 
-    template <typename T>
-    ROADS_API void AverageSmooth(ArrayList<T>& InOutContainer, size_t Iteration = 1) {
+    template<typename T>
+    ROADS_API void AverageSmooth(ArrayList<T> &InOutContainer, size_t Iteration = 1) {
         auto num_vertices = InOutContainer.size();
         ArrayList<float> NewHeights(num_vertices);
 
@@ -49,12 +63,13 @@ namespace roads {
 #pragma omp parallel for
             for (size_t i = 0; i < num_vertices; ++i) {
                 // Include the vertex itself and its neighbors.
-                ArrayList<T*> Points = InOutContainer[i].Neighbors;
+                ArrayList<T *> Points = InOutContainer[i].Neighbors;
                 Points.push_back(&InOutContainer[i]);
 
                 // Calculate the average height.
                 NewHeights[i] = std::accumulate(begin(Points), end(Points), 0.0f,
-                                                [](float sum, const T* v) { return sum + v->Height; }) / Points.size();
+                                                [](float sum, const T *v) { return sum + v->Height; }) /
+                                Points.size();
             }
 
             // Update the heights for the next iteration or the final result.
@@ -68,25 +83,44 @@ namespace roads {
     namespace energy {
         ROADS_API double CalculateStepSize(const Point2D &Pt, const Point2D &P, const Point2D &PrevX, const Point2D &CurrX);
 
-        template <typename IntLikeT>
-        IntLikeT GreatestCommonDivisor(IntLikeT i, IntLikeT j) {
-            if (0 == j)
+        template<typename IntLikeT>
+        inline IntLikeT GreatestCommonDivisor(IntLikeT i, IntLikeT j) {
+            if (0 == j) {
                 return i;
+            }
             return GreatestCommonDivisor(j, i % j);
         }
 
-        template <typename PointType = IntPoint2D>
-        void RoadsShortestPath(const PointType& StartPoint, const PointType& GoalPoint, size_t MaskK, std::map<const PointType&, const PointType&>& PredecessorList, std::map<PointType, float>& CostTo, const std::function<float(PointType, IntPoint2D)>& CostFunction) {
+        template<typename PointType = IntPoint2D>
+        void RoadsShortestPath(const PointType &StartPoint, const PointType &GoalPoint, const PointType &Bounds, const int32_t MaskK, std::unordered_map<PointType, PointType> &PredecessorList, std::unordered_map<PointType, float> &CostTo, const std::function<float(const PointType &, const PointType &)> &CostFunction) {
+            auto NewCostFunc = [CostFunction, GoalPoint, Bounds](const PointType &a, const PointType &b) -> float {
+                return CostFunction(a, b);
+                //return CostFunction(a, b) + std::abs<float>(GoalPoint[0] - b[0]) + std::abs<float>(GoalPoint[1] - b[1]);
+            };
+
             // Adding Chebyshev distance as heuristic function
-            auto NewCostFunc = [CostFunction] (const PointType& a, const PointType& b) -> float {
-                return CostFunction(a, b) + std::max(std::abs(a[0] - b[0]), std::abs(a[1] - b[1]));
+            auto Comparator = [&CostTo, &GoalPoint, &Bounds](const PointType &Point1, const PointType &Point2) {
+                //float DeltaA = std::max(std::abs<int32_t>(Point1[0] - GoalPoint[0]) / Bounds[0], std::abs<int32_t>(Point1[1] - GoalPoint[1]) / Bounds[1]);
+                //float DeltaB = std::max(std::abs<int32_t>(Point2[0] - GoalPoint[0]) / Bounds[0], std::abs<int32_t>(Point2[1] - GoalPoint[1]) / Bounds[1]);
+                float DeltaA = std::abs<int32_t>(Point1[0] - GoalPoint[0]) + std::abs<int32_t>(Point1[1] - GoalPoint[1]);
+                float DeltaB = std::abs<int32_t>(Point2[0] - GoalPoint[0]) + std::abs<int32_t>(Point2[1] - GoalPoint[1]);
+                //float DeltaA = std::sqrt(std::pow(Point1[0] - GoalPoint[0], 2) + std::pow(Point1[1] - GoalPoint[1], 2));
+                //float DeltaB = std::sqrt(std::pow(Point2[0] - GoalPoint[0], 2) + std::pow(Point2[1] - GoalPoint[1], 2));
+                float CostA = CostTo[Point1] + DeltaA;
+                float CostB = CostTo[Point2] + DeltaB;
+                return CostA > CostB;
             };
 
             // initialize a priority queue Q with the initial point StartPoint
-            auto Comparator = [&CostTo](const PointType& Point1, const PointType& Point2) { return CostTo[Point1] < CostTo[Point2]; };
-
             std::priority_queue<PointType, ArrayList<PointType>, decltype(Comparator)> Q(Comparator);
             Q.push(StartPoint);
+            CostTo[StartPoint] = 0.f;
+            for (size_t x = 0; x < Bounds[0]; x++) {
+                for (size_t y = 0; y < Bounds[1]; y++) {
+                    PointType Point {x, y};
+                    if (Point != StartPoint) { CostTo[Point] = std::numeric_limits<float>::max(); }
+                }
+            }
 
             // while Q is not empty
             while (!Q.empty()) {
@@ -99,12 +133,20 @@ namespace roads {
                     break;
                 }
 
+                printf("-- P(%d,%d) %f\n", Point[0], Point[1], CostTo[Point]);
+
                 // for all points q ∈ M_k(p_ij)
-                for (int32_t dx = -int32_t(MaskK); dx <= MaskK; ++dx) {
-                    for (int32_t dy = -int32_t(MaskK); dy <= MaskK; ++dy) {
+                for (int32_t dx = -MaskK; dx <= MaskK; ++dx) {
+                    for (int32_t dy = -MaskK; dy <= MaskK; ++dy) {
                         if (GreatestCommonDivisor(std::abs(dx), std::abs(dy)) == 1) {
-                            PointType NeighbourPoint { Point[0] + dx, Point[1] + dy };
+                            PointType NeighbourPoint{Point[0] + dx, Point[1] + dy};
+                            if (NeighbourPoint[0] < 0 || NeighbourPoint[1] < 0 || NeighbourPoint[0] >= Bounds[0] || NeighbourPoint[1] >= Bounds[1]) continue;
+
                             float NewCost = CostTo[Point] + NewCostFunc(Point, NeighbourPoint);
+                            if (NewCost < 0) {
+                                throw std::runtime_error("[Roads] Graph should not have negative weight. Check your curve !");
+                            }
+                            //printf("---- N(%d,%d) %f\n", NeighbourPoint[0], NeighbourPoint[1], NewCost);
 
                             if (CostTo.find(NeighbourPoint) == CostTo.end() || NewCost < CostTo[NeighbourPoint]) {
                                 PredecessorList[NeighbourPoint] = Point;
@@ -119,3 +161,14 @@ namespace roads {
     }// namespace energy
 
 }// namespace roads
+
+
+namespace std {
+    template<>
+    struct hash<roads::CostPoint> {
+        size_t operator()(const roads::CostPoint &rhs) const {
+            constexpr size_t Seed = 10086;
+            return (rhs[0] + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (rhs[1] + 0x9e3779b9 + (Seed << 6) + (Seed >> 2));
+        }
+    };
+}// namespace std

From 2e2c6e66d7dac62afe722a8f42a936a936c24d9e Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Thu, 24 Aug 2023 11:57:33 +0800
Subject: [PATCH 19/50] fix: remove debug message

---
 projects/Roads/utilities/include/roads/grid.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index 197c417fed..ee07556256 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -133,7 +133,7 @@ namespace roads {
                     break;
                 }
 
-                printf("-- P(%d,%d) %f\n", Point[0], Point[1], CostTo[Point]);
+                //printf("-- P(%d,%d) %f\n", Point[0], Point[1], CostTo[Point]);
 
                 // for all points q ∈ M_k(p_ij)
                 for (int32_t dx = -MaskK; dx <= MaskK; ++dx) {

From 8b568c4ffa44810cdb2a228b19c75088a2515b64 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Thu, 24 Aug 2023 15:40:37 +0800
Subject: [PATCH 20/50] feat: update heuristic function

---
 projects/Roads/utilities/include/roads/grid.h | 63 +++++++++++++++++--
 1 file changed, 58 insertions(+), 5 deletions(-)

diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index ee07556256..659308a088 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -98,14 +98,59 @@ namespace roads {
                 //return CostFunction(a, b) + std::abs<float>(GoalPoint[0] - b[0]) + std::abs<float>(GoalPoint[1] - b[1]);
             };
 
+            std::unordered_map<std::pair<PointType, PointType>, float> SimpleCost;
+            std::function<float(const PointType &, const PointType &, bool)> StraightCost = [NewCostFunc, MaskK, &SimpleCost, &StraightCost](const PointType &From, const PointType &To, bool bMoveX = true) mutable -> float {
+                auto CurrentPair = std::make_pair(From, To);
+                if (SimpleCost.count(CurrentPair)) {
+                    return SimpleCost[CurrentPair];
+                }
+
+                if (From == To) {
+                    SimpleCost[CurrentPair] = 0.0f;
+                    return 0.0f;
+                }
+
+                PointType NextPoint = From;
+
+                if (bMoveX) {
+                    if (To[0] != From[0]) {
+                        if (std::abs<int32_t>(From[0] - To[0]) <= MaskK) {
+                            NextPoint[0] = To[0];
+                        } else if (From[0] < To[0]) {
+                            NextPoint[0] += 1;
+                        } else {
+                            NextPoint[0] -= 1;
+                        }
+                    }
+                } else {
+                    if (To[1] != From[1]) {
+                        if (std::abs<int32_t>(From[1] - To[1]) <= MaskK) {
+                            NextPoint[1] = To[1];
+                        } else if (From[1] < To[1]) {
+                            NextPoint[1] += 1;
+                        } else {
+                            NextPoint[1] -= 1;
+                        }
+                    }
+                }
+
+                float Cost = NewCostFunc(From, NextPoint) + StraightCost(NextPoint, To, !bMoveX);
+                SimpleCost[CurrentPair] = Cost;
+                return Cost;
+            };
+
             // Adding Chebyshev distance as heuristic function
-            auto Comparator = [&CostTo, &GoalPoint, &Bounds](const PointType &Point1, const PointType &Point2) {
+            auto Comparator = [&CostTo, &GoalPoint, &Bounds, &StraightCost](const PointType &Point1, const PointType &Point2) {
                 //float DeltaA = std::max(std::abs<int32_t>(Point1[0] - GoalPoint[0]) / Bounds[0], std::abs<int32_t>(Point1[1] - GoalPoint[1]) / Bounds[1]);
                 //float DeltaB = std::max(std::abs<int32_t>(Point2[0] - GoalPoint[0]) / Bounds[0], std::abs<int32_t>(Point2[1] - GoalPoint[1]) / Bounds[1]);
-                float DeltaA = std::abs<int32_t>(Point1[0] - GoalPoint[0]) + std::abs<int32_t>(Point1[1] - GoalPoint[1]);
-                float DeltaB = std::abs<int32_t>(Point2[0] - GoalPoint[0]) + std::abs<int32_t>(Point2[1] - GoalPoint[1]);
+                //float DeltaA = std::abs<int32_t>(Point1[0] - GoalPoint[0]) + std::abs<int32_t>(Point1[1] - GoalPoint[1]);
+                //float DeltaB = std::abs<int32_t>(Point2[0] - GoalPoint[0]) + std::abs<int32_t>(Point2[1] - GoalPoint[1]);
                 //float DeltaA = std::sqrt(std::pow(Point1[0] - GoalPoint[0], 2) + std::pow(Point1[1] - GoalPoint[1], 2));
                 //float DeltaB = std::sqrt(std::pow(Point2[0] - GoalPoint[0], 2) + std::pow(Point2[1] - GoalPoint[1], 2));
+
+                float DeltaA = StraightCost(Point1, GoalPoint, true);
+                float DeltaB = StraightCost(Point2, GoalPoint, true);
+
                 float CostA = CostTo[Point1] + DeltaA;
                 float CostB = CostTo[Point2] + DeltaB;
                 return CostA > CostB;
@@ -117,7 +162,7 @@ namespace roads {
             CostTo[StartPoint] = 0.f;
             for (size_t x = 0; x < Bounds[0]; x++) {
                 for (size_t y = 0; y < Bounds[1]; y++) {
-                    PointType Point {x, y};
+                    PointType Point{x, y};
                     if (Point != StartPoint) { CostTo[Point] = std::numeric_limits<float>::max(); }
                 }
             }
@@ -135,7 +180,7 @@ namespace roads {
 
                 //printf("-- P(%d,%d) %f\n", Point[0], Point[1], CostTo[Point]);
 
-                // for all points q ∈ M_k(p_ij)
+                // step3. for all points q ∈ M_k(p_ij)
                 for (int32_t dx = -MaskK; dx <= MaskK; ++dx) {
                     for (int32_t dy = -MaskK; dy <= MaskK; ++dy) {
                         if (GreatestCommonDivisor(std::abs(dx), std::abs(dy)) == 1) {
@@ -171,4 +216,12 @@ namespace std {
             return (rhs[0] + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (rhs[1] + 0x9e3779b9 + (Seed << 6) + (Seed >> 2));
         }
     };
+
+    template<>
+    struct hash<std::pair<roads::CostPoint, roads::CostPoint>> {
+        size_t operator()(const std::pair<roads::CostPoint, roads::CostPoint> &rhs) const {
+            constexpr size_t Seed = 10086;
+            return (rhs.first[0] + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (rhs.first[1] + 0x9e3779b9 + (Seed << 6) + (Seed >> 2)) ^ (rhs.second[0] + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (rhs.second[1] + 0x9e3779b9 + (Seed << 6) + (Seed >> 2));
+        }
+    };
 }// namespace std

From c98e05dea122e86ad5aa964d4b9122ba90f79186 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Fri, 25 Aug 2023 12:15:21 +0800
Subject: [PATCH 21/50] feat: add visual array

---
 projects/Roads/nodes/src/cost.cpp             | 76 ++++++++++++++-----
 projects/Roads/utilities/include/roads/grid.h | 65 +++++++++-------
 projects/Roads/utilities/src/grid.cpp         | 41 +++++-----
 3 files changed, 113 insertions(+), 69 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 7e6150f828..1bb03e748b 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -156,12 +156,18 @@ namespace {
         std::string GradientChannel;
         ZENO_DECLARE_INPUT_FIELD(GradientChannel, "Gradient Channel (Vertex Attr)", false, "", "gradient");
 
-        std::string CurvatureChannel;
-        ZENO_DECLARE_INPUT_FIELD(CurvatureChannel, "Curvature Channel (Vertex Attr)", false, "", "curvature");
+        //std::string CurvatureChannel;
+        //ZENO_DECLARE_INPUT_FIELD(CurvatureChannel, "Curvature Channel (Vertex Attr)", false, "", "curvature");
 
         int ConnectiveMask;
         ZENO_DECLARE_INPUT_FIELD(ConnectiveMask, "Connective Mask", false, "", "3");
 
+        float WeightHeuristic;
+        ZENO_DECLARE_INPUT_FIELD(WeightHeuristic, "Weight of Heuristic Function", false, "", "0.3");
+
+        float CurvatureThreshold;
+        ZENO_DECLARE_INPUT_FIELD(CurvatureThreshold, "Curvature Threshold", false, "", "0.4");
+
         zeno::vec2f Start;
         ZENO_DECLARE_INPUT_FIELD(Start, "Start Point");
 
@@ -192,59 +198,89 @@ namespace {
         zeno::AttrVector<float> GradientList{};
         ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, GradientList, GradientChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
 
-        zeno::AttrVector<float> CurvatureList{};
-        ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, CurvatureList, CurvatureChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
+        //zeno::AttrVector<float> CurvatureList{};
+        //ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, CurvatureList, CurvatureChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
 
         void apply() override {
+            zeno::log_info("[Roads] Generating trajectory...");
             RoadsAssert(AutoParameter->Nx * AutoParameter->Ny <= AutoParameter->GradientList.size(), "Bad nx ny.");
 
-            CostPoint GoalPoint { static_cast<size_t>(AutoParameter->Goal[0]), static_cast<size_t>(AutoParameter->Goal[1]) };
-            CostPoint StartPoint {static_cast<size_t>(AutoParameter->Start[0]), static_cast<size_t>(AutoParameter->Start[1])};
+            CostPoint GoalPoint{static_cast<size_t>(AutoParameter->Goal[0]), static_cast<size_t>(AutoParameter->Goal[1])};
+            CostPoint StartPoint{static_cast<size_t>(AutoParameter->Start[0]), static_cast<size_t>(AutoParameter->Start[1])};
 
-            auto MapFuncGen = [](const std::shared_ptr<zeno::CurveObject> &Curve) -> std::function<float(float)> {
+            auto MapFuncGen = [](const std::shared_ptr<zeno::CurveObject> &Curve, float Threshold) -> std::function<float(float)> {
                 if (Curve) {
-                    return [Curve](float In) -> float {
+                    return [Curve, Threshold](float In) -> float {
+                        if (Threshold > 0 && In > Threshold) {
+                            return 9e06f;
+                        }
                         return Curve->eval(float(In));
                     };
                 } else {
                     zeno::log_warn("[Roads] Invalid Curve !");
-                    return [](float In) -> float {
+                    return [Threshold](float In) -> float {
+                        if (Threshold > 0 && In > Threshold) {
+                            return 9e06f;
+                        }
                         return In;
                     };
                 }
             };
 
-            auto HeightCostFunc = MapFuncGen(AutoParameter->HeightCurve);
-            auto GradientCostFunc = MapFuncGen(AutoParameter->GradientCurve);
-            auto CurvatureCostFunc = MapFuncGen(AutoParameter->CurvatureCurve);
+            auto HeightCostFunc = MapFuncGen(AutoParameter->HeightCurve, -1.0f);
+            auto GradientCostFunc = MapFuncGen(AutoParameter->GradientCurve, -1.0f);
+            auto CurvatureCostFunc = MapFuncGen(AutoParameter->CurvatureCurve, AutoParameter->CurvatureThreshold);
 
             DynamicGrid<CostPoint> CostGrid(AutoParameter->Nx, AutoParameter->Ny);
             CostGrid.resize(AutoParameter->Nx * AutoParameter->Ny);
-//#pragma omp parallel for
+            //#pragma omp parallel for
             for (size_t i = 0; i < AutoParameter->Nx * AutoParameter->Ny; ++i) {
                 size_t x = i % AutoParameter->Nx;
                 size_t y = i / AutoParameter->Ny;
-                CostGrid[i] = (CostPoint{x, y, AutoParameter->PositionList[i].at(1), AutoParameter->GradientList[i], AutoParameter->CurvatureList[i]});
+                CostGrid[i] = (CostPoint{x, y, AutoParameter->PositionList[i].at(1), AutoParameter->GradientList[i]});
             }
 
-            std::function<float(const CostPoint&, const CostPoint&)> CostFunc = [HeightCostFunc, GradientCostFunc, CurvatureCostFunc, &CostGrid, Nx = AutoParameter->Nx] (const CostPoint& A, const CostPoint& B) -> float {
+            const size_t IndexMax = AutoParameter->Nx * AutoParameter->Ny - 1;
+
+            std::function<float(const CostPoint &, const CostPoint &)> CostFunc = [&HeightCostFunc, &GradientCostFunc, &CurvatureCostFunc, &CostGrid, IndexMax, Nx = AutoParameter->Nx, Ny = AutoParameter->Ny](const CostPoint &A, const CostPoint &B) -> float {
                 size_t ia = A[0] + A[1] * Nx;
                 size_t ib = B[0] + B[1] * Nx;
-                float Cost = HeightCostFunc(float(std::abs(CostGrid[ia].Height - CostGrid[ib].Height))) + GradientCostFunc(float(std::abs(CostGrid[ia].Gradient - CostGrid[ib].Gradient)) + CurvatureCostFunc(float(std::abs(CostGrid[ia].Curvature - CostGrid[ib].Curvature))));
+
+                Eigen::Vector2f Dir = (Eigen::Vector2f(A[0], A[1]) - Eigen::Vector2f(B[0], B[1])).normalized();
+                size_t NearbyIndexPlus1 = std::clamp<size_t>(ia + 1, 0, IndexMax);
+                size_t NearbyIndexMinus1 = std::clamp<size_t>(ia - 1, 0, IndexMax);
+                size_t NearbyIndexPlusY = std::clamp<size_t>(ia + Ny, 0, IndexMax);
+                size_t NearbyIndexMinusY = std::clamp<size_t>(ia - Ny, 0, IndexMax);
+                double height_x_plus = CostGrid[NearbyIndexPlus1].Height;
+                double height_x_minus = CostGrid[NearbyIndexMinus1].Height;
+                double height_y_plus = CostGrid[NearbyIndexPlusY].Height;
+                double height_y_minus = CostGrid[NearbyIndexMinusY].Height;
+
+                float dHeight_dx = float(height_x_plus - height_x_minus) / 2.0f;
+                float dHeight_dy = float(height_y_plus - height_x_minus) / 2.0f;
+                float d2Height_dx2 = float(height_x_plus - 2 * CostGrid[ia].Height + height_x_minus);
+                float d2Height_dy2 = float(height_y_plus - 2 * CostGrid[ia].Height + height_y_minus);
+
+                Eigen::Matrix2f Hessian;
+                Hessian << d2Height_dx2, dHeight_dy, dHeight_dx, d2Height_dy2;
+
+                float Curvature = Dir.transpose() * Hessian * Dir;
+
+                float Cost = HeightCostFunc(float(std::abs(CostGrid[ia].Height - CostGrid[ib].Height))) + GradientCostFunc(float(std::abs(CostGrid[ia].Gradient - CostGrid[ib].Gradient))) + CurvatureCostFunc(float(std::abs(Curvature)));
                 return Cost;
             };
 
             std::unordered_map<CostPoint, CostPoint> Predecessor;
             std::unordered_map<CostPoint, float> CostMap;
-            roads::energy::RoadsShortestPath(StartPoint, GoalPoint, CostPoint { static_cast<size_t>(AutoParameter->Nx), static_cast<size_t>(AutoParameter->Ny) }, AutoParameter->ConnectiveMask, Predecessor, CostMap, CostFunc);
+            roads::energy::RoadsShortestPath(StartPoint, GoalPoint, CostPoint{static_cast<size_t>(AutoParameter->Nx), static_cast<size_t>(AutoParameter->Ny)}, AutoParameter->ConnectiveMask, AutoParameter->WeightHeuristic, Predecessor, CostMap, CostFunc);
 
-            zeno::log_info("qwq: {}, {}", Predecessor.size(), CostMap.size());
+            zeno::log_info("[Roads] Result Predecessor Size: {}; CostMap Size: {}", Predecessor.size(), CostMap.size());
 
             CostPoint Current = GoalPoint;
 
             ArrayList<size_t> Path;
             while (Current != StartPoint) {
-                Path.push_back( Current[0] + Current[1] * AutoParameter->Nx );
+                Path.push_back(Current[0] + Current[1] * AutoParameter->Nx);
                 Current = Predecessor[Current];
             }
             Path.push_back(StartPoint[0] + StartPoint[1] * AutoParameter->Nx);
@@ -255,7 +291,7 @@ namespace {
 
             AutoParameter->Primitive->lines.resize(Path.size() - 1);
             for (size_t i = 0; i < Path.size() - 1; ++i) {
-                AutoParameter->Primitive->lines[i] = zeno::vec2i(int(Path[i]), int(Path[i+1]));
+                AutoParameter->Primitive->lines[i] = zeno::vec2i(int(Path[i]), int(Path[i + 1]));
             }
         }
     };
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index 659308a088..fdfa1f76f4 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -23,8 +23,8 @@ namespace roads {
     struct CostPoint : std::array<size_t, 2> {
         CostPoint() = default;
 
-        CostPoint(size_t x, size_t y, double InHeight = 0.0, double InGradient = 0.0, double InCurvature = 0.0)
-            : std::array<size_t, 2>(), Height(InHeight), Gradient(InGradient), Curvature(InCurvature) {
+        CostPoint(size_t x, size_t y, double InHeight = 0.0, double InGradient = 0.0/*, double InCurvature = 0.0**/)
+            : std::array<size_t, 2>(), Height(InHeight), Gradient(InGradient) {
             at(0) = x;
             at(1) = y;
         }
@@ -35,7 +35,9 @@ namespace roads {
 
         double Height;
         double Gradient;// slope
-        double Curvature;
+
+        // We need directional curvature, we cannot pre calculate it
+        //double Curvature;
     };
 
     using EdgeWeightProperty = boost::property<boost::edge_weight_t, double>;
@@ -92,9 +94,14 @@ namespace roads {
         }
 
         template<typename PointType = IntPoint2D>
-        void RoadsShortestPath(const PointType &StartPoint, const PointType &GoalPoint, const PointType &Bounds, const int32_t MaskK, std::unordered_map<PointType, PointType> &PredecessorList, std::unordered_map<PointType, float> &CostTo, const std::function<float(const PointType &, const PointType &)> &CostFunction) {
+        void RoadsShortestPath(const PointType &StartPoint, const PointType &GoalPoint, const PointType &Bounds, const int32_t MaskK, const float WeightHeuristic, std::unordered_map<PointType, PointType> &PredecessorList, std::unordered_map<PointType, float> &CostTo, const std::function<float(const PointType &, const PointType &)> &CostFunction) {
             auto NewCostFunc = [CostFunction, GoalPoint, Bounds](const PointType &a, const PointType &b) -> float {
-                return CostFunction(a, b);
+                float Result = CostFunction(a, b);
+                if (Result < 0) {
+                    printf("[Roads] Minus cost P1(%d,%d) P2(%d,%d) Cost=%f.", a[0], a[1], b[0], b[1], Result);
+                    throw std::runtime_error("[Roads] Minus edge weight detected.");
+                }
+                return Result;
                 //return CostFunction(a, b) + std::abs<float>(GoalPoint[0] - b[0]) + std::abs<float>(GoalPoint[1] - b[1]);
             };
 
@@ -112,25 +119,22 @@ namespace roads {
 
                 PointType NextPoint = From;
 
-                if (bMoveX) {
-                    if (To[0] != From[0]) {
-                        if (std::abs<int32_t>(From[0] - To[0]) <= MaskK) {
-                            NextPoint[0] = To[0];
-                        } else if (From[0] < To[0]) {
-                            NextPoint[0] += 1;
-                        } else {
-                            NextPoint[0] -= 1;
-                        }
+                if (To[0] != From[0]) {
+                    if (std::abs<int32_t>(From[0] - To[0]) <= MaskK) {
+                        NextPoint[0] = To[0];
+                    } else if (From[0] < To[0]) {
+                        NextPoint[0] += std::max(1, MaskK / 2);
+                    } else {
+                        NextPoint[0] -= std::max(1, MaskK / 2);
                     }
-                } else {
-                    if (To[1] != From[1]) {
-                        if (std::abs<int32_t>(From[1] - To[1]) <= MaskK) {
-                            NextPoint[1] = To[1];
-                        } else if (From[1] < To[1]) {
-                            NextPoint[1] += 1;
-                        } else {
-                            NextPoint[1] -= 1;
-                        }
+                }
+                if (To[1] != From[1]) {
+                    if (std::abs<int32_t>(From[1] - To[1]) <= MaskK) {
+                        NextPoint[1] = To[1];
+                    } else if (From[1] < To[1]) {
+                        NextPoint[1] += std::max(1, MaskK / 2);
+                    } else {
+                        NextPoint[1] -= std::max(1, MaskK / 2);
                     }
                 }
 
@@ -140,7 +144,7 @@ namespace roads {
             };
 
             // Adding Chebyshev distance as heuristic function
-            auto Comparator = [&CostTo, &GoalPoint, &Bounds, &StraightCost](const PointType &Point1, const PointType &Point2) {
+            auto Comparator = [&CostTo, &GoalPoint, &Bounds, &StraightCost, WeightHeuristic](const PointType &Point1, const PointType &Point2) {
                 //float DeltaA = std::max(std::abs<int32_t>(Point1[0] - GoalPoint[0]) / Bounds[0], std::abs<int32_t>(Point1[1] - GoalPoint[1]) / Bounds[1]);
                 //float DeltaB = std::max(std::abs<int32_t>(Point2[0] - GoalPoint[0]) / Bounds[0], std::abs<int32_t>(Point2[1] - GoalPoint[1]) / Bounds[1]);
                 //float DeltaA = std::abs<int32_t>(Point1[0] - GoalPoint[0]) + std::abs<int32_t>(Point1[1] - GoalPoint[1]);
@@ -151,8 +155,8 @@ namespace roads {
                 float DeltaA = StraightCost(Point1, GoalPoint, true);
                 float DeltaB = StraightCost(Point2, GoalPoint, true);
 
-                float CostA = CostTo[Point1] + DeltaA;
-                float CostB = CostTo[Point2] + DeltaB;
+                float CostA = CostTo[Point1] + WeightHeuristic * DeltaA;
+                float CostB = CostTo[Point2] + WeightHeuristic * DeltaB;
                 return CostA > CostB;
             };
 
@@ -167,6 +171,8 @@ namespace roads {
                 }
             }
 
+            std::unordered_map<PointType, bool> Visual;
+
             // while Q is not empty
             while (!Q.empty()) {
                 // select the point p_ij from the priority queue with the smallest cost value c(p_ij)
@@ -178,6 +184,11 @@ namespace roads {
                     break;
                 }
 
+                if (Visual.find(Point) != std::end(Visual)) {
+                    continue;
+                }
+                Visual[Point] = 1;
+
                 //printf("-- P(%d,%d) %f\n", Point[0], Point[1], CostTo[Point]);
 
                 // step3. for all points q ∈ M_k(p_ij)
@@ -193,7 +204,7 @@ namespace roads {
                             }
                             //printf("---- N(%d,%d) %f\n", NeighbourPoint[0], NeighbourPoint[1], NewCost);
 
-                            if (CostTo.find(NeighbourPoint) == CostTo.end() || NewCost < CostTo[NeighbourPoint]) {
+                            if (NewCost < CostTo[NeighbourPoint]) {
                                 PredecessorList[NeighbourPoint] = Point;
                                 CostTo[NeighbourPoint] = NewCost;
                                 Q.push(NeighbourPoint);
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index 3724824130..a34e8bb0ed 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -13,8 +13,8 @@ double roads::EuclideanDistance(const Point2D &Point1, const Point2D &Point2) {
 }
 
 DynamicGrid<SlopePoint> roads::CalculateSlope(const DynamicGrid<HeightPoint> &InHeightField) {
-    constexpr static std::array<int32_t, 4> XDirection4 = { -1, 0, 1, 0 };
-    constexpr static std::array<int32_t, 4> YDirection4 = { 0, 1, 0, -1 };
+    constexpr static std::array<int32_t, 4> XDirection4 = {-1, 0, 1, 0};
+    constexpr static std::array<int32_t, 4> YDirection4 = {0, 1, 0, -1};
     constexpr static size_t DirectionSize = std::max(XDirection4.size(), YDirection4.size());
 
     const size_t SizeX = InHeightField.Nx;
@@ -46,32 +46,31 @@ DynamicGrid<SlopePoint> roads::CalculateSlope(const DynamicGrid<HeightPoint> &In
     return Result;
 }
 
-IntPoint2D InterpolatePoints(const IntPoint2D& A, const IntPoint2D& B, float t)
-{
+IntPoint2D InterpolatePoints(const IntPoint2D &A, const IntPoint2D &B, float t) {
     IntPoint2D result;
     result[0] = long(std::round(float(A[0]) + t * float(B[0] - A[0])));
     result[1] = long(std::round(float(A[1]) + t * float(B[1] - A[1])));
     return result;
 }
 
-WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint> &InCostGrid, const ConnectiveType Type, const std::function<double(double)>& HeightMappingFunc, const std::function<double(double)>& GradientMappingFunc, const std::function<double(double)>& CurvatureMappingFunc) {
-    ArrayList<IntPoint2D> Directions = { { 0, -1 }, { 0, 1 }, { -1, 0 }, { 1, 0 } };
+WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint> &InCostGrid, const ConnectiveType Type, const std::function<double(double)> &HeightMappingFunc, const std::function<double(double)> &GradientMappingFunc, const std::function<double(double)> &CurvatureMappingFunc) {
+    ArrayList<IntPoint2D> Directions = {{0, -1}, {0, 1}, {-1, 0}, {1, 0}};
     if (Type >= ConnectiveType::EIGHT) {
-        Directions.insert(Directions.end(), { { -1, -1 }, { 1, -1 }, { -1, 1 }, { 1, 1 } });
+        Directions.insert(Directions.end(), {{-1, -1}, {1, -1}, {-1, 1}, {1, 1}});
     }
     if (Type >= ConnectiveType::SIXTEEN) {
-        Directions.insert(Directions.end(), { { -1, -2 }, { 1, -2 }, { -2, -1 }, { 2, -1 }, { -2, 1 }, { 2, 1 }, { -1, 2 }, { 1, 2 } });
+        Directions.insert(Directions.end(), {{-1, -2}, {1, -2}, {-2, -1}, {2, -1}, {-2, 1}, {2, 1}, {-1, 2}, {1, 2}});
     }
     if (Type >= ConnectiveType::FOURTY) {
         // Directions.insert(Directions.end(), { { -2, -2 }, { -2, -1 }, { -2, 0 }, { -2, 1 }, { -2, 2 }, { -1, -2 }, { -1, 2 }, { 0, -2 }, { 0, 2 }, { 1, -2 }, { 1, 2 }, { 2, -2 }, { 2, -1 }, { 2, 0 }, { 2, 1 }, { 2, 2 } });
         size_t original_size = Directions.size();
-        for(size_t i = 0; i + 1 < original_size; ++i) {
+        for (size_t i = 0; i + 1 < original_size; ++i) {
             Directions.push_back(InterpolatePoints(Directions[i], Directions[i + 1], 1.0 / 3.0));
             Directions.push_back(InterpolatePoints(Directions[i], Directions[i + 1], 2.0 / 3.0));
         }
     }
 
-    WeightedGridUndirectedGraph NewGraph { InCostGrid.size() };
+    WeightedGridUndirectedGraph NewGraph{InCostGrid.size()};
 
     // boost graph library seem not provide thread safe
 #pragma omp parallel for
@@ -79,25 +78,23 @@ WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGr
         for (int32_t x = 0; x < InCostGrid.Nx; ++x) {
             const size_t OriginIdx = y * InCostGrid.Nx + x;
             boost::property_map<WeightedGridUndirectedGraph, boost::edge_weight_t>::type WeightMap = boost::get(boost::edge_weight, NewGraph);
-            for (auto & Direction : Directions) {
+            for (auto &Direction: Directions) {
                 const size_t ix = x + Direction[0];
                 const size_t iy = y + Direction[1];
                 if (ix >= InCostGrid.Nx || iy >= InCostGrid.Ny) continue;
                 const size_t TargetIdx = iy * InCostGrid.Nx + ix;
                 using EdgeDescriptor = boost::graph_traits<WeightedGridUndirectedGraph>::edge_descriptor;
                 auto [edge1, _] = boost::add_edge(OriginIdx, TargetIdx, NewGraph);
-                auto [edge2, _2] = boost::add_edge(TargetIdx, OriginIdx,  NewGraph);
-//                WeightMap[edge1] = std::pow(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] > 0 ? std::min(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 20.0, InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 10.0) : InCostGrid[TargetIdx] - InCostGrid[OriginIdx], 2);
-//                WeightMap[edge2] = std::pow(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] > 0 ? std::min(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 20.0, InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 10.0) : InCostGrid[OriginIdx] - InCostGrid[TargetIdx], 2);
+                auto [edge2, _2] = boost::add_edge(TargetIdx, OriginIdx, NewGraph);
+                //                WeightMap[edge1] = std::pow(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] > 0 ? std::min(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 20.0, InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 10.0) : InCostGrid[TargetIdx] - InCostGrid[OriginIdx], 2);
+                //                WeightMap[edge2] = std::pow(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] > 0 ? std::min(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 20.0, InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 10.0) : InCostGrid[OriginIdx] - InCostGrid[TargetIdx], 2);
                 WeightMap[edge1] =
-                    (HeightMappingFunc(InCostGrid[TargetIdx].Height - InCostGrid[OriginIdx].Height + 1.0)
-                    + GradientMappingFunc(InCostGrid[TargetIdx].Gradient - InCostGrid[OriginIdx].Gradient + 1.0)
-                    + CurvatureMappingFunc(InCostGrid[TargetIdx].Curvature - InCostGrid[OriginIdx].Curvature + 1.0)
+                    (HeightMappingFunc(InCostGrid[TargetIdx].Height - InCostGrid[OriginIdx].Height + 1.0) + GradientMappingFunc(InCostGrid[TargetIdx].Gradient - InCostGrid[OriginIdx].Gradient + 1.0)
+                     //                    + CurvatureMappingFunc(InCostGrid[TargetIdx].Curvature - InCostGrid[OriginIdx].Curvature + 1.0)
                     );
                 WeightMap[edge2] =
-                    (HeightMappingFunc(InCostGrid[OriginIdx].Height - InCostGrid[TargetIdx].Height + 1.0)
-                    + GradientMappingFunc(InCostGrid[OriginIdx].Gradient - InCostGrid[TargetIdx].Gradient + 1.0)
-                    + CurvatureMappingFunc(InCostGrid[OriginIdx].Curvature - InCostGrid[TargetIdx].Curvature + 1.0)
+                    (HeightMappingFunc(InCostGrid[OriginIdx].Height - InCostGrid[TargetIdx].Height + 1.0) + GradientMappingFunc(InCostGrid[OriginIdx].Gradient - InCostGrid[TargetIdx].Gradient + 1.0)
+                     //                    + CurvatureMappingFunc(InCostGrid[OriginIdx].Curvature - InCostGrid[TargetIdx].Curvature + 1.0)
                     );
             }
         }
@@ -107,8 +104,8 @@ WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGr
 }
 
 ArrayList<ArrayList<double>> roads::FloydWarshallShortestPath(WeightedGridUndirectedGraph &InGraph) {
-    ArrayList<ArrayList<double>> D { InGraph.m_vertices.size() };
-    ArrayList<double> d (InGraph.m_vertices.size(), (std::numeric_limits<double>::max)());
+    ArrayList<ArrayList<double>> D{InGraph.m_vertices.size()};
+    ArrayList<double> d(InGraph.m_vertices.size(), (std::numeric_limits<double>::max)());
     printf("%llu", InGraph.m_vertices.size());
     boost::floyd_warshall_all_pairs_shortest_paths(InGraph, D, boost::distance_map(&d[0]));
     return D;

From e98dca3b295ab80e942fa2670f917263c1b83583 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Fri, 25 Aug 2023 12:35:56 +0800
Subject: [PATCH 22/50] feat: update heuristic function

---
 projects/Roads/nodes/src/cost.cpp             | 50 +++++++++++--------
 projects/Roads/utilities/include/roads/grid.h |  8 +--
 2 files changed, 34 insertions(+), 24 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 1bb03e748b..9254fe6cda 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -241,30 +241,40 @@ namespace {
             }
 
             const size_t IndexMax = AutoParameter->Nx * AutoParameter->Ny - 1;
+            std::unordered_map<size_t, float> CurvatureCache;
 
-            std::function<float(const CostPoint &, const CostPoint &)> CostFunc = [&HeightCostFunc, &GradientCostFunc, &CurvatureCostFunc, &CostGrid, IndexMax, Nx = AutoParameter->Nx, Ny = AutoParameter->Ny](const CostPoint &A, const CostPoint &B) -> float {
+            std::function<float(const CostPoint &, const CostPoint &)> CostFunc = [&CurvatureCache, &HeightCostFunc, &GradientCostFunc, &CurvatureCostFunc, &CostGrid, IndexMax, Nx = AutoParameter->Nx, Ny = AutoParameter->Ny](const CostPoint &A, const CostPoint &B) mutable -> float {
                 size_t ia = A[0] + A[1] * Nx;
                 size_t ib = B[0] + B[1] * Nx;
 
-                Eigen::Vector2f Dir = (Eigen::Vector2f(A[0], A[1]) - Eigen::Vector2f(B[0], B[1])).normalized();
-                size_t NearbyIndexPlus1 = std::clamp<size_t>(ia + 1, 0, IndexMax);
-                size_t NearbyIndexMinus1 = std::clamp<size_t>(ia - 1, 0, IndexMax);
-                size_t NearbyIndexPlusY = std::clamp<size_t>(ia + Ny, 0, IndexMax);
-                size_t NearbyIndexMinusY = std::clamp<size_t>(ia - Ny, 0, IndexMax);
-                double height_x_plus = CostGrid[NearbyIndexPlus1].Height;
-                double height_x_minus = CostGrid[NearbyIndexMinus1].Height;
-                double height_y_plus = CostGrid[NearbyIndexPlusY].Height;
-                double height_y_minus = CostGrid[NearbyIndexMinusY].Height;
-
-                float dHeight_dx = float(height_x_plus - height_x_minus) / 2.0f;
-                float dHeight_dy = float(height_y_plus - height_x_minus) / 2.0f;
-                float d2Height_dx2 = float(height_x_plus - 2 * CostGrid[ia].Height + height_x_minus);
-                float d2Height_dy2 = float(height_y_plus - 2 * CostGrid[ia].Height + height_y_minus);
-
-                Eigen::Matrix2f Hessian;
-                Hessian << d2Height_dx2, dHeight_dy, dHeight_dx, d2Height_dy2;
-
-                float Curvature = Dir.transpose() * Hessian * Dir;
+                constexpr size_t Seed = 12306;
+                size_t Hash = (ia + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (ib * 0x9e3779b9 + (Seed << 2) + (Seed >> 4));
+
+                float Curvature;
+                if (CurvatureCache.find(Hash) != std::end(CurvatureCache)) {
+                    Curvature = CurvatureCache[Hash];
+                } else {
+                    Eigen::Vector2f Dir = (Eigen::Vector2f(A[0], A[1]) - Eigen::Vector2f(B[0], B[1])).normalized();
+                    size_t NearbyIndexPlus1 = std::clamp<size_t>(ia + 1, 0, IndexMax);
+                    size_t NearbyIndexMinus1 = std::clamp<size_t>(ia - 1, 0, IndexMax);
+                    size_t NearbyIndexPlusY = std::clamp<size_t>(ia + Ny, 0, IndexMax);
+                    size_t NearbyIndexMinusY = std::clamp<size_t>(ia - Ny, 0, IndexMax);
+                    double height_x_plus = CostGrid[NearbyIndexPlus1].Height;
+                    double height_x_minus = CostGrid[NearbyIndexMinus1].Height;
+                    double height_y_plus = CostGrid[NearbyIndexPlusY].Height;
+                    double height_y_minus = CostGrid[NearbyIndexMinusY].Height;
+
+                    float dHeight_dx = float(height_x_plus - height_x_minus) / 2.0f;
+                    float dHeight_dy = float(height_y_plus - height_x_minus) / 2.0f;
+                    float d2Height_dx2 = float(height_x_plus - 2 * CostGrid[ia].Height + height_x_minus);
+                    float d2Height_dy2 = float(height_y_plus - 2 * CostGrid[ia].Height + height_y_minus);
+
+                    Eigen::Matrix2f Hessian;
+                    Hessian << d2Height_dx2, dHeight_dy, dHeight_dx, d2Height_dy2;
+
+                    Curvature = Dir.transpose() * Hessian * Dir;
+                    CurvatureCache[Hash] = Curvature;
+                }
 
                 float Cost = HeightCostFunc(float(std::abs(CostGrid[ia].Height - CostGrid[ib].Height))) + GradientCostFunc(float(std::abs(CostGrid[ia].Gradient - CostGrid[ib].Gradient))) + CurvatureCostFunc(float(std::abs(Curvature)));
                 return Cost;
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index fdfa1f76f4..d4ed4e5481 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -123,18 +123,18 @@ namespace roads {
                     if (std::abs<int32_t>(From[0] - To[0]) <= MaskK) {
                         NextPoint[0] = To[0];
                     } else if (From[0] < To[0]) {
-                        NextPoint[0] += std::max(1, MaskK / 2);
+                        NextPoint[0] += MaskK;
                     } else {
-                        NextPoint[0] -= std::max(1, MaskK / 2);
+                        NextPoint[0] -= MaskK;
                     }
                 }
                 if (To[1] != From[1]) {
                     if (std::abs<int32_t>(From[1] - To[1]) <= MaskK) {
                         NextPoint[1] = To[1];
                     } else if (From[1] < To[1]) {
-                        NextPoint[1] += std::max(1, MaskK / 2);
+                        NextPoint[1] += MaskK;
                     } else {
-                        NextPoint[1] -= std::max(1, MaskK / 2);
+                        NextPoint[1] -= MaskK;
                     }
                 }
 

From 763716ad070b79bf11352b8c15becfbdb4a4a04d Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Fri, 25 Aug 2023 17:27:08 +0800
Subject: [PATCH 23/50] feat: add angle mask

---
 projects/Roads/nodes/src/cost.cpp             | 81 ++++++++++---------
 projects/Roads/utilities/include/roads/data.h |  4 +
 projects/Roads/utilities/include/roads/grid.h | 74 ++++++++++-------
 3 files changed, 91 insertions(+), 68 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 9254fe6cda..0d336eedc9 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -160,7 +160,10 @@ namespace {
         //ZENO_DECLARE_INPUT_FIELD(CurvatureChannel, "Curvature Channel (Vertex Attr)", false, "", "curvature");
 
         int ConnectiveMask;
-        ZENO_DECLARE_INPUT_FIELD(ConnectiveMask, "Connective Mask", false, "", "3");
+        ZENO_DECLARE_INPUT_FIELD(ConnectiveMask, "Connective Mask", false, "", "4");
+
+        int AngleMask;
+        ZENO_DECLARE_INPUT_FIELD(AngleMask, "Angle Mask", false, "", "8");
 
         float WeightHeuristic;
         ZENO_DECLARE_INPUT_FIELD(WeightHeuristic, "Weight of Heuristic Function", false, "", "0.3");
@@ -201,12 +204,18 @@ namespace {
         //zeno::AttrVector<float> CurvatureList{};
         //ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Primitive, CurvatureList, CurvatureChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
 
+        //std::unordered_map<size_t, float> CurvatureCache;
+
         void apply() override {
-            zeno::log_info("[Roads] Generating trajectory...");
             RoadsAssert(AutoParameter->Nx * AutoParameter->Ny <= AutoParameter->GradientList.size(), "Bad nx ny.");
 
-            CostPoint GoalPoint{static_cast<size_t>(AutoParameter->Goal[0]), static_cast<size_t>(AutoParameter->Goal[1])};
-            CostPoint StartPoint{static_cast<size_t>(AutoParameter->Start[0]), static_cast<size_t>(AutoParameter->Start[1])};
+            CostPoint GoalPoint{static_cast<size_t>(AutoParameter->Goal[0]), static_cast<size_t>(AutoParameter->Goal[1]), 0};
+            CostPoint StartPoint{static_cast<size_t>(AutoParameter->Start[0]), static_cast<size_t>(AutoParameter->Start[1]), 0};
+
+            std::unordered_map<CostPoint, CostPoint> Predecessor;
+            std::unordered_map<CostPoint, float> CostMap;
+
+            size_t Nx = AutoParameter->Nx, Ny = AutoParameter->Ny;
 
             auto MapFuncGen = [](const std::shared_ptr<zeno::CurveObject> &Curve, float Threshold) -> std::function<float(float)> {
                 if (Curve) {
@@ -237,52 +246,48 @@ namespace {
             for (size_t i = 0; i < AutoParameter->Nx * AutoParameter->Ny; ++i) {
                 size_t x = i % AutoParameter->Nx;
                 size_t y = i / AutoParameter->Ny;
-                CostGrid[i] = (CostPoint{x, y, AutoParameter->PositionList[i].at(1), AutoParameter->GradientList[i]});
+                CostGrid[i] = (CostPoint{x, y, 0, AutoParameter->PositionList[i].at(1), AutoParameter->GradientList[i]});
             }
 
-            const size_t IndexMax = AutoParameter->Nx * AutoParameter->Ny - 1;
-            std::unordered_map<size_t, float> CurvatureCache;
+            auto CalcCurvature = [&CostGrid, Nx] (const CostPoint& A, const CostPoint& B, const CostPoint& C) -> float {
+                size_t ia = A[0] + A[1] * Nx;
+                size_t ib = B[0] + B[1] * Nx;
+                size_t ic = C[0] + C[1] * Nx;
+
+                Eigen::Vector4f BA = { float(A[0] - B[0]), float(A[1] - B[1]), float(CostGrid[ia].Height - CostGrid[ib].Height), float(A[2] - B[2]) };
+
+                Eigen::Vector4f BC = { float(C[0] - B[0]), float(C[1] - B[1]), float(CostGrid[ic].Height - CostGrid[ib].Height), float(C[2] - B[2]) };
+
+                float Magnitude_BC = BC.norm();
 
-            std::function<float(const CostPoint &, const CostPoint &)> CostFunc = [&CurvatureCache, &HeightCostFunc, &GradientCostFunc, &CurvatureCostFunc, &CostGrid, IndexMax, Nx = AutoParameter->Nx, Ny = AutoParameter->Ny](const CostPoint &A, const CostPoint &B) mutable -> float {
+                Eigen::Vector4f Change = BC.normalized() - BA.normalized();
+
+                float Magnitude_Change = Change.norm();
+
+                return Magnitude_Change / (Magnitude_BC * Magnitude_BC);
+            };
+
+            const size_t IndexMax = AutoParameter->Nx * AutoParameter->Ny - 1;
+            std::function<float(const CostPoint &, const CostPoint &)> CostFunc = [ &CalcCurvature, &HeightCostFunc, &GradientCostFunc, &CurvatureCostFunc, &CostGrid, &Predecessor, IndexMax, Nx, Ny](const CostPoint &A, const CostPoint &B) mutable -> float {
                 size_t ia = A[0] + A[1] * Nx;
                 size_t ib = B[0] + B[1] * Nx;
 
-                constexpr size_t Seed = 12306;
-                size_t Hash = (ia + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (ib * 0x9e3779b9 + (Seed << 2) + (Seed >> 4));
+                //constexpr size_t Seed = 12306;
+                //size_t Hash = (ia + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (ib * 0x9e3779b9 + (Seed << 2) + (Seed >> 4));
 
-                float Curvature;
-                if (CurvatureCache.find(Hash) != std::end(CurvatureCache)) {
-                    Curvature = CurvatureCache[Hash];
-                } else {
-                    Eigen::Vector2f Dir = (Eigen::Vector2f(A[0], A[1]) - Eigen::Vector2f(B[0], B[1])).normalized();
-                    size_t NearbyIndexPlus1 = std::clamp<size_t>(ia + 1, 0, IndexMax);
-                    size_t NearbyIndexMinus1 = std::clamp<size_t>(ia - 1, 0, IndexMax);
-                    size_t NearbyIndexPlusY = std::clamp<size_t>(ia + Ny, 0, IndexMax);
-                    size_t NearbyIndexMinusY = std::clamp<size_t>(ia - Ny, 0, IndexMax);
-                    double height_x_plus = CostGrid[NearbyIndexPlus1].Height;
-                    double height_x_minus = CostGrid[NearbyIndexMinus1].Height;
-                    double height_y_plus = CostGrid[NearbyIndexPlusY].Height;
-                    double height_y_minus = CostGrid[NearbyIndexMinusY].Height;
-
-                    float dHeight_dx = float(height_x_plus - height_x_minus) / 2.0f;
-                    float dHeight_dy = float(height_y_plus - height_x_minus) / 2.0f;
-                    float d2Height_dx2 = float(height_x_plus - 2 * CostGrid[ia].Height + height_x_minus);
-                    float d2Height_dy2 = float(height_y_plus - 2 * CostGrid[ia].Height + height_y_minus);
-
-                    Eigen::Matrix2f Hessian;
-                    Hessian << d2Height_dx2, dHeight_dy, dHeight_dx, d2Height_dy2;
-
-                    Curvature = Dir.transpose() * Hessian * Dir;
-                    CurvatureCache[Hash] = Curvature;
-                }
+                // We assume that A already searched and have a predecessor
+                const CostPoint& PrevPoint = Predecessor[A];
+
+                // Calc curvature
+                float Curvature = CalcCurvature(PrevPoint, A, B);
 
                 float Cost = HeightCostFunc(float(std::abs(CostGrid[ia].Height - CostGrid[ib].Height))) + GradientCostFunc(float(std::abs(CostGrid[ia].Gradient - CostGrid[ib].Gradient))) + CurvatureCostFunc(float(std::abs(Curvature)));
                 return Cost;
             };
 
-            std::unordered_map<CostPoint, CostPoint> Predecessor;
-            std::unordered_map<CostPoint, float> CostMap;
-            roads::energy::RoadsShortestPath(StartPoint, GoalPoint, CostPoint{static_cast<size_t>(AutoParameter->Nx), static_cast<size_t>(AutoParameter->Ny)}, AutoParameter->ConnectiveMask, AutoParameter->WeightHeuristic, Predecessor, CostMap, CostFunc);
+            zeno::log_info("[Roads] Generating trajectory...");
+
+            roads::energy::RoadsShortestPath(StartPoint, GoalPoint, CostPoint{static_cast<size_t>(AutoParameter->Nx), static_cast<size_t>(AutoParameter->Ny)}, AutoParameter->ConnectiveMask, AutoParameter->AngleMask, AutoParameter->WeightHeuristic, Predecessor, CostMap, CostFunc);
 
             zeno::log_info("[Roads] Result Predecessor Size: {}; CostMap Size: {}", Predecessor.size(), CostMap.size());
 
diff --git a/projects/Roads/utilities/include/roads/data.h b/projects/Roads/utilities/include/roads/data.h
index c62133ada1..3dcf5e8fc0 100644
--- a/projects/Roads/utilities/include/roads/data.h
+++ b/projects/Roads/utilities/include/roads/data.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include "Eigen/Eigen"
+#include "boost/math/constants/constants.hpp"
 #include <array>
 #include <cassert>
 #include <cmath>
@@ -81,4 +82,7 @@ namespace roads {
         using CustomGridBase<GridPointType>::CustomGridBase;
     };
 
+    constexpr double PI = boost::math::constants::pi<double>();
+    constexpr double PI2 = boost::math::constants::pi<double>() * 2.0;
+
 }// namespace roads
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index d4ed4e5481..b77fc03678 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -20,17 +20,18 @@ namespace roads {
 
     using HeightPoint = double;
     using SlopePoint = double;
-    struct CostPoint : std::array<size_t, 2> {
+    struct CostPoint : std::array<size_t, 3> {
         CostPoint() = default;
 
-        CostPoint(size_t x, size_t y, double InHeight = 0.0, double InGradient = 0.0/*, double InCurvature = 0.0**/)
-            : std::array<size_t, 2>(), Height(InHeight), Gradient(InGradient) {
+        CostPoint(size_t x, size_t y, size_t a = 0, double InHeight = 0.0, double InGradient = 0.0/*, double InCurvature = 0.0**/)
+            : std::array<size_t, 3>(), Height(InHeight), Gradient(InGradient) {
             at(0) = x;
             at(1) = y;
+            at(2) = a;
         }
 
         bool operator==(const CostPoint &Rhs) {
-            return at(0) == Rhs.at(0) && at(1) == Rhs.at(1);
+            return at(0) == Rhs.at(0) && at(1) == Rhs.at(1) && at(2) == Rhs.at(2);
         }
 
         double Height;
@@ -94,8 +95,8 @@ namespace roads {
         }
 
         template<typename PointType = IntPoint2D>
-        void RoadsShortestPath(const PointType &StartPoint, const PointType &GoalPoint, const PointType &Bounds, const int32_t MaskK, const float WeightHeuristic, std::unordered_map<PointType, PointType> &PredecessorList, std::unordered_map<PointType, float> &CostTo, const std::function<float(const PointType &, const PointType &)> &CostFunction) {
-            auto NewCostFunc = [CostFunction, GoalPoint, Bounds](const PointType &a, const PointType &b) -> float {
+        void RoadsShortestPath(const PointType &StartPoint, const PointType &GoalPoint, const PointType &Bounds, const int32_t MaskK, const int32_t MaskA, const float WeightHeuristic, std::unordered_map<PointType, PointType> &PredecessorList, std::unordered_map<PointType, float> &CostTo, const std::function<float(const PointType &, const PointType &)> &CostFunction) {
+            auto NewCostFunc = [&CostFunction, &GoalPoint, &Bounds](const PointType &a, const PointType &b) -> float {
                 float Result = CostFunction(a, b);
                 if (Result < 0) {
                     printf("[Roads] Minus cost P1(%d,%d) P2(%d,%d) Cost=%f.", a[0], a[1], b[0], b[1], Result);
@@ -106,13 +107,13 @@ namespace roads {
             };
 
             std::unordered_map<std::pair<PointType, PointType>, float> SimpleCost;
-            std::function<float(const PointType &, const PointType &, bool)> StraightCost = [NewCostFunc, MaskK, &SimpleCost, &StraightCost](const PointType &From, const PointType &To, bool bMoveX = true) mutable -> float {
+            std::function<float(const PointType &, const PointType &, bool)> StraightCost = [&NewCostFunc, MaskK, &SimpleCost, &StraightCost](const PointType &From, const PointType &To, bool bMoveX = true) mutable -> float {
                 auto CurrentPair = std::make_pair(From, To);
                 if (SimpleCost.count(CurrentPair)) {
                     return SimpleCost[CurrentPair];
                 }
 
-                if (From == To) {
+                if (From[0] == To[0] && From[1] == To[1]) {
                     SimpleCost[CurrentPair] = 0.0f;
                     return 0.0f;
                 }
@@ -138,6 +139,8 @@ namespace roads {
                     }
                 }
 
+                //printf("---- A(%d,%d) B(%d,%d)\n", NextPoint[0], NextPoint[0], To[1], To[1]);
+
                 float Cost = NewCostFunc(From, NextPoint) + StraightCost(NextPoint, To, !bMoveX);
                 SimpleCost[CurrentPair] = Cost;
                 return Cost;
@@ -162,12 +165,19 @@ namespace roads {
 
             // initialize a priority queue Q with the initial point StartPoint
             std::priority_queue<PointType, ArrayList<PointType>, decltype(Comparator)> Q(Comparator);
+            for (size_t a = 0; a < MaskA; a++) {
+                PointType NewPoint = StartPoint;
+                NewPoint[2] = a;
+                CostTo[StartPoint] = 0.f;
+            }
+            PredecessorList[StartPoint] = StartPoint;
             Q.push(StartPoint);
-            CostTo[StartPoint] = 0.f;
             for (size_t x = 0; x < Bounds[0]; x++) {
                 for (size_t y = 0; y < Bounds[1]; y++) {
-                    PointType Point{x, y};
-                    if (Point != StartPoint) { CostTo[Point] = std::numeric_limits<float>::max(); }
+                    for (size_t a = 0; a < MaskA; a++) {
+                        PointType Point{x, y, a};
+                        if (Point[0] != StartPoint[0] || Point[1] != StartPoint[1]) { CostTo[Point] = std::numeric_limits<float>::max(); }
+                    }
                 }
             }
 
@@ -191,23 +201,26 @@ namespace roads {
 
                 //printf("-- P(%d,%d) %f\n", Point[0], Point[1], CostTo[Point]);
 
-                // step3. for all points q ∈ M_k(p_ij)
-                for (int32_t dx = -MaskK; dx <= MaskK; ++dx) {
-                    for (int32_t dy = -MaskK; dy <= MaskK; ++dy) {
-                        if (GreatestCommonDivisor(std::abs(dx), std::abs(dy)) == 1) {
-                            PointType NeighbourPoint{Point[0] + dx, Point[1] + dy};
-                            if (NeighbourPoint[0] < 0 || NeighbourPoint[1] < 0 || NeighbourPoint[0] >= Bounds[0] || NeighbourPoint[1] >= Bounds[1]) continue;
-
-                            float NewCost = CostTo[Point] + NewCostFunc(Point, NeighbourPoint);
-                            if (NewCost < 0) {
-                                throw std::runtime_error("[Roads] Graph should not have negative weight. Check your curve !");
-                            }
-                            //printf("---- N(%d,%d) %f\n", NeighbourPoint[0], NeighbourPoint[1], NewCost);
-
-                            if (NewCost < CostTo[NeighbourPoint]) {
-                                PredecessorList[NeighbourPoint] = Point;
-                                CostTo[NeighbourPoint] = NewCost;
-                                Q.push(NeighbourPoint);
+                // extended mask for angle
+                for (size_t angle = 0; angle < MaskA; ++angle) {
+                    // step3. for all points q ∈ M_k(p_ij)
+                    for (int32_t dx = -MaskK; dx <= MaskK; ++dx) {
+                        for (int32_t dy = -MaskK; dy <= MaskK; ++dy) {
+                            if (GreatestCommonDivisor(std::abs(dx), std::abs(dy)) == 1) {
+                                PointType NeighbourPoint{Point[0] + dx, Point[1] + dy, angle};
+                                if (NeighbourPoint[0] < 0 || NeighbourPoint[1] < 0 || NeighbourPoint[0] >= Bounds[0] || NeighbourPoint[1] >= Bounds[1]) continue;
+
+                                float NewCost = CostTo[Point] + NewCostFunc(Point, NeighbourPoint);
+                                if (NewCost < 0) {
+                                    throw std::runtime_error("[Roads] Graph should not have negative weight. Check your curve !");
+                                }
+                                //printf("---- N(%d,%d) %f\n", NeighbourPoint[0], NeighbourPoint[1], NewCost);
+
+                                if (NewCost < CostTo[NeighbourPoint]) {
+                                    PredecessorList[NeighbourPoint] = Point;
+                                    CostTo[NeighbourPoint] = NewCost;
+                                    Q.push(NeighbourPoint);
+                                }
                             }
                         }
                     }
@@ -224,7 +237,7 @@ namespace std {
     struct hash<roads::CostPoint> {
         size_t operator()(const roads::CostPoint &rhs) const {
             constexpr size_t Seed = 10086;
-            return (rhs[0] + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (rhs[1] + 0x9e3779b9 + (Seed << 6) + (Seed >> 2));
+            return (rhs[0] + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (rhs[1] + 0x9e3779b9 + (Seed << 6) + (Seed >> 4)) ^ (rhs[2] + 0x9e3779b9 + (Seed << 8) + (Seed >> 6));
         }
     };
 
@@ -232,7 +245,8 @@ namespace std {
     struct hash<std::pair<roads::CostPoint, roads::CostPoint>> {
         size_t operator()(const std::pair<roads::CostPoint, roads::CostPoint> &rhs) const {
             constexpr size_t Seed = 10086;
-            return (rhs.first[0] + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (rhs.first[1] + 0x9e3779b9 + (Seed << 6) + (Seed >> 2)) ^ (rhs.second[0] + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (rhs.second[1] + 0x9e3779b9 + (Seed << 6) + (Seed >> 2));
+            auto h = hash<roads::CostPoint>();
+            return (h(rhs.first) + (Seed << 4) + (Seed >> 2)) ^ (h(rhs.second) + (Seed << 6) + (Seed >> 4));
         }
     };
 }// namespace std

From f0939b12fad45f6c1c61901d988045095f0e87df Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Mon, 28 Aug 2023 11:20:54 +0800
Subject: [PATCH 24/50] feat: remove map init to make it run faster

---
 projects/Roads/nodes/src/cost.cpp             | 28 +++++++++----------
 projects/Roads/utilities/include/roads/data.h | 15 ++++++++++
 projects/Roads/utilities/include/roads/grid.h | 21 ++++++--------
 projects/Roads/utilities/include/roads/pch.h  | 13 +++++++++
 4 files changed, 49 insertions(+), 28 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 0d336eedc9..ba6d7f33d9 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -212,8 +212,8 @@ namespace {
             CostPoint GoalPoint{static_cast<size_t>(AutoParameter->Goal[0]), static_cast<size_t>(AutoParameter->Goal[1]), 0};
             CostPoint StartPoint{static_cast<size_t>(AutoParameter->Start[0]), static_cast<size_t>(AutoParameter->Start[1]), 0};
 
-            std::unordered_map<CostPoint, CostPoint> Predecessor;
-            std::unordered_map<CostPoint, float> CostMap;
+            DefaultedHashMap<CostPoint, CostPoint> Predecessor;
+            DefaultedHashMap<CostPoint, float> CostMap;
 
             size_t Nx = AutoParameter->Nx, Ny = AutoParameter->Ny;
 
@@ -249,26 +249,24 @@ namespace {
                 CostGrid[i] = (CostPoint{x, y, 0, AutoParameter->PositionList[i].at(1), AutoParameter->GradientList[i]});
             }
 
-            auto CalcCurvature = [&CostGrid, Nx] (const CostPoint& A, const CostPoint& B, const CostPoint& C) -> float {
-                size_t ia = A[0] + A[1] * Nx;
-                size_t ib = B[0] + B[1] * Nx;
-                size_t ic = C[0] + C[1] * Nx;
-
-                Eigen::Vector4f BA = { float(A[0] - B[0]), float(A[1] - B[1]), float(CostGrid[ia].Height - CostGrid[ib].Height), float(A[2] - B[2]) };
-
-                Eigen::Vector4f BC = { float(C[0] - B[0]), float(C[1] - B[1]), float(CostGrid[ic].Height - CostGrid[ib].Height), float(C[2] - B[2]) };
+            auto CalcCurvature = [&CostGrid, Nx](const CostPoint &A, const CostPoint &B, const CostPoint &C) -> float {
+                float Height_A = float(CostGrid[A[0] + A[1] * Nx].Height);
+                float Height_B = float(CostGrid[B[0] + B[1] * Nx].Height);
+                float Height_C = float(CostGrid[C[0] + C[1] * Nx].Height);
 
+                Eigen::Vector4f BA = {float(A[0] - B[0]), float(A[1] - B[1]), float(Height_A - Height_B), float(A[2] - B[2])};
+                Eigen::Vector4f BC = {float(C[0] - B[0]), float(C[1] - B[1]), float(Height_C - Height_B), float(C[2] - B[2])};
                 float Magnitude_BC = BC.norm();
 
-                Eigen::Vector4f Change = BC.normalized() - BA.normalized();
-
-                float Magnitude_Change = Change.norm();
+                Eigen::Vector4f BA_Normalized = BA.normalized();
+                Eigen::Vector4f BC_Normalized = BC.normalized();
 
+                float Magnitude_Change = (BC_Normalized - BA_Normalized).norm();
                 return Magnitude_Change / (Magnitude_BC * Magnitude_BC);
             };
 
             const size_t IndexMax = AutoParameter->Nx * AutoParameter->Ny - 1;
-            std::function<float(const CostPoint &, const CostPoint &)> CostFunc = [ &CalcCurvature, &HeightCostFunc, &GradientCostFunc, &CurvatureCostFunc, &CostGrid, &Predecessor, IndexMax, Nx, Ny](const CostPoint &A, const CostPoint &B) mutable -> float {
+            std::function<float(const CostPoint &, const CostPoint &)> CostFunc = [&CalcCurvature, &HeightCostFunc, &GradientCostFunc, &CurvatureCostFunc, &CostGrid, &Predecessor, IndexMax, Nx, Ny](const CostPoint &A, const CostPoint &B) mutable -> float {
                 size_t ia = A[0] + A[1] * Nx;
                 size_t ib = B[0] + B[1] * Nx;
 
@@ -276,7 +274,7 @@ namespace {
                 //size_t Hash = (ia + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (ib * 0x9e3779b9 + (Seed << 2) + (Seed >> 4));
 
                 // We assume that A already searched and have a predecessor
-                const CostPoint& PrevPoint = Predecessor[A];
+                const CostPoint &PrevPoint = Predecessor[A];
 
                 // Calc curvature
                 float Curvature = CalcCurvature(PrevPoint, A, B);
diff --git a/projects/Roads/utilities/include/roads/data.h b/projects/Roads/utilities/include/roads/data.h
index 3dcf5e8fc0..0533ec7136 100644
--- a/projects/Roads/utilities/include/roads/data.h
+++ b/projects/Roads/utilities/include/roads/data.h
@@ -7,6 +7,7 @@
 #include <cmath>
 #include <vector>
 #include <functional>
+#include <unordered_map>
 
 namespace roads {
 
@@ -85,4 +86,18 @@ namespace roads {
     constexpr double PI = boost::math::constants::pi<double>();
     constexpr double PI2 = boost::math::constants::pi<double>() * 2.0;
 
+    template <typename KeyType, typename ValueType>
+    struct DefaultedHashMap : public std::unordered_map<KeyType, ValueType> {
+        using std::unordered_map<KeyType, ValueType>::unordered_map;
+
+        ValueType& DefaultAt(const KeyType& InKey, const ValueType& DefaultValue = ValueType()) {
+            const auto Target = this->_Find_last(InKey, this->_Traitsobj(InKey));
+            if (Target._Duplicate) {
+                return Target._Duplicate->_Myval.second;
+            }
+
+            return const_cast<ValueType&>(DefaultValue);
+        }
+    };
+
 }// namespace roads
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index b77fc03678..33af26e88f 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -95,7 +95,7 @@ namespace roads {
         }
 
         template<typename PointType = IntPoint2D>
-        void RoadsShortestPath(const PointType &StartPoint, const PointType &GoalPoint, const PointType &Bounds, const int32_t MaskK, const int32_t MaskA, const float WeightHeuristic, std::unordered_map<PointType, PointType> &PredecessorList, std::unordered_map<PointType, float> &CostTo, const std::function<float(const PointType &, const PointType &)> &CostFunction) {
+        void RoadsShortestPath(const PointType &StartPoint, const PointType &GoalPoint, const PointType &Bounds, const int32_t MaskK, const int32_t MaskA, const float WeightHeuristic, DefaultedHashMap<PointType, PointType> &PredecessorList, DefaultedHashMap<PointType, float> &CostTo, const std::function<float(const PointType &, const PointType &)> &CostFunction) {
             auto NewCostFunc = [&CostFunction, &GoalPoint, &Bounds](const PointType &a, const PointType &b) -> float {
                 float Result = CostFunction(a, b);
                 if (Result < 0) {
@@ -158,28 +158,22 @@ namespace roads {
                 float DeltaA = StraightCost(Point1, GoalPoint, true);
                 float DeltaB = StraightCost(Point2, GoalPoint, true);
 
-                float CostA = CostTo[Point1] + WeightHeuristic * DeltaA;
-                float CostB = CostTo[Point2] + WeightHeuristic * DeltaB;
+                float CostA = CostTo.DefaultAt(Point1, std::numeric_limits<float>::max()) + WeightHeuristic * DeltaA;
+                float CostB = CostTo.DefaultAt(Point2, std::numeric_limits<float>::max()) + WeightHeuristic * DeltaB;
                 return CostA > CostB;
             };
 
             // initialize a priority queue Q with the initial point StartPoint
+            CostTo.reserve(Bounds[0]*Bounds[1]*MaskA);
             std::priority_queue<PointType, ArrayList<PointType>, decltype(Comparator)> Q(Comparator);
             for (size_t a = 0; a < MaskA; a++) {
                 PointType NewPoint = StartPoint;
                 NewPoint[2] = a;
                 CostTo[StartPoint] = 0.f;
             }
+
             PredecessorList[StartPoint] = StartPoint;
             Q.push(StartPoint);
-            for (size_t x = 0; x < Bounds[0]; x++) {
-                for (size_t y = 0; y < Bounds[1]; y++) {
-                    for (size_t a = 0; a < MaskA; a++) {
-                        PointType Point{x, y, a};
-                        if (Point[0] != StartPoint[0] || Point[1] != StartPoint[1]) { CostTo[Point] = std::numeric_limits<float>::max(); }
-                    }
-                }
-            }
 
             std::unordered_map<PointType, bool> Visual;
 
@@ -216,7 +210,7 @@ namespace roads {
                                 }
                                 //printf("---- N(%d,%d) %f\n", NeighbourPoint[0], NeighbourPoint[1], NewCost);
 
-                                if (NewCost < CostTo[NeighbourPoint]) {
+                                if (NewCost < CostTo.DefaultAt(NeighbourPoint, std::numeric_limits<float>::max())) {
                                     PredecessorList[NeighbourPoint] = Point;
                                     CostTo[NeighbourPoint] = NewCost;
                                     Q.push(NeighbourPoint);
@@ -237,7 +231,8 @@ namespace std {
     struct hash<roads::CostPoint> {
         size_t operator()(const roads::CostPoint &rhs) const {
             constexpr size_t Seed = 10086;
-            return (rhs[0] + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (rhs[1] + 0x9e3779b9 + (Seed << 6) + (Seed >> 4)) ^ (rhs[2] + 0x9e3779b9 + (Seed << 8) + (Seed >> 6));
+            size_t Result = (rhs[0] + 0x9e3779b9 + (Seed << 4) + (Seed >> 2)) ^ (rhs[1] + 0x9e3779b9 + (Seed << 6) + (Seed >> 4)) ^ (rhs[2] + 0x9e3779b9 + (Seed << 8) + (Seed >> 6));
+            return Result;
         }
     };
 
diff --git a/projects/Roads/utilities/include/roads/pch.h b/projects/Roads/utilities/include/roads/pch.h
index 062c4b1cb6..77936d106d 100644
--- a/projects/Roads/utilities/include/roads/pch.h
+++ b/projects/Roads/utilities/include/roads/pch.h
@@ -4,3 +4,16 @@
 #define ROADS_INLINE inline
 
 #include "data.h"
+#include <ctime>
+
+#define ROADS_TIMING_PRE_GENERATED \
+    clock_t start; clock_t stop;
+
+#define ROADS_TIMING_BLOCK(a, MSG) \
+    start = clock(); \
+    a; \
+    stop = clock(); \
+    printf("%s Elapsed: %f seconds\n", MSG, (double)(stop - start) / CLOCKS_PER_SEC);
+
+#define ROADS_TIMING_START start = clock();
+#define ROADS_TIMING_END(MSG) stop = clock(); printf("%s Elapsed: %f seconds\n", MSG, (double)(stop - start) / CLOCKS_PER_SEC);

From ee41c4ff7d3ff89ed345a979fdac094de8271497 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Mon, 28 Aug 2023 14:19:00 +0800
Subject: [PATCH 25/50] fix: using more efficient default value

---
 projects/Roads/nodes/src/cost.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index ba6d7f33d9..67afa27f94 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -163,13 +163,13 @@ namespace {
         ZENO_DECLARE_INPUT_FIELD(ConnectiveMask, "Connective Mask", false, "", "4");
 
         int AngleMask;
-        ZENO_DECLARE_INPUT_FIELD(AngleMask, "Angle Mask", false, "", "8");
+        ZENO_DECLARE_INPUT_FIELD(AngleMask, "Angle Mask", false, "", "4");
 
         float WeightHeuristic;
-        ZENO_DECLARE_INPUT_FIELD(WeightHeuristic, "Weight of Heuristic Function", false, "", "0.3");
+        ZENO_DECLARE_INPUT_FIELD(WeightHeuristic, "Weight of Heuristic Function", false, "", "1.0");
 
         float CurvatureThreshold;
-        ZENO_DECLARE_INPUT_FIELD(CurvatureThreshold, "Curvature Threshold", false, "", "0.4");
+        ZENO_DECLARE_INPUT_FIELD(CurvatureThreshold, "Curvature Threshold", false, "", "0.2");
 
         zeno::vec2f Start;
         ZENO_DECLARE_INPUT_FIELD(Start, "Start Point");
@@ -262,7 +262,7 @@ namespace {
                 Eigen::Vector4f BC_Normalized = BC.normalized();
 
                 float Magnitude_Change = (BC_Normalized - BA_Normalized).norm();
-                return Magnitude_Change / (Magnitude_BC * Magnitude_BC);
+                return Magnitude_Change / (Magnitude_BC * Magnitude_BC) * BC.z();
             };
 
             const size_t IndexMax = AutoParameter->Nx * AutoParameter->Ny - 1;

From 3e310ad309d0d77299c168916fd1971c84202573 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Mon, 28 Aug 2023 14:34:45 +0800
Subject: [PATCH 26/50] fix: clear clion analyser error

---
 zeno/include/zeno/utils/PropertyVisitor.h | 2 --
 1 file changed, 2 deletions(-)

diff --git a/zeno/include/zeno/utils/PropertyVisitor.h b/zeno/include/zeno/utils/PropertyVisitor.h
index ec83ed2d08..c8d747ed1d 100644
--- a/zeno/include/zeno/utils/PropertyVisitor.h
+++ b/zeno/include/zeno/utils/PropertyVisitor.h
@@ -282,8 +282,6 @@ namespace zeno {
 
         template<typename ParentType, typename InputType, size_t Hash>
         struct OutputField : public Field<ParentType, InputType, Hash> {
-            using Field<ParentType, InputType, Hash>::Parent;
-            using Field<ParentType, InputType, Hash>::Type;
             using Field<ParentType, InputType, Hash>::KeyName;
             using Field<ParentType, InputType, Hash>::ValueRef;
             using Field<ParentType, InputType, Hash>::DefaultValue;

From 60eba36562a1976ee41f2b5b3d172b731b721e36 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Mon, 28 Aug 2023 15:06:47 +0800
Subject: [PATCH 27/50] chore: add nanospline

---
 .gitmodules                             | 3 +++
 projects/Roads/CMakeLists.txt           | 2 ++
 projects/Roads/libraries/nanospline     | 1 +
 projects/Roads/utilities/CMakeLists.txt | 1 +
 4 files changed, 7 insertions(+)
 create mode 160000 projects/Roads/libraries/nanospline

diff --git a/.gitmodules b/.gitmodules
index c5394279b2..98c50b6800 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -25,3 +25,6 @@
 [submodule "projects/python/CPython"]
 	path = projects/Python/CPython
 	url = https://github.com/zenustech/python-cmake-buildsystem.git
+[submodule "projects/Roads/libraries/nanospline"]
+	path = projects/Roads/libraries/nanospline
+	url = https://github.com/qnzhou/nanospline.git
diff --git a/projects/Roads/CMakeLists.txt b/projects/Roads/CMakeLists.txt
index d16b0363cd..319ffe043c 100644
--- a/projects/Roads/CMakeLists.txt
+++ b/projects/Roads/CMakeLists.txt
@@ -5,6 +5,8 @@ IF (NOT ZENO_WITH_RPC)
     MESSAGE(FATAL_ERROR "You must enable RPC to use road system.")
 ENDIF()
 
+ADD_SUBDIRECTORY(libraries/nanospline)
+
 ADD_SUBDIRECTORY(utilities)
 ADD_SUBDIRECTORY(service)
 ADD_SUBDIRECTORY(nodes)
diff --git a/projects/Roads/libraries/nanospline b/projects/Roads/libraries/nanospline
new file mode 160000
index 0000000000..660e3db75d
--- /dev/null
+++ b/projects/Roads/libraries/nanospline
@@ -0,0 +1 @@
+Subproject commit 660e3db75d8faa43f201f7638e9bd198bd5237d5
diff --git a/projects/Roads/utilities/CMakeLists.txt b/projects/Roads/utilities/CMakeLists.txt
index b46fef4552..000e63bd18 100644
--- a/projects/Roads/utilities/CMakeLists.txt
+++ b/projects/Roads/utilities/CMakeLists.txt
@@ -42,3 +42,4 @@ ELSE()
 ENDIF()
 
 TARGET_LINK_LIBRARIES(Roads PRIVATE TBB::tbb TBB::tbbmalloc)
+TARGET_LINK_LIBRARIES(Roads PRIVATE nanospline::nanospline)

From 20a1a7c184dcedc2924f03f41276265b49e4cf34 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Tue, 29 Aug 2023 15:01:38 +0800
Subject: [PATCH 28/50] chore: remove nanospline

---
 .gitmodules                                   |  3 -
 projects/Roads/CMakeLists.txt                 |  2 -
 projects/Roads/libraries/nanospline           |  1 -
 projects/Roads/nodes/src/cost.cpp             | 60 +++++++++++++++++--
 projects/Roads/utilities/CMakeLists.txt       |  1 -
 projects/Roads/utilities/include/roads/grid.h |  3 +
 projects/Roads/utilities/include/roads/pch.h  | 19 ++++--
 7 files changed, 74 insertions(+), 15 deletions(-)
 delete mode 160000 projects/Roads/libraries/nanospline

diff --git a/.gitmodules b/.gitmodules
index 98c50b6800..c5394279b2 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -25,6 +25,3 @@
 [submodule "projects/python/CPython"]
 	path = projects/Python/CPython
 	url = https://github.com/zenustech/python-cmake-buildsystem.git
-[submodule "projects/Roads/libraries/nanospline"]
-	path = projects/Roads/libraries/nanospline
-	url = https://github.com/qnzhou/nanospline.git
diff --git a/projects/Roads/CMakeLists.txt b/projects/Roads/CMakeLists.txt
index 319ffe043c..d16b0363cd 100644
--- a/projects/Roads/CMakeLists.txt
+++ b/projects/Roads/CMakeLists.txt
@@ -5,8 +5,6 @@ IF (NOT ZENO_WITH_RPC)
     MESSAGE(FATAL_ERROR "You must enable RPC to use road system.")
 ENDIF()
 
-ADD_SUBDIRECTORY(libraries/nanospline)
-
 ADD_SUBDIRECTORY(utilities)
 ADD_SUBDIRECTORY(service)
 ADD_SUBDIRECTORY(nodes)
diff --git a/projects/Roads/libraries/nanospline b/projects/Roads/libraries/nanospline
deleted file mode 160000
index 660e3db75d..0000000000
--- a/projects/Roads/libraries/nanospline
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 660e3db75d8faa43f201f7638e9bd198bd5237d5
diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 67afa27f94..e8db72dbb3 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -285,7 +285,9 @@ namespace {
 
             zeno::log_info("[Roads] Generating trajectory...");
 
-            roads::energy::RoadsShortestPath(StartPoint, GoalPoint, CostPoint{static_cast<size_t>(AutoParameter->Nx), static_cast<size_t>(AutoParameter->Ny)}, AutoParameter->ConnectiveMask, AutoParameter->AngleMask, AutoParameter->WeightHeuristic, Predecessor, CostMap, CostFunc);
+            ROADS_TIMING_PRE_GENERATED;
+
+            ROADS_TIMING_BLOCK("AStar Extended", roads::energy::RoadsShortestPath(StartPoint, GoalPoint, CostPoint{static_cast<size_t>(AutoParameter->Nx), static_cast<size_t>(AutoParameter->Ny)}, AutoParameter->ConnectiveMask, AutoParameter->AngleMask, AutoParameter->WeightHeuristic, Predecessor, CostMap, CostFunc));
 
             zeno::log_info("[Roads] Result Predecessor Size: {}; CostMap Size: {}", Predecessor.size(), CostMap.size());
 
@@ -316,13 +318,13 @@ namespace {
         ZENO_DECLARE_INPUT_FIELD(Primitive, "Prim");
         ZENO_DECLARE_OUTPUT_FIELD(Primitive, "Prim");
 
-        bool bShouldSmooth;
+        bool bShouldSmooth = false;
         ZENO_DECLARE_INPUT_FIELD(bShouldSmooth, "Enable Smooth", false, "", "false");
 
-        float DeltaAltitudeThreshold;
+        float DeltaAltitudeThreshold = 1e-06;
         ZENO_DECLARE_INPUT_FIELD(DeltaAltitudeThreshold, "Delta Altitude Threshold", false, "", "1e-06");
 
-        float HeuristicRatio;
+        float HeuristicRatio = 0.3;
         ZENO_DECLARE_INPUT_FIELD(HeuristicRatio, "Heuristic Ratio (0 - 1)", false, "", "0.3");
 
         std::string SizeXChannel;
@@ -410,4 +412,54 @@ namespace {
             }
         }
     };
+
+    struct ZENO_CRTP(PrimLineClothoidSmooth, zeno::reflect::IParameterAutoNode) {
+        ZENO_GENERATE_NODE_BODY(PrimLineClothoidSmooth);
+
+        std::shared_ptr<zeno::PrimitiveObject> Primitive;
+        ZENO_DECLARE_INPUT_FIELD(Primitive, "Prim");
+        ZENO_DECLARE_OUTPUT_FIELD(Primitive, "Prim");
+
+        int SampleNum = 1;
+        ZENO_DECLARE_INPUT_FIELD(SampleNum, "Sample Points", false, "", "1000");
+
+        void apply() override {
+            auto& Prim = AutoParameter->Primitive;
+
+            ROADS_TIMING_PRE_GENERATED;
+            //ROADS_TIMING_BLOCK("Generate BSpline List", auto Splines = spline::GenerateBSplineWithSegment(Prim->verts, Prim->lines));
+            //ROADS_TIMING_BLOCK("Concat Splines", auto Spline = spline::Concatenate(Splines));
+            //zeno::log_info("Degree {}", Splines[0].get_degree());
+            //ROADS_TIMING_BLOCK("Resample Lines", auto Lines = spline::SamplePointsFromSpline(Splines, AutoParameter->SampleNum));
+            auto Lines = ArrayList<Eigen::Vector3f>{};
+
+            Prim = std::make_shared<zeno::PrimitiveObject>();
+
+            Prim->verts.reserve(Lines.size());
+            for (const auto& line : Lines) {
+                Prim->verts.emplace_back(line.x(), line.y(), line.z());
+            }
+
+            Prim->lines.resize(Lines.size() - 1);
+            for (int i = 0; i < Lines.size() - 1; i++) {
+                Prim->lines[i] = zeno::vec2i { i, i + 1 };
+            }
+
+            zeno::log_info("[Roads] Vertices Num: {}, Lines Num: {}", Prim->verts.size(), Prim->lines.size());
+        }
+    };
+
+#if _MSC_VER
+#include "Windows.h"
+    // Windows Debug
+    struct ZENO_CRTP(WindowsWaitForDebugger, zeno::reflect::IParameterAutoNode) {
+        ZENO_GENERATE_NODE_BODY(WindowsWaitForDebugger);
+
+        void apply() override {
+            while (!IsDebuggerPresent()) {
+                ;
+            }
+        }
+    };
+#endif
 }// namespace
diff --git a/projects/Roads/utilities/CMakeLists.txt b/projects/Roads/utilities/CMakeLists.txt
index 000e63bd18..b46fef4552 100644
--- a/projects/Roads/utilities/CMakeLists.txt
+++ b/projects/Roads/utilities/CMakeLists.txt
@@ -42,4 +42,3 @@ ELSE()
 ENDIF()
 
 TARGET_LINK_LIBRARIES(Roads PRIVATE TBB::tbb TBB::tbbmalloc)
-TARGET_LINK_LIBRARIES(Roads PRIVATE nanospline::nanospline)
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index 33af26e88f..afbc7f6d2f 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -223,6 +223,9 @@ namespace roads {
         }
     }// namespace energy
 
+    namespace spline {
+    }
+
 }// namespace roads
 
 
diff --git a/projects/Roads/utilities/include/roads/pch.h b/projects/Roads/utilities/include/roads/pch.h
index 77936d106d..5d59105c37 100644
--- a/projects/Roads/utilities/include/roads/pch.h
+++ b/projects/Roads/utilities/include/roads/pch.h
@@ -6,14 +6,25 @@
 #include "data.h"
 #include <ctime>
 
+#ifndef NDEBUG
+
 #define ROADS_TIMING_PRE_GENERATED \
     clock_t start; clock_t stop;
 
-#define ROADS_TIMING_BLOCK(a, MSG) \
+#define ROADS_TIMING_BLOCK(MSG, BLOCK) \
     start = clock(); \
-    a; \
+    BLOCK; \
     stop = clock(); \
-    printf("%s Elapsed: %f seconds\n", MSG, (double)(stop - start) / CLOCKS_PER_SEC);
+    printf("[Roads] %s Elapsed: %f seconds\n", MSG, (double)(stop - start) / CLOCKS_PER_SEC);
 
 #define ROADS_TIMING_START start = clock();
-#define ROADS_TIMING_END(MSG) stop = clock(); printf("%s Elapsed: %f seconds\n", MSG, (double)(stop - start) / CLOCKS_PER_SEC);
+#define ROADS_TIMING_END(MSG) stop = clock(); printf("[Roads] %s Elapsed: %f seconds\n", MSG, (double)(stop - start) / CLOCKS_PER_SEC);
+
+#else
+
+#define ROADS_TIMING_PRE_GENERATED
+#define ROADS_TIMING_BLOCK(MSG, BLOCK) BLOCK;
+#define ROADS_TIMING_START
+#define ROADS_TIMING_END(MSG)
+
+#endif

From ed2be87c496e1cb05b137cc1b756c98fdbb6ac9a Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Tue, 29 Aug 2023 16:48:08 +0800
Subject: [PATCH 29/50] feat: add smooth path node

---
 projects/Roads/README.md                      |   11 +
 projects/Roads/nodes/src/cost.cpp             |   19 +-
 projects/Roads/utilities/CMakeLists.txt       |    9 +-
 projects/Roads/utilities/include/roads/grid.h |   17 +-
 .../include/roads/thirdparty/parson.h         |  234 ++
 .../include/roads/thirdparty/tinyspline.h     | 3117 +++++++++++++++
 .../include/roads/thirdparty/tinysplinecxx.h  |  966 +++++
 projects/Roads/utilities/src/grid.cpp         |   34 +
 .../Roads/utilities/src/thirdparty/parson.c   | 2079 ++++++++++
 .../utilities/src/thirdparty/tinyspline.c     | 3480 +++++++++++++++++
 .../src/thirdparty/tinysplinecxx.cxx          | 1744 +++++++++
 11 files changed, 11695 insertions(+), 15 deletions(-)
 create mode 100644 projects/Roads/README.md
 create mode 100644 projects/Roads/utilities/include/roads/thirdparty/parson.h
 create mode 100644 projects/Roads/utilities/include/roads/thirdparty/tinyspline.h
 create mode 100644 projects/Roads/utilities/include/roads/thirdparty/tinysplinecxx.h
 create mode 100644 projects/Roads/utilities/src/thirdparty/parson.c
 create mode 100644 projects/Roads/utilities/src/thirdparty/tinyspline.c
 create mode 100644 projects/Roads/utilities/src/thirdparty/tinysplinecxx.cxx

diff --git a/projects/Roads/README.md b/projects/Roads/README.md
new file mode 100644
index 0000000000..dd3cec2ffa
--- /dev/null
+++ b/projects/Roads/README.md
@@ -0,0 +1,11 @@
+
+# Required Libraries
+
+1. `tinyspline`
+2. `boost`
+3. `Eigen`
+
+**For Windows**
+```powershell
+./vcpkg install boost:x64-windows Eigen:x64-windows tinyspline:x64-windows
+```
diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index e8db72dbb3..1446356855 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -426,22 +426,23 @@ namespace {
         void apply() override {
             auto& Prim = AutoParameter->Primitive;
 
+            ArrayList<std::array<float, 3>> Vertices(Prim->verts.begin(), Prim->verts.end());
+            ArrayList<std::array<int, 2>> Lines(Prim->lines.begin(), Prim->lines.end());
+
             ROADS_TIMING_PRE_GENERATED;
-            //ROADS_TIMING_BLOCK("Generate BSpline List", auto Splines = spline::GenerateBSplineWithSegment(Prim->verts, Prim->lines));
-            //ROADS_TIMING_BLOCK("Concat Splines", auto Spline = spline::Concatenate(Splines));
-            //zeno::log_info("Degree {}", Splines[0].get_degree());
-            //ROADS_TIMING_BLOCK("Resample Lines", auto Lines = spline::SamplePointsFromSpline(Splines, AutoParameter->SampleNum));
-            auto Lines = ArrayList<Eigen::Vector3f>{};
+            ROADS_TIMING_BLOCK("Resample segments", auto Result = spline::SmoothAndResampleSegments(Vertices, Lines, AutoParameter->SampleNum));
+
+            //auto Lines = ArrayList<Eigen::Vector3f>{};
 
             Prim = std::make_shared<zeno::PrimitiveObject>();
 
-            Prim->verts.reserve(Lines.size());
-            for (const auto& line : Lines) {
+            Prim->verts.reserve(Result.size());
+            for (const auto& line : Result) {
                 Prim->verts.emplace_back(line.x(), line.y(), line.z());
             }
 
-            Prim->lines.resize(Lines.size() - 1);
-            for (int i = 0; i < Lines.size() - 1; i++) {
+            Prim->lines.resize(Result.size() - 1);
+            for (int i = 0; i < Result.size() - 1; i++) {
                 Prim->lines[i] = zeno::vec2i { i, i + 1 };
             }
 
diff --git a/projects/Roads/utilities/CMakeLists.txt b/projects/Roads/utilities/CMakeLists.txt
index b46fef4552..672b920006 100644
--- a/projects/Roads/utilities/CMakeLists.txt
+++ b/projects/Roads/utilities/CMakeLists.txt
@@ -3,9 +3,9 @@ SET(Boost_ADDITIONAL_VERSIONS "1.82.0" "1.82")
 FIND_PACKAGE(Eigen3 REQUIRED NO_MODULE)
 FIND_PACKAGE(OpenMP REQUIRED)
 FIND_PACKAGE(Boost COMPONENTS graph)
-find_package(TBB CONFIG REQUIRED COMPONENTS tbb tbbmalloc)
+#find_package(TBB CONFIG REQUIRED COMPONENTS tbb tbbmalloc)
 
-FILE(GLOB_RECURSE SOURCE_FILES src/*.cpp src/*.h)
+FILE(GLOB_RECURSE SOURCE_FILES src/*.cpp src/*.h src/thirdparty/parson.c src/thirdparty/tinyspline.c src/thirdparty/tinysplinecxx.cxx)
 
 ADD_LIBRARY(Roads STATIC ${SOURCE_FILES})
 
@@ -41,4 +41,7 @@ ELSE()
     MESSAGE(FATAL_ERROR "Roads Module is depending on Boost::graph. You have to install it first.")
 ENDIF()
 
-TARGET_LINK_LIBRARIES(Roads PRIVATE TBB::tbb TBB::tbbmalloc)
+#TARGET_LINK_LIBRARIES(Roads PRIVATE TBB::tbb TBB::tbbmalloc)
+
+ADD_COMPILE_DEFINITIONS(TINYSPLINE_EXPORT=1)
+ADD_COMPILE_DEFINITIONS(TINYSPLINE_FLOAT_PRECISION=1)
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index afbc7f6d2f..65e49b7391 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -11,6 +11,10 @@
 #include "boost/graph/graph_concepts.hpp"
 #include "boost/graph/graph_traits.hpp"
 
+namespace tinyspline {
+    class BSpline;
+}
+
 namespace roads {
 
     struct AdvancePoint {
@@ -23,7 +27,7 @@ namespace roads {
     struct CostPoint : std::array<size_t, 3> {
         CostPoint() = default;
 
-        CostPoint(size_t x, size_t y, size_t a = 0, double InHeight = 0.0, double InGradient = 0.0/*, double InCurvature = 0.0**/)
+        CostPoint(size_t x, size_t y, size_t a = 0, double InHeight = 0.0, double InGradient = 0.0 /*, double InCurvature = 0.0**/)
             : std::array<size_t, 3>(), Height(InHeight), Gradient(InGradient) {
             at(0) = x;
             at(1) = y;
@@ -164,7 +168,7 @@ namespace roads {
             };
 
             // initialize a priority queue Q with the initial point StartPoint
-            CostTo.reserve(Bounds[0]*Bounds[1]*MaskA);
+            CostTo.reserve(Bounds[0] * Bounds[1] * MaskA);
             std::priority_queue<PointType, ArrayList<PointType>, decltype(Comparator)> Q(Comparator);
             for (size_t a = 0; a < MaskA; a++) {
                 PointType NewPoint = StartPoint;
@@ -224,7 +228,14 @@ namespace roads {
     }// namespace energy
 
     namespace spline {
-    }
+        //template<typename PointContainerType = ArrayList<std::array<float, 3>>, typename SegmentContainerType = std::array<int, 2>>
+        class tinyspline::BSpline GenerateBSplineFromSegment(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments);
+
+        //template<typename PointContainerType = ArrayList<std::array<float, 3>>, typename SegmentContainerType = ArrayList<std::array<int, 2>>>
+        ArrayList<Eigen::Vector3f> GenerateAndSamplePointsFromSegments(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments, int32_t SamplePoints = 1000);
+
+        ArrayList<Eigen::Vector3f> SmoothAndResampleSegments(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments, int32_t SamplePoints = 1000);
+    }// namespace spline
 
 }// namespace roads
 
diff --git a/projects/Roads/utilities/include/roads/thirdparty/parson.h b/projects/Roads/utilities/include/roads/thirdparty/parson.h
new file mode 100644
index 0000000000..6438c9367a
--- /dev/null
+++ b/projects/Roads/utilities/include/roads/thirdparty/parson.h
@@ -0,0 +1,234 @@
+/*
+ Parson ( http://kgabis.github.com/parson/ )
+ Copyright (c) 2012 - 2017 Krzysztof Gabis
+
+ Permission is hereby granted, free of charge, to any person obtaining a copy
+ of this software and associated documentation files (the "Software"), to deal
+ in the Software without restriction, including without limitation the rights
+ to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ copies of the Software, and to permit persons to whom the Software is
+ furnished to do so, subject to the following conditions:
+
+ The above copyright notice and this permission notice shall be included in
+ all copies or substantial portions of the Software.
+
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ THE SOFTWARE.
+*/
+
+#ifndef parson_parson_h
+#define parson_parson_h
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include <stddef.h>   /* size_t */
+
+/* Types and enums */
+typedef struct json_object_t JSON_Object;
+typedef struct json_array_t  JSON_Array;
+typedef struct json_value_t  JSON_Value;
+
+enum json_value_type {
+    JSONError   = -1,
+    JSONNull    = 1,
+    JSONString  = 2,
+    JSONNumber  = 3,
+    JSONObject  = 4,
+    JSONArray   = 5,
+    JSONBoolean = 6
+};
+typedef int JSON_Value_Type;
+
+enum json_result_t {
+    JSONSuccess = 0,
+    JSONFailure = -1
+};
+typedef int JSON_Status;
+
+typedef void * (*JSON_Malloc_Function)(size_t);
+typedef void   (*JSON_Free_Function)(void *);
+
+/* Call only once, before calling any other function from parson API. If not called, malloc and free
+   from stdlib will be used for all allocations */
+void json_set_allocation_functions(JSON_Malloc_Function malloc_fun, JSON_Free_Function free_fun);
+
+/* Parses first JSON value in a file, returns NULL in case of error */
+JSON_Value * json_parse_file(const char *filename);
+
+/* Parses first JSON value in a file and ignores comments (/ * * / and //),
+   returns NULL in case of error */
+JSON_Value * json_parse_file_with_comments(const char *filename);
+
+/*  Parses first JSON value in a string, returns NULL in case of error */
+JSON_Value * json_parse_string(const char *string);
+
+/*  Parses first JSON value in a string and ignores comments (/ * * / and //),
+    returns NULL in case of error */
+JSON_Value * json_parse_string_with_comments(const char *string);
+
+/* Serialization */
+size_t      json_serialization_size(const JSON_Value *value); /* returns 0 on fail */
+JSON_Status json_serialize_to_buffer(const JSON_Value *value, char *buf, size_t buf_size_in_bytes);
+JSON_Status json_serialize_to_file(const JSON_Value *value, const char *filename);
+char *      json_serialize_to_string(const JSON_Value *value);
+
+/* Pretty serialization */
+size_t      json_serialization_size_pretty(const JSON_Value *value); /* returns 0 on fail */
+JSON_Status json_serialize_to_buffer_pretty(const JSON_Value *value, char *buf, size_t buf_size_in_bytes);
+JSON_Status json_serialize_to_file_pretty(const JSON_Value *value, const char *filename);
+char *      json_serialize_to_string_pretty(const JSON_Value *value);
+
+void        json_free_serialized_string(char *string); /* frees string from json_serialize_to_string and json_serialize_to_string_pretty */
+
+/* Comparing */
+int  json_value_equals(const JSON_Value *a, const JSON_Value *b);
+
+/* Validation
+   This is *NOT* JSON Schema. It validates json by checking if object have identically
+   named fields with matching types.
+   For example schema {"name":"", "age":0} will validate
+   {"name":"Joe", "age":25} and {"name":"Joe", "age":25, "gender":"m"},
+   but not {"name":"Joe"} or {"name":"Joe", "age":"Cucumber"}.
+   In case of arrays, only first value in schema is checked against all values in tested array.
+   Empty objects ({}) validate all objects, empty arrays ([]) validate all arrays,
+   null validates values of every type.
+ */
+JSON_Status json_validate(const JSON_Value *schema, const JSON_Value *value);
+
+/*
+ * JSON Object
+ */
+JSON_Value  * json_object_get_value  (const JSON_Object *object, const char *name);
+const char  * json_object_get_string (const JSON_Object *object, const char *name);
+JSON_Object * json_object_get_object (const JSON_Object *object, const char *name);
+JSON_Array  * json_object_get_array  (const JSON_Object *object, const char *name);
+double        json_object_get_number (const JSON_Object *object, const char *name); /* returns 0 on fail */
+int           json_object_get_boolean(const JSON_Object *object, const char *name); /* returns -1 on fail */
+
+/* dotget functions enable addressing values with dot notation in nested objects,
+ just like in structs or c++/java/c# objects (e.g. objectA.objectB.value).
+ Because valid names in JSON can contain dots, some values may be inaccessible
+ this way. */
+JSON_Value  * json_object_dotget_value  (const JSON_Object *object, const char *name);
+const char  * json_object_dotget_string (const JSON_Object *object, const char *name);
+JSON_Object * json_object_dotget_object (const JSON_Object *object, const char *name);
+JSON_Array  * json_object_dotget_array  (const JSON_Object *object, const char *name);
+double        json_object_dotget_number (const JSON_Object *object, const char *name); /* returns 0 on fail */
+int           json_object_dotget_boolean(const JSON_Object *object, const char *name); /* returns -1 on fail */
+
+/* Functions to get available names */
+size_t        json_object_get_count   (const JSON_Object *object);
+const char  * json_object_get_name    (const JSON_Object *object, size_t index);
+JSON_Value  * json_object_get_value_at(const JSON_Object *object, size_t index);
+JSON_Value  * json_object_get_wrapping_value(const JSON_Object *object);
+
+/* Functions to check if object has a value with a specific name. Returned value is 1 if object has
+ * a value and 0 if it doesn't. dothas functions behave exactly like dotget functions. */
+int json_object_has_value        (const JSON_Object *object, const char *name);
+int json_object_has_value_of_type(const JSON_Object *object, const char *name, JSON_Value_Type type);
+
+int json_object_dothas_value        (const JSON_Object *object, const char *name);
+int json_object_dothas_value_of_type(const JSON_Object *object, const char *name, JSON_Value_Type type);
+
+/* Creates new name-value pair or frees and replaces old value with a new one.
+ * json_object_set_value does not copy passed value so it shouldn't be freed afterwards. */
+JSON_Status json_object_set_value(JSON_Object *object, const char *name, JSON_Value *value);
+JSON_Status json_object_set_string(JSON_Object *object, const char *name, const char *string);
+JSON_Status json_object_set_number(JSON_Object *object, const char *name, double number);
+JSON_Status json_object_set_boolean(JSON_Object *object, const char *name, int boolean);
+JSON_Status json_object_set_null(JSON_Object *object, const char *name);
+
+/* Works like dotget functions, but creates whole hierarchy if necessary.
+ * json_object_dotset_value does not copy passed value so it shouldn't be freed afterwards. */
+JSON_Status json_object_dotset_value(JSON_Object *object, const char *name, JSON_Value *value);
+JSON_Status json_object_dotset_string(JSON_Object *object, const char *name, const char *string);
+JSON_Status json_object_dotset_number(JSON_Object *object, const char *name, double number);
+JSON_Status json_object_dotset_boolean(JSON_Object *object, const char *name, int boolean);
+JSON_Status json_object_dotset_null(JSON_Object *object, const char *name);
+
+/* Frees and removes name-value pair */
+JSON_Status json_object_remove(JSON_Object *object, const char *name);
+
+/* Works like dotget function, but removes name-value pair only on exact match. */
+JSON_Status json_object_dotremove(JSON_Object *object, const char *key);
+
+/* Removes all name-value pairs in object */
+JSON_Status json_object_clear(JSON_Object *object);
+
+/*
+ *JSON Array
+ */
+JSON_Value  * json_array_get_value  (const JSON_Array *array, size_t index);
+const char  * json_array_get_string (const JSON_Array *array, size_t index);
+JSON_Object * json_array_get_object (const JSON_Array *array, size_t index);
+JSON_Array  * json_array_get_array  (const JSON_Array *array, size_t index);
+double        json_array_get_number (const JSON_Array *array, size_t index); /* returns 0 on fail */
+int           json_array_get_boolean(const JSON_Array *array, size_t index); /* returns -1 on fail */
+size_t        json_array_get_count  (const JSON_Array *array);
+JSON_Value  * json_array_get_wrapping_value(const JSON_Array *array);
+    
+/* Frees and removes value at given index, does nothing and returns JSONFailure if index doesn't exist.
+ * Order of values in array may change during execution.  */
+JSON_Status json_array_remove(JSON_Array *array, size_t i);
+
+/* Frees and removes from array value at given index and replaces it with given one.
+ * Does nothing and returns JSONFailure if index doesn't exist.
+ * json_array_replace_value does not copy passed value so it shouldn't be freed afterwards. */
+JSON_Status json_array_replace_value(JSON_Array *array, size_t i, JSON_Value *value);
+JSON_Status json_array_replace_string(JSON_Array *array, size_t i, const char* string);
+JSON_Status json_array_replace_number(JSON_Array *array, size_t i, double number);
+JSON_Status json_array_replace_boolean(JSON_Array *array, size_t i, int boolean);
+JSON_Status json_array_replace_null(JSON_Array *array, size_t i);
+
+/* Frees and removes all values from array */
+JSON_Status json_array_clear(JSON_Array *array);
+
+/* Appends new value at the end of array.
+ * json_array_append_value does not copy passed value so it shouldn't be freed afterwards. */
+JSON_Status json_array_append_value(JSON_Array *array, JSON_Value *value);
+JSON_Status json_array_append_string(JSON_Array *array, const char *string);
+JSON_Status json_array_append_number(JSON_Array *array, double number);
+JSON_Status json_array_append_boolean(JSON_Array *array, int boolean);
+JSON_Status json_array_append_null(JSON_Array *array);
+
+/*
+ *JSON Value
+ */
+JSON_Value * json_value_init_object (void);
+JSON_Value * json_value_init_array  (void);
+JSON_Value * json_value_init_string (const char *string); /* copies passed string */
+JSON_Value * json_value_init_number (double number);
+JSON_Value * json_value_init_boolean(int boolean);
+JSON_Value * json_value_init_null   (void);
+JSON_Value * json_value_deep_copy   (const JSON_Value *value);
+void         json_value_free        (JSON_Value *value);
+
+JSON_Value_Type json_value_get_type   (const JSON_Value *value);
+JSON_Object *   json_value_get_object (const JSON_Value *value);
+JSON_Array  *   json_value_get_array  (const JSON_Value *value);
+const char  *   json_value_get_string (const JSON_Value *value);
+double          json_value_get_number (const JSON_Value *value);
+int             json_value_get_boolean(const JSON_Value *value);
+JSON_Value  *   json_value_get_parent (const JSON_Value *value);
+
+/* Same as above, but shorter */
+JSON_Value_Type json_type   (const JSON_Value *value);
+JSON_Object *   json_object (const JSON_Value *value);
+JSON_Array  *   json_array  (const JSON_Value *value);
+const char  *   json_string (const JSON_Value *value);
+double          json_number (const JSON_Value *value);
+int             json_boolean(const JSON_Value *value);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/projects/Roads/utilities/include/roads/thirdparty/tinyspline.h b/projects/Roads/utilities/include/roads/thirdparty/tinyspline.h
new file mode 100644
index 0000000000..56f00f3f14
--- /dev/null
+++ b/projects/Roads/utilities/include/roads/thirdparty/tinyspline.h
@@ -0,0 +1,3117 @@
+/** @file */
+
+#ifndef TINYSPLINE_H
+#define TINYSPLINE_H
+
+#include <stddef.h>
+
+
+
+/*! @name Deprecation
+ *
+ * The macro \c TS_DEPRECATED can be used to mark functions as
+ * deprecated.
+ *
+ * @{
+ */
+#if defined(__GNUC__) || defined(__clang__)
+#define TS_DEPRECATED __attribute__((deprecated))
+#elif defined(_MSC_VER)
+#define TS_DEPRECATED __declspec(deprecated)
+#elif defined(SWIG)
+#define TS_DEPRECATED
+#else
+#warning "WARNING: TS_DEPRECATED is not supported by the compiler"
+#define TS_DEPRECATED
+#endif
+/*! @} */
+
+
+
+/*! @name Library Export/Import
+ *
+ * If TinySpline is built for Windows, the macros \c TINYSPLINE_SHARED_EXPORT
+ * and \c TINYSPLINE_SHARED_IMPORT define the Microsoft specific directives \c
+ * __declspec(dllexport) and \c __declspec(dllimport), respectively. More
+ * information on these directives can be found at:
+ *
+ *     https://docs.microsoft.com/en-us/cpp/cpp/dllexport-dllimport
+ *
+ * If TinySpline is built to the ELF (most Unix like environments) or Mach (OS
+ * X) object format, \c TINYSPLINE_SHARED_EXPORT defines the directive
+ * <tt>__attribute__ ((visibility ("default")))</tt> which, in combination with
+ * \c -fvisibility=hidden, behaves similar to \c __declspec(dllexport). \c
+ * TINYSPLINE_SHARED_IMPORT is set empty (i.e., it defines nothing).
+ *
+ * If none of the above applies, \c TINYSPLINE_SHARED_EXPORT and \c
+ * TINYSPLINE_SHARED_IMPORT are set empty (i.e., they define nothing).
+ *
+ * Depending on whether TinySpline is compiled <em>as shared library</em> or
+ * linked against <em>as shared library</em>, \c TINYSPLINE_API points to \c
+ * TINYSPLINE_SHARED_EXPORT (compiled) or \c TINYSPLINE_SHARED_IMPORT (linked
+ * against). All elements of TinySpline that needs to be exported/imported are
+ * annotated with \c TINYSPLINE_API. This eliminates the need for a
+ * module-definition (.def) file. If TinySpline is compiled or linked against
+ * <em>as static library</em>, \c TINYSPLINE_API is set empty (i.e., it defines
+ * nothing).
+ *
+ * If you consume TinySpline as shared library built for Windows, all you need
+ * is to define \c TINYSPLINE_SHARED. This will automatically import all
+ * required symbols. When compiling TinySpline, the build system should set all
+ * necessary defines.
+ *
+ * @{
+ */
+#if !defined(TINYSPLINE_API)
+#if defined(_WIN32) || defined(__CYGWIN__)
+#define TINYSPLINE_SHARED_EXPORT __declspec(dllexport)
+#define TINYSPLINE_SHARED_IMPORT __declspec(dllimport)
+#elif defined(__ELF__) || defined(__MACH__)
+#define TINYSPLINE_SHARED_EXPORT __attribute__ ((visibility ("default")))
+#define TINYSPLINE_SHARED_IMPORT
+#else
+#define TINYSPLINE_SHARED_EXPORT
+#define TINYSPLINE_SHARED_IMPORT
+#endif
+
+#ifdef TINYSPLINE_SHARED
+#ifdef TINYSPLINE_EXPORT
+#define TINYSPLINE_API TINYSPLINE_SHARED_EXPORT
+#else
+#define TINYSPLINE_API TINYSPLINE_SHARED_IMPORT
+#endif
+#else
+#define TINYSPLINE_API
+#endif
+#endif
+
+#ifdef	__cplusplus
+extern "C" {
+#endif
+/*! @} */
+
+
+
+/*! @name Predefined Constants
+ *
+ * The following constants have been adjusted to maintain internal consistency
+ * and should only be changed with great caution! The values chosen should be
+ * suitable for most environments and can be used with float (single) and
+ * double precision (see ::tsReal). If changes are necessary, please read the
+ * documentation of the constants in advance.
+ *
+ * @{
+ */
+/**
+ * The mathematical constant pi.
+ */
+#define TS_PI 3.14159265358979323846
+
+/**
+ * The maximum number of knots a spline can have. This constant is strongly
+ * related to ::TS_KNOT_EPSILON in that the larger ::TS_MAX_NUM_KNOTS is, the
+ * less precise ::TS_KNOT_EPSILON has to be (i.e., knots with greater distance
+ * are considered equal). Likewise, the more precise ::TS_KNOT_EPSILON is
+ * (i.e., knots with smaller distance are considered equal), the less
+ * ::TS_MAX_NUM_KNOTS has to be. By default, the relation between
+ * ::TS_MAX_NUM_KNOTS and ::TS_KNOT_EPSILON is as follows:
+ *
+ *     TS_MAX_NUM_KNOTS = 1 / TS_KNOTS_EPSILON
+ */
+#define TS_MAX_NUM_KNOTS 10000
+
+/**
+ * The minimum of the domain of newly created splines. Must be less than
+ * ::TS_DOMAIN_DEFAULT_MAX. This constant is used only when creating new
+ * splines. After creation, the domain of a spline can be adjusted as needed.
+ */
+#define TS_DOMAIN_DEFAULT_MIN 0.0f
+
+/**
+ * The maximum of the domain of newly created splines. Must be greater than
+ * ::TS_DOMAIN_DEFAULT_MIN. This constant is used only when creating new
+ * splines. After creation, the domain of a spline can be adjusted as needed.
+ */
+#define TS_DOMAIN_DEFAULT_MAX 1.0f
+
+/**
+ * If the distance between two knots falls below this threshold, they are
+ * considered equal. Must be positive ( > 0 ). This constant is strongly
+ * related to ::TS_MAX_NUM_KNOTS in that the more precise ::TS_KNOT_EPSILON is
+ * (i.e., knots with smaller distance are considered equal), the less
+ * ::TS_MAX_NUM_KNOTS has to be. Likewise, the larger ::TS_MAX_NUM_KNOTS is,
+ * the less precise ::TS_KNOT_EPSILON has to be (i.e., knots with greater
+ * distance are considered equal). By default, the relation between
+ * ::TS_KNOT_EPSILON and ::TS_MAX_NUM_KNOTS is as follows:
+ *
+ *     TS_KNOT_EPSILON = 1 / TS_MAX_NUM_KNOTS
+ *
+ * It is recommended that ::TS_KNOT_EPSILON is aligned to the span of
+ * ::TS_DOMAIN_DEFAULT_MIN and ::TS_DOMAIN_DEFAULT_MAX. That is, adjacent
+ * floating point values in the domain [::TS_DOMAIN_DEFAULT_MIN,
+ * ::TS_DOMAIN_DEFAULT_MAX] should not be equal according to
+ * ::TS_KNOT_EPSILON. This is in particular recommended when ::TS_KNOT_EPSILON
+ * and ::TS_MAX_NUM_KNOTS are related to each other as described above.
+ */
+#define TS_KNOT_EPSILON 1e-4f
+
+/**
+ * If the distance between two (control) points is less than or equal to this
+ * threshold, they are considered equal. This constant is not used directly by
+ * the C interface. Rather, it serves as a viable default value for functions
+ * requiring an epsilon environment to decide whether two (control) points are
+ * equal or not. The C++ interface, for example, uses this as default value for
+ * optional parameters.
+ */
+#ifdef TINYSPLINE_FLOAT_PRECISION
+#define TS_POINT_EPSILON 1e-3f
+#else
+#define TS_POINT_EPSILON 1e-5f
+#endif
+
+/**
+ * If the length of an element (e.g., a vector) is less than this threshold,
+ * the length is considered \c 0. Must be positive ( > 0 ).
+ */
+#ifdef TINYSPLINE_FLOAT_PRECISION
+#define TS_LENGTH_ZERO 1e-3f
+#else
+#define TS_LENGTH_ZERO 1e-4f
+#endif
+/*! @} */
+
+
+
+/*! @name API Configuration
+ *
+ * In the following section, different aspects of TinySpline's API can be
+ * configured (compile-time). It is recommended to configure the API by
+ * supplying the corresponding preprocessor definition(s). That said, there is
+ * nothing wrong with editing the source code directly.
+ *
+ * @{
+ */
+/**
+ * TinySpline uses its own typedef for floating point numbers. Supported are
+ * floats (single precision) and doubles (double precision). By default,
+ * doubles are used. Note that this typedef affects the entire API (i.e., types
+ * are not mixed; all structs and functions rely only on tsReal). Float
+ * precision is primarily used in combination with GLUT because GLUT's API
+ * doesn't support doubles:
+ *
+ *     https://www.glprogramming.com/red/chapter12.html
+ *
+ * Generally, double precision is the right choice. Floats are mainly supported
+ * for legacy reasons. Yet, floats are not considered deprecated! If necessary,
+ * tsReal can also be typedefed to any other floating point representation. In
+ * this case, make sure to adjust TS_MAX_NUM_KNOTS and TS_KNOT_EPSILON
+ * (cf. Section "Predefined Constants").
+ */
+#ifdef TINYSPLINE_FLOAT_PRECISION
+typedef float tsReal;
+#else
+typedef double tsReal;
+#endif
+/*! @} */
+
+
+
+/*! @name Error Handling
+ *
+ * There are three types of error handling in TinySpline.
+ *
+ * 1. Return value: Functions that can fail return a special error code
+ * (::tsError). If the error code is not \c 0 (::TS_SUCCESS), an error occurred
+ * during execution. For example:
+ *
+ *     if ( ts_bspline_to_beziers(&spline, &beziers, NULL) ) {
+ *          ... An error occurred ...
+ *     }
+ *
+ *   It is of course possible to check the actual type of error:
+ *
+ *       tsError error = ts_bspline_to_beziers(&spline, &beziers, NULL);
+ *       if (error == TS_MALLOC) {
+ *            ... Out of memory ...
+ *       } else if (error == ...
+ *
+ *   This type of error handling is used in many C programs. The disadvantage
+ *   is that there is no additional error message besides the error code (with
+ *   which the cause of an error could be specified in more detail). Some
+ *   libraries make do with global variables in which error messages are stored
+ *   for later purpose (e.g., \a errno and \a strerror). Unfortunately,
+ *   however, this approach (by design) is often not thread-safe. The second
+ *   error handling option solves this issue.
+ *
+ * 2. ::tsStatus objects: Functions that can fail do not only return an error
+ * code, but also take a pointer to a ::tsStatus object as an optional
+ * parameter. In the event of an error, and if the supplied pointer is not
+ * NULL, the error message is stored in tsStatus#message and can be accessed by
+ * the caller. Using a ::tsStatus object, the example given in 1. can be
+ * modified as follows:
+ *
+ *     tsStatus status;
+ *     if ( ts_bspline_to_beziers(&spline, &beziers, &status) ) {
+ *         status.code; // error code
+ *         status.message; // error message
+ *     }
+ *
+ *   Note that ::tsStatus objects can be reused:
+ *
+ *       tsStatus status;
+ *       if ( ts_bspline_to_beziers(&spline, &beziers, &status) ) {
+ *           ...
+ *       }
+ *       ...
+ *       if ( ts_bspline_derive(&beziers, 1, 0.001, &beziers, &status) ) {
+ *           ...
+ *       }
+ *
+ *   If you would like to use this type of error handling in your own functions
+ *   (in particular the optional ::tsStatus parameter), you may wonder whether
+ *   there is an easy way to return error codes and format error messages. This
+ *   is where the macros ::TS_RETURN_0 -- ::TS_RETURN_4 come into play. They
+ *   can, for example, be used as follows:
+ *
+ *       tsError my_function(..., tsStatus *status, ...)
+ *       {
+ *           ...
+ *           tsReal *points = (tsReal *) malloc(len * sizeof(tsReal));
+ *           if (!points)
+ *               TS_RETURN_0(status, TS_MALLOC, "out of memory")
+ *           ...
+ *       }
+ *
+ *   The \c N in \c TS_RETURN_<N> denotes the number of format specifier in the
+ *   supplied format string (cf. sprintf(char *, const char *, ... )).
+ *
+ * 3. Try-catch-finally blocks: TinySpline provides a set of macros that can be
+ * used when a complex control flow is necessary. The macros create a structure
+ * that is similar to the exception handling mechanisms of high-level languages
+ * such as C++. The basic structure is as follows:
+ *
+ *     TS_TRY(try, error, status) // `status' may be NULL
+ *         ...
+ *         TS_CALL( try, error, ts_bspline_to_beziers(
+ *             &spline, &beziers, status) )
+ *         ...
+ *     TS_CATCH(error)
+ *         ... Executed in case of an error ...
+ *         ... `error' (tsError) indicates the type of error.
+ *         ... `status' (tsStatus) contains the error code and message ...
+ *     TS_FINALLY
+ *         ... Executed in any case ...
+ *     TS_END_TRY
+ *
+ *   ::TS_TRY and ::TS_END_TRY mark the boundaries of a try-catch-finally
+ *   block. Every block has an identifier (name) that must be unique within a
+ *   scope. The name of a block is set via the first argument of ::TS_TRY (\c
+ *   try in the example listed above). The control flow of a try-catch-finally
+ *   block is directed via the second and third argument of ::TS_TRY (\c error
+ *   and \c status in the example listed above) and the utility macro
+ *   ::TS_CALL. The second argument of ::TS_TRY, a ::tsError, is mandatory. The
+ *   third argument of ::TS_TRY, a ::tsStatus object, is optional, that is, it
+ *   may be \c NULL. ::TS_CALL serves as a wrapper for functions with return
+ *   type ::tsError. If the called functions fails (more on that later),
+ *   ::TS_CALL immediately jumps into the ::TS_CATCH section where \c error and
+ *   \c status can be evaluated as needed (note that \c status may be \c
+ *   NULL). The ::TS_FINALLY section is executed in any case and is in
+ *   particularly helpful when resources (such as heap-memory, file-handles
+ *   etc.) must be released.
+ *
+ *   While ::TS_CALL can be used to wrap functions with return type ::tsError,
+ *   sooner or later it will be necessary to delegate the failure of other
+ *   kinds of functions (i.e., functions outside of TinySpline; e.g.,
+ *   malloc(size_t)). This is the purpose of the ::TS_THROW_0 -- ::TS_THROW_4
+ *   macros. It is not by coincidence that the signature of the \c TS_THROW_<N>
+ *   macros is quite similar to that of the \c TS_RETURN_<N> macros. Both
+ *   "macro groups" are used to report errors. The difference between \c
+ *   TS_RETURN_<N> and TS_THROW_<N>, however, is that the former exits a
+ *   function (i.e., a \c return statement is inserted by these macros) while
+ *   the latter jumps into a catch block (the catch block to jump into is set
+ *   via the first argument of \c TS_THROW_<N>):
+ *
+ *       tsBSpline spline = ts_bspline_init();
+ *       tsReal *points = NULL;
+ *       TS_TRY(try, error, status)
+ *           ...
+ *           tsReal *points = (tsReal *) malloc(len * sizeof(tsReal));
+ *           if (!points)
+ *               TS_THROW_0(try, status, TS_MALLOC, "out of memory")
+ *           ...
+ *           TS_CALL( try, error, ts_bspline_interpolate_cubic_natural(
+ *               points, len / dim, dim, &spline, status) )
+ *           ...
+ *       TS_CATCH(error)
+ *           ... Log error message ...
+ *       TS_FINALLY
+ *           ts_bspline_free(&spline);
+ *           if (points)
+ *               free(points);
+ *       TS_END_TRY
+ *
+ *   In all likelihood, you are already familiar with this kind error
+ *   handling. Actually, there are a plethora of examples available online
+ *   showing how exception-like error handling can be implemented in C. What
+ *   most of these examples have in common is that they suggest to wrap the
+ *   functions \c setjmp and \c longjmp (see setjmp.h) with macros. While this
+ *   undoubtedly is a clever trick, \c setjmp and \c longjmp have no viable
+ *   (i.e, thread-safe) solution for propagating the cause of an error (in the
+ *   form of a human-readable error message) back to the client of a
+ *   library. Therefore, TinySpline implements try-catch-finally blocks with \c
+ *   if statements, labels, and \c goto statements (TS_THROW_<N>).
+ *
+ *   ::TS_TRY is flexible enough to be used in functions that are in turn
+ *   embedded into TinySpline's error handling system:
+ *
+ *       tsError my_function(..., tsStatus *status, ...)
+ *       {
+ *           tsError error;
+ *           TS_TRY(try, error, status)
+ *               ...
+ *           TS_END_TRY
+ *           return error;
+ *       }
+ *
+ *   as well as functions forming the root of a call stack that uses
+ *   TinySpline's error handling system:
+ *
+ *       tsStatus status;
+ *       TS_TRY(try, status.code, &status)
+ *           ...
+ *       TS_END_TRY
+ *
+ *   There is some utility macros that might be useful when dealing with
+ *   try-catch-finally blocks:
+ *
+ *   - ::TS_END_TRY_RETURN: Returns the supplied error code immediately after
+ *     completing the corresponding block. Can be used as follows:
+ *
+ *         tsError my_function(..., tsStatus *status, ...)
+ *         {
+ *             tsError error;
+ *             TS_TRY(try, error, status)
+ *                 ...
+ *             TS_END_TRY_RETURN(error)
+ *         }
+ *
+ *   - ::TS_END_TRY_ROE: Like ::TS_END_TRY_RETURN but returns the supplied
+ *     error code, \c e, if \c e is not ::TS_SUCCESS (\c ROE means
+ *     <b>R</b>eturn <b>O</b>n <b>E</b>rror). Can be used as follows:
+ *
+ *         tsError my_function(..., tsStatus *status, ...)
+ *         {
+ *             tsError error;
+ *             TS_TRY(try, error, status)
+ *                 ...
+ *             TS_END_TRY_ROE(error)
+ *             ... Additional code. The code is executed only if `error' is
+ *                 TS_SUCCESS, that is, if no error occurred in the try block
+ *                 above ...
+ *         }
+ *
+ *   - ::TS_CALL_ROE: Calls the supplied function and returns its error code,
+ *     \c e, if \c e is not ::TS_SUCCESS. This macro can be seen as a \e
+ *     minified try block (a dedicated try block is not needed).
+ *
+ *   - ::TS_RETURN_SUCCESS: Shortcut for ::TS_RETURN_0 with error code
+ *     ::TS_SUCCESS and an empty error message.
+ *
+ * @{
+ */
+/**
+ * Defines different error codes.
+ */
+typedef enum
+{
+	/** No error. */
+	TS_SUCCESS = 0,
+
+	/** Memory cannot be allocated (malloc, realloc etc.). */
+	TS_MALLOC = -1,
+
+	/** Points have dimensionality 0. */
+	TS_DIM_ZERO = -2,
+
+	/** degree >= num(control_points). */
+	TS_DEG_GE_NCTRLP = -3,
+
+	/** Knot is not within the domain. */
+	TS_U_UNDEFINED = -4,
+
+	/** multiplicity(knot) > order */
+	TS_MULTIPLICITY = -5,
+
+	/** Decreasing knot vector. */
+	TS_KNOTS_DECR = -6,
+
+	/** Unexpected number of knots. */
+	TS_NUM_KNOTS = -7,
+
+	/** Spline is not derivable. */
+	TS_UNDERIVABLE = -8,
+
+	/** len(control_points) % dimension != 0. */
+	TS_LCTRLP_DIM_MISMATCH = -10,
+
+	/** Error while reading/writing a file. */
+	TS_IO_ERROR = -11,
+
+	/** Error while parsing a serialized entity. */
+	TS_PARSE_ERROR = -12,
+
+	/** Index does not exist (e.g., when accessing an array). */
+	TS_INDEX_ERROR = -13,
+
+	/** Function returns without result (e.g., approximations). */
+	TS_NO_RESULT = -14,
+
+	/** Unexpected number of points. */
+	TS_NUM_POINTS = -15
+} tsError;
+
+/**
+ * Stores an error code (see ::tsError) with corresponding message.
+ */
+typedef struct
+{
+	/** The error code. */
+	tsError code;
+
+	/**
+	 * The corresponding error message (encoded as C string). Memory is
+	 * allocated on stack so as to be able to provide a meaningful message
+	 * in the event of memory issues (cf. ::TS_MALLOC).
+	 */
+	char message[100];
+} tsStatus;
+
+#define TS_TRY(label, error, status)         \
+{                                            \
+	(error) = TS_SUCCESS;                \
+	if ((status) != NULL) {              \
+		(status)->code = TS_SUCCESS; \
+		(status)->message[0] = '\0'; \
+	}                                    \
+	__ ## label ## __:                   \
+	if (!(error)) {
+
+#define TS_CALL(label, error, call)                  \
+		(error) = (call);                    \
+		if ((error)) goto __ ## label ## __;
+
+#define TS_CATCH(error) \
+	} if ((error)) {
+
+#define TS_FINALLY \
+	} {
+
+#define TS_END_TRY \
+	}          \
+}
+
+#define TS_END_TRY_RETURN(error)   \
+	TS_END_TRY return (error);
+
+#define TS_END_TRY_ROE(error)                 \
+	TS_END_TRY if ((error)) return error;
+
+#define TS_CALL_ROE(error, call)   \
+{                                  \
+	(error) = (call);          \
+	if ((error)) return error; \
+}
+
+#define TS_RETURN_SUCCESS(status)            \
+{                                            \
+	if ((status) != NULL) {              \
+		(status)->code = TS_SUCCESS; \
+		(status)->message[0] = '\0'; \
+	}                                    \
+	return TS_SUCCESS;                   \
+}
+
+#define TS_RETURN_0(status, error, msg)          \
+{                                                \
+	if ((status) != NULL) {                  \
+		(status)->code = error;          \
+		sprintf((status)->message, msg); \
+	}                                        \
+	return error;                            \
+}
+
+#define TS_RETURN_1(status, error, msg, arg1)          \
+{                                                      \
+	if ((status) != NULL) {                        \
+		(status)->code = error;                \
+		sprintf((status)->message, msg, arg1); \
+	}                                              \
+	return error;                                  \
+}
+
+#define TS_RETURN_2(status, error, msg, arg1, arg2)          \
+{                                                            \
+	if ((status) != NULL) {                              \
+		(status)->code = error;                      \
+		sprintf((status)->message, msg, arg1, arg2); \
+	}                                                    \
+	return error;                                        \
+}
+
+#define TS_RETURN_3(status, error, msg, arg1, arg2, arg3)          \
+{                                                                  \
+	if ((status) != NULL) {                                    \
+		(status)->code = error;                            \
+		sprintf((status)->message, msg, arg1, arg2, arg3); \
+	}                                                          \
+	return error;                                              \
+}
+
+#define TS_RETURN_4(status, error, msg, arg1, arg2, arg3, arg4)          \
+{                                                                        \
+	if ((status) != NULL) {                                          \
+		(status)->code = error;                                  \
+		sprintf((status)->message, msg, arg1, arg2, arg3, arg4); \
+	}                                                                \
+	return error;                                                    \
+}
+
+#define TS_THROW_0(label, error, status, val, msg) \
+{                                                  \
+	(error) = val;                             \
+	if ((status) != NULL) {                    \
+		(status)->code = val;              \
+		sprintf((status)->message, msg);   \
+	}                                          \
+	goto __ ## label ## __;                    \
+}
+
+#define TS_THROW_1(label, error, status, val, msg, arg1) \
+{                                                        \
+	(error) = val;                                   \
+	if ((status) != NULL) {                          \
+		(status)->code = val;                    \
+		sprintf((status)->message, msg, arg1);   \
+	}                                                \
+	goto __ ## label ## __;                          \
+}
+
+#define TS_THROW_2(label, error, status, val, msg, arg1, arg2) \
+{                                                              \
+	(error) = val;                                         \
+	if ((status) != NULL) {                                \
+		(status)->code = val;                          \
+		sprintf((status)->message, msg, arg1, arg2);   \
+	}                                                      \
+	goto __ ## label ## __;                                \
+}
+
+#define TS_THROW_3(label, error, status, val, msg, arg1, arg2, arg3) \
+{                                                                    \
+	(error) = val;                                               \
+	if ((status) != NULL) {                                      \
+		(status)->code = val;                                \
+		sprintf((status)->message, msg, arg1, arg2, arg3);   \
+	}                                                            \
+	goto __ ## label ## __;                                      \
+}
+
+#define TS_THROW_4(label, error, status, val, msg, arg1, arg2, arg3, arg4) \
+{                                                                          \
+	(error) = val;                                                     \
+	if ((status) != NULL) {                                            \
+		(status)->code = val;                                      \
+		sprintf((status)->message, msg, arg1, arg2, arg3, arg4);   \
+	}                                                                  \
+	goto __ ## label ## __;                                            \
+}
+/*! @} */
+
+
+
+/*! @name Utility Structs and Enums
+ *
+ * @{
+ */
+/**
+ * Describes the structure of the knot vector. More details can be found at:
+ *
+ *     www.cs.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/bspline-curve.html
+ */
+typedef enum
+{
+	/** Uniformly spaced knot vector with opened end knots. */
+	TS_OPENED = 0,
+
+	/** Uniformly spaced knot vector with clamped end knots. */
+	TS_CLAMPED = 1,
+
+	/**
+	 * Uniformly spaced knot vector where the multiplicity of each knot is
+	 * equal to the order of the spline.
+	 */
+	TS_BEZIERS = 2
+} tsBSplineType;
+
+/**
+ * A three-dimensional TNB-vector with position. More details can be found at:
+ *
+ *     https://en.wikipedia.org/wiki/Frenet-Serret_formulas
+ *     https://www.math.tamu.edu/~tkiffe/calc3/tnb/tnb.html
+ *
+ * TNB stands for \e tangent, \e normal, and \e binormal.
+ */
+typedef struct
+{
+	/** Position of the TNB-vector. */
+	tsReal position[3];
+
+	/** Tangent of the TNB-vector. */
+	tsReal tangent[3];
+
+	/** Normal of the TNB-vector. */
+	tsReal normal[3];
+
+	/** Binormal of the TNB-vector. */
+	tsReal binormal[3];
+} tsFrame;
+/*! @} */
+
+
+
+/*! @name B-Spline Data
+ *
+ * The internal state of ::tsBSpline is protected using the PIMPL design
+ * pattern (see https://en.cppreference.com/w/cpp/language/pimpl for more
+ * details). The data of an instance can be accessed with the functions listed
+ * in this section.
+ *
+ * @{
+ */
+/**
+ * Represents a B-Spline, which may also be used for NURBS, Bezier curves,
+ * lines, and points. NURBS use homogeneous coordinates to store their control
+ * points (i.e. the last component of a control point stores the weight).
+ * Bezier curves are B-Splines with 'num_control_points == order' and a
+ * clamped knot vector, which lets them pass through their first and last
+ * control point (a property which does not necessarily apply to B-Splines and
+ * NURBS). Lines and points, on that basis, are Bezier curves of degree 1
+ * (lines) and 0 (points).
+ *
+ * Two dimensional control points are stored as follows:
+ *
+ *     [x_0, y_0, x_1, y_1, ..., x_n-1, y_n-1]
+ *
+ * Tree dimensional control points are stored as follows:
+ *
+ *     [x_0, y_0, z_0, x_1, y_1, z_1, ..., x_n-1, y_n-1, z_n-1]
+ *
+ * ... and so on. As already mentioned, NURBS use homogeneous coordinates to
+ * store their control points. For example, a NURBS in 2D stores its control
+ * points as follows:
+ *
+ *     [x_0*w_0, y_0*w_0, w_0, x_1*w_1, y_1*w_1, w_1, ...]
+ *
+ * where 'w_i' is the weight of the i'th control point.
+ */
+typedef struct
+{
+	struct tsBSplineImpl *pImpl; /**< The actual implementation. */
+} tsBSpline;
+
+/**
+ * Returns the degree of \p spline.
+ *
+ * @param[in] spline
+ * 	The spline whose degree is read.
+ * @return
+ * 	The degree of \p spline.
+ */
+size_t TINYSPLINE_API
+ts_bspline_degree(const tsBSpline *spline);
+
+/**
+ * Returns the order (degree + 1) of \p spline.
+ *
+ * @param[in] spline
+ * 	The spline whose order is read.
+ * @return
+ * 	The order of \p spline.
+ */
+size_t TINYSPLINE_API
+ts_bspline_order(const tsBSpline *spline);
+
+/**
+ * Returns the dimensionality of \p spline, that is, the number of components
+ * of its control points (::ts_bspline_control_points). One-dimensional splines
+ * are possible, albeit their benefit might be questionable.
+ *
+ * @param[in] spline
+ * 	The spline whose dimension is read.
+ * @return
+ * 	The dimension of \p spline (>= 1).
+ */
+size_t TINYSPLINE_API
+ts_bspline_dimension(const tsBSpline *spline);
+
+/**
+ * Returns the length of the control point array of \p spline.
+ *
+ * @param[in] spline
+ * 	The spline whose length of the control point array is read.
+ * @return
+ * 	The length of the control point array of \p spline.
+ */
+size_t TINYSPLINE_API
+ts_bspline_len_control_points(const tsBSpline *spline);
+
+/**
+ * Returns the number of control points of \p spline.
+ *
+ * @param[in] spline
+ * 	The spline whose number of control points is read.
+ * @return
+ * 	The number of control points of \p spline.
+ */
+size_t TINYSPLINE_API
+ts_bspline_num_control_points(const tsBSpline *spline);
+
+/**
+ * Returns the size of the control point array of \p spline. This function may
+ * be useful when copying control points using \e memcpy or \e memmove.
+ *
+ * @param[in] spline
+ * 	The spline whose size of the control point array is read.
+ * @return
+ * 	The size of the control point array of \p spline.
+ */
+size_t TINYSPLINE_API
+ts_bspline_sof_control_points(const tsBSpline *spline);
+
+/**
+ * Returns the pointer to the control point array of \p spline. Note that the
+ * return type of this function is \c const for a reason. Clients should only
+ * read the returned array. When suppressing the constness and writing to the
+ * array against better knowledge, the client is on its own with regard to the
+ * consistency of the internal state of \p spline. If the control points of a
+ * spline need to be changed, use ::ts_bspline_control_points to obtain a copy
+ * of the control point array and ::ts_bspline_set_control_points to copy the
+ * changed values back to the spline.
+ *
+ * @param[in] spline
+ * 	The spline whose pointer to the control point array is returned.
+ * @return
+ * 	Pointer to the control point array of \p spline.
+ */
+const tsReal TINYSPLINE_API *
+ts_bspline_control_points_ptr(const tsBSpline *spline);
+
+/**
+ * Returns a deep copy of the control points of \p spline.
+ *
+ * @param[in] spline
+ * 	The spline whose control points are read.
+ * @param[out] ctrlp
+ * 	The output array. \b Note: It is the responsibility of the client to
+ * 	release the allocated memory after use.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_control_points(const tsBSpline *spline,
+                          tsReal **ctrlp,
+                          tsStatus *status);
+
+/**
+ * Returns the pointer to the control point of \p spline at \p index. Note that
+ * the type of the out parameter \p ctrlp is \c const for a reason. Clients
+ * should only read the returned array. When suppressing the constness of \p
+ * ctrlp and writing to the array against better knowledge, the client is on
+ * its own with regard to the consistency of the internal state of \p
+ * spline. If one of the control points of a spline needs to be changed, use
+ * ::ts_bspline_set_control_points to copy the new control point to the spline.
+ *
+ * @param[in] spline
+ * 	The spline whose pointer to the control point at \p index is returned.
+ * @param[in] index
+ * 	Zero-based index of the control point to be returned.
+ * @param[out] ctrlp
+ * 	Pointer to the control point of \p spline at \p index.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_INDEX_ERROR
+ * 	If \p index is out of range.
+ */
+tsError TINYSPLINE_API
+ts_bspline_control_point_at_ptr(const tsBSpline *spline,
+                                size_t index,
+                                const tsReal **ctrlp,
+                                tsStatus *status);
+
+/**
+ * Sets the control points of \p spline. Creates a deep copy of \p ctrlp.
+ *
+ * @pre
+ * 	\p ctrlp has length ::ts_bspline_len_control_points.
+ * @param[out] spline
+ * 	The spline whose control points are set.
+ * @param[in] ctrlp
+ * 	The value.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ */
+tsError TINYSPLINE_API
+ts_bspline_set_control_points(tsBSpline *spline,
+                              const tsReal *ctrlp,
+                              tsStatus *status);
+
+/**
+ * Sets the control point of \p spline at \p index. Creates a deep copy of
+ * \p ctrlp.
+ *
+ * @pre
+ * 	\p ctrlp has length ::ts_bspline_dimension.
+ * @param[out] spline
+ * 	The spline whose control point is set at \p index.
+ * @param[in] index
+ * 	Zero-based index of the control point to be set.
+ * @param[in] ctrlp
+ * 	The value.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_INDEX_ERROR
+ * 	If \p index is out of range.
+ */
+tsError TINYSPLINE_API
+ts_bspline_set_control_point_at(tsBSpline *spline,
+                                size_t index,
+                                const tsReal *ctrlp,
+                                tsStatus *status);
+
+/**
+ * Returns the number of knots of \p spline.
+ *
+ * @param[in] spline
+ * 	The spline whose number of knots is read.
+ * @return
+ * 	The number of knots of \p spline.
+ */
+size_t TINYSPLINE_API
+ts_bspline_num_knots(const tsBSpline *spline);
+
+/**
+ * Returns the size of the knot array of \p spline. This function may be useful
+ * when copying knots using \e memcpy or \e memmove.
+ *
+ * @param[in] spline
+ * 	The spline whose size of the knot array is read.
+ * @return TS_SUCCESS
+ * 	The size of the knot array of \p spline.
+ */
+size_t TINYSPLINE_API
+ts_bspline_sof_knots(const tsBSpline *spline);
+
+/**
+ * Returns the pointer to the knot vector of \p spline. Note that the return
+ * type of this function is \c const for a reason. Clients should only read the
+ * returned array. When suppressing the constness and writing to the array
+ * against better knowledge, the client is on its own with regard to the
+ * consistency of the internal state of \p spline. If the knot vector of a
+ * spline needs to be changed, use ::ts_bspline_knots to obtain a copy of the
+ * knot vector and ::ts_bspline_set_knots to copy the changed values back to
+ * the spline.
+ *
+ * @param[in] spline
+ * 	The spline whose pointer to the knot vector is returned.
+ * @return
+ * 	Pointer to the knot vector of \p spline.
+ */
+const tsReal TINYSPLINE_API *
+ts_bspline_knots_ptr(const tsBSpline *spline);
+
+/**
+ * Returns a deep copy of the knots of \p spline.
+ *
+ * @param[in] spline
+ * 	The spline whose knots are read.
+ * @param[out] knots
+ * 	The output array. \b Note: It is the responsibility of the client to
+ * 	release the allocated memory after use.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_knots(const tsBSpline *spline,
+                 tsReal **knots,
+                 tsStatus *status);
+
+/**
+ * Returns the knot of \p spline at \p index.
+ *
+ * @param[in] spline
+ * 	The spline whose knot is read at \p index.
+ * @param[in] index
+ * 	Zero-based index of the knot to be read.
+ * @param[out] knot
+ * 	The output value.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_INDEX_ERROR
+ * 	If \p index is out of range.
+ */
+tsError TINYSPLINE_API
+ts_bspline_knot_at(const tsBSpline *spline,
+                   size_t index,
+                   tsReal *knot,
+                   tsStatus *status);
+
+/**
+ * Sets the knots of \p spline. Creates a deep copy of \p knots.
+ *
+ * @pre
+ * 	\p knots has length ::ts_bspline_num_knots.
+ * @param[out] spline
+ * 	The spline whose knots are set.
+ * @param[in] knots
+ * 	The value.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_KNOTS_DECR
+ * 	If the knot vector is decreasing.
+ * @return TS_MULTIPLICITY
+ * 	If there is a knot with multiplicity > order
+ */
+tsError TINYSPLINE_API
+ts_bspline_set_knots(tsBSpline *spline,
+                     const tsReal *knots,
+                     tsStatus *status);
+
+/**
+ * Sets the knots of \p spline supplied as varargs. As all splines have at
+ * least two knots, the first two knots have a named parameter. Note that, by
+ * design of varargs in C, the last named parameter must not be float. Thus,
+ * \p knot1 is of type double instead of ::tsReal.
+ *
+ * @pre
+ * 	::ts_bspline_num_knots knots are supplied as varargs.
+ * @param[out] spline
+ *	The spline whose knots are set.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @param[in] knot0
+ * 	The first knot.
+ * @param[in] knot1
+ * 	the second knot.
+ * @param[in] ...
+ * 	The remaining knots.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ * @return TS_KNOTS_DECR
+ * 	If the knot vector is decreasing.
+ * @return TS_MULTIPLICITY
+ * 	If there is a knot with multiplicity > order
+ */
+tsError TINYSPLINE_API
+ts_bspline_set_knots_varargs(tsBSpline *spline,
+                             tsStatus *status,
+                             tsReal knot0,
+                             double knot1,
+                             ...);
+
+/**
+ * Sets the knot of \p spline at \p index.
+ *
+ * @param[in] spline
+ * 	The spline whose knot is set at \p index.
+ * @param[in] index
+ * 	Zero-based index of the knot to be set.
+ * @param[in] knot
+ * 	The value.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_INDEX_ERROR
+ * 	If \p index is out of range.
+ * @return TS_KNOTS_DECR
+ * 	If setting the knot at \p index results in a decreasing knot vector.
+ * @return TS_MULTIPLICITY
+ * 	If setting the knot at \p index results in a knot vector containing
+ * 	\p knot with multiplicity greater than the order of \p spline.
+ */
+tsError TINYSPLINE_API
+ts_bspline_set_knot_at(tsBSpline *spline,
+                       size_t index,
+                       tsReal knot,
+                       tsStatus *status);
+/*! @} */
+
+
+
+/*! @name B-Spline Initialization
+ *
+ * The following functions are used to create and release ::tsBSpline instances
+ * as well as to copy and move the internal data of a ::tsBSpline instance to
+ * another instance.
+ *
+ * \b Note: It is recommended to initialize an instance with
+ * ::ts_bspline_init. This way, ::ts_bspline_free can be called in ::TS_CATCH
+ * and ::TS_FINALLY blocks (see Section Error Handling for more details)
+ * without further checking. For example:
+ *
+ *     tsBSpline spline = ts_bspline_init();
+ *     TS_TRY(...)
+ *         ...
+ *     TS_FINALLY
+ *         ts_bspline_free(&spline);
+ *     TS_END_TRY
+ *
+ * @{
+ */
+/**
+ * Creates a new spline whose data points to NULL.
+ *
+ * @return
+ * 	A new spline whose data points to NULL.
+ */
+tsBSpline TINYSPLINE_API
+ts_bspline_init(void);
+
+/**
+ * Creates a new spline and stores the result in \p spline.
+ *
+ * @param[in] num_control_points
+ * 	The number of control points of \p spline.
+ * @param[in] dimension
+ * 	The dimension of the control points of \p spline.
+ * @param[in] degree
+ * 	The degree of \p spline.
+ * @param[in] type
+ * 	How to setup the knot vector of \p spline.
+ * @param[out] spline
+ * 	The output spline.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_DIM_ZERO
+ * 	If \p dimension is \c 0.
+ * @return TS_DEG_GE_NCTRLP
+ * 	If \p degree >= \p num_control_points.
+ * @return TS_NUM_KNOTS
+ * 	If \p type is ::TS_BEZIERS and
+ * 	(\p num_control_points % \p degree + 1) != 0.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_new(size_t num_control_points,
+               size_t dimension,
+               size_t degree,
+               tsBSplineType type,
+               tsBSpline *spline,
+               tsStatus *status);
+
+/**
+ * Creates a new spline with given control points (varargs) and stores the
+ * result in \p spline. As all splines have at least one control point (with
+ * minimum dimensionality one), the first component of the first control point
+ * has a named parameter. Note that, by design of varargs in C, the last named
+ * parameter must not be float. Thus, \p first is of type double instead of
+ * ::tsReal.
+ *
+ * @param[in] num_control_points
+ * 	The number of control points of \p spline.
+ * @param[in] dimension
+ * 	The dimension of the control points of \p spline.
+ * @param[in] degree
+ * 	The degree of \p spline.
+ * @param[in] type
+ * 	How to setup the knot vector of \p spline.
+ * @param[out] spline
+ * 	The output spline.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @param[in] first
+ * 	The first component of the first control point.
+ * @param[in] ...
+ * 	The remaining components (control points).
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_DIM_ZERO
+ * 	If \p dimension is \c 0.
+ * @return TS_DEG_GE_NCTRLP
+ * 	If \p degree >= \p num_control_points.
+ * @return TS_NUM_KNOTS
+ * 	If \p type is ::TS_BEZIERS and
+ * 	(\p num_control_points % \p degree + 1) != 0.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_new_with_control_points(size_t num_control_points,
+                                   size_t dimension,
+                                   size_t degree,
+                                   tsBSplineType type,
+                                   tsBSpline *spline,
+                                   tsStatus *status,
+                                   double first,
+                                   ...);
+
+/**
+ * Creates a deep copy of \p src and stores the copied data in \p dest. \p src
+ * and \p dest can be the same instance.
+ *
+ * \b Note: Unlike \e memcpy and \e memmove, the first parameter is the source
+ * and the second parameter is the destination.
+ *
+ * @param[in] src
+ * 	The spline to be deep copied.
+ * @param[out] dest
+ * 	The output spline.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_copy(const tsBSpline *src,
+                tsBSpline *dest,
+                tsStatus *status);
+
+/**
+ * Moves the ownership of the data of \p src to \p dest. After calling this
+ * function, the data of \p src points to NULL. Does not release the data of \p
+ * dest. \p src and \p dest can be the same instance (in this case, the data of
+ * \p src remains).
+ *
+ * @param[in, out] src
+ * 	The spline whose data is moved to \p dest.
+ * @param[out] dest
+ * 	The spline that receives the data of \p src.
+ */
+void TINYSPLINE_API
+ts_bspline_move(tsBSpline *src,
+                tsBSpline *dest);
+
+/**
+ * Releases the data of \p spline. After calling this function, the data of \p
+ * spline points to NULL.
+ *
+ * @param[out] spline
+ * 	The spline to be released.
+ */
+void TINYSPLINE_API
+ts_bspline_free(tsBSpline *spline);
+/*! @} */
+
+
+
+/*! @name De Boor Net Data
+ *
+ * The internal state of ::tsDeBoorNet is protected using the PIMPL design
+ * pattern (see https://en.cppreference.com/w/cpp/language/pimpl for more
+ * details). The data of an instance can be accessed with the functions listed
+ * in this section.
+ *
+ * @{
+ */
+/**
+ * Represents the output of De Boor's algorithm. De Boor's algorithm is used to
+ * evaluate a spline at a certain knot by iteratively computing a net of
+ * intermediate points until the resulting point is available:
+ *
+ *     https://en.wikipedia.org/wiki/De_Boor%27s_algorithm
+ *     https://www.cs.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/de-Boor.html
+ *
+ * All points of a net are stored in \c points (::ts_deboornet_points). The
+ * resulting point is the last point in \c points and, for the sake of
+ * convenience, can be accessed with ::ts_deboornet_result.
+ *
+ * Two dimensional points are stored as follows:
+ *
+ *     [x_0, y_0, x_1, y_1, ..., x_n-1, y_n-1]
+ *
+ * Tree dimensional points are stored as follows:
+ *
+ *     [x_0, y_0, z_0, x_1, y_1, z_1, ..., x_n-1, y_n-1, z_n-1]
+ *
+ * ... and so on. The output also supports homogeneous coordinates
+ * (cf. ::tsBSpline).
+ *
+ * There is a special case in which the evaluation of a knot \c u returns two
+ * results instead of one. It occurs when the multiplicity of \c u (\c s(u)) is
+ * equals to the order of the evaluated spline, indicating that the spline is
+ * discontinuous at \c u. This is common practice for B-Splines (and NURBS)
+ * consisting of connected Bezier curves where the endpoint of curve \c c_i is
+ * equal to the start point of curve \c c_i+1. Yet, the end point of \c c_i and
+ * the start point of \c c_i+1 may still be completely different, yielding to
+ * visible gaps (if distance of the points is large enough). In such case (\c
+ * s(u) == \c order), ::ts_deboornet_points stores only the two resulting
+ * points (there is no net to calculate) and ::ts_deboornet_result points to
+ * the \e first point in ::ts_deboornet_points. Since having gaps in splines is
+ * unusual, both points in ::ts_deboornet_points are generally equal, making it
+ * easy to handle this special case by simply calling
+ * ::ts_deboornet_result. However, one can access both points if necessary:
+ *
+ *     ts_deboornet_result(...)[0] ...       // Access the first component of
+ *                                           // the first result.
+ *
+ *     ts_deboornet_result(...)[dim(spline)] // Access the first component of
+ *                                           // the second result.
+ *
+ * As if this wasn't complicated enough, there is an exception for this special
+ * case, yielding to exactly one result (just like the regular case) even if \c
+ * s(u) == \c order. It occurs when \c u is the lower or upper bound of the
+ * domain of the evaluated spline. For instance, if \c b is a spline with
+ * domain [0, 1] and \c b is evaluated at \c u = \c 0 or \c u = \c 1, then
+ * ::ts_deboornet_result is \e always a single point regardless of the
+ * multiplicity of \c u.
+ *
+ * In summary, there are three different types of evaluation:
+ *
+ * 1. The regular case, in which all points of the net are returned.
+ *
+ * 2. A special case, in which two results are returned (required for splines
+ * with gaps).
+ *
+ * 3. The exception of 2., in which exactly one result is returned (even if \c
+ * s(u) == \c order).
+ *
+ * All in all this looks quite complex (and actually it is), but for most
+ * applications you do not have to deal with this. Just use
+ * ::ts_deboornet_result to access the outcome of De Boor's algorithm.
+ */
+typedef struct
+{
+	struct tsDeBoorNetImpl *pImpl; /**< The actual implementation. */
+} tsDeBoorNet;
+
+/**
+ * Returns the knot (sometimes also referred to as \c u or \c t) of \p net.
+ *
+ * @param[in] net
+ * 	The net whose knot is read.
+ * @return
+ * 	The knot of \p net.
+ */
+tsReal TINYSPLINE_API
+ts_deboornet_knot(const tsDeBoorNet *net);
+
+/**
+ * Returns the index of the knot of \p net.
+ *
+ * @param[in] net
+ * 	The net whose index is read.
+ * @return
+ * 	The index [u_k, u_k+1) with \c u being the knot of \p net.
+ */
+size_t TINYSPLINE_API
+ts_deboornet_index(const tsDeBoorNet *net);
+
+/**
+ * Returns the multiplicity of the knot of \p net.
+ *
+ * @param[in] net
+ * 	The net whose multiplicity is read.
+ * @return
+ * 	The multiplicity of the knot of \p net.
+ */
+size_t TINYSPLINE_API
+ts_deboornet_multiplicity(const tsDeBoorNet *net);
+
+/**
+ * Returns the number of insertion that were necessary to evaluate the knot of
+ * \p net.
+ *
+ * @param[in] net
+ * 	The net whose number of insertions of its knot is read.
+ * @return
+ * 	The number of insertions that were necessary to evaluate the knot of \p
+ * 	net.
+ */
+size_t TINYSPLINE_API
+ts_deboornet_num_insertions(const tsDeBoorNet *net);
+
+/**
+ * Returns the dimensionality of \p net, that is, the number of components of
+ * its points (::ts_deboornet_points) and result (::ts_deboornet_result).
+ * One-dimensional nets are possible, albeit their benefit might be
+ * questionable.
+ *
+ * @param[in] net
+ * 	The net whose dimension is read.
+ * @return
+ * 	The dimensionality of \p net (>= 1).
+ */
+size_t TINYSPLINE_API
+ts_deboornet_dimension(const tsDeBoorNet *net);
+
+/**
+ * Returns the length of the point array of \p net.
+ *
+ * @param[in] net
+ * 	The net whose length of the point array is read.
+ * @return
+ * 	The length of the point array of \p net.
+ */
+size_t TINYSPLINE_API
+ts_deboornet_len_points(const tsDeBoorNet *net);
+
+/**
+ * Returns the number of points of \p net.
+ *
+ * @param[in] net
+ * 	The net whose number of points is read.
+ * @return
+ * 	The number of points of \p net.
+ */
+size_t TINYSPLINE_API
+ts_deboornet_num_points(const tsDeBoorNet *net);
+
+/**
+ * Returns the size of the point array of \p net. This function may be useful
+ * when copying points using \e memcpy or \e memmove.
+ *
+ * @param[in] net
+ * 	The net whose size of the point array is read.
+ * @return
+ * 	The size of the point array of \p net.
+ */
+size_t TINYSPLINE_API
+ts_deboornet_sof_points(const tsDeBoorNet *net);
+
+/**
+ * Returns the pointer to the point array of \p net. Note that the return type
+ * of this function is \c const for a reason. Clients should only read the
+ * returned array. When suppressing the constness and writing to the array
+ * against better knowledge, the client is on its own with regard to the
+ * consistency of the internal state of \p net. To obtain a copy of the points
+ * of \p net, use ::ts_deboornet_points.
+ *
+ * @param[in] net
+ * 	The net whose pointer to the point array is returned.
+ * @return
+ * 	Pointer to the point array of \p net.
+ */
+const tsReal TINYSPLINE_API *
+ts_deboornet_points_ptr(const tsDeBoorNet *net);
+
+/**
+ * Returns a deep copy of the points of \p net.
+ *
+ * @param[in] net
+ * 	The net whose points are read.
+ * @param[out] points
+ * 	The output array. \b Note: It is the responsibility of the client to
+ * 	release the allocated memory after use.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_deboornet_points(const tsDeBoorNet *net,
+                    tsReal **points,
+                    tsStatus *status);
+
+/**
+ * Returns the length of the result array of \p net.
+ *
+ * @param[in] net
+ * 	The net whose length of the result array is read.
+ * @return
+ * 	The length of the result array of \p net.
+ */
+size_t TINYSPLINE_API
+ts_deboornet_len_result(const tsDeBoorNet *net);
+
+/**
+ * Returns the number of points in the result array of \p net
+ * (1 <= num_result <= 2).
+ *
+ * @param[in] net
+ * 	The net whose number of points in the result array is read.
+ * @return
+ * 	The number of points in the result array of \p net.
+ */
+size_t TINYSPLINE_API
+ts_deboornet_num_result(const tsDeBoorNet *net);
+
+/**
+ * Returns the size of the result array of \p net. This function may be useful
+ * when copying results using \e memcpy or \e memmove.
+ *
+ * @param[in] net
+ * 	The net whose size of the result array is read.
+ * @return TS_SUCCESS
+ * 	The size of the result array of \p net.
+ */
+size_t TINYSPLINE_API
+ts_deboornet_sof_result(const tsDeBoorNet *net);
+
+/**
+ * Returns the pointer to the result array of \p net. Note that the return type
+ * of this function is \c const for a reason. Clients should only read the
+ * returned array. When suppressing the constness and writing to the array
+ * against better knowledge, the client is on its own with regard to the
+ * consistency of the internal state of \p net. To obtain a copy of the result
+ * of \p net, use ::ts_deboornet_result.
+ *
+ * @param[in] net
+ * 	The net whose pointer to the result array is returned.
+ * @return
+ * 	Pointer to the result array of \p net.
+ */
+const tsReal TINYSPLINE_API *
+ts_deboornet_result_ptr(const tsDeBoorNet *net);
+
+/**
+ * Returns a deep copy of the result of \p net.
+ *
+ * @param[in] net
+ * 	The net whose result is read.
+ * @param[out] result
+ * 	The output array. \b Note: It is the responsibility of the client to
+ * 	release the allocated memory after use.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_deboornet_result(const tsDeBoorNet *net,
+                    tsReal **result,
+                    tsStatus *status);
+/*! @} */
+
+
+
+/*! @name De Boor Net Initialization
+ *
+ * The following functions are used to create and release ::tsDeBoorNet
+ * instances as well as to copy and move the internal data of a ::tsDeBoorNet
+ * instance to another instance.
+ *
+ * \b Note: It is recommended to initialize an instance with
+ * ::ts_deboornet_init. This way, ::ts_deboornet_free can be called in
+ * ::TS_CATCH and ::TS_FINALLY blocks (see Section Error Handling for more
+ * details) without further checking. For example:
+ *
+ *     tsDeBoorNet net = ts_deboornet_init();
+ *     TS_TRY(...)
+ *         ...
+ *     TS_FINALLY
+ *         ts_deboornet_free(&net);
+ *     TS_END_TRY
+ *
+ * @{
+ */
+/**
+ * Creates a new net whose data points to NULL.
+ *
+ * @return
+ * 	A new net whose data points to NULL.
+ */
+tsDeBoorNet TINYSPLINE_API
+ts_deboornet_init(void);
+
+/**
+ * Creates a deep copy of \p src and stores the copied data in \p dest. \p src
+ * and \p dest can be the same instance.
+ *
+ * \b Note: Unlike \e memcpy and \e memmove, the first parameter is the source
+ * and the second parameter is the destination.
+ *
+ * @param[in] src
+ * 	The net to be deep copied.
+ * @param[out] dest
+ * 	The output net.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_deboornet_copy(const tsDeBoorNet *src,
+                  tsDeBoorNet *dest,
+                  tsStatus *status);
+
+/**
+ * Moves the ownership of the data of \p src to \p dest. After calling this
+ * function, the data of \p src points to NULL. Does not release the data of \p
+ * dest. \p src and \p dest can be the same instance (in this case, the data of
+ * \p src remains).
+ *
+ * @param[out] src
+ * 	The net whose data is moved to \p dest.
+ * @param[out] dest
+ * 	The net that receives the data of \p src.
+ */
+void TINYSPLINE_API
+ts_deboornet_move(tsDeBoorNet *src,
+                  tsDeBoorNet *dest);
+
+/**
+ * Releases the data of \p net. After calling this function, the data of \p net
+ * points to NULL.
+ *
+ * @param[out] net
+ * 	The net to be released.
+ */
+void TINYSPLINE_API
+ts_deboornet_free(tsDeBoorNet *net);
+/*! @} */
+
+
+
+/*! @name Interpolation and Approximation Functions
+ *
+ * Given a set (or a sequence) of points, interpolate/approximate a spline that
+ * follows these points.
+ *
+ * Note: Approximations have not yet been implemented. Pull requests are
+ * welcome.
+ *
+ * @{
+ */
+/**
+ * Interpolates a cubic spline with natural end conditions. For more details
+ * see:
+ *
+ *     https://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm
+ *     http://www.math.ucla.edu/~baker/149.1.02w/handouts/dd_splines.pdf
+ *     http://www.bakoma-tex.com/doc/generic/pst-bspline/pst-bspline-doc.pdf
+ *
+ * The interpolated spline is a sequence of bezier curves connecting each point
+ * in \p points. Each bezier curve is of degree \c 3 with dimensionality \p
+ * dimension. The total number of control points is:
+ *
+ *     min(1, \p num_points - 1) * 4
+ *
+ * Note: \p num_points is the number of points in \p points and not the length
+ * of \p points. For instance, the following point vector has
+ * \p num_points = 4 and \p dimension = 2:
+ *
+ *     [x0, y0, x1, y1, x2, y2, x3, y3]
+ *
+ * @param[in] points
+ * 	The points to be interpolated.
+ * @param[in] num_points
+ * 	The number of points in \p points. If \c 1, a cubic point (i.e., a
+ * 	spline with four times the same control point) is created.
+ * @param[in] dimension
+ * 	The dimensionality of the points.
+ * @param[out] spline
+ * 	The interpolated spline.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_DIM_ZERO
+ * 	If \p dimension is 0.
+ * @return TS_NUM_POINTS
+ * 	If \p num_points is 0.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_interpolate_cubic_natural(const tsReal *points,
+                                     size_t num_points,
+                                     size_t dimension,
+                                     tsBSpline *spline,
+                                     tsStatus *status);
+
+/**
+ * Interpolates a piecewise cubic spline by translating the given catmull-rom
+ * control points into a sequence of bezier curves. In order to avoid division
+ * by zero, successive control points with distance less than or equal to \p
+ * epsilon are filtered out. If the resultant sequence contains only a single
+ * point, a cubic point (i.e., a spline with four times the same control point)
+ * is created. Optionally, the first and last control point can be specified
+ * (see \p first and \p last).
+ *
+ * @param[in] points
+ * 	The points to be interpolated.
+ * @param[in] num_points
+ * 	The number of points in \p points. If \c 1, a cubic point (i.e., a
+ * 	spline with four times the same control point) is created.
+ * @param[in] dimension
+ * 	The dimensionality of the points.
+ * @param[in] alpha
+ * 	Knot parameterization: 0 => uniform, 0.5 => centripetal, 1 => chordal.
+ * 	The input value is clamped to the domain [0, 1].
+ * @param[in] first
+ * 	The first control point of the catmull-rom sequence. If NULL, an
+ * 	appropriate point is generated based on the first two points in
+ * 	\p points. If the distance between \p first and the first control point
+ * 	in \p points is less than or equals to \p epsilon, \p first is treated
+ * 	as NULL. This is necessary to avoid division by zero.
+ * @param[in] last
+ * 	The last control point of the catmull-rom sequence. If NULL, an
+ * 	appropriate point is generated based on the last two points in
+ * 	\p points. If the distance between \p last and the last control point
+ * 	in \p points is less than or equals to \p epsilon, \p last is treated
+ * 	as NULL. This is necessary to avoid division by zero.
+ * @param[in] epsilon
+ * 	The maximum distance between points with "same" coordinates. That is,
+ * 	if the distance between neighboring points is less than or equal to
+ * 	\p epsilon, they are considered equal. For the sake of fail-safeness,
+ * 	the sign is removed with fabs. It is advisable to pass a value greater
+ * 	than zero, however, it is not necessary.
+ * @param[out] spline
+ * 	The interpolated spline.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_DIM_ZERO
+ * 	If \p dimension is 0.
+ * @return TS_NUM_POINTS
+ * 	If \p num_points is 0.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_interpolate_catmull_rom(const tsReal *points,
+                                   size_t num_points,
+                                   size_t dimension,
+                                   tsReal alpha,
+                                   const tsReal *first,
+                                   const tsReal *last,
+                                   tsReal epsilon,
+                                   tsBSpline *spline,
+                                   tsStatus *status);
+/*! @} */
+
+
+
+/*! @name Query Functions
+ *
+ * Functions for querying different kinds of data from splines.
+ *
+ * @{
+ */
+/**
+ * Evaluates \p spline at \p knot and stores the result (see ::tsDeBoorNet) in
+ * \p net.
+ *
+ * @param[in] spline
+ * 	The spline to evaluate.
+ * @param[in] knot
+ * 	The knot to evaluate \p spline at.
+ * @param[out] net
+ * 	Stores the evaluation result.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_U_UNDEFINED
+ * 	If \p spline is not defined at \p knot.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_eval(const tsBSpline *spline,
+                tsReal knot,
+                tsDeBoorNet *net,
+                tsStatus *status);
+
+/**
+ * Evaluates \p spline at each knot in \p knots and stores the evaluated points
+ * (see ::ts_deboornet_result) in \p points. If \p knots contains one or more
+ * knots where \p spline is discontinuous at, only the first point of the
+ * corresponding evaluation result is taken. After calling this function \p
+ * points contains exactly \code num * ts_bspline_dimension(spline) \endcode
+ * values.
+ *
+ * This function is in particular useful in cases where a multitude of knots
+ * need to be evaluated, because only a single instance of ::tsDeBoorNet is
+ * created and reused for all evaluation tasks (therefore the memory footprint
+ * is reduced to a minimum).
+ *
+ * @param[in] spline
+ * 	The spline to evaluate.
+ * @param[in] knots
+ * 	The knot values to evaluate.
+ * @param[in] num
+ * 	The number of knots in \p us.
+ * @param[out] points
+ * 	The output parameter.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_U_UNDEFINED
+ * 	If \p spline is not defined at one of the knot values in \p us.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_eval_all(const tsBSpline *spline,
+                    const tsReal *knots,
+                    size_t num,
+                    tsReal **points,
+                    tsStatus *status);
+
+/**
+ * Generates a sequence of \p num different knots, passes this sequence to
+ * ::ts_bspline_eval_all, and stores the resultant points in \p points. The
+ * sequence of knots is generated using ::ts_bspline_uniform_knot_seq. If \p
+ * num is 0, the default value \c 100 is used as fallback.
+ *
+ * For the sake of stability regarding future changes, the actual number of
+ * generated knots (which only differs from \p num if \p num is 0) is stored in
+ * \p actual_num. If \p num is 1, the point located at the minimum of the
+ * domain of \p spline is evaluated.
+ *
+ * @param[in] spline
+ * 	The spline to be evaluate.
+ * @param[in] num
+ * 	The number of knots to be generate.
+ * @param[out] points
+ * 	The output parameter.
+ * @param[out] actual_num
+ * 	The actual number of generated knots. Differs from \p num only if
+ * 	\p num is 0. Must not be NULL.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_sample(const tsBSpline *spline,
+                  size_t num,
+                  tsReal **points,
+                  size_t *actual_num,
+                  tsStatus *status);
+
+/**
+ * Tries to find a point P on \p spline such that:
+ *
+ *     ts_distance(P[index], value, 1) <= fabs(epsilon)
+ *
+ * This function is using the bisection method to determine P. Accordingly, it
+ * is expected that the control points of \p spline are sorted at component
+ * \p index either in ascending order (if \p ascending != 0) or in descending
+ * order (if \p ascending == 0). If the control points of \p spline are not
+ * sorted at component \p index, the behaviour of this function is undefined.
+ * For the sake of fail-safeness, the distance of P[index] and \p value is
+ * compared with the absolute value of \p epsilon (using fabs).
+ *
+ * The bisection method is an iterative approach which minimizes the error
+ * (\p epsilon) with each iteration step until an "optimum" was found. However,
+ * there may be no point P satisfying the distance condition. Thus, the number
+ * of iterations must be limited (\p max_iter). Depending on the domain of the
+ * control points of \p spline at component \p index and \p epsilon,
+ * \p max_iter ranges from 7 to 50. In most cases \p max_iter == 30 should be
+ * fine though. The parameter \p persnickety allows to define the behaviour of
+ * this function is case no point was found after \p max_iter iterations. If
+ * enabled (!= 0), TS_NO_RESULT is returned. If disabled (== 0), the best
+ * fitting point is returned.
+ *
+ * @param[in] spline
+ * 	The spline to evaluate
+ * @param[in] value
+ * 	The value (point at component \p index) to find.
+ * @param[in] epsilon
+ * 	The maximum distance (inclusive).
+ * @param[in] persnickety
+ * 	Indicates whether TS_NO_RESULT should be returned if there is no point
+ * 	P satisfying the distance condition (!= 0 to enable, == 0 to disable).
+ * 	If disabled, the best fitting point is returned.
+ * @param[in] index
+ * 	The point's component.
+ * @param[in] ascending
+ * 	Indicates whether the control points of \p spline are sorted in
+ * 	ascending (!= 0) or in descending (== 0) order at component \p index.
+ * @param[in] max_iter
+ * 	The maximum number of iterations (30 is a sane default value).
+ * @param[out] net
+ * 	The output parameter.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_INDEX_ERROR
+ * 	If the dimension of the control points of \p spline <= \p index.
+ * @return TS_NO_RESULT
+ * 	If \p persnickety is enabled (!= 0) and there is no point P satisfying
+ * 	the distance condition.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_bisect(const tsBSpline *spline,
+                  tsReal value,
+                  tsReal epsilon,
+                  int persnickety,
+                  size_t index,
+                  int ascending,
+                  size_t max_iter,
+                  tsDeBoorNet *net,
+                  tsStatus *status);
+
+/**
+ * Returns the domain of \p spline.
+ *
+ * @param[in] spline
+ * 	The spline to query.
+ * @param[out] min
+ * 	The lower bound of the domain of \p spline.
+ * @param[out] max
+ * 	The upper bound of the domain of \p spline.
+ */
+void TINYSPLINE_API
+ts_bspline_domain(const tsBSpline *spline,
+                  tsReal *min,
+                  tsReal *max);
+
+/**
+ * Checks whether the distance of the endpoints of \p spline is less than or
+ * equal to \p epsilon for the first 'ts_bspline_degree - 1' derivatives
+ * (starting with the zeroth derivative).
+ *
+ * @param[in] spline
+ * 	The spline to query.
+ * @param[in] epsilon
+ * 	The maximum distance.
+ * @param[out] closed
+ * 	The output parameter. 1 if true, 0 otherwise.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_is_closed(const tsBSpline *spline,
+                     tsReal epsilon,
+                     int *closed,
+                     tsStatus *status);
+
+/**
+ * Computes a sequence of three-dimensional frames (see ::tsFrame) for the
+ * spline \p spline. The position of the frames corresponds to the knots in \p
+ * knots. The implementation is based on:
+ *
+ *     @article{10.1145/1330511.1330513,
+ *       author =       {Wang, Wenping and J\"{u}ttler, Bert and Zheng, Dayue
+ *                       and Liu, Yang},
+ *       title =        {Computation of Rotation Minimizing Frames},
+ *       year =         {2008},
+ *       issue_date =   {March 2008},
+ *       publisher =    {Association for Computing Machinery},
+ *       address =      {New York, NY, USA},
+ *       volume =       {27},
+ *       number =       {1},
+ *       issn =         {0730-0301},
+ *       url =          {https://doi.org/10.1145/1330511.1330513},
+ *       doi =          {10.1145/1330511.1330513},
+ *       abstract =     {Due to its minimal twist, the rotation minimizing
+ *                       frame (RMF) is widely used in computer graphics,
+ *                       including sweep or blending surface modeling, motion
+ *                       design and control in computer animation and
+ *                       robotics, streamline visualization, and tool path
+ *                       planning in CAD/CAM. We present a novel simple and
+ *                       efficient method for accurate and stable computation
+ *                       of RMF of a curve in 3D. This method, called the
+ *                       double reflection method, uses two reflections to
+ *                       compute each frame from its preceding one to yield a
+ *                       sequence of frames to approximate an exact RMF. The
+ *                       double reflection method has the fourth order global
+ *                       approximation error, thus it is much more accurate
+ *                       than the two currently prevailing methods with the
+ *                       second order approximation error—the projection
+ *                       method by Klok and the rotation method by
+ *                       Bloomenthal, while all these methods have nearly the
+ *                       same per-frame computational cost. Furthermore, the
+ *                       double reflection method is much simpler and faster
+ *                       than using the standard fourth order Runge-Kutta
+ *                       method to integrate the defining ODE of the RMF,
+ *                       though they have the same accuracy. We also
+ *                       investigate further properties and extensions of the
+ *                       double reflection method, and discuss the
+ *                       variational principles in design moving frames with
+ *                       boundary conditions, based on RMF.},
+ *       journal =      {ACM Trans. Graph.},
+ *       month =        mar,
+ *       articleno =    {2},
+ *       numpages =     {18},
+ *       keywords =     {motion design, sweep surface, motion, differential
+ *                       geometry, Curve, rotation minimizing frame}
+ *     }
+ *
+ * @pre \p knots and \p frames have \p num entries.
+ * @param[in] spline
+ * 	The spline to query.
+ * @param[in] knots
+ * 	The knots to query \p spline at.
+ * @param[in] num
+ * 	Number of elements in \p knots and \p frames. Can be \c 0.
+ * @param[in] has_first_normal
+ * 	Indicates whether the normal of the first element of \p frames should
+ * 	be taken as starting value for the algorithm. If \c 0, the starting
+ * 	normal is determined based on the tangent of \p spline at \c knots[0].
+ * 	Note that, if the argument value is not \c 0, it is up to the caller of
+ * 	this function to ensure that the supplied normal is valid. The function
+ * 	only normalizes the supplied value.
+ * @param[in, out] frames
+ * 	Stores the computed frames.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If memory allocation failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_compute_rmf(const tsBSpline *spline,
+                       const tsReal *knots,
+                       size_t num,
+                       int has_first_normal,
+                       tsFrame *frames,
+                       tsStatus *status);
+
+/**
+ * Computes the cumulative chord lengths of the points of the given
+ * knots. Note that the first length (i.e., <tt>lengths[0]</tt>) is
+ * always \c 0, even if the minimum of the domain of \p spline is less
+ * than the first knot (i.e., <tt>knots[0]</tt>). Also, the returned
+ * lengths may be inaccurate if \p spline is discontinuous (i.e., the
+ * multiplicity of one of the interior knots is equal to the order of
+ * \p spline) with unequal evaluation points---in such case only the
+ * first result of the evaluation is included in the calculation.
+ *
+ * @pre \p knots and \p lengths have length \p num.
+ * @param[in] spline
+ * 	The spline to query.
+ * @param[in] knots
+ * 	The knots to evaluate \p spline at.
+ * @param[in] num
+ * 	Number of knots in \p knots.
+ * @param[out] lengths
+ * 	The cumulative chord lengths. <tt>lengths[i]</tt> is the length of \p
+ * 	spline at knot <tt>i</tt> (<tt>knots[i]</tt>).
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_U_UNDEFINED
+ * 	If \p spline is not defined at one of the knots in \p knots.
+ * @return TS_KNOTS_DECR
+ * 	If \p knots is not monotonically increasing.
+ * @return TS_MALLOC
+ * 	If memory allocation failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_chord_lengths(const tsBSpline *spline,
+                         const tsReal *knots,
+                         size_t num,
+                         tsReal *lengths,
+                         tsStatus *status);
+
+/**
+ * Extracts a sub-spline from \p spline with respect to the given domain
+ * <tt>[knot0, knot1]</tt>. The knots \p knot0 and \p knot1 must lie within the
+ * domain of \p spline and must not be equal according to
+ * ::ts_knots_equal. However, \p knot0 can be greater than \p knot1. In this
+ * case, the control points of the sub-spline are reversed.
+
+ * @param[in] spline
+ * 	The spline to query.
+ * @param[in] knot0
+ * 	Lower bound of the domain of the queried sub-spline if \p knot0 is less
+ * 	than \p knot1. Upper bound otherwise.
+ * @param[in] knot1
+ * 	Upper bound of the domain of the queried sub-spline if \p knot1 is
+ * 	greater than \p knot0. Lower bound otherwise.
+ * @param[out] sub
+ * 	The queried sub-spline.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_U_UNDEFINED
+ * 	If \p spline is not defined at \p knot0 or \p knot1.
+ * @return TS_NO_RESULT
+ * 	If \p knot0 and \p knot1 are equal according to ::ts_knots_equal.
+ * @return TS_MALLOC
+ * 	If memory allocation failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_sub_spline(const tsBSpline *spline,
+                      tsReal knot0,
+                      tsReal knot1,
+                      tsBSpline *sub,
+                      tsStatus *status);
+
+/**
+ * Generates a sequence of \p num knots with uniform distribution. \e Uniform
+ * means that consecutive knots in \p knots have the same distance.
+ *
+ * @param[in] spline
+ * 	The spline to query.
+ * @param[in] num
+ * 	Number of knots in \p knots.
+ * @param[out] knots
+ * 	Stores the generated knot sequence.
+ */
+void TINYSPLINE_API
+ts_bspline_uniform_knot_seq(const tsBSpline *spline,
+                            size_t num,
+                            tsReal *knots);
+
+/**
+ * Short-cut function for ::ts_chord_lengths_equidistant_knot_seq. The ordering
+ * of the parameters (in particular, \p num_samples after \p knots) is aligned
+ * to ::ts_bspline_uniform_knot_seq so that it is easier for users to replace
+ * one call with the other.
+ *
+ * @param[in] spline
+ * 	The spline to query.
+ * @param[in] num
+ * 	Number of knots in \p knots.
+ * @param[out] knots
+ * 	Stores the generated knot sequence.
+ * @param[in] num_samples
+ * 	Number of knots to be sampled for the 'reparametrization by arc length'.
+ * 	\c 200 yields a quite precise mapping (subpixel accuracy). For very,
+ * 	very high precision requirements, \c 500 should be sufficient. If \c 0,
+ * 	the default value \c 200 is used as fallback.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If memory allocation failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_equidistant_knot_seq(const tsBSpline *spline,
+                                size_t num,
+                                tsReal *knots,
+                                size_t num_samples,
+                                tsStatus *status);
+/*! @} */
+
+
+
+/*! @name Transformation Functions
+ *
+ * Transformations modify the internal state of a spline---e.g., the number of
+ * control points, the structure of the knot vector, the degree, and so on. Some
+ * transformations modify the state without changing the shape of the spline
+ * (e.g., ::ts_bspline_elevate_degree). All transformations specify at least
+ * three parameters: i) an input spline (the spline to be transformed), ii) an
+ * output spline (the spline which receives the result of the transformation),
+ * and iii) a ::tsStatus object (output parameter for error handling). Along
+ * with these parameters, additional parameters may be necessary to i) calculate
+ * the transformation (such as ::ts_bspline_tension) and ii) store additional
+ * results (such as ::ts_bspline_insert_knot). Unless stated otherwise, the
+ * order of the parameters of a transformation function, \c t, is:
+ *
+ *     t(input, [additional_input], output, [additional_output], status)
+ *
+ * \b Note: Transformation functions do not releases the memory of the output
+ * spline before assigning the result of the transformation to it. Thus, when
+ * using the same output spline multiple times, make sure to release its memory
+ * before each call (after the first one). If not, severe memory leaks are to be
+ * expected:
+ *
+ *     tsBSpline in = ...                   // an arbitrary spline
+ *     tsBSpline out = ts_bspline_init();   // stores the result
+ *
+ *     ts_bspline_to_beziers(&in, &out);    // first transformation
+ *     ...                                  // some code
+ *     ts_bspline_free(&out);               // avoid memory leak.
+ *     ts_bspline_tension(&in, 0.85, &out); // next transformation
+ *
+ * It is possible to pass a spline as input and output argument at the same
+ * time. In this case, the called transformation uses a temporary buffer to
+ * store the working data. If the transformation succeeds, the memory of the
+ * supplied spline is released and the result is assigned to it. So even if a
+ * transformation fails, the internal state of the supplied splines stays intact
+ * (i.e., it remains unchanged).
+ *
+ * \b Note: It is not necessary to release the memory of a spline which is
+ * passed as input and output argument at the same time before calling the next
+ * transformation (in fact that would fail due to a null pointer):
+ *
+ *     tsBSpline spline = ...                      // an arbitrary spline
+ *     ts_bspline_to_beziers(&spline, &spline);    // first transformation
+ *     ts_bspline_tension(&spline, 0.85, &spline); // next transformation
+ *
+ * @{
+ */
+/**
+ * Returns the \p n'th derivative of \p spline as ::tsBSpline instance. The
+ * derivative of a spline \c s of degree \c d (\c d > 0) with \c m control
+ * points and \c n knots is another spline \c s' of degree \c d-1 with \c m-1
+ * control points and \c n-2 knots, defined over \c s as:
+ *
+ * \f{eqnarray*}{
+ *   s'(u) &=& \sum_{i=0}^{n-1} N_{i+1,p-1}(u)    *
+ *                              (P_{i+1} - P_{i}) *
+ *                              p / (u_{i+p+1}-u_{i+1}) \\
+ *         &=& \sum_{i=1}^{n} N_{i,p-1}(u)      *
+ *                            (P_{i} - P_{i-1}) *
+ *                            p / (u_{i+p}-u_{i})
+ * \f}
+ *
+ * If \c s has a clamped knot vector, it can be shown that:
+ *
+ * \f{eqnarray*}{
+ *   s'(u) &=& \sum_{i=0}^{n-1} N_{i,p-1}(u)      *
+ *                              (P_{i+1} - P_{i}) *
+ *                              p / (u_{i+p+1}-u_{i+1})
+ * \f}
+ *
+ * where the multiplicity of the first and the last knot value \c u is \c p
+ * rather than \c p+1. The derivative of a point (degree 0) is another point
+ * with coordinate 0. For more details, see:
+ *
+ *     http://www.cs.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/bspline-derv.html
+ *
+ * If \p spline != \p deriv, the internal state of \p spline is not modified,
+ * that is, \p deriv is a new, independent ::tsBSpline instance.
+ *
+ * @param[in] spline
+ * 	The spline to be derived.
+ * @param[in] n
+ * 	Number of derivations.
+ * @param[in] epsilon
+ * 	The maximum distance of discontinuous points. If negative,
+ * 	discontinuity is ignored and the derivative is computed based on the
+ * 	first result of the corresponding ::tsDeBoorNet.
+ * @param[out] deriv
+ *	The derivative of \p spline.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_UNDERIVABLE
+ * 	If \p spline is discontinuous at an internal knot and the distance
+ * 	between the corresponding points is greater than \p epsilon.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_derive(const tsBSpline *spline,
+                  size_t n,
+                  tsReal epsilon,
+                  tsBSpline *deriv,
+                  tsStatus *status);
+
+/**
+ * Inserts \p knot \p num times into the knot vector of \p spline. The
+ * operation fails if \p result would have an invalid knot vector (i.e.,
+ * multiplicity(knot) > order(result)). If \p spline != \p result, the internal
+ * state of \p spline is not modified, that is, \p result is a new, independent
+ * ::tsBSpline instance.
+ *
+ * @param[in] spline
+ * 	The spline into which \p knot is inserted \p num times.
+ * @param[in] knot
+ * 	The knot to be inserted.
+ * @param[in] num
+ * 	Number of insertions.
+ * @param[out] result
+ * 	The output spline.
+ * @param[out] k
+ * 	Stores the last index of \p knot in \p result.
+ * @param status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_U_UNDEFINED
+ * 	If \p knot is not within the domain of \p spline.
+ * @return TS_MULTIPLICITY
+ * 	If the multiplicity of \p knot in \p spline plus \p num is greater than
+ * 	the order of \p spline.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_insert_knot(const tsBSpline *spline,
+                       tsReal knot, size_t num,
+                       tsBSpline *result,
+                       size_t *k,
+                       tsStatus *status);
+
+/**
+ * Splits \p spline at \p knot. That is, \p knot is inserted into \p spline \c
+ * n times such that the multiplicity of \p knot is equal the spline's order.
+ * If \p spline != \p split, the internal state of \p spline is not modified,
+ * that is, \p split is a new, independent ::tsBSpline instance.
+ *
+ * @param[in] spline
+ * 	The spline to be split.
+ * @param[in] knot
+ * 	The split point (knot).
+ * @param[out] split
+ * 	The split spline.
+ * @param[out] k
+ * 	Stores the last index of \p knot in \p split.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_U_UNDEFINED
+ * 	If \p spline is not defined at \p knot.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_split(const tsBSpline *spline,
+                 tsReal knot,
+                 tsBSpline *split,
+                 size_t *k,
+                 tsStatus *status);
+
+/**
+ * Straightens the control points of \p spline according to \p beta (0 yields a
+ * line connecting the first and the last control point; 1 keeps the original
+ * shape). The value of \p beta is clamped to [0, 1]. If \p spline != \p out,
+ * the internal state of \p spline is not modified, that is, \p out is a new,
+ * independent ::tsBSpline instance.
+ *
+ * This function is based on:
+ *
+ *     @ARTICLE{10.1109/TVCG.2006.147,
+ *       author =       {Holten, Danny},
+ *       journal =      {IEEE Transactions on Visualization and Computer
+ *                       Graphics},
+ *       title =        {Hierarchical Edge Bundles: Visualization of
+ *                       Adjacency Relations in Hierarchical Data},
+ *       year =         {2006},
+ *       volume =       {12},
+ *       number =       {5},
+ *       pages =        {741-748},
+ *       abstract =     {A compound graph is a frequently encountered type of
+ *                       data set. Relations are given between items, and a
+ *                       hierarchy is defined on the items as well. We
+ *                       present a new method for visualizing such compound
+ *                       graphs. Our approach is based on visually bundling
+ *                       the adjacency edges, i.e., non-hierarchical edges,
+ *                       together. We realize this as follows. We assume that
+ *                       the hierarchy is shown via a standard tree
+ *                       visualization method. Next, we bend each adjacency
+ *                       edge, modeled as a B-spline curve, toward the
+ *                       polyline defined by the path via the inclusion edges
+ *                       from one node to another. This hierarchical bundling
+ *                       reduces visual clutter and also visualizes implicit
+ *                       adjacency edges between parent nodes that are the
+ *                       result of explicit adjacency edges between their
+ *                       respective child nodes. Furthermore, hierarchical
+ *                       edge bundling is a generic method which can be used
+ *                       in conjunction with existing tree visualization
+ *                       techniques. We illustrate our technique by providing
+ *                       example visualizations and discuss the results based
+ *                       on an informal evaluation provided by potential
+ *                       users of such visualizations.},
+ *       keywords =     {},
+ *       doi =          {10.1109/TVCG.2006.147},
+ *       ISSN =         {1941-0506},
+ *       month =        {Sep.},
+ *     }
+ *
+ * Holten calls it "straightening" (page 744, equation 1).
+ *
+ * @param[in] spline
+ * 	The spline to be straightened.
+ * @param[in] beta
+ * 	The straightening factor. The value is clamped to the domain [0, 1].
+ * @param[out] out
+ * 	The straightened spline.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_tension(const tsBSpline *spline,
+                   tsReal beta,
+                   tsBSpline *out,
+                   tsStatus *status);
+
+/**
+ * Decomposes \p spline into a sequence of Bezier curves by splitting it at
+ * each internal knot. If \p spline != \p beziers, the internal state of \p
+ * spline is not modified, that is, \p beziers is a new, independent
+ * ::tsBSpline instance.
+ *
+ * @param[in] spline
+ * 	The spline to be decomposed.
+ * @param[out] beziers
+ * 	The bezier decomposition of \p spline.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_to_beziers(const tsBSpline *spline,
+                      tsBSpline *beziers,
+                      tsStatus *status);
+
+/**
+ * Elevates the degree of \p spline by \p amount and stores the result in
+ * \p elevated. If \p spline != \p elevated, the internal state of \p spline is
+ * not modified, that is, \p elevated is a new, independent ::tsBSpline
+ * instance.
+ *
+ * @param[in] spline
+ * 	The spline to be elevated.
+ * @param[in] amount
+ * 	How often to elevate the degree of \p spline.
+ * @param[in] epsilon
+ * 	In order to elevate the degree of a spline, it must be decomposed into
+ * 	a sequence of bezier curves (see ::ts_bspline_to_beziers). After degree
+ * 	elevation, the split points of the bezier curves are merged again. This
+ * 	parameter is used to distinguish between the split points of the
+ * 	decomposition process and discontinuity points that should be retained.
+ * 	A viable default value is ::TS_POINT_EPSILON. If negative, the resulting
+ * 	spline, \p elevated, forms a sequence of bezier curves.
+ * @param[out] elevated
+ * 	The elevated spline.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If memory allocation failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_elevate_degree(const tsBSpline *spline,
+                          size_t amount,
+                          tsReal epsilon,
+                          tsBSpline *elevated,
+                          tsStatus *status);
+
+/**
+ * Modifies the splines \p s1 and \p s2 such that they have same the degree and
+ * number of control points/knots (without modifying the shape of \p s1 and \p
+ * s2). The resulting splines are stored in \p s1_out and \p s2_out. If \p s1 !=
+ * \p s1_out, the internal state of \p s1 is not modified, that is, \p s1_out is
+ * a new, independent ::tsBSpline instance. The same is true for \p s2 and \p
+ * s2_out.
+ *
+ * @param[in] s1
+ * 	The spline which is to be aligned with \p s2.
+ * @param[in] s2
+ * 	The spline which is to be aligned with \p s1.
+ * @param[in] epsilon
+ * 	Spline alignment relies on degree elevation. This parameter is used in
+ * 	::ts_bspline_elevate_degree to check whether two control points, \c p1
+ * 	and \c p2, are "equal", that is, the distance between \c p1 and \c p2
+ * 	is less than or equal to \p epsilon. A viable default value is
+ * 	::TS_POINT_EPSILON.
+ * @param[out] s1_out
+ * 	The aligned version of \p s1.
+ * @param[out] s2_out
+ * 	The aligned version of \p s2.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If memory allocation failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_align(const tsBSpline *s1,
+                 const tsBSpline *s2,
+                 tsReal epsilon,
+                 tsBSpline *s1_out,
+                 tsBSpline *s2_out,
+                 tsStatus *status);
+
+/**
+ * Interpolates between \p origin and \p target with respect to the time
+ * parameter \p t (domain: [0, 1]; clamped if necessary). The resulting spline
+ * is stored in \p out. Because it is to be expected that this function is
+ * called several times in a row (e.g., to have a smooth transition from one
+ * spline to another), memory for \p out is allocated only if it points to NULL
+ * or if it has to be enlarged to store the result of the interpolation (which
+ * can only happen if \p origin or \p target---or both---have been changed
+ * since the last call). This way, this function can be used as follows:
+ *
+ *     tsReal t;
+ *     tsBSpline origin = ...
+ *     tsBSpline target = ...
+ *     tsBSpline morph = ts_bspline_init();
+ *     for (t = (tsReal) 0.0; t <= (tsReal) 1.0; t += (tsReal) 0.001)
+ *         ts_bspline_morph(&origin, &target, t, ..., &morph, ...);
+ *     ts_bspline_free(&morph);
+ *
+ * It should be noted that this function, if necessary, aligns \p origin and \p
+ * target using ::ts_bspline_align. In order to avoid the overhead of spline
+ * alignment, \p origin and \p target should be aligned in advance.
+ *
+ * @param[in] origin
+ * 	Origin spline.
+ * @param[in] target
+ * 	Target spline.
+ * @param[in] t
+ * 	The time parameter. If 0, \p out becomes \p origin. If 1, \p out becomes
+ * 	\p target. Note that the value passed is clamped to the domain [0, 1].
+ * @param[in] epsilon
+ * 	If \p origin and \p target must be aligned, this parameter is passed
+ * 	::ts_bspline_elevate_degree to check whether two control points, \c p1
+ * 	and \c p2, are "equal", that is, the distance between \c p1 and \c p2
+ * 	is less than or equal to \p epsilon. A viable default value is
+ * 	::TS_POINT_EPSILON.
+ * @param[out] out
+ * 	The resulting spline.
+ * @return[out] TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If memory allocation failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_morph(const tsBSpline *origin,
+                 const tsBSpline *target,
+                 tsReal t,
+                 tsReal epsilon,
+                 tsBSpline *out,
+                 tsStatus *status);
+/*! @} */
+
+
+
+/*! @name Serialization and Persistence
+ *
+ * The following functions can be used to serialize and persist (i.e., store
+ * the serialized data in a file) splines. There are also functions to load
+ * serialized splines.
+ *
+ * @{
+ */
+/**
+ * Serializes \p spline to a null-terminated JSON string and stores the result
+ * in \p json.
+ *
+ * @param[in] spline
+ * 	The spline to be serialized.
+ * @param[out] json
+ * 	The serialized JSON string.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_to_json(const tsBSpline *spline,
+                   char **json,
+                   tsStatus *status);
+
+/**
+ * Parses \p json and stores the result in \p spline.
+ *
+ * @param[in] json
+ * 	The JSON string to be parsed.
+ * @param[out] spline
+ * 	The output spline.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_PARSE_ERROR
+ * 	If an error occurred while parsing \p json.
+ * @return TS_DIM_ZERO
+ * 	If the dimension is \c 0.
+ * @return TS_LCTRLP_DIM_MISMATCH
+ * 	If the length of the control point array modulo dimension is not \c 0.
+ * @return TS_DEG_GE_NCTRLP
+ * 	If the degree is greater or equals to the number of control points.
+ * @return TS_NUM_KNOTS
+ * 	If the number of knots does not match to the number of control points
+ * 	plus the degree of the spline.
+ * @return TS_KNOTS_DECR
+ * 	If the knot vector is decreasing.
+ * @return TS_MULTIPLICITY
+ * 	If there is a knot with multiplicity greater than order.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_parse_json(const char *json,
+                      tsBSpline *spline,
+                      tsStatus *status);
+
+/**
+ * Saves \p spline as JSON ASCII file.
+ *
+ * @param[in] spline
+ * 	The spline to be saved.
+ * @param[in] path
+ * 	Path of the JSON file.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_IO_ERROR
+ * 	If an error occurred while saving \p spline.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_save(const tsBSpline *spline,
+                const char *path,
+                tsStatus *status);
+
+/**
+ * Loads \p spline from a JSON ASCII file.
+ *
+ * @param[in] path
+ * 	Path of the JSON file to be loaded.
+ * @param[out] spline
+ * 	The output spline.
+ * @param[ou] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_IO_ERROR
+ * 	If \p path does not exist.
+ * @return TS_PARSE_ERROR
+ * 	If an error occurred while parsing the contents of \p path.
+ * @return TS_DIM_ZERO
+ * 	If the dimension is \c 0.
+ * @return TS_LCTRLP_DIM_MISMATCH
+ * 	If the length of the control point array modulo dimension is not \c 0.
+ * @return TS_DEG_GE_NCTRLP
+ * 	If the degree is greater or equals to the number of control points.
+ * @return TS_NUM_KNOTS
+ * 	If the number of knots does not match to the number of control points
+ * 	plus the degree of the spline.
+ * @return TS_KNOTS_DECR
+ * 	If the knot vector is decreasing.
+ * @return TS_MULTIPLICITY
+ * 	If there is a knot with multiplicity greater than order.
+ * @return TS_MALLOC
+ * 	If allocating memory failed.
+ */
+tsError TINYSPLINE_API
+ts_bspline_load(const char *path,
+                tsBSpline *spline,
+                tsStatus *status);
+
+
+
+/*! @name Vector Math
+ *
+ * Vector math is a not insignificant part of TinySpline, and so it's not
+ * surprising that some utility functions around vectors are needed. Because
+ * these functions might be useful for others, they are part of TinySpline's
+ * public API. However, note that the code is \b not highly optimized (with,
+ * for example, instruction set extensions like SSE). If high performance
+ * vector math is needed, other libraries should be used instead.
+ *
+ * @{
+ */
+/**
+ * Initializes vector \p out with \p x and \p y.
+ *
+ * @pre
+ * 	\p out has dimensionality \c 2.
+ * @param[out] out
+ * 	Target vector.
+ * @param[in] x
+ * 	The x value.
+ * @param[in] y
+ * 	The y value.
+ */
+void TINYSPLINE_API
+ts_vec2_init(tsReal *out,
+             tsReal x,
+             tsReal y);
+
+/**
+ * Initializes vector \p out with \p x, \p y, and \p z.
+ *
+ * @pre
+ * 	\p out has dimensionality \c 3.
+ * @param[out] out
+ * 	Target vector.
+ * @param[in] x
+ * 	The x value.
+ * @param[in] y
+ * 	The y value.
+ * @param[in] z
+ * 	The z value.
+ */
+void TINYSPLINE_API
+ts_vec3_init(tsReal *out,
+             tsReal x,
+             tsReal y,
+             tsReal z);
+
+/**
+ * Initializes vector \p out with \p x, \p y, \p z, and \p w.
+ *
+ * @pre
+ * 	\p out has dimensionality \c 4.
+ * @param[out] out
+ * 	Target vector.
+ * @param[in] x
+ * 	The x value.
+ * @param[in] y
+ * 	The y value.
+ * @param[in] z
+ * 	The z value.
+ * @param[in] w
+ * 	The w value.
+ */
+void TINYSPLINE_API
+ts_vec4_init(tsReal *out,
+             tsReal x,
+             tsReal y,
+             tsReal z,
+             tsReal w);
+
+/**
+ * Copies the values of vector \p x (a vector with dimensionality \p dim) to
+ * vector \p out (a vector with dimensionality \c 2). If \p dim is less than \c
+ * 2, the remaining values of \p out are set to \c 0. Excess values in \p x
+ * (i.e., \p dim is greater than \c 2) are ignored.
+ *
+ * @pre
+ * 	\p out has dimensionality \c 2.
+ * @param[out] out
+ * 	Target vector.
+ * @param[in] x
+ * 	Vector to read the values from.
+ * @param[in] dim
+ * 	Dimensionality of \p x.
+ */
+void TINYSPLINE_API
+ts_vec2_set(tsReal *out,
+            const tsReal *x,
+            size_t dim);
+
+/**
+ * Copies the values of vector \p x (a vector with dimensionality \p dim) to
+ * vector \p out (a vector with dimensionality \c 3). If \p dim is less than \c
+ * 3, the remaining values of \p out are set to \c 0. Excess values in \p x
+ * (i.e., \p dim is greater than \c 3) are ignored.
+ *
+ * @pre
+ * 	\p out has dimensionality \c 3.
+ * @param[out] out
+ * 	Target vector.
+ * @param[in] x
+ * 	Vector to read the values from.
+ * @param[in] dim
+ * 	Dimensionality of \p x.
+ */
+void TINYSPLINE_API
+ts_vec3_set(tsReal *out,
+            const tsReal *x,
+            size_t dim);
+
+/**
+ * Copies the values of vector \p x (a vector with dimensionality \p dim) to
+ * vector \p out (a vector with dimensionality \c 4). If \p dim is less than \c
+ * 4, the remaining values of \p out are set to \c 0. Excess values in \p x
+ * (i.e., \p dim is greater than \c 4) are ignored.
+ *
+ * @pre
+ * 	\p out has dimensionality \c 4.
+ * @param[out] out
+ * 	Target vector.
+ * @param[in] x
+ * 	Vector to read the values from.
+ * @param[in] dim
+ * 	Dimensionality of \p x.
+ */
+void TINYSPLINE_API
+ts_vec4_set(tsReal *out,
+            const tsReal *x,
+            size_t dim);
+
+/**
+ * Adds vector \p y to vector \p x and stores the result in vector \p out.
+ *
+ * @param[in] x
+ * 	First vector.
+ * @param[in] y
+ * 	Second vector.
+ * @param[in] dim
+ * 	Dimensionality of \p x, \p y, and \p out.
+ * @param[out] out
+ * 	Result vector. Can be same as \p x or \p y, i.e., the result can be
+ * 	stored in-place.
+ */
+void TINYSPLINE_API
+ts_vec_add(const tsReal *x,
+           const tsReal *y,
+           size_t dim,
+           tsReal *out);
+
+/**
+ * Subtracts vector \p y from vector \p x and stores the result in vector \p
+ * out.
+ *
+ * @param[in] x
+ * 	First vector.
+ * @param[in] y
+ * 	Second vector.
+ * @param[in] dim
+ * 	Dimensionality of \p x, \p y, and \p out.
+ * @param[out] out
+ * 	Result vector. Can be same as \p x or \p y, i.e., the result can be
+ * 	stored in-place.
+ */
+void TINYSPLINE_API
+ts_vec_sub(const tsReal *x,
+           const tsReal *y,
+           size_t dim,
+           tsReal *out);
+
+/**
+ * Computes the dot product (also known as scalar product) of the vectors \p x
+ * and \p y.
+ *
+ * @post
+ * 	\c 0 if \p dim is \c 0.
+ * @param[in] x
+ * 	First vector.
+ * @param[in] y
+ * 	Second vector.
+ * @param[in] dim
+ * 	Dimensionality of \p x and \p y.
+ * @return
+ * 	The dot product of \p x and \y.
+ */
+tsReal TINYSPLINE_API
+ts_vec_dot(const tsReal *x,
+           const tsReal *y,
+           size_t dim);
+
+/**
+ * Computes the angle in degrees between the vectors \p x and \p y. The angle
+ * returned is unsigned, that is, the smaller of the two possible angles is
+ * computed. The nullable parameter \p buf servers as a buffer in case \p x or
+ * \p y (or both) are not normalized. If \p buf is \c NULL, it is expected that
+ * \p x and \p y are already normalized. If \p buf is not \c NULL, a storage
+ * twice the size of \p dim is expected in which the normalized vectors of \p x
+ * and \p y are stored.
+ *
+ * @pre
+ * 	\p buf is either \c NULL or has length <tt>2 * dim</tt>.
+ * @param[in] x
+ * 	First vector.
+ * @param[in] y
+ * 	Second vector.
+ * @param[out] buf
+ * 	A buffer in which the normalized vectors of \p x and \y are stored. If
+ * 	\c NULL, it is expected that \p x and \p y are already normalized.
+ * @param[in] dim
+ * 	Dimensionality of \p x and \p y.
+ * @return
+ * 	The angle between \p x and \y with <tt>0.0 <= angle <= 180.0</tt>.
+ */
+tsReal TINYSPLINE_API
+ts_vec_angle(const tsReal *x,
+             const tsReal *y,
+             tsReal *buf,
+             size_t dim);
+
+/**
+ * Computes the cross product (also known as vector product or directed area
+ * product) of the vectors \p x and \p y.
+ *
+ * @pre \p x and \p y have dimensionality \c 3.
+ * @param[in] x
+ * 	First vector.
+ * @param[in] y
+ * 	Second vector.
+ * @param[out] out
+ * 	Result vector. Can be same as \p x or \p y, i.e., the result can be
+ * 	stored in-place.
+ */
+void TINYSPLINE_API
+ts_vec3_cross(const tsReal *x,
+              const tsReal *y,
+              tsReal *out);
+
+/**
+ * Normalizes vector \p x.
+ *
+ * @post
+ * 	\c 0 if the length of \p x (see ::ts_vec_mag) is less than
+ * 	::TS_LENGTH_ZERO.
+ * @param[in] x
+ * 	A vector.
+ * @param[in] dim
+ * 	Dimensionality of \p x.
+ * @param[out] out
+ * 	Result vector. Can be same as \p x, i.e., the result can be stored
+ * 	in-place.
+ */
+void TINYSPLINE_API
+ts_vec_norm(const tsReal *x,
+            size_t dim,
+            tsReal *out);
+
+/**
+ * Determines the length of vector \p x.
+ *
+ * @post
+ * 	\c 0 if \p dim is \c 0.
+ * @param[in] x
+ * 	A vector.
+ * @param[in] dim
+ * 	Dimensionality of \p x.
+ */
+tsReal TINYSPLINE_API
+ts_vec_mag(const tsReal *x,
+           size_t dim);
+
+/**
+ * Multiplies vector \p x with scalar \p val and stores the result in vector \p
+ * out.
+ *
+ * @param[in] x
+ * 	A vector.
+ * @param[in] dim
+ * 	Dimensionality of \p x.
+ * @param[in] val
+ * 	Scalar value.
+ * @param[out] out
+ * 	Result vector. Can be same as \p x, i.e., the result can be stored
+ * 	in-place.
+ */
+void TINYSPLINE_API
+ts_vec_mul(const tsReal *x,
+           size_t dim,
+           tsReal val,
+           tsReal *out);
+/*! @} */
+
+
+
+/*! @name Chord Length Method
+ *
+ * Functions for processing the cumulative chord lengths of a spline
+ * as computed by ::ts_bspline_chord_lengths.
+ *
+ * @{
+ */
+/**
+ * Maps \p len to a knot, \c k, such that <tt>ts_bspline_eval(..., k ...)</tt>
+ * yields a point whose length, with respect to <tt>knots[0]</tt>, is close to
+ * \p len. Note that \p len is clamped to the domain of \p lengths. The domain
+ * of the result, \p knot, is <tt>[knots[0], knots[num-1]]</tt>.
+ *
+ * The precision of the mapping depends on the resolution of \p knots and \p
+ * lengths. That is, the more chord lengths were computed, the more precise the
+ * length-to-knot-mapping becomes. Generally, \c 200 chord lengths yields a
+ * quite precise mapping (subpixel accuracy). For very, very high precision
+ * requirements, \c 500 chord lengths should be sufficient.
+ *
+ * Returns ::TS_NO_RESULT if \p num is \c 0.
+ *
+ * @pre
+ * 	\p lengths is monotonically increasing and contains only non-negative
+ * 	values (distances cannot be negative).
+ * @param[in] knots
+ * 	Knots that were passed to ::ts_bspline_chord_lengths.
+ * @param[in] lengths
+ * 	Cumulative chord lengths as computed by ::ts_bspline_chord_lengths.
+ * @param[in] num
+ * 	Number of values in \p knots and \p lengths.
+ * @param[in] len
+ * 	Length to be mapped. Clamped to the domain of \p lengths.
+ * @param[out] knot
+ * 	A knot, such that <tt>ts_bspline_eval(..., knot ...)</tt> yields a
+ * 	point whose length, with respect to <tt>knots[0]</tt>, is close to \p
+ * 	len.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_NO_RESULT
+ * 	If \p num is \c 0.
+ */
+tsError TINYSPLINE_API
+ts_chord_lengths_length_to_knot(const tsReal *knots,
+                                const tsReal *lengths,
+                                size_t num,
+                                tsReal len,
+                                tsReal *knot,
+                                tsStatus *status);
+
+/**
+ * Same as ::ts_chord_lengths_length_to_knot, except that this function takes a
+ * chord length parameter, \p t, with domain [0, 1] (clamped), which indicates
+ * the to be evaluated relative proportion of the total length
+ * (<tt>lengths[num-1]</tt>).
+ *
+ * @pre
+ * 	\p lengths is monotonically increasing and contains only non-negative
+ * 	values (distances cannot be negative).
+ * @param[in] knots
+ * 	Knots that were passed to ::ts_bspline_chord_lengths.
+ * @param[in] lengths
+ * 	Cumulative chord lengths as computed by ::ts_bspline_chord_lengths.
+ * @param[in] num
+ * 	Number of values in \p knots and \p lengths.
+ * @param[in] t
+ * 	Chord length parameter (relative proportion of the total length).
+ * 	Clamped to the domain [0, 1].
+ * @param[out] knot
+ * 	A knot, such that <tt>ts_bspline_eval(..., knot ...)</tt> yields a
+ * 	point whose length, with respect to <tt>knots[0]</tt>, is close to
+ * 	<tt>t * lengths[num-1]<tt>
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_NO_RESULT
+ * 	If \p num is \c 0.
+ */
+tsError TINYSPLINE_API
+ts_chord_lengths_t_to_knot(const tsReal *knots,
+                           const tsReal *lengths,
+                           size_t num,
+                           tsReal t,
+                           tsReal *knot,
+                           tsStatus *status);
+
+/**
+ * Generates a sequence of \p num_knot_seq knots with equidistant distribution.
+ * \e Equidistant means that the points evaluated from consecutive knots in \p
+ * knot_seq have the same distance along the spline. This is also known as
+ * 'reparametrization by arc length'.
+ *
+ * @pre
+ * 	\p lengths is monotonically increasing and contains only non-negative
+ * 	values (distances cannot be negative).
+ * @param[in] knots
+ * 	Knots that were passed to ::ts_bspline_chord_lengths.
+ * @param[in] lengths
+ * 	Cumulative chord lengths as computed by ::ts_bspline_chord_lengths.
+ * @param[in] num
+ * 	Number of values in \p knots and \p lengths.
+ * @param[in] num_knot_seq
+ * 	Number of knots in \p knot_seq.
+ * @param[out] knot_seq
+ * 	Stores the generated knot sequence.
+ * @param[out] status
+ * 	The status of this function. May be NULL.
+ * @return TS_SUCCESS
+ * 	On success.
+ * @return TS_NO_RESULT
+ * 	If \p num is \c 0.
+ */
+tsError TINYSPLINE_API
+ts_chord_lengths_equidistant_knot_seq(const tsReal *knots,
+                                      const tsReal *lengths,
+                                      size_t num,
+                                      size_t num_knot_seq,
+                                      tsReal *knot_seq,
+                                      tsStatus *status);
+/*! @} */
+
+
+
+/*! @name Utility Functions
+ *
+ * @{
+ */
+/**
+ * Returns whether the knots \p x and \p y are equal with respect to the epsilon
+ * environment ::TS_KNOT_EPSILON (i.e., their distance is <u>less</u> than
+ * ::TS_KNOT_EPSILON).
+ *
+ * @param[in] x
+ * 	First knot.
+ * @param[in] y
+ * 	Second knot.
+ * @return 1
+ * 	If \p x and \p y are equal.
+ * @return 0
+ * 	If \p x and \p y are not equal.
+ */
+int TINYSPLINE_API
+ts_knots_equal(tsReal x,
+               tsReal y);
+
+/**
+ * Fills the given array \p arr with \p val.
+ *
+ * @param[in] arr
+ * 	The array to be filled.
+ * @param[in] num
+ * 	Fill length.
+ * @param[in] val
+ * 	The value to fill into \p arr.
+ */
+void TINYSPLINE_API
+ts_arr_fill(tsReal *arr,
+            size_t num,
+            tsReal val);
+
+/**
+ * Returns the euclidean distance of the points \p x and \p y.
+ *
+ * @param[in] x
+ * 	First point.
+ * @param[in] y
+ * 	Second point.
+ * @param[in] dim
+ * 	Dimensionality of \p x and \p y.
+ * @return
+ * 	The euclidean distance of the points \p x and \p y.
+ */
+tsReal TINYSPLINE_API
+ts_distance(const tsReal *x,
+            const tsReal *y,
+            size_t dim);
+/*! @} */
+
+
+
+#ifdef	__cplusplus
+}
+#endif
+
+#endif	/* TINYSPLINE_H */
+
diff --git a/projects/Roads/utilities/include/roads/thirdparty/tinysplinecxx.h b/projects/Roads/utilities/include/roads/thirdparty/tinysplinecxx.h
new file mode 100644
index 0000000000..2f9d87fd26
--- /dev/null
+++ b/projects/Roads/utilities/include/roads/thirdparty/tinysplinecxx.h
@@ -0,0 +1,966 @@
+/** @file */
+
+#pragma once
+
+#include "tinyspline.h"
+#include <vector>
+#include <string>
+
+#ifndef TINYSPLINECXX_API
+#define TINYSPLINECXX_API TINYSPLINE_API
+#endif
+
+
+
+/*! @name Emscripten Extensions
+ *
+ * Please see the following references for more details on how to use
+ * Emscripten with C++:
+ *
+ * https://emscripten.org/docs/porting/connecting_cpp_and_javascript/embind.html
+ * https://emscripten.org/docs/api_reference/bind.h.html
+ *
+ * @{
+ */
+#ifdef TINYSPLINE_EMSCRIPTEN
+
+// Additional includes and namespaces.
+#include <stdexcept>
+#include <emscripten/bind.h>
+using namespace emscripten;
+
+/* Used by value_objects to guard read-only properties. */
+void inline
+cannotWrite()
+{ throw std::runtime_error("cannot write read-only property"); }
+
+/* Allow clients to read exception messages in the Javascript binding. */
+std::string
+exceptionMessage(int ptr)
+{ return std::string(reinterpret_cast<std::exception *>(ptr)->what()); }
+
+// Map: std::vector <--> JS array
+// https://github.com/emscripten-core/emscripten/issues/11070#issuecomment-717675128
+namespace emscripten {
+namespace internal {
+template <typename T, typename Allocator>
+struct BindingType<std::vector<T, Allocator>> {
+	using ValBinding = BindingType<val>;
+	using WireType = ValBinding::WireType;
+
+	static WireType
+	toWireType(const std::vector<T, Allocator> &vec)
+	{ return ValBinding::toWireType(val::array(vec)); }
+
+	static std::vector<T, Allocator>
+	fromWireType(WireType value)
+	{ return vecFromJSArray<T>(ValBinding::fromWireType(value)); }
+};
+
+template <typename T>
+struct TypeID<T, // this is ridiculous...
+              typename std::enable_if_t<std::is_same<
+                  typename Canonicalized<T>::type,
+                  std::vector<typename Canonicalized<T>::type::value_type,
+                  typename Canonicalized<T>::type::allocator_type>>::value>> {
+	static constexpr TYPEID
+	get() { return TypeID<val>::get(); }
+};
+}  // namespace internal
+}  // namespace emscripten
+#endif
+/*! @} */
+
+
+
+namespace tinyspline {
+
+/*! @name API Configuration
+ *
+ * @{
+ */
+typedef tsReal real;
+/*! @} */
+
+
+
+/*! @name Swig Type Mapping
+ *
+ * Methods that do not return or set the value of a class attribute (let's call
+ * these methods `non-accessor methods') must return/take instances of
+ * std::vector as pointer. Otherwise, they aren't type mapped by Swig to the
+ * std::vector representation of the target language.
+ *
+ * @{
+ */
+#ifdef SWIG
+using std_real_vector_in = std::vector<tinyspline::real> *;
+using std_real_vector_out = std::vector<tinyspline::real> *;
+#else
+using std_real_vector_in = const std::vector<tinyspline::real> &;
+using std_real_vector_out = std::vector<tinyspline::real>;
+#endif
+/*! @} */
+
+
+
+/*! @name Vector Math
+ *
+ * Wrapper classes for TinySpline's vector math.
+ *
+ * @{
+ */
+class TINYSPLINECXX_API Vec2 {
+public:
+	Vec2();
+	Vec2(real x, real y);
+
+	Vec2 operator+(const Vec2 &other);
+	Vec2 operator-(const Vec2 &other);
+	Vec2 operator*(real scalar);
+
+	real x() const;
+	void setX(real val);
+	real y() const;
+	void setY(real val);
+	std::vector<real> values() const;
+
+	Vec2 add(const Vec2 &other) const;
+	Vec2 subtract(const Vec2 &other) const;
+	Vec2 multiply(real scalar) const;
+	Vec2 normalize() const;
+	real magnitude() const;
+	real dot(const Vec2 &other) const;
+	real angle(const Vec2 &other) const;
+	real distance(const Vec2 &other) const;
+
+	std::string toString() const;
+
+private:
+	real m_vals[2];
+
+#ifdef TINYSPLINE_EMSCRIPTEN
+public:
+	void setValues(std::vector<real>) { cannotWrite(); }
+#endif
+};
+
+class TINYSPLINECXX_API Vec3 {
+public:
+	Vec3();
+	Vec3(real x, real y, real z);
+
+	Vec3 operator+(const Vec3 &other);
+	Vec3 operator-(const Vec3 &other);
+	Vec3 operator*(real scalar);
+
+	real x() const;
+	void setX(real val);
+	real y() const;
+	void setY(real val);
+	real z() const;
+	void setZ(real val);
+	std::vector<real> values() const;
+
+	Vec3 add(const Vec3 &other) const;
+	Vec3 subtract(const Vec3 &other) const;
+	Vec3 multiply(real scalar) const;
+	Vec3 cross(const Vec3 &other) const;
+	Vec3 normalize() const;
+	real magnitude() const;
+	real dot(const Vec3 &other) const;
+	real angle(const Vec3 &other) const;
+	real distance(const Vec3 &other) const;
+
+	std::string toString() const;
+
+private:
+	real m_vals[3];
+
+#ifdef TINYSPLINE_EMSCRIPTEN
+public:
+	void setValues(std::vector<real>) { cannotWrite(); }
+#endif
+};
+
+class TINYSPLINECXX_API Vec4 {
+public:
+	Vec4();
+	Vec4(real x, real y, real z, real w);
+
+	Vec4 operator+(const Vec4 &other);
+	Vec4 operator-(const Vec4 &other);
+	Vec4 operator*(real scalar);
+
+	real x() const;
+	void setX(real val);
+	real y() const;
+	void setY(real val);
+	real z() const;
+	void setZ(real val);
+	real w() const;
+	void setW(real val);
+	std::vector<real> values() const;
+
+	Vec4 add(const Vec4 &other) const;
+	Vec4 subtract(const Vec4 &other) const;
+	Vec4 multiply(real scalar) const;
+	Vec4 normalize() const;
+	real magnitude() const;
+	real dot(const Vec4 &other) const;
+	real angle(const Vec4 &other) const;
+	real distance(const Vec4 &other) const;
+
+	std::string toString() const;
+
+private:
+	real m_vals[4];
+
+#ifdef TINYSPLINE_EMSCRIPTEN
+public:
+	void setValues(std::vector<real>) { cannotWrite(); }
+#endif
+};
+
+#ifdef TINYSPLINE_EMSCRIPTEN
+class VecMath {
+public:
+	/*! @name Add
+	*
+	* @{
+	*/
+	static Vec2
+	add2(const Vec2 &a, const Vec2 &b)
+	{ return a.add(b); }
+
+	static Vec3
+	add3(const Vec3 &a, const Vec3 &b)
+	{ return a.add(b); }
+
+	static Vec4
+	add4(const Vec4 &a, const Vec4 &b)
+	{ return a.add(b); }
+	/*! @} */
+
+	/*! @name Subtract
+	*
+	* @{
+	*/
+	static Vec2
+	subtract2(const Vec2 &a, const Vec2 &b)
+	{ return a.subtract(b); }
+
+	static Vec3
+	subtract3(const Vec3 &a, const Vec3 &b)
+	{ return a.subtract(b); }
+
+	static Vec4
+	subtract4(const Vec4 &a, const Vec4 &b)
+	{ return a.subtract(b); }
+	/*! @} */
+
+	/*! @name Multiply
+	*
+	* @{
+	*/
+	static Vec2
+	multiply2(const Vec2 &vec, real scalar)
+	{ return vec.multiply(scalar); }
+
+	static Vec3
+	multiply3(const Vec3 &vec, real scalar)
+	{ return vec.multiply(scalar); }
+
+	static Vec4
+	multiply4(const Vec4 &vec, real scalar)
+	{ return vec.multiply(scalar); }
+	/*! @} */
+
+	/*! @name Cross
+	*
+	* @{
+	*/
+	static Vec3
+	cross3(const Vec3 &a, Vec3 &b)
+	{ return a.cross(b); }
+	/*! @} */
+
+	/*! @name Normalize
+	*
+	* @{
+	*/
+	static Vec2
+	normalize2(const Vec2 &vec)
+	{ return vec.normalize(); }
+
+	static Vec3
+	normalize3(const Vec3 &vec)
+	{ return vec.normalize(); }
+
+	static Vec4
+	normalize4(const Vec4 &vec)
+	{ return vec.normalize(); }
+	/*! @} */
+
+	/*! @name Magnitude
+	*
+	* @{
+	*/
+	static real
+	magnitude2(const Vec2 &vec)
+	{ return vec.magnitude(); }
+
+	static real
+	magnitude3(const Vec3 &vec)
+	{ return vec.magnitude(); }
+
+	static real
+	magnitude4(const Vec4 &vec)
+	{ return vec.magnitude(); }
+	/*! @} */
+
+	/*! @name Dot
+	*
+	* @{
+	*/
+	static real
+	dot2(const Vec2 &a, Vec2 &b)
+	{ return a.dot(b); }
+
+	static real
+	dot3(const Vec3 &a, Vec3 &b)
+	{ return a.dot(b); }
+
+	static real
+	dot4(const Vec4 &a, Vec4 &b)
+	{ return a.dot(b); }
+	/*! @} */
+
+	/*! @name Angle
+	*
+	* @{
+	*/
+	static real
+	angle2(const Vec2 &a, Vec2 &b)
+	{ return a.angle(b); }
+
+	static real
+	angle3(const Vec3 &a, Vec3 &b)
+	{ return a.angle(b); }
+
+	static real
+	angle4(const Vec4 &a, Vec4 &b)
+	{ return a.angle(b); }
+	/*! @} */
+
+	/*! @name Distance
+	*
+	* @{
+	*/
+	static real
+	distance2(const Vec2 &a, Vec2 &b)
+	{ return a.distance(b); }
+
+	static real
+	distance3(const Vec3 &a, Vec3 &b)
+	{ return a.distance(b); }
+
+	static real
+	distance4(const Vec4 &a, Vec4 &b)
+	{ return a.distance(b); }
+	/*! @} */
+};
+#endif
+/*! @} */
+
+
+
+/*! @name Spline Framing
+ *
+ * Wrapper classes for ::tsFrame (::Frame) and sequences of ::tsFrame
+ * (::FrameSeq). Instances of ::FrameSeq are created by ::BSpline::computeRMF.
+ *
+ * @{
+ */
+class TINYSPLINECXX_API Frame {
+public:
+	Frame(Vec3 &position,
+	      Vec3 &tangent,
+	      Vec3 &normal,
+	      Vec3 &binormal);
+
+	Vec3 position() const;
+	Vec3 tangent() const;
+	Vec3 normal() const;
+	Vec3 binormal() const;
+
+	std::string toString() const;
+
+private:
+	Vec3 m_position,
+	     m_tangent,
+	     m_normal,
+	     m_binormal;
+
+#ifdef TINYSPLINE_EMSCRIPTEN
+public:
+	Frame() {}
+	void setPosition(Vec3) { cannotWrite(); }
+	void setTangent(Vec3) { cannotWrite(); }
+	void setNormal(Vec3) { cannotWrite(); }
+	void setBinormal(Vec3) { cannotWrite(); }
+#endif
+};
+
+class TINYSPLINECXX_API FrameSeq {
+public:
+	FrameSeq();
+	FrameSeq(const FrameSeq &other);
+	FrameSeq(FrameSeq &&other);
+	virtual ~FrameSeq();
+
+	FrameSeq &operator=(const FrameSeq &other);
+	FrameSeq &operator=(FrameSeq &&other);
+
+	size_t size() const;
+	Frame at(size_t idx) const;
+
+	std::string toString() const;
+
+private:
+	tsFrame *m_frames;
+	size_t m_size;
+	FrameSeq(tsFrame *frames,
+	         size_t len);
+	friend class BSpline;
+};
+/*! @} */
+
+
+
+/*! @name Utility Classes
+ *
+ * Little helper classes, such as value classes or classes with only static
+ * methods. These are primarily classes that do not really fit into another
+ * group or are too small to form their own group.
+ *
+ * @{
+ */
+class TINYSPLINECXX_API Domain {
+public:
+	Domain(real min, real max);
+
+	real min() const;
+	real max() const;
+
+	std::string toString() const;
+
+private:
+	real m_min,
+	     m_max;
+
+#ifdef TINYSPLINE_EMSCRIPTEN
+public:
+	Domain()
+	: Domain(TS_DOMAIN_DEFAULT_MIN, TS_DOMAIN_DEFAULT_MAX)
+	{}
+	void setMin(real) { cannotWrite(); }
+	void setMax(real) { cannotWrite(); }
+#endif
+};
+/*! @} */
+
+
+
+/*! @name DeBoorNet
+ *
+ * Wrapper class for ::tsDeBoorNet.
+ *
+ * @{
+ */
+class TINYSPLINECXX_API DeBoorNet {
+public:
+	DeBoorNet(const DeBoorNet &other);
+	DeBoorNet(DeBoorNet &&other);
+	virtual ~DeBoorNet();
+
+	DeBoorNet & operator=(const DeBoorNet &other);
+	DeBoorNet & operator=(DeBoorNet &&other);
+
+	real knot() const;
+	size_t index() const;
+	size_t multiplicity() const;
+	size_t numInsertions() const;
+	size_t dimension() const;
+	std::vector<real> points() const;
+	std::vector<real> result() const;
+
+	/**
+	* Returns the result at \p idx as ::Vec2. Note that, by design, \p idx
+	* cannot be greater than \c 1. It is safe to call this method even if
+	* ::dimension is less than \c 2. In this case, the missing components
+	* are set to \c 0. If ::dimension is greater than \c 2, the excess
+	* values are ignored.
+	*
+	* @return
+	* 	The result at \p idx as ::Vec2.
+	* @throws std::out_of_range
+	* 	If \p idx is greater than \c 1, or if \p idx is \c 1, but there
+	* 	is only one result.
+	*/
+	Vec2 resultVec2(size_t idx = 0) const;
+
+	/**
+	* Returns the result at \p idx as ::Vec3. Note that, by design, \p idx
+	* cannot be greater than \c 1. It is safe to call this method even if
+	* ::dimension is less than \c 3. In this case, the missing components
+	* are set to \c 0. If ::dimension is greater than \c 3, the excess
+	* values are ignored.
+	*
+	* @return
+	* 	The result at \p idx as ::Vec3.
+	* @throws std::out_of_range
+	* 	If \p idx is greater than \c 1, or if \p idx is \c 1, but there
+	* 	is only one result.
+	*/
+	Vec3 resultVec3(size_t idx = 0) const;
+
+	/**
+	* Returns the result at \p idx as ::Vec4. Note that, by design, \p idx
+	* cannot be greater than \c 1. It is safe to call this method even if
+	* ::dimension is less than \c 4. In this case, the missing components
+	* are set to \c 0. If ::dimension is greater than \c 4, the excess
+	* values are ignored.
+	*
+	* @return
+	* 	The result at \p idx as ::Vec4.
+	* @throws std::out_of_range
+	* 	If \p idx is greater than \c 1, or if \p idx is \c 1, but there
+	* 	is only one result.
+	*/
+	Vec4 resultVec4(size_t idx = 0) const;
+
+	std::string toString() const;
+
+private:
+	tsDeBoorNet m_net;
+
+	/* Constructors & Destructors */
+	explicit DeBoorNet(tsDeBoorNet &data);
+
+	friend class BSpline;
+
+#ifdef TINYSPLINE_EMSCRIPTEN
+public:
+	DeBoorNet() : m_net(ts_deboornet_init()) {}
+	void setKnot(real) { cannotWrite(); }
+	void setIndex(size_t) { cannotWrite(); }
+	void setMultiplicity(size_t) { cannotWrite(); }
+	void setNumInsertions(size_t) { cannotWrite(); }
+	void setDimension(size_t) { cannotWrite(); }
+	void setPoints(std::vector<real>) { cannotWrite(); }
+	void setResult(std::vector<real>) { cannotWrite(); }
+#endif
+};
+/*! @} */
+
+
+
+/*! @name BSpline
+ *
+ * Wrapper class for ::tsBSpline.
+ *
+ * @{
+ */
+class Morphism;
+class ChordLengths;
+class TINYSPLINECXX_API BSpline {
+public:
+	enum Type { Opened, Clamped, Beziers };
+
+	/* Constructors & Destructors */
+	BSpline();
+	BSpline(const BSpline &other);
+	BSpline(BSpline &&other);
+	explicit BSpline(size_t numControlPoints,
+	                 size_t dimension = 2,
+	                 size_t degree = 3,
+	                 Type type = Type::Clamped);
+	virtual ~BSpline();
+
+	/* Create from static method */
+	static BSpline interpolateCubicNatural(std_real_vector_in points,
+	                                       size_t dimension);
+	static BSpline interpolateCatmullRom(std_real_vector_in points,
+	                                     size_t dimension,
+	                                     real alpha = (real) 0.5,
+	                                     std::vector<real> *first = nullptr,
+	                                     std::vector<real> *last = nullptr,
+	                                     real epsilon = TS_POINT_EPSILON);
+	static BSpline parseJson(std::string json);
+	static BSpline load(std::string path);
+
+	static bool knotsEqual(real x, real y);
+
+	/* Operators */
+	BSpline & operator=(const BSpline &other);
+	BSpline & operator=(BSpline &&other);
+	DeBoorNet operator()(real knot) const;
+
+	/* Accessors */
+	size_t degree() const;
+	size_t order() const;
+	size_t dimension() const;
+	std::vector<real> controlPoints() const;
+	Vec2 controlPointVec2At(size_t idx) const;
+	Vec3 controlPointVec3At(size_t idx) const;
+	Vec4 controlPointVec4At(size_t idx) const;
+	std::vector<real> knots() const;
+	real knotAt(size_t idx) const;
+
+	/* Query */
+	size_t numControlPoints() const;
+	DeBoorNet eval(real knot) const;
+	std_real_vector_out evalAll(std_real_vector_in knots) const;
+	std_real_vector_out sample(size_t num = 0) const;
+	DeBoorNet bisect(real value,
+	                 real epsilon = (real) 0.0,
+	                 bool persnickety = false,
+	                 size_t index = 0,
+	                 bool ascending = true,
+	                 size_t maxIter = 50) const;
+	Domain domain() const;
+	bool isClosed(real epsilon = TS_POINT_EPSILON) const;
+	FrameSeq computeRMF(std_real_vector_in knots,
+	                    Vec3 *firstNormal = nullptr) const;
+	BSpline subSpline(real knot0, real knot1) const;
+	std_real_vector_out uniformKnotSeq(size_t num = 100) const;
+	std_real_vector_out equidistantKnotSeq(size_t num = 100,
+	                                       size_t numSamples = 0) const;
+	ChordLengths chordLengths(std_real_vector_in knots) const;
+	ChordLengths chordLengths(size_t numSamples = 200) const;
+
+	/* Serialization */
+	std::string toJson() const;
+	void save(std::string path) const;
+
+	/* Modifications */
+	void setControlPoints(const std::vector<real> &ctrlp);
+	void setControlPointVec2At(size_t idx, Vec2 &cp);
+	void setControlPointVec3At(size_t idx, Vec3 &cp);
+	void setControlPointVec4At(size_t idx, Vec4 &cp);
+	void setKnots(const std::vector<real> &knots);
+	void setKnotAt(size_t idx, real knot);
+
+	/* Transformations */
+	BSpline insertKnot(real knot, size_t num) const;
+	BSpline split(real knot) const;
+	BSpline tension(real beta) const;
+	BSpline toBeziers() const;
+	BSpline derive(size_t num = 1,
+	               real eps = TS_POINT_EPSILON) const;
+	BSpline elevateDegree(size_t amount,
+	                      real eps = TS_POINT_EPSILON) const;
+	BSpline alignWith(const BSpline &other,
+	                  BSpline &otherAligned,
+	                  real eps = TS_POINT_EPSILON) const;
+	Morphism morphTo(const BSpline &other,
+	                 real eps = TS_POINT_EPSILON) const;
+
+	/* Debug */
+	std::string toString() const;
+
+private:
+	tsBSpline m_spline;
+
+	/* Constructors & Destructors */
+	explicit BSpline(tsBSpline &data);
+
+	/* Needs to access ::spline. */
+	friend class Morphism;
+
+#ifdef TINYSPLINE_EMSCRIPTEN
+public:
+	std_real_vector_out sample0() const { return sample(); }
+	std_real_vector_out sample1(size_t num) const { return sample(num); }
+	BSpline derive0() const { return derive(); }
+	BSpline derive1(size_t n) const { return derive(n); }
+	BSpline derive2(size_t n, real eps) const { return derive(n, eps); }
+#endif
+};
+/*! @} */
+
+
+
+/*! @name Spline Morphing
+ *
+ * @{
+ */
+class TINYSPLINECXX_API Morphism {
+public:
+	Morphism(const BSpline &origin,
+		 const BSpline &target,
+		 real epsilon = TS_POINT_EPSILON);
+
+	BSpline origin() const;
+	BSpline target() const;
+	real epsilon() const;
+
+	BSpline eval(real t);
+	BSpline operator()(real t);
+
+	std::string toString() const;
+private:
+	BSpline m_origin, m_target;
+	real m_epsilon;
+	BSpline m_originAligned, m_targetAligned;
+	BSpline m_buffer;
+};
+/*! @} */
+
+
+
+/*! @name Reparametrization by Arc Length
+ * @{
+ */
+class TINYSPLINECXX_API ChordLengths {
+public:
+	ChordLengths();
+	ChordLengths(const ChordLengths &other);
+	ChordLengths(ChordLengths &&other);
+	virtual ~ChordLengths();
+
+	ChordLengths &operator=(const ChordLengths &other);
+	ChordLengths &operator=(ChordLengths &&other);
+
+	BSpline spline() const;
+	std::vector<real> knots() const;
+	std::vector<real> lengths() const;
+	TS_DEPRECATED std::vector<real> values() const;
+	size_t size() const;
+
+	real arcLength() const;
+	real lengthToKnot(real len) const;
+	real tToKnot(real t) const;
+	std_real_vector_out equidistantKnotSeq(size_t num = 100) const;
+
+	std::string toString() const;
+private:
+	BSpline m_spline;
+	real *m_knots, *m_lengths;
+	size_t m_size;
+	ChordLengths(const BSpline &spline,
+	             real *knots,
+	             real *lengths,
+	             size_t size);
+	friend class BSpline;
+};
+/*! @} */
+
+}
+
+
+
+#ifdef TINYSPLINE_EMSCRIPTEN
+using namespace tinyspline;
+EMSCRIPTEN_BINDINGS(tinyspline) {
+	function("exceptionMessage", &exceptionMessage);
+
+	// Vector Math
+	value_object<Vec2>("Vec2")
+		.field("x",
+		       &Vec2::x,
+		       &Vec2::setX)
+		.field("y",
+		       &Vec2::y,
+		       &Vec2::setY)
+		.field("values",
+		       &Vec2::values,
+		       &Vec2::setValues)
+	;
+	value_object<Vec3>("Vec3")
+		.field("x",
+		       &Vec3::x,
+		       &Vec3::setX)
+		.field("y",
+		       &Vec3::y,
+		       &Vec3::setY)
+		.field("z",
+		       &Vec3::z,
+		       &Vec3::setZ)
+		.field("values",
+		       &Vec3::values,
+		       &Vec3::setValues)
+	;
+	value_object<Vec4>("Vec4")
+		.field("x",
+		       &Vec4::x,
+		       &Vec4::setX)
+		.field("y",
+		       &Vec4::y,
+		       &Vec4::setY)
+		.field("z",
+		       &Vec4::z,
+		       &Vec4::setZ)
+		.field("w",
+		       &Vec4::w,
+		       &Vec4::setW)
+		.field("values",
+		       &Vec4::values,
+		       &Vec4::setValues)
+	;
+	class_<VecMath>("VecMath")
+		.class_function("add", &VecMath::add2)
+		.class_function("add", &VecMath::add3)
+		.class_function("add", &VecMath::add4)
+		.class_function("subtract", &VecMath::subtract2)
+		.class_function("subtract", &VecMath::subtract3)
+		.class_function("subtract", &VecMath::subtract4)
+		.class_function("multiply", &VecMath::multiply2)
+		.class_function("multiply", &VecMath::multiply3)
+		.class_function("multiply", &VecMath::multiply4)
+		.class_function("cross", &VecMath::cross3)
+		.class_function("normalize", &VecMath::normalize2)
+		.class_function("normalize", &VecMath::normalize3)
+		.class_function("normalize", &VecMath::normalize4)
+		.class_function("magnitude", &VecMath::magnitude2)
+		.class_function("magnitude", &VecMath::magnitude3)
+		.class_function("magnitude", &VecMath::magnitude4)
+		.class_function("dot", &VecMath::dot2)
+		.class_function("dot", &VecMath::dot3)
+		.class_function("dot", &VecMath::dot4)
+		.class_function("angle", &VecMath::angle2)
+		.class_function("angle", &VecMath::angle3)
+		.class_function("angle", &VecMath::angle4)
+		.class_function("distance", &VecMath::distance2)
+		.class_function("distance", &VecMath::distance3)
+		.class_function("distance", &VecMath::distance4)
+	;
+
+	// Spline Framing
+	value_object<Frame>("Frame")
+		.field("position",
+		       &Frame::position,
+		       &Frame::setPosition)
+		.field("tangent",
+		       &Frame::tangent,
+		       &Frame::setTangent)
+		.field("normal",
+		       &Frame::normal,
+		       &Frame::setNormal)
+		.field("binormal",
+		       &Frame::normal,
+		       &Frame::setNormal)
+	;
+	class_<FrameSeq>("FrameSeq")
+		.constructor<>()
+		.constructor<FrameSeq>()
+		.function("size", &FrameSeq::size)
+		.function("at", &FrameSeq::at)
+		.function("toString", &FrameSeq::toString)
+	;
+
+	// Utility Classes
+	value_object<Domain>("Domain")
+		.field("min",
+		       &Domain::min,
+		       &Domain::setMin)
+		.field("max",
+		       &Domain::max,
+		       &Domain::setMax)
+	;
+
+	value_object<DeBoorNet>("DeBoorNet")
+		.field("knot",
+		       &DeBoorNet::knot,
+		       &DeBoorNet::setKnot)
+		.field("index",
+		       &DeBoorNet::index,
+		       &DeBoorNet::setIndex)
+		.field("multiplicity",
+		       &DeBoorNet::multiplicity,
+		       &DeBoorNet::setMultiplicity)
+		.field("numInsertions",
+		       &DeBoorNet::numInsertions,
+		       &DeBoorNet::setNumInsertions)
+		.field("dimension",
+		       &DeBoorNet::dimension,
+		       &DeBoorNet::setDimension)
+		.field("points",
+		       &DeBoorNet::points,
+		       &DeBoorNet::setPoints)
+		.field("result",
+		       &DeBoorNet::result,
+		       &DeBoorNet::setResult)
+	;
+
+	class_<BSpline>("BSpline")
+	        .constructor<>()
+	        //.constructor<BSpline>()
+	        .constructor<size_t>()
+	        .constructor<size_t, size_t>()
+	        .constructor<size_t, size_t, size_t>()
+	        .constructor<size_t, size_t, size_t, BSpline::Type>()
+
+	        .class_function("interpolateCubicNatural",
+			&BSpline::interpolateCubicNatural)
+	        .class_function("interpolateCatmullRom",
+			&BSpline::interpolateCatmullRom,
+			allow_raw_pointers())
+	        .class_function("parseJson", &BSpline::parseJson)
+
+	        .property("degree", &BSpline::degree)
+	        .property("order", &BSpline::order)
+	        .property("dimension", &BSpline::dimension)
+	        .property("controlPoints", &BSpline::controlPoints,
+			&BSpline::setControlPoints)
+	        .property("knots", &BSpline::knots, &BSpline::setKnots)
+	        .property("domain", &BSpline::domain)
+
+	        /* Property by index */
+	        .function("knotAt", &BSpline::knotAt)
+	        .function("setKnotAt", &BSpline::setKnotAt)
+
+	        /* Query */
+	        .function("numControlPoints", &BSpline::numControlPoints)
+	        .function("eval", &BSpline::eval)
+	        .function("evalAll", &BSpline::evalAll)
+	        .function("sample",
+			select_overload<std_real_vector_out() const>
+			(&BSpline::sample0))
+	        .function("sample",
+			select_overload<std_real_vector_out(size_t) const>
+			(&BSpline::sample1))
+	        .function("sample", &BSpline::sample)
+	        .function("bisect", &BSpline::bisect)
+	        .function("isClosed", &BSpline::isClosed)
+
+		/* Serialization */
+	        .function("toJson", &BSpline::toJson)
+
+	        /* Transformations */
+	        .function("insertKnot", &BSpline::insertKnot)
+	        .function("split", &BSpline::split)
+	        .function("tension", &BSpline::tension)
+	        .function("toBeziers", &BSpline::toBeziers)
+	        .function("derive",
+			select_overload<BSpline() const>
+			(&BSpline::derive0))
+	        .function("derive",
+			select_overload<BSpline(size_t) const>
+			(&BSpline::derive1))
+	        .function("derive",
+			select_overload<BSpline(size_t, real) const>
+			(&BSpline::derive2))
+
+	        /* Debug */
+	        .function("toString", &BSpline::toString)
+	;
+
+	enum_<BSpline::Type>("BSplineType")
+	        .value("Opened", BSpline::Type::Opened)
+	        .value("Clamped", BSpline::Type::Clamped)
+	        .value("Beziers", BSpline::Type::Beziers)
+	;
+}
+#endif
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index a34e8bb0ed..d4ebbbfc96 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -2,6 +2,8 @@
 #include "Eigen/Eigen"
 #include "boost/graph/floyd_warshall_shortest.hpp"
 
+#include "roads/thirdparty/tinysplinecxx.h"
+
 using namespace roads;
 
 double roads::EuclideanDistance(const Point &Point1, const Point &Point2) {
@@ -119,3 +121,35 @@ double roads::energy::CalculateStepSize(const Point2D &Pt, const Point2D &P, con
 
     return 0.0;
 }
+
+ArrayList<Eigen::Vector3f> spline::SmoothAndResampleSegments(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments, int32_t SamplePoints) {
+    return GenerateAndSamplePointsFromSegments(InPoints, Segments, SamplePoints);
+}
+
+tinyspline::BSpline spline::GenerateBSplineFromSegment(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments) {
+    using namespace tinyspline;
+
+    ArrayList<float> Points;
+
+    for (const auto& Seg : Segments) {
+        Points.insert(std::end(Points), { float(InPoints[Seg[0]][0]), float(InPoints[Seg[0]][1]), float(InPoints[Seg[0]][2]) });
+    }
+    Points.insert(std::end(Points), { float(InPoints[Segments[Segments.size() - 1][1]][0]), float(InPoints[Segments[Segments.size() - 1][1]][1]), float(InPoints[Segments[Segments.size() - 1][1]][2]) });
+
+    return BSpline::interpolateCubicNatural(Points, 3);
+}
+
+ArrayList<Eigen::Vector3f> spline::GenerateAndSamplePointsFromSegments(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments, int32_t SamplePoints) {
+    auto Spline = GenerateBSplineFromSegment(InPoints, Segments);
+    float Step = 1.0f / float(SamplePoints);
+
+    ArrayList<Eigen::Vector3f> Result;
+    Result.reserve(SamplePoints);
+
+    for (int32_t i = 0; i < SamplePoints; i++) {
+        auto Point = Spline.eval(float(i) * Step).resultVec3();
+        Result.push_back( Eigen::Vector3f { Point.x(), Point.y(), Point.z() } );
+    }
+
+    return Result;
+}
diff --git a/projects/Roads/utilities/src/thirdparty/parson.c b/projects/Roads/utilities/src/thirdparty/parson.c
new file mode 100644
index 0000000000..c662de8e23
--- /dev/null
+++ b/projects/Roads/utilities/src/thirdparty/parson.c
@@ -0,0 +1,2079 @@
+/*
+ Parson ( http://kgabis.github.com/parson/ )
+ Copyright (c) 2012 - 2017 Krzysztof Gabis
+
+ Permission is hereby granted, free of charge, to any person obtaining a copy
+ of this software and associated documentation files (the "Software"), to deal
+ in the Software without restriction, including without limitation the rights
+ to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ copies of the Software, and to permit persons to whom the Software is
+ furnished to do so, subject to the following conditions:
+
+ The above copyright notice and this permission notice shall be included in
+ all copies or substantial portions of the Software.
+
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ THE SOFTWARE.
+*/
+#ifdef _MSC_VER
+#ifndef _CRT_SECURE_NO_WARNINGS
+#define _CRT_SECURE_NO_WARNINGS
+#endif /* _CRT_SECURE_NO_WARNINGS */
+#endif /* _MSC_VER */
+
+#include "roads/thirdparty/parson.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <ctype.h>
+#include <math.h>
+#include <errno.h>
+
+/* Suppress some useless MSVC warnings. */
+#ifdef _MSC_VER
+#pragma warning(push)
+/* address of dllimport */
+#pragma warning(disable:4232)
+/* function not inlined */
+#pragma warning(disable:4710)
+/* byte padding */
+#pragma warning(disable:4820)
+/* meaningless deprecation */
+#pragma warning(disable:4996)
+/* Spectre mitigation */
+#pragma warning(disable:5045)
+#endif
+
+/* Apparently sscanf is not implemented in some "standard" libraries, so don't use it, if you
+ * don't have to. */
+#define sscanf THINK_TWICE_ABOUT_USING_SSCANF
+
+#define STARTING_CAPACITY 16
+#define MAX_NESTING       2048
+
+#define FLOAT_FORMAT "%1.17g" /* do not increase precision without incresing NUM_BUF_SIZE */
+#define NUM_BUF_SIZE 64 /* double printed with "%1.17g" shouldn't be longer than 25 bytes so let's be paranoid and use 64 */
+
+#define SIZEOF_TOKEN(a)       (sizeof(a) - 1)
+#define SKIP_CHAR(str)        ((*str)++)
+#define SKIP_WHITESPACES(str) while (isspace((unsigned char)(**str))) { SKIP_CHAR(str); }
+#define MAX(a, b)             ((a) > (b) ? (a) : (b))
+
+#undef malloc
+#undef free
+
+#if defined(isnan) && defined(isinf)
+#define IS_NUMBER_INVALID(x) (isnan((x)) || isinf((x)))
+#else
+#define IS_NUMBER_INVALID(x) (((x) * 0.0) != 0.0)
+#endif
+
+static JSON_Malloc_Function parson_malloc = malloc;
+static JSON_Free_Function parson_free = free;
+
+#define IS_CONT(b) (((unsigned char)(b) & 0xC0) == 0x80) /* is utf-8 continuation byte */
+
+/* Type definitions */
+typedef union json_value_value {
+    char        *string;
+    double       number;
+    JSON_Object *object;
+    JSON_Array  *array;
+    int          boolean;
+    int          null;
+} JSON_Value_Value;
+
+struct json_value_t {
+    JSON_Value      *parent;
+    JSON_Value_Type  type;
+    JSON_Value_Value value;
+};
+
+struct json_object_t {
+    JSON_Value  *wrapping_value;
+    char       **names;
+    JSON_Value **values;
+    size_t       count;
+    size_t       capacity;
+};
+
+struct json_array_t {
+    JSON_Value  *wrapping_value;
+    JSON_Value **items;
+    size_t       count;
+    size_t       capacity;
+};
+
+/* Various */
+static char * read_file(const char *filename);
+static void   remove_comments(char *string, const char *start_token, const char *end_token);
+static char * parson_strndup(const char *string, size_t n);
+static char * parson_strdup(const char *string);
+static int    hex_char_to_int(char c);
+static int    parse_utf16_hex(const char *string, unsigned int *result);
+static int    num_bytes_in_utf8_sequence(unsigned char c);
+static int    verify_utf8_sequence(const unsigned char *string, int *len);
+static int    is_valid_utf8(const char *string, size_t string_len);
+static int    is_decimal(const char *string, size_t length);
+
+/* JSON Object */
+static JSON_Object * json_object_init(JSON_Value *wrapping_value);
+static JSON_Status   json_object_add(JSON_Object *object, const char *name, JSON_Value *value);
+static JSON_Status   json_object_addn(JSON_Object *object, const char *name, size_t name_len, JSON_Value *value);
+static JSON_Status   json_object_resize(JSON_Object *object, size_t new_capacity);
+static JSON_Value  * json_object_getn_value(const JSON_Object *object, const char *name, size_t name_len);
+static JSON_Status   json_object_remove_internal(JSON_Object *object, const char *name, int free_value);
+static JSON_Status   json_object_dotremove_internal(JSON_Object *object, const char *name, int free_value);
+static void          json_object_free(JSON_Object *object);
+
+/* JSON Array */
+static JSON_Array * json_array_init(JSON_Value *wrapping_value);
+static JSON_Status  json_array_add(JSON_Array *array, JSON_Value *value);
+static JSON_Status  json_array_resize(JSON_Array *array, size_t new_capacity);
+static void         json_array_free(JSON_Array *array);
+
+/* JSON Value */
+static JSON_Value * json_value_init_string_no_copy(char *string);
+
+/* Parser */
+static JSON_Status  skip_quotes(const char **string);
+static int          parse_utf16(const char **unprocessed, char **processed);
+static char *       process_string(const char *input, size_t len);
+static char *       get_quoted_string(const char **string);
+static JSON_Value * parse_object_value(const char **string, size_t nesting);
+static JSON_Value * parse_array_value(const char **string, size_t nesting);
+static JSON_Value * parse_string_value(const char **string);
+static JSON_Value * parse_boolean_value(const char **string);
+static JSON_Value * parse_number_value(const char **string);
+static JSON_Value * parse_null_value(const char **string);
+static JSON_Value * parse_value(const char **string, size_t nesting);
+
+/* Serialization */
+static int    json_serialize_to_buffer_r(const JSON_Value *value, char *buf, int level, int is_pretty, char *num_buf);
+static int    json_serialize_string(const char *string, char *buf);
+static int    append_indent(char *buf, int level);
+static int    append_string(char *buf, const char *string);
+
+/* Various */
+static char * parson_strndup(const char *string, size_t n) {
+    char *output_string = (char*)parson_malloc(n + 1);
+    if (!output_string) {
+        return NULL;
+    }
+    output_string[n] = '\0';
+#if __GNUC__ >= 8
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wstringop-truncation"
+#endif
+    strncpy(output_string, string, n);
+#if __GNUC__ >= 8
+#pragma GCC diagnostic pop
+#endif
+    return output_string;
+}
+
+static char * parson_strdup(const char *string) {
+    return parson_strndup(string, strlen(string));
+}
+
+static int hex_char_to_int(char c) {
+    if (c >= '0' && c <= '9') {
+        return c - '0';
+    } else if (c >= 'a' && c <= 'f') {
+        return c - 'a' + 10;
+    } else if (c >= 'A' && c <= 'F') {
+        return c - 'A' + 10;
+    }
+    return -1;
+}
+
+static int parse_utf16_hex(const char *s, unsigned int *result) {
+    int x1, x2, x3, x4;
+    if (s[0] == '\0' || s[1] == '\0' || s[2] == '\0' || s[3] == '\0') {
+        return 0;
+    }
+    x1 = hex_char_to_int(s[0]);
+    x2 = hex_char_to_int(s[1]);
+    x3 = hex_char_to_int(s[2]);
+    x4 = hex_char_to_int(s[3]);
+    if (x1 == -1 || x2 == -1 || x3 == -1 || x4 == -1) {
+        return 0;
+    }
+    *result = (unsigned int)((x1 << 12) | (x2 << 8) | (x3 << 4) | x4);
+    return 1;
+}
+
+static int num_bytes_in_utf8_sequence(unsigned char c) {
+    if (c == 0xC0 || c == 0xC1 || c > 0xF4 || IS_CONT(c)) {
+        return 0;
+    } else if ((c & 0x80) == 0) {    /* 0xxxxxxx */
+        return 1;
+    } else if ((c & 0xE0) == 0xC0) { /* 110xxxxx */
+        return 2;
+    } else if ((c & 0xF0) == 0xE0) { /* 1110xxxx */
+        return 3;
+    } else if ((c & 0xF8) == 0xF0) { /* 11110xxx */
+        return 4;
+    }
+    return 0; /* won't happen */
+}
+
+static int verify_utf8_sequence(const unsigned char *string, int *len) {
+    unsigned int cp = 0;
+    *len = num_bytes_in_utf8_sequence(string[0]);
+
+    if (*len == 1) {
+        cp = string[0];
+    } else if (*len == 2 && IS_CONT(string[1])) {
+        cp = string[0] & 0x1F;
+        cp = (cp << 6) | (string[1] & 0x3F);
+    } else if (*len == 3 && IS_CONT(string[1]) && IS_CONT(string[2])) {
+        cp = ((unsigned char)string[0]) & 0xF;
+        cp = (cp << 6) | (string[1] & 0x3F);
+        cp = (cp << 6) | (string[2] & 0x3F);
+    } else if (*len == 4 && IS_CONT(string[1]) && IS_CONT(string[2]) && IS_CONT(string[3])) {
+        cp = string[0] & 0x7;
+        cp = (cp << 6) | (string[1] & 0x3F);
+        cp = (cp << 6) | (string[2] & 0x3F);
+        cp = (cp << 6) | (string[3] & 0x3F);
+    } else {
+        return 0;
+    }
+
+    /* overlong encodings */
+    if ((cp < 0x80    && *len > 1) ||
+        (cp < 0x800   && *len > 2) ||
+        (cp < 0x10000 && *len > 3)) {
+        return 0;
+    }
+
+    /* invalid unicode */
+    if (cp > 0x10FFFF) {
+        return 0;
+    }
+
+    /* surrogate halves */
+    if (cp >= 0xD800 && cp <= 0xDFFF) {
+        return 0;
+    }
+
+    return 1;
+}
+
+static int is_valid_utf8(const char *string, size_t string_len) {
+    int len = 0;
+    const char *string_end =  string + string_len;
+    while (string < string_end) {
+        if (!verify_utf8_sequence((const unsigned char*)string, &len)) {
+            return 0;
+        }
+        string += len;
+    }
+    return 1;
+}
+
+static int is_decimal(const char *string, size_t length) {
+    if (length > 1 && string[0] == '0' && string[1] != '.') {
+        return 0;
+    }
+    if (length > 2 && !strncmp(string, "-0", 2) && string[2] != '.') {
+        return 0;
+    }
+    while (length--) {
+        if (strchr("xX", string[length])) {
+            return 0;
+        }
+    }
+    return 1;
+}
+
+static char * read_file(const char * filename) {
+    FILE *fp = fopen(filename, "r");
+    size_t size_to_read = 0;
+    size_t size_read = 0;
+    long pos;
+    char *file_contents;
+    if (!fp) {
+        return NULL;
+    }
+    fseek(fp, 0L, SEEK_END);
+    pos = ftell(fp);
+    if (pos < 0) {
+        fclose(fp);
+        return NULL;
+    }
+    size_to_read = pos;
+    rewind(fp);
+    file_contents = (char*)parson_malloc(sizeof(char) * (size_to_read + 1));
+    if (!file_contents) {
+        fclose(fp);
+        return NULL;
+    }
+    size_read = fread(file_contents, 1, size_to_read, fp);
+    if (size_read == 0 || ferror(fp)) {
+        fclose(fp);
+        parson_free(file_contents);
+        return NULL;
+    }
+    fclose(fp);
+    file_contents[size_read] = '\0';
+    return file_contents;
+}
+
+static void remove_comments(char *string, const char *start_token, const char *end_token) {
+    int in_string = 0, escaped = 0;
+    size_t i;
+    char *ptr = NULL, current_char;
+    size_t start_token_len = strlen(start_token);
+    size_t end_token_len = strlen(end_token);
+    if (start_token_len == 0 || end_token_len == 0) {
+        return;
+    }
+    while ((current_char = *string) != '\0') {
+        if (current_char == '\\' && !escaped) {
+            escaped = 1;
+            string++;
+            continue;
+        } else if (current_char == '\"' && !escaped) {
+            in_string = !in_string;
+        } else if (!in_string && strncmp(string, start_token, start_token_len) == 0) {
+            for(i = 0; i < start_token_len; i++) {
+                string[i] = ' ';
+            }
+            string = string + start_token_len;
+            ptr = strstr(string, end_token);
+            if (!ptr) {
+                return;
+            }
+            for (i = 0; i < (ptr - string) + end_token_len; i++) {
+                string[i] = ' ';
+            }
+            string = ptr + end_token_len - 1;
+        }
+        escaped = 0;
+        string++;
+    }
+}
+
+/* JSON Object */
+static JSON_Object * json_object_init(JSON_Value *wrapping_value) {
+    JSON_Object *new_obj = (JSON_Object*)parson_malloc(sizeof(JSON_Object));
+    if (new_obj == NULL) {
+        return NULL;
+    }
+    new_obj->wrapping_value = wrapping_value;
+    new_obj->names = (char**)NULL;
+    new_obj->values = (JSON_Value**)NULL;
+    new_obj->capacity = 0;
+    new_obj->count = 0;
+    return new_obj;
+}
+
+static JSON_Status json_object_add(JSON_Object *object, const char *name, JSON_Value *value) {
+    if (name == NULL) {
+        return JSONFailure;
+    }
+    return json_object_addn(object, name, strlen(name), value);
+}
+
+static JSON_Status json_object_addn(JSON_Object *object, const char *name, size_t name_len, JSON_Value *value) {
+    size_t index = 0;
+    if (object == NULL || name == NULL || value == NULL) {
+        return JSONFailure;
+    }
+    if (json_object_getn_value(object, name, name_len) != NULL) {
+        return JSONFailure;
+    }
+    if (object->count >= object->capacity) {
+        size_t new_capacity = MAX(object->capacity * 2, STARTING_CAPACITY);
+        if (json_object_resize(object, new_capacity) == JSONFailure) {
+            return JSONFailure;
+        }
+    }
+    index = object->count;
+    object->names[index] = parson_strndup(name, name_len);
+    if (object->names[index] == NULL) {
+        return JSONFailure;
+    }
+    value->parent = json_object_get_wrapping_value(object);
+    object->values[index] = value;
+    object->count++;
+    return JSONSuccess;
+}
+
+static JSON_Status json_object_resize(JSON_Object *object, size_t new_capacity) {
+    char **temp_names = NULL;
+    JSON_Value **temp_values = NULL;
+
+    if ((object->names == NULL && object->values != NULL) ||
+        (object->names != NULL && object->values == NULL) ||
+        new_capacity == 0) {
+            return JSONFailure; /* Shouldn't happen */
+    }
+    temp_names = (char**)parson_malloc(new_capacity * sizeof(char*));
+    if (temp_names == NULL) {
+        return JSONFailure;
+    }
+    temp_values = (JSON_Value**)parson_malloc(new_capacity * sizeof(JSON_Value*));
+    if (temp_values == NULL) {
+        parson_free(temp_names);
+        return JSONFailure;
+    }
+    if (object->names != NULL && object->values != NULL && object->count > 0) {
+        memcpy(temp_names, object->names, object->count * sizeof(char*));
+        memcpy(temp_values, object->values, object->count * sizeof(JSON_Value*));
+    }
+    parson_free(object->names);
+    parson_free(object->values);
+    object->names = temp_names;
+    object->values = temp_values;
+    object->capacity = new_capacity;
+    return JSONSuccess;
+}
+
+static JSON_Value * json_object_getn_value(const JSON_Object *object, const char *name, size_t name_len) {
+    size_t i, name_length;
+    for (i = 0; i < json_object_get_count(object); i++) {
+        name_length = strlen(object->names[i]);
+        if (name_length != name_len) {
+            continue;
+        }
+        if (strncmp(object->names[i], name, name_len) == 0) {
+            return object->values[i];
+        }
+    }
+    return NULL;
+}
+
+static JSON_Status json_object_remove_internal(JSON_Object *object, const char *name, int free_value) {
+    size_t i = 0, last_item_index = 0;
+    if (object == NULL || json_object_get_value(object, name) == NULL) {
+        return JSONFailure;
+    }
+    last_item_index = json_object_get_count(object) - 1;
+    for (i = 0; i < json_object_get_count(object); i++) {
+        if (strcmp(object->names[i], name) == 0) {
+            parson_free(object->names[i]);
+            if (free_value) {
+                json_value_free(object->values[i]);
+            }
+            if (i != last_item_index) { /* Replace key value pair with one from the end */
+                object->names[i] = object->names[last_item_index];
+                object->values[i] = object->values[last_item_index];
+            }
+            object->count -= 1;
+            return JSONSuccess;
+        }
+    }
+    return JSONFailure; /* No execution path should end here */
+}
+
+static JSON_Status json_object_dotremove_internal(JSON_Object *object, const char *name, int free_value) {
+    JSON_Value *temp_value = NULL;
+    JSON_Object *temp_object = NULL;
+    const char *dot_pos = strchr(name, '.');
+    if (dot_pos == NULL) {
+        return json_object_remove_internal(object, name, free_value);
+    }
+    temp_value = json_object_getn_value(object, name, dot_pos - name);
+    if (json_value_get_type(temp_value) != JSONObject) {
+        return JSONFailure;
+    }
+    temp_object = json_value_get_object(temp_value);
+    return json_object_dotremove_internal(temp_object, dot_pos + 1, free_value);
+}
+
+static void json_object_free(JSON_Object *object) {
+    size_t i;
+    for (i = 0; i < object->count; i++) {
+        parson_free(object->names[i]);
+        json_value_free(object->values[i]);
+    }
+    parson_free(object->names);
+    parson_free(object->values);
+    parson_free(object);
+}
+
+/* JSON Array */
+static JSON_Array * json_array_init(JSON_Value *wrapping_value) {
+    JSON_Array *new_array = (JSON_Array*)parson_malloc(sizeof(JSON_Array));
+    if (new_array == NULL) {
+        return NULL;
+    }
+    new_array->wrapping_value = wrapping_value;
+    new_array->items = (JSON_Value**)NULL;
+    new_array->capacity = 0;
+    new_array->count = 0;
+    return new_array;
+}
+
+static JSON_Status json_array_add(JSON_Array *array, JSON_Value *value) {
+    if (array->count >= array->capacity) {
+        size_t new_capacity = MAX(array->capacity * 2, STARTING_CAPACITY);
+        if (json_array_resize(array, new_capacity) == JSONFailure) {
+            return JSONFailure;
+        }
+    }
+    value->parent = json_array_get_wrapping_value(array);
+    array->items[array->count] = value;
+    array->count++;
+    return JSONSuccess;
+}
+
+static JSON_Status json_array_resize(JSON_Array *array, size_t new_capacity) {
+    JSON_Value **new_items = NULL;
+    if (new_capacity == 0) {
+        return JSONFailure;
+    }
+    new_items = (JSON_Value**)parson_malloc(new_capacity * sizeof(JSON_Value*));
+    if (new_items == NULL) {
+        return JSONFailure;
+    }
+    if (array->items != NULL && array->count > 0) {
+        memcpy(new_items, array->items, array->count * sizeof(JSON_Value*));
+    }
+    parson_free(array->items);
+    array->items = new_items;
+    array->capacity = new_capacity;
+    return JSONSuccess;
+}
+
+static void json_array_free(JSON_Array *array) {
+    size_t i;
+    for (i = 0; i < array->count; i++) {
+        json_value_free(array->items[i]);
+    }
+    parson_free(array->items);
+    parson_free(array);
+}
+
+/* JSON Value */
+static JSON_Value * json_value_init_string_no_copy(char *string) {
+    JSON_Value *new_value = (JSON_Value*)parson_malloc(sizeof(JSON_Value));
+    if (!new_value) {
+        return NULL;
+    }
+    new_value->parent = NULL;
+    new_value->type = JSONString;
+    new_value->value.string = string;
+    return new_value;
+}
+
+/* Parser */
+static JSON_Status skip_quotes(const char **string) {
+    if (**string != '\"') {
+        return JSONFailure;
+    }
+    SKIP_CHAR(string);
+    while (**string != '\"') {
+        if (**string == '\0') {
+            return JSONFailure;
+        } else if (**string == '\\') {
+            SKIP_CHAR(string);
+            if (**string == '\0') {
+                return JSONFailure;
+            }
+        }
+        SKIP_CHAR(string);
+    }
+    SKIP_CHAR(string);
+    return JSONSuccess;
+}
+
+static int parse_utf16(const char **unprocessed, char **processed) {
+    unsigned int cp, lead, trail;
+    int parse_succeeded = 0;
+    char *processed_ptr = *processed;
+    const char *unprocessed_ptr = *unprocessed;
+    unprocessed_ptr++; /* skips u */
+    parse_succeeded = parse_utf16_hex(unprocessed_ptr, &cp);
+    if (!parse_succeeded) {
+        return JSONFailure;
+    }
+    if (cp < 0x80) {
+        processed_ptr[0] = (char)cp; /* 0xxxxxxx */
+    } else if (cp < 0x800) {
+        processed_ptr[0] = ((cp >> 6) & 0x1F) | 0xC0; /* 110xxxxx */
+        processed_ptr[1] = ((cp)      & 0x3F) | 0x80; /* 10xxxxxx */
+        processed_ptr += 1;
+    } else if (cp < 0xD800 || cp > 0xDFFF) {
+        processed_ptr[0] = ((cp >> 12) & 0x0F) | 0xE0; /* 1110xxxx */
+        processed_ptr[1] = ((cp >> 6)  & 0x3F) | 0x80; /* 10xxxxxx */
+        processed_ptr[2] = ((cp)       & 0x3F) | 0x80; /* 10xxxxxx */
+        processed_ptr += 2;
+    } else if (cp >= 0xD800 && cp <= 0xDBFF) { /* lead surrogate (0xD800..0xDBFF) */
+        lead = cp;
+        unprocessed_ptr += 4; /* should always be within the buffer, otherwise previous sscanf would fail */
+        if (*unprocessed_ptr++ != '\\' || *unprocessed_ptr++ != 'u') {
+            return JSONFailure;
+        }
+        parse_succeeded = parse_utf16_hex(unprocessed_ptr, &trail);
+        if (!parse_succeeded || trail < 0xDC00 || trail > 0xDFFF) { /* valid trail surrogate? (0xDC00..0xDFFF) */
+            return JSONFailure;
+        }
+        cp = ((((lead - 0xD800) & 0x3FF) << 10) | ((trail - 0xDC00) & 0x3FF)) + 0x010000;
+        processed_ptr[0] = (((cp >> 18) & 0x07) | 0xF0); /* 11110xxx */
+        processed_ptr[1] = (((cp >> 12) & 0x3F) | 0x80); /* 10xxxxxx */
+        processed_ptr[2] = (((cp >> 6)  & 0x3F) | 0x80); /* 10xxxxxx */
+        processed_ptr[3] = (((cp)       & 0x3F) | 0x80); /* 10xxxxxx */
+        processed_ptr += 3;
+    } else { /* trail surrogate before lead surrogate */
+        return JSONFailure;
+    }
+    unprocessed_ptr += 3;
+    *processed = processed_ptr;
+    *unprocessed = unprocessed_ptr;
+    return JSONSuccess;
+}
+
+
+/* Copies and processes passed string up to supplied length.
+Example: "\u006Corem ipsum" -> lorem ipsum */
+static char* process_string(const char *input, size_t len) {
+    const char *input_ptr = input;
+    size_t initial_size = (len + 1) * sizeof(char);
+    size_t final_size = 0;
+    char *output = NULL, *output_ptr = NULL, *resized_output = NULL;
+    output = (char*)parson_malloc(initial_size);
+    if (output == NULL) {
+        goto error;
+    }
+    output_ptr = output;
+    while ((*input_ptr != '\0') && (size_t)(input_ptr - input) < len) {
+        if (*input_ptr == '\\') {
+            input_ptr++;
+            switch (*input_ptr) {
+                case '\"': *output_ptr = '\"'; break;
+                case '\\': *output_ptr = '\\'; break;
+                case '/':  *output_ptr = '/';  break;
+                case 'b':  *output_ptr = '\b'; break;
+                case 'f':  *output_ptr = '\f'; break;
+                case 'n':  *output_ptr = '\n'; break;
+                case 'r':  *output_ptr = '\r'; break;
+                case 't':  *output_ptr = '\t'; break;
+                case 'u':
+                    if (parse_utf16(&input_ptr, &output_ptr) == JSONFailure) {
+                        goto error;
+                    }
+                    break;
+                default:
+                    goto error;
+            }
+        } else if ((unsigned char)*input_ptr < 0x20) {
+            goto error; /* 0x00-0x19 are invalid characters for json string (http://www.ietf.org/rfc/rfc4627.txt) */
+        } else {
+            *output_ptr = *input_ptr;
+        }
+        output_ptr++;
+        input_ptr++;
+    }
+    *output_ptr = '\0';
+    /* resize to new length */
+    final_size = (size_t)(output_ptr-output) + 1;
+    /* todo: don't resize if final_size == initial_size */
+    resized_output = (char*)parson_malloc(final_size);
+    if (resized_output == NULL) {
+        goto error;
+    }
+    memcpy(resized_output, output, final_size);
+    parson_free(output);
+    return resized_output;
+error:
+    parson_free(output);
+    return NULL;
+}
+
+/* Return processed contents of a string between quotes and
+   skips passed argument to a matching quote. */
+static char * get_quoted_string(const char **string) {
+    const char *string_start = *string;
+    size_t string_len = 0;
+    JSON_Status status = skip_quotes(string);
+    if (status != JSONSuccess) {
+        return NULL;
+    }
+    string_len = *string - string_start - 2; /* length without quotes */
+    return process_string(string_start + 1, string_len);
+}
+
+static JSON_Value * parse_value(const char **string, size_t nesting) {
+    if (nesting > MAX_NESTING) {
+        return NULL;
+    }
+    SKIP_WHITESPACES(string);
+    switch (**string) {
+        case '{':
+            return parse_object_value(string, nesting + 1);
+        case '[':
+            return parse_array_value(string, nesting + 1);
+        case '\"':
+            return parse_string_value(string);
+        case 'f': case 't':
+            return parse_boolean_value(string);
+        case '-':
+        case '0': case '1': case '2': case '3': case '4':
+        case '5': case '6': case '7': case '8': case '9':
+            return parse_number_value(string);
+        case 'n':
+            return parse_null_value(string);
+        default:
+            return NULL;
+    }
+}
+
+static JSON_Value * parse_object_value(const char **string, size_t nesting) {
+    JSON_Value *output_value = NULL, *new_value = NULL;
+    JSON_Object *output_object = NULL;
+    char *new_key = NULL;
+    output_value = json_value_init_object();
+    if (output_value == NULL) {
+        return NULL;
+    }
+    if (**string != '{') {
+        json_value_free(output_value);
+        return NULL;
+    }
+    output_object = json_value_get_object(output_value);
+    SKIP_CHAR(string);
+    SKIP_WHITESPACES(string);
+    if (**string == '}') { /* empty object */
+        SKIP_CHAR(string);
+        return output_value;
+    }
+    while (**string != '\0') {
+        new_key = get_quoted_string(string);
+        if (new_key == NULL) {
+            json_value_free(output_value);
+            return NULL;
+        }
+        SKIP_WHITESPACES(string);
+        if (**string != ':') {
+            parson_free(new_key);
+            json_value_free(output_value);
+            return NULL;
+        }
+        SKIP_CHAR(string);
+        new_value = parse_value(string, nesting);
+        if (new_value == NULL) {
+            parson_free(new_key);
+            json_value_free(output_value);
+            return NULL;
+        }
+        if (json_object_add(output_object, new_key, new_value) == JSONFailure) {
+            parson_free(new_key);
+            json_value_free(new_value);
+            json_value_free(output_value);
+            return NULL;
+        }
+        parson_free(new_key);
+        SKIP_WHITESPACES(string);
+        if (**string != ',') {
+            break;
+        }
+        SKIP_CHAR(string);
+        SKIP_WHITESPACES(string);
+    }
+    SKIP_WHITESPACES(string);
+    if (**string != '}' || /* Trim object after parsing is over */
+        json_object_resize(output_object, json_object_get_count(output_object)) == JSONFailure) {
+            json_value_free(output_value);
+            return NULL;
+    }
+    SKIP_CHAR(string);
+    return output_value;
+}
+
+static JSON_Value * parse_array_value(const char **string, size_t nesting) {
+    JSON_Value *output_value = NULL, *new_array_value = NULL;
+    JSON_Array *output_array = NULL;
+    output_value = json_value_init_array();
+    if (output_value == NULL) {
+        return NULL;
+    }
+    if (**string != '[') {
+        json_value_free(output_value);
+        return NULL;
+    }
+    output_array = json_value_get_array(output_value);
+    SKIP_CHAR(string);
+    SKIP_WHITESPACES(string);
+    if (**string == ']') { /* empty array */
+        SKIP_CHAR(string);
+        return output_value;
+    }
+    while (**string != '\0') {
+        new_array_value = parse_value(string, nesting);
+        if (new_array_value == NULL) {
+            json_value_free(output_value);
+            return NULL;
+        }
+        if (json_array_add(output_array, new_array_value) == JSONFailure) {
+            json_value_free(new_array_value);
+            json_value_free(output_value);
+            return NULL;
+        }
+        SKIP_WHITESPACES(string);
+        if (**string != ',') {
+            break;
+        }
+        SKIP_CHAR(string);
+        SKIP_WHITESPACES(string);
+    }
+    SKIP_WHITESPACES(string);
+    if (**string != ']' || /* Trim array after parsing is over */
+        json_array_resize(output_array, json_array_get_count(output_array)) == JSONFailure) {
+            json_value_free(output_value);
+            return NULL;
+    }
+    SKIP_CHAR(string);
+    return output_value;
+}
+
+static JSON_Value * parse_string_value(const char **string) {
+    JSON_Value *value = NULL;
+    char *new_string = get_quoted_string(string);
+    if (new_string == NULL) {
+        return NULL;
+    }
+    value = json_value_init_string_no_copy(new_string);
+    if (value == NULL) {
+        parson_free(new_string);
+        return NULL;
+    }
+    return value;
+}
+
+static JSON_Value * parse_boolean_value(const char **string) {
+    size_t true_token_size = SIZEOF_TOKEN("true");
+    size_t false_token_size = SIZEOF_TOKEN("false");
+    if (strncmp("true", *string, true_token_size) == 0) {
+        *string += true_token_size;
+        return json_value_init_boolean(1);
+    } else if (strncmp("false", *string, false_token_size) == 0) {
+        *string += false_token_size;
+        return json_value_init_boolean(0);
+    }
+    return NULL;
+}
+
+static JSON_Value * parse_number_value(const char **string) {
+    char *end;
+    double number = 0;
+    errno = 0;
+    number = strtod(*string, &end);
+    if (errno || !is_decimal(*string, end - *string)) {
+        return NULL;
+    }
+    *string = end;
+    return json_value_init_number(number);
+}
+
+static JSON_Value * parse_null_value(const char **string) {
+    size_t token_size = SIZEOF_TOKEN("null");
+    if (strncmp("null", *string, token_size) == 0) {
+        *string += token_size;
+        return json_value_init_null();
+    }
+    return NULL;
+}
+
+/* Serialization */
+#define APPEND_STRING(str) do { written = append_string(buf, (str));\
+                                if (written < 0) { return -1; }\
+                                if (buf != NULL) { buf += written; }\
+                                written_total += written; } while(0)
+
+#define APPEND_INDENT(level) do { written = append_indent(buf, (level));\
+                                  if (written < 0) { return -1; }\
+                                  if (buf != NULL) { buf += written; }\
+                                  written_total += written; } while(0)
+
+static int json_serialize_to_buffer_r(const JSON_Value *value, char *buf, int level, int is_pretty, char *num_buf)
+{
+    const char *key = NULL, *string = NULL;
+    JSON_Value *temp_value = NULL;
+    JSON_Array *array = NULL;
+    JSON_Object *object = NULL;
+    size_t i = 0, count = 0;
+    double num = 0.0;
+    int written = -1, written_total = 0;
+
+    switch (json_value_get_type(value)) {
+        case JSONArray:
+            array = json_value_get_array(value);
+            count = json_array_get_count(array);
+            APPEND_STRING("[");
+            if (count > 0 && is_pretty) {
+                APPEND_STRING("\n");
+            }
+            for (i = 0; i < count; i++) {
+                if (is_pretty) {
+                    APPEND_INDENT(level+1);
+                }
+                temp_value = json_array_get_value(array, i);
+                written = json_serialize_to_buffer_r(temp_value, buf, level+1, is_pretty, num_buf);
+                if (written < 0) {
+                    return -1;
+                }
+                if (buf != NULL) {
+                    buf += written;
+                }
+                written_total += written;
+                if (i < (count - 1)) {
+                    APPEND_STRING(",");
+                }
+                if (is_pretty) {
+                    APPEND_STRING("\n");
+                }
+            }
+            if (count > 0 && is_pretty) {
+                APPEND_INDENT(level);
+            }
+            APPEND_STRING("]");
+            return written_total;
+        case JSONObject:
+            object = json_value_get_object(value);
+            count  = json_object_get_count(object);
+            APPEND_STRING("{");
+            if (count > 0 && is_pretty) {
+                APPEND_STRING("\n");
+            }
+            for (i = 0; i < count; i++) {
+                key = json_object_get_name(object, i);
+                if (key == NULL) {
+                    return -1;
+                }
+                if (is_pretty) {
+                    APPEND_INDENT(level+1);
+                }
+                written = json_serialize_string(key, buf);
+                if (written < 0) {
+                    return -1;
+                }
+                if (buf != NULL) {
+                    buf += written;
+                }
+                written_total += written;
+                APPEND_STRING(":");
+                if (is_pretty) {
+                    APPEND_STRING(" ");
+                }
+                temp_value = json_object_get_value(object, key);
+                written = json_serialize_to_buffer_r(temp_value, buf, level+1, is_pretty, num_buf);
+                if (written < 0) {
+                    return -1;
+                }
+                if (buf != NULL) {
+                    buf += written;
+                }
+                written_total += written;
+                if (i < (count - 1)) {
+                    APPEND_STRING(",");
+                }
+                if (is_pretty) {
+                    APPEND_STRING("\n");
+                }
+            }
+            if (count > 0 && is_pretty) {
+                APPEND_INDENT(level);
+            }
+            APPEND_STRING("}");
+            return written_total;
+        case JSONString:
+            string = json_value_get_string(value);
+            if (string == NULL) {
+                return -1;
+            }
+            written = json_serialize_string(string, buf);
+            if (written < 0) {
+                return -1;
+            }
+            if (buf != NULL) {
+                buf += written;
+            }
+            written_total += written;
+            return written_total;
+        case JSONBoolean:
+            if (json_value_get_boolean(value)) {
+                APPEND_STRING("true");
+            } else {
+                APPEND_STRING("false");
+            }
+            return written_total;
+        case JSONNumber:
+            num = json_value_get_number(value);
+            if (buf != NULL) {
+                num_buf = buf;
+            }
+            written = sprintf(num_buf, FLOAT_FORMAT, num);
+            if (written < 0) {
+                return -1;
+            }
+            if (buf != NULL) {
+                buf += written;
+            }
+            written_total += written;
+            return written_total;
+        case JSONNull:
+            APPEND_STRING("null");
+            return written_total;
+        case JSONError:
+            return -1;
+        default:
+            return -1;
+    }
+}
+
+static int json_serialize_string(const char *string, char *buf) {
+    size_t i = 0, len = strlen(string);
+    char c = '\0';
+    int written = -1, written_total = 0;
+    APPEND_STRING("\"");
+    for (i = 0; i < len; i++) {
+        c = string[i];
+        switch (c) {
+            case '\"': APPEND_STRING("\\\""); break;
+            case '\\': APPEND_STRING("\\\\"); break;
+            case '/':  APPEND_STRING("\\/"); break; /* to make json embeddable in xml\/html */
+            case '\b': APPEND_STRING("\\b"); break;
+            case '\f': APPEND_STRING("\\f"); break;
+            case '\n': APPEND_STRING("\\n"); break;
+            case '\r': APPEND_STRING("\\r"); break;
+            case '\t': APPEND_STRING("\\t"); break;
+            case '\x00': APPEND_STRING("\\u0000"); break;
+            case '\x01': APPEND_STRING("\\u0001"); break;
+            case '\x02': APPEND_STRING("\\u0002"); break;
+            case '\x03': APPEND_STRING("\\u0003"); break;
+            case '\x04': APPEND_STRING("\\u0004"); break;
+            case '\x05': APPEND_STRING("\\u0005"); break;
+            case '\x06': APPEND_STRING("\\u0006"); break;
+            case '\x07': APPEND_STRING("\\u0007"); break;
+            /* '\x08' duplicate: '\b' */
+            /* '\x09' duplicate: '\t' */
+            /* '\x0a' duplicate: '\n' */
+            case '\x0b': APPEND_STRING("\\u000b"); break;
+            /* '\x0c' duplicate: '\f' */
+            /* '\x0d' duplicate: '\r' */
+            case '\x0e': APPEND_STRING("\\u000e"); break;
+            case '\x0f': APPEND_STRING("\\u000f"); break;
+            case '\x10': APPEND_STRING("\\u0010"); break;
+            case '\x11': APPEND_STRING("\\u0011"); break;
+            case '\x12': APPEND_STRING("\\u0012"); break;
+            case '\x13': APPEND_STRING("\\u0013"); break;
+            case '\x14': APPEND_STRING("\\u0014"); break;
+            case '\x15': APPEND_STRING("\\u0015"); break;
+            case '\x16': APPEND_STRING("\\u0016"); break;
+            case '\x17': APPEND_STRING("\\u0017"); break;
+            case '\x18': APPEND_STRING("\\u0018"); break;
+            case '\x19': APPEND_STRING("\\u0019"); break;
+            case '\x1a': APPEND_STRING("\\u001a"); break;
+            case '\x1b': APPEND_STRING("\\u001b"); break;
+            case '\x1c': APPEND_STRING("\\u001c"); break;
+            case '\x1d': APPEND_STRING("\\u001d"); break;
+            case '\x1e': APPEND_STRING("\\u001e"); break;
+            case '\x1f': APPEND_STRING("\\u001f"); break;
+            default:
+                if (buf != NULL) {
+                    buf[0] = c;
+                    buf += 1;
+                }
+                written_total += 1;
+                break;
+        }
+    }
+    APPEND_STRING("\"");
+    return written_total;
+}
+
+static int append_indent(char *buf, int level) {
+    int i;
+    int written = -1, written_total = 0;
+    for (i = 0; i < level; i++) {
+        APPEND_STRING("    ");
+    }
+    return written_total;
+}
+
+static int append_string(char *buf, const char *string) {
+    if (buf == NULL) {
+        return (int)strlen(string);
+    }
+    return sprintf(buf, "%s", string);
+}
+
+#undef APPEND_STRING
+#undef APPEND_INDENT
+
+/* Parser API */
+JSON_Value * json_parse_file(const char *filename) {
+    char *file_contents = read_file(filename);
+    JSON_Value *output_value = NULL;
+    if (file_contents == NULL) {
+        return NULL;
+    }
+    output_value = json_parse_string(file_contents);
+    parson_free(file_contents);
+    return output_value;
+}
+
+JSON_Value * json_parse_file_with_comments(const char *filename) {
+    char *file_contents = read_file(filename);
+    JSON_Value *output_value = NULL;
+    if (file_contents == NULL) {
+        return NULL;
+    }
+    output_value = json_parse_string_with_comments(file_contents);
+    parson_free(file_contents);
+    return output_value;
+}
+
+JSON_Value * json_parse_string(const char *string) {
+    if (string == NULL) {
+        return NULL;
+    }
+    if (string[0] == '\xEF' && string[1] == '\xBB' && string[2] == '\xBF') {
+        string = string + 3; /* Support for UTF-8 BOM */
+    }
+    return parse_value((const char**)&string, 0);
+}
+
+JSON_Value * json_parse_string_with_comments(const char *string) {
+    JSON_Value *result = NULL;
+    char *string_mutable_copy = NULL, *string_mutable_copy_ptr = NULL;
+    string_mutable_copy = parson_strdup(string);
+    if (string_mutable_copy == NULL) {
+        return NULL;
+    }
+    remove_comments(string_mutable_copy, "/*", "*/");
+    remove_comments(string_mutable_copy, "//", "\n");
+    string_mutable_copy_ptr = string_mutable_copy;
+    result = parse_value((const char**)&string_mutable_copy_ptr, 0);
+    parson_free(string_mutable_copy);
+    return result;
+}
+
+/* JSON Object API */
+
+JSON_Value * json_object_get_value(const JSON_Object *object, const char *name) {
+    if (object == NULL || name == NULL) {
+        return NULL;
+    }
+    return json_object_getn_value(object, name, strlen(name));
+}
+
+const char * json_object_get_string(const JSON_Object *object, const char *name) {
+    return json_value_get_string(json_object_get_value(object, name));
+}
+
+double json_object_get_number(const JSON_Object *object, const char *name) {
+    return json_value_get_number(json_object_get_value(object, name));
+}
+
+JSON_Object * json_object_get_object(const JSON_Object *object, const char *name) {
+    return json_value_get_object(json_object_get_value(object, name));
+}
+
+JSON_Array * json_object_get_array(const JSON_Object *object, const char *name) {
+    return json_value_get_array(json_object_get_value(object, name));
+}
+
+int json_object_get_boolean(const JSON_Object *object, const char *name) {
+    return json_value_get_boolean(json_object_get_value(object, name));
+}
+
+JSON_Value * json_object_dotget_value(const JSON_Object *object, const char *name) {
+    const char *dot_position = strchr(name, '.');
+    if (!dot_position) {
+        return json_object_get_value(object, name);
+    }
+    object = json_value_get_object(json_object_getn_value(object, name, dot_position - name));
+    return json_object_dotget_value(object, dot_position + 1);
+}
+
+const char * json_object_dotget_string(const JSON_Object *object, const char *name) {
+    return json_value_get_string(json_object_dotget_value(object, name));
+}
+
+double json_object_dotget_number(const JSON_Object *object, const char *name) {
+    return json_value_get_number(json_object_dotget_value(object, name));
+}
+
+JSON_Object * json_object_dotget_object(const JSON_Object *object, const char *name) {
+    return json_value_get_object(json_object_dotget_value(object, name));
+}
+
+JSON_Array * json_object_dotget_array(const JSON_Object *object, const char *name) {
+    return json_value_get_array(json_object_dotget_value(object, name));
+}
+
+int json_object_dotget_boolean(const JSON_Object *object, const char *name) {
+    return json_value_get_boolean(json_object_dotget_value(object, name));
+}
+
+size_t json_object_get_count(const JSON_Object *object) {
+    return object ? object->count : 0;
+}
+
+const char * json_object_get_name(const JSON_Object *object, size_t index) {
+    if (object == NULL || index >= json_object_get_count(object)) {
+        return NULL;
+    }
+    return object->names[index];
+}
+
+JSON_Value * json_object_get_value_at(const JSON_Object *object, size_t index) {
+    if (object == NULL || index >= json_object_get_count(object)) {
+        return NULL;
+    }
+    return object->values[index];
+}
+
+JSON_Value *json_object_get_wrapping_value(const JSON_Object *object) {
+    return object->wrapping_value;
+}
+
+int json_object_has_value (const JSON_Object *object, const char *name) {
+    return json_object_get_value(object, name) != NULL;
+}
+
+int json_object_has_value_of_type(const JSON_Object *object, const char *name, JSON_Value_Type type) {
+    JSON_Value *val = json_object_get_value(object, name);
+    return val != NULL && json_value_get_type(val) == type;
+}
+
+int json_object_dothas_value (const JSON_Object *object, const char *name) {
+    return json_object_dotget_value(object, name) != NULL;
+}
+
+int json_object_dothas_value_of_type(const JSON_Object *object, const char *name, JSON_Value_Type type) {
+    JSON_Value *val = json_object_dotget_value(object, name);
+    return val != NULL && json_value_get_type(val) == type;
+}
+
+/* JSON Array API */
+JSON_Value * json_array_get_value(const JSON_Array *array, size_t index) {
+    if (array == NULL || index >= json_array_get_count(array)) {
+        return NULL;
+    }
+    return array->items[index];
+}
+
+const char * json_array_get_string(const JSON_Array *array, size_t index) {
+    return json_value_get_string(json_array_get_value(array, index));
+}
+
+double json_array_get_number(const JSON_Array *array, size_t index) {
+    return json_value_get_number(json_array_get_value(array, index));
+}
+
+JSON_Object * json_array_get_object(const JSON_Array *array, size_t index) {
+    return json_value_get_object(json_array_get_value(array, index));
+}
+
+JSON_Array * json_array_get_array(const JSON_Array *array, size_t index) {
+    return json_value_get_array(json_array_get_value(array, index));
+}
+
+int json_array_get_boolean(const JSON_Array *array, size_t index) {
+    return json_value_get_boolean(json_array_get_value(array, index));
+}
+
+size_t json_array_get_count(const JSON_Array *array) {
+    return array ? array->count : 0;
+}
+
+JSON_Value * json_array_get_wrapping_value(const JSON_Array *array) {
+    return array->wrapping_value;
+}
+
+/* JSON Value API */
+JSON_Value_Type json_value_get_type(const JSON_Value *value) {
+    return value ? value->type : JSONError;
+}
+
+JSON_Object * json_value_get_object(const JSON_Value *value) {
+    return json_value_get_type(value) == JSONObject ? value->value.object : NULL;
+}
+
+JSON_Array * json_value_get_array(const JSON_Value *value) {
+    return json_value_get_type(value) == JSONArray ? value->value.array : NULL;
+}
+
+const char * json_value_get_string(const JSON_Value *value) {
+    return json_value_get_type(value) == JSONString ? value->value.string : NULL;
+}
+
+double json_value_get_number(const JSON_Value *value) {
+    return json_value_get_type(value) == JSONNumber ? value->value.number : 0;
+}
+
+int json_value_get_boolean(const JSON_Value *value) {
+    return json_value_get_type(value) == JSONBoolean ? value->value.boolean : -1;
+}
+
+JSON_Value * json_value_get_parent (const JSON_Value *value) {
+    return value ? value->parent : NULL;
+}
+
+void json_value_free(JSON_Value *value) {
+    switch (json_value_get_type(value)) {
+        case JSONObject:
+            json_object_free(value->value.object);
+            break;
+        case JSONString:
+            parson_free(value->value.string);
+            break;
+        case JSONArray:
+            json_array_free(value->value.array);
+            break;
+        default:
+            break;
+    }
+    parson_free(value);
+}
+
+JSON_Value * json_value_init_object(void) {
+    JSON_Value *new_value = (JSON_Value*)parson_malloc(sizeof(JSON_Value));
+    if (!new_value) {
+        return NULL;
+    }
+    new_value->parent = NULL;
+    new_value->type = JSONObject;
+    new_value->value.object = json_object_init(new_value);
+    if (!new_value->value.object) {
+        parson_free(new_value);
+        return NULL;
+    }
+    return new_value;
+}
+
+JSON_Value * json_value_init_array(void) {
+    JSON_Value *new_value = (JSON_Value*)parson_malloc(sizeof(JSON_Value));
+    if (!new_value) {
+        return NULL;
+    }
+    new_value->parent = NULL;
+    new_value->type = JSONArray;
+    new_value->value.array = json_array_init(new_value);
+    if (!new_value->value.array) {
+        parson_free(new_value);
+        return NULL;
+    }
+    return new_value;
+}
+
+JSON_Value * json_value_init_string(const char *string) {
+    char *copy = NULL;
+    JSON_Value *value;
+    size_t string_len = 0;
+    if (string == NULL) {
+        return NULL;
+    }
+    string_len = strlen(string);
+    if (!is_valid_utf8(string, string_len)) {
+        return NULL;
+    }
+    copy = parson_strndup(string, string_len);
+    if (copy == NULL) {
+        return NULL;
+    }
+    value = json_value_init_string_no_copy(copy);
+    if (value == NULL) {
+        parson_free(copy);
+    }
+    return value;
+}
+
+JSON_Value * json_value_init_number(double number) {
+    JSON_Value *new_value = NULL;
+#ifdef __GNUC__
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wfloat-equal"
+#endif
+    if (IS_NUMBER_INVALID(number)) {
+#ifdef __GNUC__
+#pragma GCC diagnostic pop
+#endif
+        return NULL;
+    }
+    new_value = (JSON_Value*)parson_malloc(sizeof(JSON_Value));
+    if (new_value == NULL) {
+        return NULL;
+    }
+    new_value->parent = NULL;
+    new_value->type = JSONNumber;
+    new_value->value.number = number;
+    return new_value;
+}
+
+JSON_Value * json_value_init_boolean(int boolean) {
+    JSON_Value *new_value = (JSON_Value*)parson_malloc(sizeof(JSON_Value));
+    if (!new_value) {
+        return NULL;
+    }
+    new_value->parent = NULL;
+    new_value->type = JSONBoolean;
+    new_value->value.boolean = boolean ? 1 : 0;
+    return new_value;
+}
+
+JSON_Value * json_value_init_null(void) {
+    JSON_Value *new_value = (JSON_Value*)parson_malloc(sizeof(JSON_Value));
+    if (!new_value) {
+        return NULL;
+    }
+    new_value->parent = NULL;
+    new_value->type = JSONNull;
+    return new_value;
+}
+
+JSON_Value * json_value_deep_copy(const JSON_Value *value) {
+    size_t i = 0;
+    JSON_Value *return_value = NULL, *temp_value_copy = NULL, *temp_value = NULL;
+    const char *temp_string = NULL, *temp_key = NULL;
+    char *temp_string_copy = NULL;
+    JSON_Array *temp_array = NULL, *temp_array_copy = NULL;
+    JSON_Object *temp_object = NULL, *temp_object_copy = NULL;
+
+    switch (json_value_get_type(value)) {
+        case JSONArray:
+            temp_array = json_value_get_array(value);
+            return_value = json_value_init_array();
+            if (return_value == NULL) {
+                return NULL;
+            }
+            temp_array_copy = json_value_get_array(return_value);
+            for (i = 0; i < json_array_get_count(temp_array); i++) {
+                temp_value = json_array_get_value(temp_array, i);
+                temp_value_copy = json_value_deep_copy(temp_value);
+                if (temp_value_copy == NULL) {
+                    json_value_free(return_value);
+                    return NULL;
+                }
+                if (json_array_add(temp_array_copy, temp_value_copy) == JSONFailure) {
+                    json_value_free(return_value);
+                    json_value_free(temp_value_copy);
+                    return NULL;
+                }
+            }
+            return return_value;
+        case JSONObject:
+            temp_object = json_value_get_object(value);
+            return_value = json_value_init_object();
+            if (return_value == NULL) {
+                return NULL;
+            }
+            temp_object_copy = json_value_get_object(return_value);
+            for (i = 0; i < json_object_get_count(temp_object); i++) {
+                temp_key = json_object_get_name(temp_object, i);
+                temp_value = json_object_get_value(temp_object, temp_key);
+                temp_value_copy = json_value_deep_copy(temp_value);
+                if (temp_value_copy == NULL) {
+                    json_value_free(return_value);
+                    return NULL;
+                }
+                if (json_object_add(temp_object_copy, temp_key, temp_value_copy) == JSONFailure) {
+                    json_value_free(return_value);
+                    json_value_free(temp_value_copy);
+                    return NULL;
+                }
+            }
+            return return_value;
+        case JSONBoolean:
+            return json_value_init_boolean(json_value_get_boolean(value));
+        case JSONNumber:
+            return json_value_init_number(json_value_get_number(value));
+        case JSONString:
+            temp_string = json_value_get_string(value);
+            if (temp_string == NULL) {
+                return NULL;
+            }
+            temp_string_copy = parson_strdup(temp_string);
+            if (temp_string_copy == NULL) {
+                return NULL;
+            }
+            return_value = json_value_init_string_no_copy(temp_string_copy);
+            if (return_value == NULL) {
+                parson_free(temp_string_copy);
+            }
+            return return_value;
+        case JSONNull:
+            return json_value_init_null();
+        case JSONError:
+            return NULL;
+        default:
+            return NULL;
+    }
+}
+
+size_t json_serialization_size(const JSON_Value *value) {
+    char num_buf[NUM_BUF_SIZE]; /* recursively allocating buffer on stack is a bad idea, so let's do it only once */
+    int res = json_serialize_to_buffer_r(value, NULL, 0, 0, num_buf);
+    return res < 0 ? 0 : (size_t)(res + 1);
+}
+
+JSON_Status json_serialize_to_buffer(const JSON_Value *value, char *buf, size_t buf_size_in_bytes) {
+    int written = -1;
+    size_t needed_size_in_bytes = json_serialization_size(value);
+    if (needed_size_in_bytes == 0 || buf_size_in_bytes < needed_size_in_bytes) {
+        return JSONFailure;
+    }
+    written = json_serialize_to_buffer_r(value, buf, 0, 0, NULL);
+    if (written < 0) {
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_serialize_to_file(const JSON_Value *value, const char *filename) {
+    JSON_Status return_code = JSONSuccess;
+    FILE *fp = NULL;
+    char *serialized_string = json_serialize_to_string(value);
+    if (serialized_string == NULL) {
+        return JSONFailure;
+    }
+    fp = fopen(filename, "w");
+    if (fp == NULL) {
+        json_free_serialized_string(serialized_string);
+        return JSONFailure;
+    }
+    if (fputs(serialized_string, fp) == EOF) {
+        return_code = JSONFailure;
+    }
+    if (fclose(fp) == EOF) {
+        return_code = JSONFailure;
+    }
+    json_free_serialized_string(serialized_string);
+    return return_code;
+}
+
+char * json_serialize_to_string(const JSON_Value *value) {
+    JSON_Status serialization_result = JSONFailure;
+    size_t buf_size_bytes = json_serialization_size(value);
+    char *buf = NULL;
+    if (buf_size_bytes == 0) {
+        return NULL;
+    }
+    buf = (char*)parson_malloc(buf_size_bytes);
+    if (buf == NULL) {
+        return NULL;
+    }
+    serialization_result = json_serialize_to_buffer(value, buf, buf_size_bytes);
+    if (serialization_result == JSONFailure) {
+        json_free_serialized_string(buf);
+        return NULL;
+    }
+    return buf;
+}
+
+size_t json_serialization_size_pretty(const JSON_Value *value) {
+    char num_buf[NUM_BUF_SIZE]; /* recursively allocating buffer on stack is a bad idea, so let's do it only once */
+    int res = json_serialize_to_buffer_r(value, NULL, 0, 1, num_buf);
+    return res < 0 ? 0 : (size_t)(res + 1);
+}
+
+JSON_Status json_serialize_to_buffer_pretty(const JSON_Value *value, char *buf, size_t buf_size_in_bytes) {
+    int written = -1;
+    size_t needed_size_in_bytes = json_serialization_size_pretty(value);
+    if (needed_size_in_bytes == 0 || buf_size_in_bytes < needed_size_in_bytes) {
+        return JSONFailure;
+    }
+    written = json_serialize_to_buffer_r(value, buf, 0, 1, NULL);
+    if (written < 0) {
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_serialize_to_file_pretty(const JSON_Value *value, const char *filename) {
+    JSON_Status return_code = JSONSuccess;
+    FILE *fp = NULL;
+    char *serialized_string = json_serialize_to_string_pretty(value);
+    if (serialized_string == NULL) {
+        return JSONFailure;
+    }
+    fp = fopen(filename, "w");
+    if (fp == NULL) {
+        json_free_serialized_string(serialized_string);
+        return JSONFailure;
+    }
+    if (fputs(serialized_string, fp) == EOF) {
+        return_code = JSONFailure;
+    }
+    if (fclose(fp) == EOF) {
+        return_code = JSONFailure;
+    }
+    json_free_serialized_string(serialized_string);
+    return return_code;
+}
+
+char * json_serialize_to_string_pretty(const JSON_Value *value) {
+    JSON_Status serialization_result = JSONFailure;
+    size_t buf_size_bytes = json_serialization_size_pretty(value);
+    char *buf = NULL;
+    if (buf_size_bytes == 0) {
+        return NULL;
+    }
+    buf = (char*)parson_malloc(buf_size_bytes);
+    if (buf == NULL) {
+        return NULL;
+    }
+    serialization_result = json_serialize_to_buffer_pretty(value, buf, buf_size_bytes);
+    if (serialization_result == JSONFailure) {
+        json_free_serialized_string(buf);
+        return NULL;
+    }
+    return buf;
+}
+
+void json_free_serialized_string(char *string) {
+    parson_free(string);
+}
+
+JSON_Status json_array_remove(JSON_Array *array, size_t ix) {
+    size_t to_move_bytes = 0;
+    if (array == NULL || ix >= json_array_get_count(array)) {
+        return JSONFailure;
+    }
+    json_value_free(json_array_get_value(array, ix));
+    to_move_bytes = (json_array_get_count(array) - 1 - ix) * sizeof(JSON_Value*);
+    memmove(array->items + ix, array->items + ix + 1, to_move_bytes);
+    array->count -= 1;
+    return JSONSuccess;
+}
+
+JSON_Status json_array_replace_value(JSON_Array *array, size_t ix, JSON_Value *value) {
+    if (array == NULL || value == NULL || value->parent != NULL || ix >= json_array_get_count(array)) {
+        return JSONFailure;
+    }
+    json_value_free(json_array_get_value(array, ix));
+    value->parent = json_array_get_wrapping_value(array);
+    array->items[ix] = value;
+    return JSONSuccess;
+}
+
+JSON_Status json_array_replace_string(JSON_Array *array, size_t i, const char* string) {
+    JSON_Value *value = json_value_init_string(string);
+    if (value == NULL) {
+        return JSONFailure;
+    }
+    if (json_array_replace_value(array, i, value) == JSONFailure) {
+        json_value_free(value);
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_array_replace_number(JSON_Array *array, size_t i, double number) {
+    JSON_Value *value = json_value_init_number(number);
+    if (value == NULL) {
+        return JSONFailure;
+    }
+    if (json_array_replace_value(array, i, value) == JSONFailure) {
+        json_value_free(value);
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_array_replace_boolean(JSON_Array *array, size_t i, int boolean) {
+    JSON_Value *value = json_value_init_boolean(boolean);
+    if (value == NULL) {
+        return JSONFailure;
+    }
+    if (json_array_replace_value(array, i, value) == JSONFailure) {
+        json_value_free(value);
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_array_replace_null(JSON_Array *array, size_t i) {
+    JSON_Value *value = json_value_init_null();
+    if (value == NULL) {
+        return JSONFailure;
+    }
+    if (json_array_replace_value(array, i, value) == JSONFailure) {
+        json_value_free(value);
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_array_clear(JSON_Array *array) {
+    size_t i = 0;
+    if (array == NULL) {
+        return JSONFailure;
+    }
+    for (i = 0; i < json_array_get_count(array); i++) {
+        json_value_free(json_array_get_value(array, i));
+    }
+    array->count = 0;
+    return JSONSuccess;
+}
+
+JSON_Status json_array_append_value(JSON_Array *array, JSON_Value *value) {
+    if (array == NULL || value == NULL || value->parent != NULL) {
+        return JSONFailure;
+    }
+    return json_array_add(array, value);
+}
+
+JSON_Status json_array_append_string(JSON_Array *array, const char *string) {
+    JSON_Value *value = json_value_init_string(string);
+    if (value == NULL) {
+        return JSONFailure;
+    }
+    if (json_array_append_value(array, value) == JSONFailure) {
+        json_value_free(value);
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_array_append_number(JSON_Array *array, double number) {
+    JSON_Value *value = json_value_init_number(number);
+    if (value == NULL) {
+        return JSONFailure;
+    }
+    if (json_array_append_value(array, value) == JSONFailure) {
+        json_value_free(value);
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_array_append_boolean(JSON_Array *array, int boolean) {
+    JSON_Value *value = json_value_init_boolean(boolean);
+    if (value == NULL) {
+        return JSONFailure;
+    }
+    if (json_array_append_value(array, value) == JSONFailure) {
+        json_value_free(value);
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_array_append_null(JSON_Array *array) {
+    JSON_Value *value = json_value_init_null();
+    if (value == NULL) {
+        return JSONFailure;
+    }
+    if (json_array_append_value(array, value) == JSONFailure) {
+        json_value_free(value);
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_object_set_value(JSON_Object *object, const char *name, JSON_Value *value) {
+    size_t i = 0;
+    JSON_Value *old_value;
+    if (object == NULL || name == NULL || value == NULL || value->parent != NULL) {
+        return JSONFailure;
+    }
+    old_value = json_object_get_value(object, name);
+    if (old_value != NULL) { /* free and overwrite old value */
+        json_value_free(old_value);
+        for (i = 0; i < json_object_get_count(object); i++) {
+            if (strcmp(object->names[i], name) == 0) {
+                value->parent = json_object_get_wrapping_value(object);
+                object->values[i] = value;
+                return JSONSuccess;
+            }
+        }
+    }
+    /* add new key value pair */
+    return json_object_add(object, name, value);
+}
+
+JSON_Status json_object_set_string(JSON_Object *object, const char *name, const char *string) {
+    return json_object_set_value(object, name, json_value_init_string(string));
+}
+
+JSON_Status json_object_set_number(JSON_Object *object, const char *name, double number) {
+    return json_object_set_value(object, name, json_value_init_number(number));
+}
+
+JSON_Status json_object_set_boolean(JSON_Object *object, const char *name, int boolean) {
+    return json_object_set_value(object, name, json_value_init_boolean(boolean));
+}
+
+JSON_Status json_object_set_null(JSON_Object *object, const char *name) {
+    return json_object_set_value(object, name, json_value_init_null());
+}
+
+JSON_Status json_object_dotset_value(JSON_Object *object, const char *name, JSON_Value *value) {
+    const char *dot_pos = NULL;
+    JSON_Value *temp_value = NULL, *new_value = NULL;
+    JSON_Object *temp_object = NULL, *new_object = NULL;
+    JSON_Status status = JSONFailure;
+    size_t name_len = 0;
+    if (object == NULL || name == NULL || value == NULL) {
+        return JSONFailure;
+    }
+    dot_pos = strchr(name, '.');
+    if (dot_pos == NULL) {
+        return json_object_set_value(object, name, value);
+    }
+    name_len = dot_pos - name;
+    temp_value = json_object_getn_value(object, name, name_len);
+    if (temp_value) {
+        /* Don't overwrite existing non-object (unlike json_object_set_value, but it shouldn't be changed at this point) */
+        if (json_value_get_type(temp_value) != JSONObject) {
+            return JSONFailure;
+        }
+        temp_object = json_value_get_object(temp_value);
+        return json_object_dotset_value(temp_object, dot_pos + 1, value);
+    }
+    new_value = json_value_init_object();
+    if (new_value == NULL) {
+        return JSONFailure;
+    }
+    new_object = json_value_get_object(new_value);
+    status = json_object_dotset_value(new_object, dot_pos + 1, value);
+    if (status != JSONSuccess) {
+        json_value_free(new_value);
+        return JSONFailure;
+    }
+    status = json_object_addn(object, name, name_len, new_value);
+    if (status != JSONSuccess) {
+        json_object_dotremove_internal(new_object, dot_pos + 1, 0);
+        json_value_free(new_value);
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_object_dotset_string(JSON_Object *object, const char *name, const char *string) {
+    JSON_Value *value = json_value_init_string(string);
+    if (value == NULL) {
+        return JSONFailure;
+    }
+    if (json_object_dotset_value(object, name, value) == JSONFailure) {
+        json_value_free(value);
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_object_dotset_number(JSON_Object *object, const char *name, double number) {
+    JSON_Value *value = json_value_init_number(number);
+    if (value == NULL) {
+        return JSONFailure;
+    }
+    if (json_object_dotset_value(object, name, value) == JSONFailure) {
+        json_value_free(value);
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_object_dotset_boolean(JSON_Object *object, const char *name, int boolean) {
+    JSON_Value *value = json_value_init_boolean(boolean);
+    if (value == NULL) {
+        return JSONFailure;
+    }
+    if (json_object_dotset_value(object, name, value) == JSONFailure) {
+        json_value_free(value);
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_object_dotset_null(JSON_Object *object, const char *name) {
+    JSON_Value *value = json_value_init_null();
+    if (value == NULL) {
+        return JSONFailure;
+    }
+    if (json_object_dotset_value(object, name, value) == JSONFailure) {
+        json_value_free(value);
+        return JSONFailure;
+    }
+    return JSONSuccess;
+}
+
+JSON_Status json_object_remove(JSON_Object *object, const char *name) {
+    return json_object_remove_internal(object, name, 1);
+}
+
+JSON_Status json_object_dotremove(JSON_Object *object, const char *name) {
+    return json_object_dotremove_internal(object, name, 1);
+}
+
+JSON_Status json_object_clear(JSON_Object *object) {
+    size_t i = 0;
+    if (object == NULL) {
+        return JSONFailure;
+    }
+    for (i = 0; i < json_object_get_count(object); i++) {
+        parson_free(object->names[i]);
+        json_value_free(object->values[i]);
+    }
+    object->count = 0;
+    return JSONSuccess;
+}
+
+JSON_Status json_validate(const JSON_Value *schema, const JSON_Value *value) {
+    JSON_Value *temp_schema_value = NULL, *temp_value = NULL;
+    JSON_Array *schema_array = NULL, *value_array = NULL;
+    JSON_Object *schema_object = NULL, *value_object = NULL;
+    JSON_Value_Type schema_type = JSONError, value_type = JSONError;
+    const char *key = NULL;
+    size_t i = 0, count = 0;
+    if (schema == NULL || value == NULL) {
+        return JSONFailure;
+    }
+    schema_type = json_value_get_type(schema);
+    value_type = json_value_get_type(value);
+    if (schema_type != value_type && schema_type != JSONNull) { /* null represents all values */
+        return JSONFailure;
+    }
+    switch (schema_type) {
+        case JSONArray:
+            schema_array = json_value_get_array(schema);
+            value_array = json_value_get_array(value);
+            count = json_array_get_count(schema_array);
+            if (count == 0) {
+                return JSONSuccess; /* Empty array allows all types */
+            }
+            /* Get first value from array, rest is ignored */
+            temp_schema_value = json_array_get_value(schema_array, 0);
+            for (i = 0; i < json_array_get_count(value_array); i++) {
+                temp_value = json_array_get_value(value_array, i);
+                if (json_validate(temp_schema_value, temp_value) == JSONFailure) {
+                    return JSONFailure;
+                }
+            }
+            return JSONSuccess;
+        case JSONObject:
+            schema_object = json_value_get_object(schema);
+            value_object = json_value_get_object(value);
+            count = json_object_get_count(schema_object);
+            if (count == 0) {
+                return JSONSuccess; /* Empty object allows all objects */
+            } else if (json_object_get_count(value_object) < count) {
+                return JSONFailure; /* Tested object mustn't have less name-value pairs than schema */
+            }
+            for (i = 0; i < count; i++) {
+                key = json_object_get_name(schema_object, i);
+                temp_schema_value = json_object_get_value(schema_object, key);
+                temp_value = json_object_get_value(value_object, key);
+                if (temp_value == NULL) {
+                    return JSONFailure;
+                }
+                if (json_validate(temp_schema_value, temp_value) == JSONFailure) {
+                    return JSONFailure;
+                }
+            }
+            return JSONSuccess;
+        case JSONString: case JSONNumber: case JSONBoolean: case JSONNull:
+            return JSONSuccess; /* equality already tested before switch */
+        case JSONError: default:
+            return JSONFailure;
+    }
+}
+
+int json_value_equals(const JSON_Value *a, const JSON_Value *b) {
+    JSON_Object *a_object = NULL, *b_object = NULL;
+    JSON_Array *a_array = NULL, *b_array = NULL;
+    const char *a_string = NULL, *b_string = NULL;
+    const char *key = NULL;
+    size_t a_count = 0, b_count = 0, i = 0;
+    JSON_Value_Type a_type, b_type;
+    a_type = json_value_get_type(a);
+    b_type = json_value_get_type(b);
+    if (a_type != b_type) {
+        return 0;
+    }
+    switch (a_type) {
+        case JSONArray:
+            a_array = json_value_get_array(a);
+            b_array = json_value_get_array(b);
+            a_count = json_array_get_count(a_array);
+            b_count = json_array_get_count(b_array);
+            if (a_count != b_count) {
+                return 0;
+            }
+            for (i = 0; i < a_count; i++) {
+                if (!json_value_equals(json_array_get_value(a_array, i),
+                                       json_array_get_value(b_array, i))) {
+                    return 0;
+                }
+            }
+            return 1;
+        case JSONObject:
+            a_object = json_value_get_object(a);
+            b_object = json_value_get_object(b);
+            a_count = json_object_get_count(a_object);
+            b_count = json_object_get_count(b_object);
+            if (a_count != b_count) {
+                return 0;
+            }
+            for (i = 0; i < a_count; i++) {
+                key = json_object_get_name(a_object, i);
+                if (!json_value_equals(json_object_get_value(a_object, key),
+                                       json_object_get_value(b_object, key))) {
+                    return 0;
+                }
+            }
+            return 1;
+        case JSONString:
+            a_string = json_value_get_string(a);
+            b_string = json_value_get_string(b);
+            if (a_string == NULL || b_string == NULL) {
+                return 0; /* shouldn't happen */
+            }
+            return strcmp(a_string, b_string) == 0;
+        case JSONBoolean:
+            return json_value_get_boolean(a) == json_value_get_boolean(b);
+        case JSONNumber:
+            return fabs(json_value_get_number(a) - json_value_get_number(b)) < 0.000001; /* EPSILON */
+        case JSONError:
+            return 1;
+        case JSONNull:
+            return 1;
+        default:
+            return 1;
+    }
+}
+
+JSON_Value_Type json_type(const JSON_Value *value) {
+    return json_value_get_type(value);
+}
+
+JSON_Object * json_object (const JSON_Value *value) {
+    return json_value_get_object(value);
+}
+
+JSON_Array * json_array  (const JSON_Value *value) {
+    return json_value_get_array(value);
+}
+
+const char * json_string (const JSON_Value *value) {
+    return json_value_get_string(value);
+}
+
+double json_number (const JSON_Value *value) {
+    return json_value_get_number(value);
+}
+
+int json_boolean(const JSON_Value *value) {
+    return json_value_get_boolean(value);
+}
+
+void json_set_allocation_functions(JSON_Malloc_Function malloc_fun, JSON_Free_Function free_fun) {
+    parson_malloc = malloc_fun;
+    parson_free = free_fun;
+}
+
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif
diff --git a/projects/Roads/utilities/src/thirdparty/tinyspline.c b/projects/Roads/utilities/src/thirdparty/tinyspline.c
new file mode 100644
index 0000000000..2e67dd6dd6
--- /dev/null
+++ b/projects/Roads/utilities/src/thirdparty/tinyspline.c
@@ -0,0 +1,3480 @@
+#define TINYSPLINE_EXPORT
+#include "roads/thirdparty/tinyspline.h"
+#include "roads/thirdparty/parson.h" /* serialization */
+
+#include <stdlib.h> /* malloc, free */
+#include <math.h>   /* fabs, sqrt, acos */
+#include <string.h> /* memcpy, memmove */
+#include <stdio.h>  /* FILE, fopen */
+#include <stdarg.h> /* varargs */
+
+/* Suppress some useless MSVC warnings. */
+#ifdef _MSC_VER
+#pragma warning(push)
+/* address of dllimport */
+#pragma warning(disable:4232)
+/* function not inlined */
+#pragma warning(disable:4710)
+/* byte padding */
+#pragma warning(disable:4820)
+/* meaningless deprecation */
+#pragma warning(disable:4996)
+/* Spectre mitigation */
+#pragma warning(disable:5045)
+#endif
+
+#define INIT_OUT_BSPLINE(in, out)              \
+	if ((in) != (out))                     \
+		ts_int_bspline_init(out);
+
+
+
+/*! @name Internal Structs and Functions
+ *
+ * Internal functions are prefixed with \e ts_int (int for internal).
+ *
+ * @{
+ */
+/**
+ * Stores the private data of ::tsBSpline.
+ */
+struct tsBSplineImpl
+{
+	size_t deg; /**< Degree of B-Spline basis function. */
+	size_t dim; /**< Dimensionality of the control points (2D => x, y). */
+	size_t n_ctrlp; /**< Number of control points. */
+	size_t n_knots; /**< Number of knots (n_ctrlp + deg + 1). */
+};
+
+/**
+ * Stores the private data of ::tsDeBoorNet.
+ */
+struct tsDeBoorNetImpl
+{
+	tsReal u; /**< The evaluated knot. */
+	size_t k; /**< The index [u_k, u_k+1) */
+	size_t s; /**< Multiplicity of u_k. */
+	size_t h; /**< Number of insertions required to obtain result. */
+	size_t dim; /**< Dimensionality of the points (2D => x, y). */
+	size_t n_points; /** Number of points in `points'. */
+};
+
+void
+ts_int_bspline_init(tsBSpline *spline)
+{
+	spline->pImpl = NULL;
+}
+
+size_t
+ts_int_bspline_sof_state(const tsBSpline *spline)
+{
+	return sizeof(struct tsBSplineImpl) +
+	       ts_bspline_sof_control_points(spline) +
+	       ts_bspline_sof_knots(spline);
+}
+
+tsReal *
+ts_int_bspline_access_ctrlp(const tsBSpline *spline)
+{
+	return (tsReal *) (& spline->pImpl[1]);
+}
+
+tsReal *
+ts_int_bspline_access_knots(const tsBSpline *spline)
+{
+	return ts_int_bspline_access_ctrlp(spline) +
+	       ts_bspline_len_control_points(spline);
+}
+
+tsError
+ts_int_bspline_access_ctrlp_at(const tsBSpline *spline,
+                               size_t index,
+                               tsReal **ctrlp,
+                               tsStatus *status)
+{
+	const size_t num = ts_bspline_num_control_points(spline);
+	if (index >= num) {
+		TS_RETURN_2(status, TS_INDEX_ERROR,
+		            "index (%lu) >= num(control_points) (%lu)",
+		            (unsigned long) index,
+		            (unsigned long) num)
+	}
+	*ctrlp = ts_int_bspline_access_ctrlp(spline) +
+	         index * ts_bspline_dimension(spline);
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_int_bspline_access_knot_at(const tsBSpline *spline,
+                              size_t index,
+                              tsReal *knot,
+                              tsStatus *status)
+{
+	const size_t num = ts_bspline_num_knots(spline);
+	if (index >= num) {
+		TS_RETURN_2(status, TS_INDEX_ERROR,
+		            "index (%lu) >= num(knots) (%lu)",
+		            (unsigned long) index,
+		            (unsigned long) num)
+	}
+	*knot = ts_int_bspline_access_knots(spline)[index];
+	TS_RETURN_SUCCESS(status)
+}
+
+void
+ts_int_deboornet_init(tsDeBoorNet *net)
+{
+	net->pImpl = NULL;
+}
+
+size_t
+ts_int_deboornet_sof_state(const tsDeBoorNet *net)
+{
+	return sizeof(struct tsDeBoorNetImpl) +
+	       ts_deboornet_sof_points(net) +
+	       ts_deboornet_sof_result(net);
+}
+
+tsReal *
+ts_int_deboornet_access_points(const tsDeBoorNet *net)
+{
+	return (tsReal *) (& net->pImpl[1]);
+}
+
+tsReal *
+ts_int_deboornet_access_result(const tsDeBoorNet *net)
+{
+	if (ts_deboornet_num_result(net) == 2) {
+		return ts_int_deboornet_access_points(net);
+	} else {
+		return ts_int_deboornet_access_points(net) +
+		       /* Last point in `points`. */
+		       (ts_deboornet_len_points(net) -
+		        ts_deboornet_dimension(net));
+	}
+}
+/*! @} */
+
+
+
+/*! @name B-Spline Data
+ *
+ * @{
+ */
+size_t
+ts_bspline_degree(const tsBSpline *spline)
+{
+	return spline->pImpl->deg;
+}
+
+size_t
+ts_bspline_order(const tsBSpline *spline)
+{
+	return ts_bspline_degree(spline) + 1;
+}
+
+size_t
+ts_bspline_dimension(const tsBSpline *spline)
+{
+	return spline->pImpl->dim;
+}
+
+size_t
+ts_bspline_len_control_points(const tsBSpline *spline)
+{
+	return ts_bspline_num_control_points(spline) *
+	       ts_bspline_dimension(spline);
+}
+
+size_t
+ts_bspline_num_control_points(const tsBSpline *spline)
+{
+	return spline->pImpl->n_ctrlp;
+}
+
+size_t
+ts_bspline_sof_control_points(const tsBSpline *spline)
+{
+	return ts_bspline_len_control_points(spline) * sizeof(tsReal);
+}
+
+const tsReal *
+ts_bspline_control_points_ptr(const tsBSpline *spline)
+{
+	return ts_int_bspline_access_ctrlp(spline);
+}
+
+tsError
+ts_bspline_control_points(const tsBSpline *spline,
+                          tsReal **ctrlp,
+                          tsStatus *status)
+{
+	const size_t size = ts_bspline_sof_control_points(spline);
+	*ctrlp = (tsReal*) malloc(size);
+	if (!*ctrlp) TS_RETURN_0(status, TS_MALLOC, "out of memory")
+	memcpy(*ctrlp, ts_int_bspline_access_ctrlp(spline), size);
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_bspline_control_point_at_ptr(const tsBSpline *spline,
+                                size_t index,
+                                const tsReal **ctrlp,
+                                tsStatus *status)
+{
+	tsReal *vals;
+	tsError err;
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_int_bspline_access_ctrlp_at(
+		        spline, index, &vals, status))
+		*ctrlp = vals;
+	TS_CATCH(err)
+		*ctrlp = NULL;
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_set_control_points(tsBSpline *spline,
+                              const tsReal *ctrlp,
+                              tsStatus *status)
+{
+	const size_t size = ts_bspline_sof_control_points(spline);
+	memmove(ts_int_bspline_access_ctrlp(spline), ctrlp, size);
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_bspline_set_control_point_at(tsBSpline *spline,
+                                size_t index,
+                                const tsReal *ctrlp,
+                                tsStatus *status)
+{
+	tsReal *to;
+	size_t size;
+	tsError err;
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_int_bspline_access_ctrlp_at(
+		        spline, index, &to, status))
+		size = ts_bspline_dimension(spline) * sizeof(tsReal);
+		memcpy(to, ctrlp, size);
+	TS_END_TRY_RETURN(err)
+}
+
+size_t
+ts_bspline_num_knots(const tsBSpline *spline)
+{
+	return spline->pImpl->n_knots;
+}
+
+size_t
+ts_bspline_sof_knots(const tsBSpline *spline)
+{
+	return ts_bspline_num_knots(spline) * sizeof(tsReal);
+}
+
+const tsReal *
+ts_bspline_knots_ptr(const tsBSpline *spline)
+{
+	return ts_int_bspline_access_knots(spline);
+}
+
+tsError
+ts_bspline_knots(const tsBSpline *spline,
+                 tsReal **knots,
+                 tsStatus *status)
+{
+	const size_t size = ts_bspline_sof_knots(spline);
+	*knots = (tsReal*) malloc(size);
+	if (!*knots) TS_RETURN_0(status, TS_MALLOC, "out of memory")
+	memcpy(*knots, ts_int_bspline_access_knots(spline), size);
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_bspline_knot_at(const tsBSpline *spline,
+                   size_t index,
+                   tsReal *knot,
+                   tsStatus *status)
+{
+	return ts_int_bspline_access_knot_at(spline, index, knot, status);
+}
+
+tsError
+ts_bspline_set_knots(tsBSpline *spline,
+                     const tsReal *knots,
+                     tsStatus *status)
+{
+	const size_t size = ts_bspline_sof_knots(spline);
+	const size_t num_knots = ts_bspline_num_knots(spline);
+	const size_t order = ts_bspline_order(spline);
+	size_t idx, mult;
+	tsReal lst_knot, knot;
+	lst_knot = knots[0];
+	mult = 1;
+	for (idx = 1; idx < num_knots; idx++) {
+		knot = knots[idx];
+		if (ts_knots_equal(lst_knot, knot)) {
+			mult++;
+		} else if (lst_knot > knot) {
+			TS_RETURN_1(status, TS_KNOTS_DECR,
+			            "decreasing knot vector at index: %lu",
+			            (unsigned long) idx)
+		} else {
+			mult = 0;
+		}
+		if (mult > order) {
+			TS_RETURN_3(status, TS_MULTIPLICITY,
+			            "mult(%f) (%lu) > order (%lu)",
+			            knot, (unsigned long) mult,
+			            (unsigned long) order)
+		}
+		lst_knot = knot;
+	}
+	memmove(ts_int_bspline_access_knots(spline), knots, size);
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_bspline_set_knots_varargs(tsBSpline *spline,
+                             tsStatus *status,
+                             tsReal knot0,
+                             double knot1,
+                             ...)
+{
+	tsReal *values = NULL;
+	va_list argp;
+	size_t idx;
+	tsError err;
+
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_bspline_knots(
+		        spline, &values, status))
+
+		values[0] = knot0;
+		values[1] = (tsReal) knot1;
+		va_start(argp, knot1);
+		for (idx = 2; idx < ts_bspline_num_knots(spline); idx++)
+			values[idx] = (tsReal) va_arg(argp, double);
+		va_end(argp);
+
+		TS_CALL(try, err, ts_bspline_set_knots(
+		        spline, values, status))
+	TS_FINALLY
+		if (values) free(values);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_set_knot_at(tsBSpline *spline,
+                       size_t index,
+                       tsReal knot,
+                       tsStatus *status)
+{
+	tsError err;
+	tsReal *knots = NULL;
+	/* This is only for initialization. */
+	tsReal oldKnot = ts_int_bspline_access_knots(spline)[0];
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_int_bspline_access_knot_at(
+		        spline, index, &oldKnot, status))
+		/* knots must be set after reading oldKnot because the catch
+		 * block assumes that oldKnot contains the correct value if
+		 * knots is not NULL. */
+		knots = ts_int_bspline_access_knots(spline);
+		knots[index] = knot;
+		TS_CALL(try, err, ts_bspline_set_knots(
+		        spline, knots, status))
+	TS_CATCH(err)
+		/* If knots is not NULL, oldKnot contains the correct value. */
+		if (knots) knots[index] = oldKnot;
+	TS_END_TRY_RETURN(err)
+}
+/*! @} */
+
+
+
+/*! @name B-Spline Initialization
+ *
+ * @{
+ */
+tsBSpline
+ts_bspline_init(void)
+{
+	tsBSpline spline;
+	ts_int_bspline_init(&spline);
+	return spline;
+}
+
+tsError
+ts_int_bspline_generate_knots(const tsBSpline *spline,
+                              tsBSplineType type,
+                              tsStatus *status)
+{
+	const size_t n_knots = ts_bspline_num_knots(spline);
+	const size_t deg = ts_bspline_degree(spline);
+	const size_t order = ts_bspline_order(spline);
+	tsReal fac; /**< Factor used to calculate the knot values. */
+	size_t i; /**< Used in for loops. */
+	tsReal *knots; /**< Pointer to the knots of \p _result_. */
+
+	/* order >= 1 implies 2*order >= 2 implies n_knots >= 2 */
+	if (type == TS_BEZIERS && n_knots % order != 0) {
+		TS_RETURN_2(status, TS_NUM_KNOTS,
+		            "num(knots) (%lu) %% order (%lu) != 0",
+		            (unsigned long) n_knots, (unsigned long) order)
+	}
+
+	knots = ts_int_bspline_access_knots(spline);
+
+	if (type == TS_OPENED) {
+		knots[0] = TS_DOMAIN_DEFAULT_MIN; /* n_knots >= 2 */
+		fac = (TS_DOMAIN_DEFAULT_MAX - TS_DOMAIN_DEFAULT_MIN)
+		      / (n_knots - 1); /* n_knots >= 2 */
+		for (i = 1; i < n_knots-1; i++)
+			knots[i] = TS_DOMAIN_DEFAULT_MIN + i*fac;
+		knots[i] = TS_DOMAIN_DEFAULT_MAX; /* n_knots >= 2 */
+	} else if (type == TS_CLAMPED) {
+		/* n_knots >= 2*order == 2*(deg+1) == 2*deg + 2 > 2*deg - 1 */
+		fac = (TS_DOMAIN_DEFAULT_MAX - TS_DOMAIN_DEFAULT_MIN)
+		      / (n_knots - 2*deg - 1);
+		ts_arr_fill(knots, order, TS_DOMAIN_DEFAULT_MIN);
+		for (i = order ;i < n_knots-order; i++)
+			knots[i] = TS_DOMAIN_DEFAULT_MIN + (i-deg)*fac;
+		ts_arr_fill(knots + i, order, TS_DOMAIN_DEFAULT_MAX);
+	} else if (type == TS_BEZIERS) {
+		/* n_knots >= 2*order implies n_knots/order >= 2 */
+		fac = (TS_DOMAIN_DEFAULT_MAX - TS_DOMAIN_DEFAULT_MIN)
+		      / (n_knots/order - 1);
+		ts_arr_fill(knots, order, TS_DOMAIN_DEFAULT_MIN);
+		for (i = order; i < n_knots-order; i += order)
+			ts_arr_fill(knots + i,
+			            order,
+			            TS_DOMAIN_DEFAULT_MIN + (i/order)*fac);
+		ts_arr_fill(knots + i, order, TS_DOMAIN_DEFAULT_MAX);
+	}
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_bspline_new(size_t num_control_points,
+               size_t dimension,
+               size_t degree,
+               tsBSplineType type,
+               tsBSpline *spline,
+               tsStatus *status)
+{
+	const size_t order = degree + 1;
+	const size_t num_knots = num_control_points + order;
+	const size_t len_ctrlp = num_control_points * dimension;
+
+	const size_t sof_real = sizeof(tsReal);
+	const size_t sof_impl = sizeof(struct tsBSplineImpl);
+	const size_t sof_ctrlp_vec = len_ctrlp * sof_real;
+	const size_t sof_knots_vec = num_knots * sof_real;
+	const size_t sof_spline = sof_impl + sof_ctrlp_vec + sof_knots_vec;
+	tsError err;
+
+	ts_int_bspline_init(spline);
+
+	if (dimension < 1) {
+		TS_RETURN_0(status, TS_DIM_ZERO, "unsupported dimension: 0")
+	}
+	if (num_knots > TS_MAX_NUM_KNOTS) {
+		TS_RETURN_2(status, TS_NUM_KNOTS,
+		            "unsupported number of knots: %lu > %i",
+		            (unsigned long) num_knots, TS_MAX_NUM_KNOTS)
+	}
+	if (degree >= num_control_points) {
+		TS_RETURN_2(status, TS_DEG_GE_NCTRLP,
+		            "degree (%lu) >= num(control_points) (%lu)",
+		            (unsigned long) degree,
+		            (unsigned long) num_control_points)
+	}
+
+	spline->pImpl = (struct tsBSplineImpl *) malloc(sof_spline);
+	if (!spline->pImpl) TS_RETURN_0(status, TS_MALLOC, "out of memory")
+
+	spline->pImpl->deg = degree;
+	spline->pImpl->dim = dimension;
+	spline->pImpl->n_ctrlp = num_control_points;
+	spline->pImpl->n_knots = num_knots;
+
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_int_bspline_generate_knots(
+		        spline, type, status))
+	TS_CATCH(err)
+		ts_bspline_free(spline);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_new_with_control_points(size_t num_control_points,
+                                   size_t dimension,
+                                   size_t degree,
+                                   tsBSplineType type,
+                                   tsBSpline *spline,
+                                   tsStatus *status,
+                                   double first,
+                                   ...)
+{
+	tsReal *ctrlp = NULL;
+	va_list argp;
+	size_t i;
+	tsError err;
+
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_bspline_new(
+		        num_control_points, dimension,
+		        degree, type, spline, status))
+	TS_CATCH(err)
+		ts_bspline_free(spline);
+	TS_END_TRY_ROE(err)
+	ctrlp = ts_int_bspline_access_ctrlp(spline);
+
+	ctrlp[0] = (tsReal) first;
+	va_start(argp, first);
+	for (i = 1; i < ts_bspline_len_control_points(spline); i++)
+		ctrlp[i] = (tsReal) va_arg(argp, double);
+	va_end(argp);
+
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_bspline_copy(const tsBSpline *src,
+                tsBSpline *dest,
+                tsStatus *status)
+{
+	size_t size;
+	if (src == dest) TS_RETURN_SUCCESS(status)
+	ts_int_bspline_init(dest);
+	size = ts_int_bspline_sof_state(src);
+	dest->pImpl = (struct tsBSplineImpl *) malloc(size);
+	if (!dest->pImpl) TS_RETURN_0(status, TS_MALLOC, "out of memory")
+	memcpy(dest->pImpl, src->pImpl, size);
+	TS_RETURN_SUCCESS(status)
+}
+
+void
+ts_bspline_move(tsBSpline *src,
+                tsBSpline *dest)
+{
+	if (src == dest) return;
+	dest->pImpl = src->pImpl;
+	ts_int_bspline_init(src);
+}
+
+void
+ts_bspline_free(tsBSpline *spline)
+{
+	if (spline->pImpl) free(spline->pImpl);
+	ts_int_bspline_init(spline);
+}
+/*! @} */
+
+
+
+/*! @name De Boor Net Data
+ *
+ * @{
+ */
+tsReal
+ts_deboornet_knot(const tsDeBoorNet *net)
+{
+	return net->pImpl->u;
+}
+
+size_t
+ts_deboornet_index(const tsDeBoorNet *net)
+{
+	return net->pImpl->k;
+}
+
+size_t
+ts_deboornet_multiplicity(const tsDeBoorNet *net)
+{
+	return net->pImpl->s;
+}
+
+size_t
+ts_deboornet_num_insertions(const tsDeBoorNet *net)
+{
+	return net->pImpl->h;
+}
+
+size_t
+ts_deboornet_dimension(const tsDeBoorNet *net)
+{
+	return net->pImpl->dim;
+}
+
+size_t
+ts_deboornet_len_points(const tsDeBoorNet *net)
+{
+	return ts_deboornet_num_points(net) *
+	       ts_deboornet_dimension(net);
+}
+
+size_t
+ts_deboornet_num_points(const tsDeBoorNet *net)
+{
+	return net->pImpl->n_points;
+}
+
+size_t
+ts_deboornet_sof_points(const tsDeBoorNet *net)
+{
+	return ts_deboornet_len_points(net) * sizeof(tsReal);
+}
+
+const tsReal *
+ts_deboornet_points_ptr(const tsDeBoorNet *net)
+{
+	return ts_int_deboornet_access_points(net);
+}
+
+tsError
+ts_deboornet_points(const tsDeBoorNet *net,
+                    tsReal **points,
+                    tsStatus *status)
+{
+	const size_t size = ts_deboornet_sof_points(net);
+	*points = (tsReal*) malloc(size);
+	if (!*points) TS_RETURN_0(status, TS_MALLOC, "out of memory")
+	memcpy(*points, ts_int_deboornet_access_points(net), size);
+	TS_RETURN_SUCCESS(status)
+}
+
+size_t
+ts_deboornet_len_result(const tsDeBoorNet *net)
+{
+	return ts_deboornet_num_result(net) *
+	       ts_deboornet_dimension(net);
+}
+
+size_t
+ts_deboornet_num_result(const tsDeBoorNet *net)
+{
+	return ts_deboornet_num_points(net) == 2 ? 2 : 1;
+}
+
+size_t
+ts_deboornet_sof_result(const tsDeBoorNet *net)
+{
+	return ts_deboornet_len_result(net) * sizeof(tsReal);
+}
+
+const tsReal *
+ts_deboornet_result_ptr(const tsDeBoorNet *net)
+{
+	return ts_int_deboornet_access_result(net);
+}
+
+tsError
+ts_deboornet_result(const tsDeBoorNet *net,
+                    tsReal **result,
+                    tsStatus *status)
+{
+	const size_t size = ts_deboornet_sof_result(net);
+	*result = (tsReal*) malloc(size);
+	if (!*result) TS_RETURN_0(status, TS_MALLOC, "out of memory")
+	memcpy(*result, ts_int_deboornet_access_result(net), size);
+	TS_RETURN_SUCCESS(status)
+}
+/*! @} */
+
+
+
+/*! @name De Boor Net Initialization
+ *
+ * @{
+ */
+tsDeBoorNet
+ts_deboornet_init(void)
+{
+	tsDeBoorNet net;
+	ts_int_deboornet_init(&net);
+	return net;
+}
+
+tsError
+ts_int_deboornet_new(const tsBSpline *spline,
+                     tsDeBoorNet *net,
+                     tsStatus *status)
+{
+	const size_t dim = ts_bspline_dimension(spline);
+	const size_t deg = ts_bspline_degree(spline);
+	const size_t order = ts_bspline_order(spline);
+	const size_t num_points = (size_t)(order * (order+1) * 0.5f);
+	/* Handle `order == 1' which generates too few points. */
+	const size_t fixed_num_points = num_points < 2 ? 2 : num_points;
+
+	const size_t sof_real = sizeof(tsReal);
+	const size_t sof_impl = sizeof(struct tsDeBoorNetImpl);
+	const size_t sof_points_vec = fixed_num_points * dim * sof_real;
+	const size_t sof_net = sof_impl * sof_points_vec;
+
+	net->pImpl = (struct tsDeBoorNetImpl *) malloc(sof_net);
+	if (!net->pImpl) TS_RETURN_0(status, TS_MALLOC, "out of memory")
+
+	net->pImpl->u = 0.f;
+	net->pImpl->k = 0;
+	net->pImpl->s = 0;
+	net->pImpl->h = deg;
+	net->pImpl->dim = dim;
+	net->pImpl->n_points = fixed_num_points;
+	TS_RETURN_SUCCESS(status)
+}
+
+void
+ts_deboornet_free(tsDeBoorNet *net)
+{
+	if (net->pImpl) free(net->pImpl);
+	ts_int_deboornet_init(net);
+}
+
+tsError
+ts_deboornet_copy(const tsDeBoorNet *src,
+                  tsDeBoorNet *dest,
+                  tsStatus *status)
+{
+	size_t size;
+	if (src == dest) TS_RETURN_SUCCESS(status)
+	ts_int_deboornet_init(dest);
+	size = ts_int_deboornet_sof_state(src);
+	dest->pImpl = (struct tsDeBoorNetImpl *) malloc(size);
+	if (!dest->pImpl) TS_RETURN_0(status, TS_MALLOC, "out of memory")
+	memcpy(dest->pImpl, src->pImpl, size);
+	TS_RETURN_SUCCESS(status)
+}
+
+void
+ts_deboornet_move(tsDeBoorNet *src,
+                  tsDeBoorNet *dest)
+{
+	if (src == dest) return;
+	dest->pImpl = src->pImpl;
+	ts_int_deboornet_init(src);
+}
+/*! @} */
+
+
+
+/*! @name Interpolation and Approximation Functions
+ *
+ * @{
+ */
+tsError
+ts_int_cubic_point(const tsReal *point,
+                   size_t dim,
+                   tsBSpline *spline,
+                   tsStatus *status)
+{
+	const size_t size = dim * sizeof(tsReal);
+	tsReal *ctrlp = NULL;
+	size_t i;
+	tsError err;
+	TS_CALL_ROE(err, ts_bspline_new(
+	            4, dim, 3,
+	            TS_CLAMPED, spline, status))
+	ctrlp = ts_int_bspline_access_ctrlp(spline);
+	for (i = 0; i < 4; i++) {
+		memcpy(ctrlp + i*dim,
+		       point,
+		       size);
+	}
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_int_thomas_algorithm(const tsReal *a,
+                        const tsReal *b,
+                        const tsReal *c,
+                        size_t num,
+                        size_t dim,
+                        tsReal *d,
+                        tsStatus *status)
+{
+	size_t i, j, k, l;
+	tsReal m, *cc = NULL;
+	tsError err;
+
+	if (dim == 0) {
+		TS_RETURN_0(status, TS_DIM_ZERO,
+		            "unsupported dimension: 0")
+	}
+	if (num <= 1) {
+		TS_RETURN_1(status, TS_NUM_POINTS,
+		            "num(points) (%lu) <= 1",
+		            (unsigned long) num)
+	}
+	cc = (tsReal *) malloc(num * sizeof(tsReal));
+	if (!cc) TS_RETURN_0(status, TS_MALLOC, "out of memory")
+
+	TS_TRY(try, err, status)
+		/* Forward sweep. */
+		if (fabs(b[0]) <= fabs(c[0])) {
+			TS_THROW_2(try, err, status, TS_NO_RESULT,
+			           "error: |%f| <= |%f|", b[0], c[0])
+		}
+		/* |b[i]| > |c[i]| implies that |b[i]| > 0. Thus, the following
+		 * statements cannot evaluate to division by zero.*/
+		cc[0] = c[0] / b[0];
+		for (i = 0; i < dim; i++)
+			d[i] = d[i] / b[0];
+		for (i = 1; i < num; i++) {
+			if (fabs(b[i]) <= fabs(a[i]) + fabs(c[i])) {
+				TS_THROW_3(try, err, status, TS_NO_RESULT,
+				           "error: |%f| <= |%f| + |%f|",
+				           b[i], a[i], c[i])
+			}
+			/* |a[i]| < |b[i]| and cc[i - 1] < 1. Therefore, the
+			 * following statement cannot evaluate to division by
+			 * zero. */
+			m = 1.f / (b[i] - a[i] * cc[i - 1]);
+			/* |b[i]| > |a[i]| + |c[i]| implies that there must be
+			 * an eps > 0 such that |b[i]| = |a[i]| + |c[i]| + eps.
+			 * Even if |a[i]| is 0 (by which the result of the
+			 * following statement becomes maximum), |c[i]| is less
+			 * than |b[i]| by an amount of eps. By substituting the
+			 * previous and the following statements (under the
+			 * assumption that |a[i]| is 0), we obtain c[i] / b[i],
+			 * which must be less than 1. */
+			cc[i] = c[i] * m;
+			for (j = 0; j < dim; j++) {
+				k = i * dim + j;
+				l = (i-1) * dim + j;
+				d[k] = (d[k] - a[i] * d[l]) * m;
+			}
+		}
+
+		/* Back substitution. */
+		for (i = num-1; i > 0; i--) {
+			for (j = 0; j < dim; j++) {
+				k = (i-1) * dim + j;
+				l = i * dim + j;
+				d[k] -= cc[i-1] * d[l];
+			}
+		}
+	TS_FINALLY
+		free(cc);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_int_relaxed_uniform_cubic_bspline(const tsReal *points,
+                                     size_t n,
+                                     size_t dim,
+                                     tsBSpline *spline,
+                                     tsStatus *status)
+{
+	const size_t order = 4;    /**< Order of spline to interpolate. */
+	const tsReal as = 1.f/6.f; /**< The value 'a sixth'. */
+	const tsReal at = 1.f/3.f; /**< The value 'a third'. */
+	const tsReal tt = 2.f/3.f; /**< The value 'two third'. */
+	size_t sof_ctrlp;          /**< Size of a single control point. */
+	const tsReal* b = points;  /**< Array of the b values. */
+	tsReal* s;                 /**< Array of the s values. */
+	size_t i, d;               /**< Used in for loops */
+	size_t j, k, l;            /**< Used as temporary indices. */
+	tsReal *ctrlp; /**< Pointer to the control points of \p _spline_. */
+	tsError err;
+
+	/* input validation */
+	if (dim == 0)
+		TS_RETURN_0(status, TS_DIM_ZERO, "unsupported dimension: 0")
+	if (n <= 1) {
+		TS_RETURN_1(status, TS_NUM_POINTS,
+		            "num(points) (%lu) <= 1",
+		            (unsigned long) n)
+	}
+	/* in the following n >= 2 applies */
+
+	sof_ctrlp = dim * sizeof(tsReal); /* dim > 0 implies sof_ctrlp > 0 */
+
+	s = NULL;
+	TS_TRY(try, err, status)
+		/* n >= 2 implies n-1 >= 1 implies (n-1)*4 >= 4 */
+		TS_CALL(try, err, ts_bspline_new(
+		        (n-1) * 4, dim, order - 1,
+		        TS_BEZIERS, spline, status))
+		ctrlp = ts_int_bspline_access_ctrlp(spline);
+
+		s = (tsReal*) malloc(n * sof_ctrlp);
+		if (!s) {
+			TS_THROW_0(try, err, status, TS_MALLOC,
+			           "out of memory")
+		}
+
+		/* set s_0 to b_0 and s_n = b_n */
+		memcpy(s, b, sof_ctrlp);
+		memcpy(s + (n-1)*dim, b + (n-1)*dim, sof_ctrlp);
+
+		/* set s_i = 1/6*b_{i-1} + 2/3*b_{i} + 1/6*b_{i+1}*/
+		for (i = 1; i < n-1; i++) {
+			for (d = 0; d < dim; d++) {
+				j = (i-1)*dim+d;
+				k = i*dim+d;
+				l = (i+1)*dim+d;
+				s[k] = as * b[j];
+				s[k] += tt * b[k];
+				s[k] += as * b[l];
+			}
+		}
+
+		/* create beziers from b and s */
+		for (i = 0; i < n-1; i++) {
+			for (d = 0; d < dim; d++) {
+				j = i*dim+d;
+				k = i*4*dim+d;
+				l = (i+1)*dim+d;
+				ctrlp[k] = s[j];
+				ctrlp[k+dim] = tt*b[j] + at*b[l];
+				ctrlp[k+2*dim] = at*b[j] + tt*b[l];
+				ctrlp[k+3*dim] = s[l];
+			}
+		}
+	TS_CATCH(err)
+		ts_bspline_free(spline);
+	TS_FINALLY
+		if (s)
+			free(s);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_interpolate_cubic_natural(const tsReal *points,
+                                     size_t num_points,
+                                     size_t dimension,
+                                     tsBSpline *spline,
+                                     tsStatus *status)
+{
+	const size_t sof_ctrlp = dimension * sizeof(tsReal);
+	const size_t len_points = num_points * dimension;
+	const size_t num_int_points = num_points - 2;
+	const size_t len_int_points = num_int_points * dimension;
+	tsReal *buffer, *a, *b, *c, *d;
+	size_t i, j, k, l;
+	tsError err;
+
+	ts_int_bspline_init(spline);
+	if (num_points == 0)
+		TS_RETURN_0(status, TS_NUM_POINTS, "num(points) == 0")
+	if (num_points == 1) {
+		TS_CALL_ROE(err, ts_int_cubic_point(
+		            points, dimension, spline, status))
+		TS_RETURN_SUCCESS(status)
+	}
+	if (num_points == 2) {
+		return ts_int_relaxed_uniform_cubic_bspline(
+			points, num_points, dimension, spline, status);
+	}
+	/* `num_points` >= 3 */
+	buffer = NULL;
+	TS_TRY(try, err, status)
+		buffer = (tsReal *) malloc(
+			/* `a', `b', `c' (note that `c' is equal to `a') */
+			2 * num_int_points * sizeof(tsReal) +
+			/* At first: `d' Afterwards: The result of the thomas
+			 * algorithm including the first and last point to be
+			 * interpolated. */
+			num_points * dimension * sizeof(tsReal));
+		if (!buffer) {
+			TS_THROW_0(try, err, status, TS_MALLOC,
+			           "out of memory")
+		}
+		/* The system of linear equations is taken from:
+		 *     http://www.bakoma-tex.com/doc/generic/pst-bspline/
+		 *     pst-bspline-doc.pdf */
+		a = c = buffer;
+		ts_arr_fill(a, num_int_points, 1);
+		b = a + num_int_points;
+		ts_arr_fill(b, num_int_points, 4);
+		d = b + num_int_points /* shift to the beginning of `d' */
+		      + dimension; /* make space for the first point */
+		/* 6 * S_{i+1} */
+		for (i = 0; i < num_int_points; i++) {
+			for (j = 0; j < dimension; j++) {
+				k = i * dimension + j;
+				l = (i+1) * dimension + j;
+				d[k] = 6 * points[l];
+			}
+		}
+		for (i = 0; i < dimension; i++) {
+			/* 6 * S_{1} - S_{0} */
+			d[i] -= points[i];
+			/* 6 * S_{n-1} - S_{n} */
+			k = len_int_points - (i+1);
+			l = len_points - (i+1);
+			d[k] -= points[l];
+		}
+		/* The Thomas algorithm requires at least two points. Hence,
+		 * `num_int_points` == 1 must be handled separately (let's call
+		 * it "Mini Thomas"). */
+		if (num_int_points == 1) {
+			for (i = 0; i < dimension; i++)
+				d[i] *= (tsReal) 0.25f;
+		} else {
+			TS_CALL(try, err, ts_int_thomas_algorithm(
+			        a, b, c, num_int_points, dimension, d,
+			        status))
+		}
+		memcpy(d - dimension, points, sof_ctrlp);
+		memcpy(d + num_int_points * dimension,
+		       points + (num_points-1) * dimension,
+		       sof_ctrlp);
+		TS_CALL(try, err, ts_int_relaxed_uniform_cubic_bspline(
+		        d - dimension, num_points, dimension, spline, status))
+	TS_CATCH(err)
+		ts_bspline_free(spline);
+	TS_FINALLY
+		if (buffer) free(buffer);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_interpolate_catmull_rom(const tsReal *points,
+                                   size_t num_points,
+                                   size_t dimension,
+                                   tsReal alpha,
+                                   const tsReal *first,
+                                   const tsReal *last,
+                                   tsReal epsilon,
+                                   tsBSpline *spline,
+                                   tsStatus *status)
+{
+	const size_t sof_real = sizeof(tsReal);
+	const size_t sof_ctrlp = dimension * sof_real;
+	const tsReal eps = (tsReal) fabs(epsilon);
+	tsReal *bs_ctrlp; /* Points to the control points of `spline`. */
+	tsReal *cr_ctrlp; /**< The points to interpolate based on `points`. */
+	size_t i, d; /**< Used in for loops. */
+	tsError err; /**< Local error handling. */
+	/* [https://en.wikipedia.org/wiki/
+	 * Centripetal_Catmull%E2%80%93Rom_spline] */
+	tsReal t0, t1, t2, t3; /**< Catmull-Rom knots. */
+	/* [https://stackoverflow.com/questions/30748316/
+	 * catmull-rom-interpolation-on-svg-paths/30826434#30826434] */
+	tsReal c1, c2, d1, d2, m1, m2; /**< Used to calculate derivatives. */
+	tsReal *p0, *p1, *p2, *p3; /**< Processed Catmull-Rom points. */
+
+	ts_int_bspline_init(spline);
+	if (dimension == 0)
+		TS_RETURN_0(status, TS_DIM_ZERO, "unsupported dimension: 0")
+	if (num_points == 0)
+		TS_RETURN_0(status, TS_NUM_POINTS, "num(points) == 0")
+	if (alpha < (tsReal) 0.0) alpha = (tsReal) 0.0;
+	if (alpha > (tsReal) 1.0) alpha = (tsReal) 1.0;
+
+	/* Copy `points` to `cr_ctrlp`. Add space for `first` and `last`. */
+	cr_ctrlp = (tsReal *) malloc((num_points + 2) * sof_ctrlp);
+	if (!cr_ctrlp)
+		TS_RETURN_0(status, TS_MALLOC, "out of memory")
+	memcpy(cr_ctrlp + dimension, points, num_points * sof_ctrlp);
+
+	/* Remove redundant points from `cr_ctrlp`. Update `num_points`. */
+	for (i = 1 /* 0 (`first`) is not assigned yet */;
+	     i < num_points /* skip last point (inclusive end) */;
+	     i++) {
+		p0 = cr_ctrlp + (i * dimension);
+		p1 = p0 + dimension;
+		if (ts_distance(p0, p1, dimension) <= eps) {
+			/* Are there any other points (after the one that is
+			 * to be removed) that need to be shifted? */
+			if (i < num_points - 1) {
+				memmove(p1, p1 + dimension,
+				        (num_points - (i + 1)) * sof_ctrlp);
+			}
+			num_points--;
+			i--;
+		}
+	}
+
+	/* Check if there are still enough points for interpolation. */
+	if (num_points == 1) { /* `num_points` can't be 0 */
+		free(cr_ctrlp); /* The point is copied from `points`. */
+		TS_CALL_ROE(err, ts_int_cubic_point(
+		            points, dimension, spline, status))
+		TS_RETURN_SUCCESS(status)
+	}
+
+	/* Add or generate `first` and `last`. Update `num_points`. */
+	p0 = cr_ctrlp + dimension;
+	if (first && ts_distance(first, p0, dimension) > eps) {
+		memcpy(cr_ctrlp, first, sof_ctrlp);
+	} else {
+		p1 = p0 + dimension;
+		for (d = 0; d < dimension; d++)
+			cr_ctrlp[d] = p0[d] + (p0[d] - p1[d]);
+	}
+	p1 = cr_ctrlp + (num_points * dimension);
+	if (last && ts_distance(p1, last, dimension) > eps) {
+		memcpy(cr_ctrlp + ((num_points + 1) * dimension),
+		       last, sof_ctrlp);
+	} else {
+		p0 = p1 - dimension;
+		for (d = 0; d < dimension; d++) {
+			cr_ctrlp[((num_points + 1) * dimension) + d] =
+				p1[d] + (p1[d] - p0[d]);
+		}
+	}
+	num_points = num_points + 2;
+
+	/* Transform the sequence of Catmull-Rom splines. */
+	bs_ctrlp = NULL;
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_bspline_new(
+		        (num_points - 3) * 4, dimension, 3,
+		        TS_BEZIERS, spline, status))
+		bs_ctrlp = ts_int_bspline_access_ctrlp(spline);
+	TS_CATCH(err)
+		free(cr_ctrlp);
+	TS_END_TRY_ROE(err)
+	for (i = 0; i < ts_bspline_num_control_points(spline) / 4; i++) {
+		p0 = cr_ctrlp + ((i+0) * dimension);
+		p1 = cr_ctrlp + ((i+1) * dimension);
+		p2 = cr_ctrlp + ((i+2) * dimension);
+		p3 = cr_ctrlp + ((i+3) * dimension);
+
+		t0 = (tsReal) 0.f;
+		t1 = t0 + (tsReal) pow(ts_distance(p0, p1, dimension), alpha);
+		t2 = t1 + (tsReal) pow(ts_distance(p1, p2, dimension), alpha);
+		t3 = t2 + (tsReal) pow(ts_distance(p2, p3, dimension), alpha);
+
+		c1 = (t2-t1) / (t2-t0);
+		c2 = (t1-t0) / (t2-t0);
+		d1 = (t3-t2) / (t3-t1);
+		d2 = (t2-t1) / (t3-t1);
+
+		for (d = 0; d < dimension; d++) {
+			m1 = (t2-t1)*(c1*(p1[d]-p0[d])/(t1-t0)
+			              + c2*(p2[d]-p1[d])/(t2-t1));
+			m2 = (t2-t1)*(d1*(p2[d]-p1[d])/(t2-t1)
+			              + d2*(p3[d]-p2[d])/(t3-t2));
+			bs_ctrlp[((i*4 + 0) * dimension) + d] = p1[d];
+			bs_ctrlp[((i*4 + 1) * dimension) + d] = p1[d] + m1/3;
+			bs_ctrlp[((i*4 + 2) * dimension) + d] = p2[d] - m2/3;
+			bs_ctrlp[((i*4 + 3) * dimension) + d] = p2[d];
+		}
+	}
+	free(cr_ctrlp);
+	TS_RETURN_SUCCESS(status)
+}
+/*! @} */
+
+
+
+/*! @name Query Functions
+ *
+ * @{
+ */
+tsError
+ts_int_bspline_find_knot(const tsBSpline *spline,
+                         tsReal *knot, /* in: knot; out: actual knot */
+                         size_t *idx,  /* out: index of `knot' */
+                         size_t *mult, /* out: multiplicity of `knot' */
+                         tsStatus *status)
+{
+	const size_t deg = ts_bspline_degree(spline);
+	const size_t num_knots = ts_bspline_num_knots(spline);
+	const tsReal *knots = ts_int_bspline_access_knots(spline);
+	tsReal min, max;
+	size_t low, high;
+
+	ts_bspline_domain(spline, &min, &max);
+	if (*knot < min) {
+		/* Avoid infinite loop (issue #222) */
+		if (ts_knots_equal(*knot, min)) *knot = min;
+		else {
+			TS_RETURN_2(status, TS_U_UNDEFINED,
+			            "knot (%f) < min(domain) (%f)",
+			            *knot, min)
+		}
+	}
+	else if (*knot > max && !ts_knots_equal(*knot, max)) {
+		TS_RETURN_2(status, TS_U_UNDEFINED,
+		            "knot (%f) > max(domain) (%f)",
+		            *knot, max)
+	}
+
+	/* Based on 'The NURBS Book' (Les Piegl and Wayne Tiller). */
+	if (ts_knots_equal(*knot, knots[num_knots - 1])) {
+		*idx = num_knots - 1;
+	} else {
+		low = 0;
+		high = num_knots - 1;
+		*idx = (low+high) / 2;
+		while (*knot < knots[*idx] || *knot >= knots[*idx + 1]) {
+			if (*knot < knots[*idx])
+				high = *idx;
+			else
+				low = *idx;
+			*idx = (low+high) / 2;
+		}
+	}
+
+	/* Handle floating point errors. */
+	while (*idx < num_knots - 1 && /* there is a next knot */
+	       ts_knots_equal(*knot, knots[*idx + 1])) {
+		(*idx)++;
+	}
+	if (ts_knots_equal(*knot, knots[*idx]))
+		*knot = knots[*idx]; /* set actual knot */
+
+	/* Calculate knot's multiplicity. */
+	for (*mult = deg + 1; *mult > 0 ; (*mult)--) {
+		if (ts_knots_equal(*knot, knots[*idx - (*mult-1)]))
+			break;
+	}
+
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_int_bspline_eval_woa(const tsBSpline *spline,
+                        tsReal u,
+                        tsDeBoorNet *net,
+                        tsStatus *status)
+{
+	const size_t deg = ts_bspline_degree(spline);
+	const size_t order = ts_bspline_order(spline);
+	const size_t dim = ts_bspline_dimension(spline);
+	const size_t num_knots = ts_bspline_num_knots(spline);
+	const size_t sof_ctrlp = dim * sizeof(tsReal);
+
+	const tsReal *ctrlp = ts_int_bspline_access_ctrlp(spline);
+	const tsReal *knots = ts_int_bspline_access_knots(spline);
+	tsReal *points = NULL;  /**< Pointer to the points of \p net. */
+
+	size_t k;        /**< Index of \p u. */
+	size_t s;        /**< Multiplicity of \p u. */
+
+	size_t from;     /**< Offset used to copy values. */
+	size_t fst;      /**< First affected control point, inclusive. */
+	size_t lst;      /**< Last affected control point, inclusive. */
+	size_t N;        /**< Number of affected control points. */
+
+	/* The following indices are used to create the DeBoor net. */
+	size_t lidx;     /**< Current left index. */
+	size_t ridx;     /**< Current right index. */
+	size_t tidx;     /**< Current to index. */
+	size_t r, i, d;  /**< Used in for loop. */
+	tsReal ui;       /**< Knot value at index i. */
+	tsReal a, a_hat; /**< Weighting factors of control points. */
+
+	tsError err;
+
+	points = ts_int_deboornet_access_points(net);
+
+	/* 1. Find index k such that u is in between [u_k, u_k+1).
+	 * 2. Setup already known values.
+	 * 3. Decide by multiplicity of u how to calculate point P(u). */
+
+	/* 1. */
+	k = s = 0;
+	TS_CALL_ROE(err, ts_int_bspline_find_knot(
+	            spline, &u, &k, &s, status))
+
+	/* 2. */
+	net->pImpl->u = u;
+	net->pImpl->k = k;
+	net->pImpl->s = s;
+	net->pImpl->h = deg < s ? 0 : deg-s; /* prevent underflow */
+
+	/* 3. (by 1. s <= order)
+	 *
+	 * 3a) Check for s = order.
+	 *     Take the two points k-s and k-s + 1. If one of
+	 *     them doesn't exist, take only the other.
+	 * 3b) Use de boor algorithm to find point P(u). */
+	if (s == order) {
+		/* only one of the two control points exists */
+		if (k == deg || /* only the first */
+		    k == num_knots - 1) { /* only the last */
+			from = k == deg ? 0 : (k-s) * dim;
+			net->pImpl->n_points = 1;
+			memcpy(points, ctrlp + from, sof_ctrlp);
+		} else {
+			from = (k-s) * dim;
+			net->pImpl->n_points = 2;
+			memcpy(points, ctrlp + from, 2 * sof_ctrlp);
+		}
+	} else { /* by 3a) s <= deg (order = deg+1) */
+		fst = k-deg; /* by 1. k >= deg */
+		lst = k-s; /* s <= deg <= k */
+		N = lst-fst + 1; /* lst <= fst implies N >= 1 */
+
+		net->pImpl->n_points = (size_t)(N * (N+1) * 0.5f);
+
+		/* copy initial values to output */
+		memcpy(points, ctrlp + fst*dim, N * sof_ctrlp);
+
+		lidx = 0;
+		ridx = dim;
+		tidx = N*dim; /* N >= 1 implies tidx > 0 */
+		r = 1;
+		for (;r <= ts_deboornet_num_insertions(net); r++) {
+			i = fst + r;
+			for (; i <= lst; i++) {
+				ui = knots[i];
+				a = (ts_deboornet_knot(net) - ui) /
+					(knots[i+deg-r+1] - ui);
+				a_hat = 1.f-a;
+
+				for (d = 0; d < dim; d++) {
+					points[tidx++] =
+						a_hat * points[lidx++] +
+						a     * points[ridx++];
+				}
+			}
+			lidx += dim;
+			ridx += dim;
+		}
+	}
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_bspline_eval(const tsBSpline *spline,
+                tsReal knot,
+                tsDeBoorNet *net,
+                tsStatus *status)
+{
+	tsError err;
+	ts_int_deboornet_init(net);
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_int_deboornet_new(
+		        spline, net, status))
+		TS_CALL(try, err, ts_int_bspline_eval_woa(
+		        spline, knot, net, status))
+	TS_CATCH(err)
+		ts_deboornet_free(net);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_eval_all(const tsBSpline *spline,
+                    const tsReal *knots,
+                    size_t num,
+                    tsReal **points,
+                    tsStatus *status)
+{
+	const size_t dim = ts_bspline_dimension(spline);
+	const size_t sof_point = dim * sizeof(tsReal);
+	const size_t sof_points = num * sof_point;
+	tsDeBoorNet net = ts_deboornet_init();
+	tsReal *result;
+	size_t i;
+	tsError err;
+	TS_TRY(try, err, status)
+		*points = (tsReal *) malloc(sof_points);
+		if (!*points) {
+			TS_THROW_0(try, err, status, TS_MALLOC,
+			           "out of memory")
+		}
+		TS_CALL(try, err, ts_int_deboornet_new(
+		        spline,&net, status))
+		for (i = 0; i < num; i++) {
+			TS_CALL(try, err, ts_int_bspline_eval_woa(
+			        spline, knots[i], &net, status))
+			result = ts_int_deboornet_access_result(&net);
+			memcpy((*points) + i * dim, result, sof_point);
+		}
+	TS_CATCH(err)
+		if (*points)
+			free(*points);
+		*points = NULL;
+	TS_FINALLY
+		ts_deboornet_free(&net);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_sample(const tsBSpline *spline,
+                  size_t num,
+                  tsReal **points,
+                  size_t *actual_num,
+                  tsStatus *status)
+{
+	tsError err;
+	tsReal *knots;
+
+	num = num == 0 ? 100 : num;
+	*actual_num = num;
+	knots = (tsReal *) malloc(num * sizeof(tsReal));
+	if (!knots) {
+		*points = NULL;
+		TS_RETURN_0(status, TS_MALLOC, "out of memory")
+	}
+	ts_bspline_uniform_knot_seq(spline, num, knots);
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_bspline_eval_all(
+		        spline, knots, num, points, status))
+	TS_FINALLY
+		free(knots);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_bisect(const tsBSpline *spline,
+                  tsReal value,
+                  tsReal epsilon,
+                  int persnickety,
+                  size_t index,
+                  int ascending,
+                  size_t max_iter,
+                  tsDeBoorNet *net,
+                  tsStatus *status)
+{
+	tsError err;
+	const size_t dim = ts_bspline_dimension(spline);
+	const tsReal eps = (tsReal) fabs(epsilon);
+	size_t i = 0;
+	tsReal dist = 0;
+	tsReal min, max, mid;
+	tsReal *P;
+
+	ts_int_deboornet_init(net);
+
+	if (dim < index) {
+		TS_RETURN_2(status, TS_INDEX_ERROR,
+		            "dimension (%lu) <= index (%lu)",
+		            (unsigned long) dim,
+		            (unsigned long) index)
+	}
+	if(max_iter == 0)
+		TS_RETURN_0(status, TS_NO_RESULT, "0 iterations")
+
+	ts_bspline_domain(spline, &min, &max);
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_int_deboornet_new(
+		        spline, net, status))
+		do {
+			mid = (tsReal) ((min + max) / 2.0);
+			TS_CALL(try, err, ts_int_bspline_eval_woa(
+			        spline, mid, net, status))
+			P = ts_int_deboornet_access_result(net);
+			dist = ts_distance(&P[index], &value, 1);
+			if (dist <= eps)
+				TS_RETURN_SUCCESS(status)
+			if (ascending) {
+				if (P[index] < value)
+					min = mid;
+				else
+					max = mid;
+			} else {
+				if (P[index] < value)
+					max = mid;
+				else
+					min = mid;
+			}
+		} while (i++ < max_iter);
+		if (persnickety) {
+			TS_THROW_1(try, err, status, TS_NO_RESULT,
+			           "maximum iterations (%lu) exceeded",
+			           (unsigned long) max_iter)
+		}
+	TS_CATCH(err)
+		ts_deboornet_free(net);
+	TS_END_TRY_RETURN(err)
+}
+
+void ts_bspline_domain(const tsBSpline *spline,
+                       tsReal *min,
+                       tsReal *max)
+{
+	*min = ts_int_bspline_access_knots(spline)
+		[ts_bspline_degree(spline)];
+	*max = ts_int_bspline_access_knots(spline)
+		[ts_bspline_num_knots(spline) - ts_bspline_order(spline)];
+}
+
+tsError
+ts_bspline_is_closed(const tsBSpline *spline,
+                     tsReal epsilon,
+                     int *closed,
+                     tsStatus *status)
+{
+	const size_t deg = ts_bspline_degree(spline);
+	const size_t dim = ts_bspline_dimension(spline);
+	tsBSpline derivative;
+	tsReal min, max;
+	tsDeBoorNet first, last;
+	size_t i;
+	tsError err;
+
+	ts_int_bspline_init(&derivative);
+	ts_int_deboornet_init(&first);
+	ts_int_deboornet_init(&last);
+
+	TS_TRY(try, err, status)
+		for (i = 0; i < deg; i++) {
+			TS_CALL(try, err, ts_bspline_derive(
+			        spline, i, -1.f, &derivative, status))
+			ts_bspline_domain(&derivative, &min, &max);
+			TS_CALL(try, err, ts_bspline_eval(
+			        &derivative, min, &first, status))
+			TS_CALL(try, err, ts_bspline_eval(
+			        &derivative, max, &last, status))
+			*closed = ts_distance(
+				ts_int_deboornet_access_result(&first),
+				ts_int_deboornet_access_result(&last),
+				dim) <= epsilon ? 1 : 0;
+			ts_bspline_free(&derivative);
+			ts_deboornet_free(&first);
+			ts_deboornet_free(&last);
+			if (!*closed)
+				TS_RETURN_SUCCESS(status)
+		}
+	TS_FINALLY
+		ts_bspline_free(&derivative);
+		ts_deboornet_free(&first);
+		ts_deboornet_free(&last);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_compute_rmf(const tsBSpline *spline,
+                       const tsReal *knots,
+                       size_t num,
+                       int has_first_normal,
+                       tsFrame *frames,
+                       tsStatus *status)
+{
+	tsError err;
+	size_t i;
+	tsReal fx, fy, fz, fmin;
+	tsReal xc[3], xn[3], v1[3], c1, v2[3], c2, rL[3], tL[3];
+	tsBSpline deriv = ts_bspline_init();
+	tsDeBoorNet curr = ts_deboornet_init();
+	tsDeBoorNet next = ts_deboornet_init();
+
+	if (num < 1)
+		TS_RETURN_SUCCESS(status);
+
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_int_deboornet_new(
+		        spline, &curr, status))
+		TS_CALL(try, err, ts_int_deboornet_new(
+		        spline, &next, status))
+		TS_CALL(try, err, ts_bspline_derive(
+		        spline, 1, (tsReal) -1.0, &deriv, status))
+
+		/* Set position. */
+		TS_CALL(try, err, ts_int_bspline_eval_woa(
+		        spline, knots[0], &curr, status))
+		ts_vec3_set(frames[0].position,
+		            ts_int_deboornet_access_result(&curr),
+		            ts_bspline_dimension(spline));
+		/* Set tangent. */
+		TS_CALL(try, err, ts_int_bspline_eval_woa(
+		        &deriv, knots[0], &curr, status))
+		ts_vec3_set(frames[0].tangent,
+		            ts_int_deboornet_access_result(&curr),
+		            ts_bspline_dimension(&deriv));
+		ts_vec_norm(frames[0].tangent, 3, frames[0].tangent);
+		/* Set normal. */
+		if (!has_first_normal) {
+			fx = (tsReal) fabs(frames[0].tangent[0]);
+			fy = (tsReal) fabs(frames[0].tangent[1]);
+			fz = (tsReal) fabs(frames[0].tangent[2]);
+			fmin = fx; /* x is min => 1, 0, 0 */
+			ts_vec3_init(frames[0].normal,
+			             (tsReal) 1.0,
+			             (tsReal) 0.0,
+			             (tsReal) 0.0);
+			if (fy < fmin) { /* y is min => 0, 1, 0 */
+				fmin = fy;
+				ts_vec3_init(frames[0].normal,
+				             (tsReal) 0.0,
+				             (tsReal) 1.0,
+				             (tsReal) 0.0);
+			}
+			if (fz < fmin) { /* z is min => 0, 0, 1 */
+				ts_vec3_init(frames[0].normal,
+				             (tsReal) 0.0,
+				             (tsReal) 0.0,
+				             (tsReal) 1.0);
+			}
+			ts_vec3_cross(frames[0].tangent,
+			              frames[0].normal,
+			              frames[0].normal);
+			ts_vec_norm(frames[0].normal, 3, frames[0].normal);
+			if (ts_bspline_dimension(spline) >= 3) {
+				/* In 3D (and higher) an additional rotation of
+				   the normal along the tangent is needed in
+				   order to let the normal extend sideways (as
+				   it does in 2D and lower). */
+				ts_vec3_cross(frames[0].tangent,
+				              frames[0].normal,
+				              frames[0].normal);
+			}
+		} else {
+			/* Never trust user input! */
+			ts_vec_norm(frames[0].normal, 3, frames[0].normal);
+		}
+		/* Set binormal. */
+		ts_vec3_cross(frames[0].tangent,
+		              frames[0].normal,
+		              frames[0].binormal);
+
+		for (i = 0; i < num - 1; i++) {
+			/* Eval current and next point. */
+			TS_CALL(try, err, ts_int_bspline_eval_woa(
+			        spline, knots[i], &curr, status))
+			TS_CALL(try, err, ts_int_bspline_eval_woa(
+			        spline, knots[i+1], &next, status))
+			ts_vec3_set(xc, /* xc is now the current point */
+			            ts_int_deboornet_access_result(&curr),
+			            ts_bspline_dimension(spline));
+			ts_vec3_set(xn, /* xn is now the next point */
+			            ts_int_deboornet_access_result(&next),
+			            ts_bspline_dimension(spline));
+
+			/* Set position of U_{i+1}. */
+			ts_vec3_set(frames[i+1].position, xn, 3);
+
+			/* Compute reflection vector of R_{1}. */
+			ts_vec_sub(xn, xc, 3, v1);
+			c1 = ts_vec_dot(v1, v1, 3);
+
+			/* Compute r_{i}^{L} = R_{1} * r_{i}. */
+			rL[0] = (tsReal) 2.0 / c1;
+			rL[1] = ts_vec_dot(v1, frames[i].normal, 3);
+			rL[2] = rL[0] * rL[1];
+			ts_vec_mul(v1, 3, rL[2], rL);
+			ts_vec_sub(frames[i].normal, rL, 3, rL);
+
+			/* Compute t_{i}^{L} = R_{1} * t_{i}. */
+			tL[0] = (tsReal) 2.0 / c1;
+			tL[1] = ts_vec_dot(v1, frames[i].tangent, 3);
+			tL[2] = tL[0] * tL[1];
+			ts_vec_mul(v1, 3, tL[2], tL);
+			ts_vec_sub(frames[i].tangent, tL, 3, tL);
+
+			/* Compute reflection vector of R_{2}. */
+			TS_CALL(try, err, ts_int_bspline_eval_woa(
+			        &deriv, knots[i+1], &next, status))
+			ts_vec3_set(xn, /* xn is now the next tangent */
+			            ts_int_deboornet_access_result(&next),
+			            ts_bspline_dimension(&deriv));
+			ts_vec_norm(xn, 3, xn);
+			ts_vec_sub(xn, tL, 3, v2);
+			c2 = ts_vec_dot(v2, v2, 3);
+
+			/* Compute r_{i+1} = R_{2} * r_{i}^{L}. */
+			ts_vec3_set(xc, /* xc is now the next normal */
+			            frames[i+1].normal, 3);
+			xc[0] = (tsReal) 2.0 / c2;
+			xc[1] = ts_vec_dot(v2, rL, 3);
+			xc[2] = xc[0] * xc[1];
+			ts_vec_mul(v2, 3, xc[2], xc);
+			ts_vec_sub(rL, xc, 3, xc);
+			ts_vec_norm(xc, 3, xc);
+
+			/* Compute vector s_{i+1} of U_{i+1}. */
+			ts_vec3_cross(xn, xc, frames[i+1].binormal);
+
+			/* Set vectors t_{i+1} and r_{i+1} of U_{i+1}. */
+			ts_vec3_set(frames[i+1].tangent, xn, 3);
+			ts_vec3_set(frames[i+1].normal, xc, 3);
+		}
+	TS_FINALLY
+		ts_bspline_free(&deriv);
+		ts_deboornet_free(&curr);
+		ts_deboornet_free(&next);
+	TS_END_TRY_RETURN(err)
+}
+
+
+tsError
+ts_bspline_chord_lengths(const tsBSpline *spline,
+                         const tsReal *knots,
+                         size_t num,
+                         tsReal *lengths,
+                         tsStatus *status)
+{
+	tsError err;
+	tsReal dist, lst_knot, cur_knot;
+	size_t i, dim = ts_bspline_dimension(spline);
+	tsDeBoorNet lst = ts_deboornet_init();
+	tsDeBoorNet cur = ts_deboornet_init();
+	tsDeBoorNet tmp = ts_deboornet_init();
+
+	if (num == 0) TS_RETURN_SUCCESS(status);
+
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_int_deboornet_new(
+		        spline, &lst, status))
+		TS_CALL(try, err, ts_int_deboornet_new(
+		        spline, &cur, status))
+
+		/* num >= 1 */
+		TS_CALL(try, err, ts_int_bspline_eval_woa(
+		        spline, knots[0], &lst, status));
+		lengths[0] = (tsReal) 0.0;
+
+		for (i = 1; i < num; i++) {
+			TS_CALL(try, err, ts_int_bspline_eval_woa(
+			        spline, knots[i], &cur, status));
+			lst_knot = ts_deboornet_knot(&lst);
+			cur_knot = ts_deboornet_knot(&cur);
+			if (cur_knot < lst_knot) {
+				TS_THROW_1(try, err, status, TS_KNOTS_DECR,
+				            "decreasing knot at index: %lu",
+				            (unsigned long) i)
+			}
+			dist = ts_distance(ts_deboornet_result_ptr(&lst),
+			                   ts_deboornet_result_ptr(&cur),
+			                   dim);
+			lengths[i] = lengths[i-1] + dist;
+			ts_deboornet_move(&lst, &tmp);
+			ts_deboornet_move(&cur, &lst);
+			ts_deboornet_move(&tmp, &cur);
+		}
+	TS_FINALLY
+		ts_deboornet_free(&lst);
+		ts_deboornet_free(&cur);
+	TS_END_TRY_RETURN(err)
+}
+
+
+tsError
+ts_bspline_sub_spline(const tsBSpline *spline,
+                      tsReal knot0,
+                      tsReal knot1,
+                      tsBSpline *sub,
+                      tsStatus *status)
+{
+	int reverse; /* reverse `spline`? (if `knot0 > knot1`) */
+	tsReal *tmp = NULL; /* a buffer to swap control points */
+	tsReal min, max; /* domain of `spline` */
+	size_t dim, deg, order; /* properties of `spline` (and `sub`) */
+	tsBSpline worker; /* stores the result of the `split' operations */
+	tsReal *ctrlp, *knots; /* control points and knots of `worker` */
+	size_t k0, k1; /* indices returned by the `split' operations */
+	size_t c0, c1; /* indices of the control points to be moved */
+	size_t nc, nk; /* number of control points and knots of `sub` */
+	size_t i; /* for various needs */
+	tsError err; /* for local try-catch block */
+
+	/* Make sure that `worker` points to `NULL'. This allows us to call
+	 * `ts_bspline_free` in `TS_CATCH` without further checks. Also, `NULL'
+	 * serves as an indicator of whether `ts_bspline_split` has been called
+	 * on `spline` at least once (if not, `worker` needs to be initialized
+	 * manually). */
+	ts_int_bspline_init(&worker);
+	INIT_OUT_BSPLINE(spline, sub)
+
+	ts_bspline_domain(spline, &min, &max);
+	dim = ts_bspline_dimension(spline);
+	deg = ts_bspline_degree(spline);
+	order = ts_bspline_order(spline);
+
+	/* Cannot create valid knot vector from empty domain. */
+	if (ts_knots_equal(knot0, knot1)) {
+		TS_RETURN_0(status,
+		            TS_NO_RESULT,
+		            "empty domain")
+	}
+
+	/* Check for `reverse mode'. Reverse mode means that the copied sequence
+	 * of (sub) control points need to be reversed, forming a `backwards'
+	 * spline. */
+	reverse = knot0 > knot1;
+	if (reverse) { /* swap `knot0` and `knot1` */
+		tmp = (tsReal *) malloc(dim * sizeof(tsReal));
+		if (!tmp) TS_RETURN_0(status, TS_MALLOC, "out of memory");
+		*tmp = knot0; /* `tmp` can  hold at least one value */
+		knot0 = knot1;
+		knot1 = *tmp;
+	}
+
+	TS_TRY(try, err, status)
+		if (!ts_knots_equal(knot0 , min)) {
+			TS_CALL(try , err, ts_bspline_split(
+			        spline, knot0, &worker, &k0, status))
+		} else { k0 = deg; }
+		if (!ts_knots_equal(knot1, max)) {
+			TS_CALL(try , err, ts_bspline_split(
+			        /* If `NULL', the split operation
+			           above was not called. */
+			        !worker.pImpl ? spline : &worker,
+			        knot1, &worker, &k1, status))
+		} else {
+			k1 = ts_bspline_num_knots(
+			        /* If `NULL', the split operation
+			           above was not called. */
+			        !worker.pImpl ? spline : &worker) - 1;
+		}
+
+		/* Set up `worker`. */
+		if (!worker.pImpl) { /* => no split applied */
+			TS_CALL(try, err, ts_bspline_copy(
+			        spline, &worker, status))
+			/* Needed in `reverse mode'. */
+			ctrlp = ts_int_bspline_access_ctrlp(&worker);
+			knots = ts_int_bspline_access_knots(&worker);
+			nc = ts_bspline_num_control_points(&worker);
+		} else {
+			c0 = (k0-deg) * dim;
+			c1 = (k1-order) * dim;
+			nc = ((c1-c0) / dim) + 1;
+			nk = (k1-k0) + order;
+
+			/* Also needed in `reverse mode'. */
+			ctrlp = ts_int_bspline_access_ctrlp(&worker);
+			knots = ts_int_bspline_access_knots(&worker);
+
+			/* Move control points. */
+			memmove(ctrlp,
+			        ctrlp + c0,
+			        nc * dim * sizeof(tsReal));
+			/* Move knots. */
+			memmove(ctrlp + nc * dim,
+			        knots + (k0-deg),
+			        nk * sizeof(tsReal));
+
+			/* Remove superfluous control points and knots from
+			 * the memory of `worker`. */
+			worker.pImpl->n_knots = nk;
+			worker.pImpl->n_ctrlp = nc;
+			i = ts_int_bspline_sof_state(&worker);
+			worker.pImpl = realloc(worker.pImpl, i);
+			if (worker.pImpl == NULL) { /* unlikely to fail */
+				TS_THROW_0(try, err, status, TS_MALLOC,
+				           "out of memory")
+			}
+		}
+
+		/* Reverse control points (if necessary). */
+		if (reverse) {
+			for (i = 0; i < nc / 2; i++) {
+				memcpy(tmp,
+				       ctrlp  +     i      * dim,
+				       dim * sizeof(tsReal));
+				memmove(ctrlp +     i      * dim,
+				        ctrlp + (nc-1 - i) * dim,
+				        dim * sizeof(tsReal));
+				memcpy(ctrlp  + (nc-1 - i) * dim,
+				       tmp,
+				       dim * sizeof(tsReal));
+			}
+		}
+
+		/* Move `worker' to output parameter. */
+		if (spline == sub) ts_bspline_free(sub);
+		ts_bspline_move(&worker, sub);
+	TS_CATCH(err)
+		ts_bspline_free(&worker);
+	TS_FINALLY
+		if (tmp) free(tmp);
+	TS_END_TRY_RETURN(err)
+}
+
+void
+ts_bspline_uniform_knot_seq(const tsBSpline *spline,
+                            size_t num,
+                            tsReal *knots)
+{
+	size_t i;
+	tsReal min, max;
+	if (num == 0) return;
+	ts_bspline_domain(spline, &min, &max);
+	for (i = 0; i < num; i++) {
+		knots[i] = max - min;
+		knots[i] *= (tsReal) i / (num - 1);
+		knots[i] += min;
+	}
+	/* Set `knots[0]` after `knots[num - 1]` to ensure
+	   that `knots[0] = min` if `num` is `1'. */
+	knots[num - 1] = max;
+	knots[0] = min;
+}
+
+tsError
+ts_bspline_equidistant_knot_seq(const tsBSpline *spline,
+                                size_t num,
+                                tsReal *knots,
+                                size_t num_samples,
+                                tsStatus *status)
+{
+	tsError err;
+	tsReal *samples = NULL, *lengths = NULL;
+
+	if (num == 0) TS_RETURN_SUCCESS(status);
+	if (num_samples == 0) num_samples = 200;
+
+	samples = (tsReal *) malloc(2 * num_samples * sizeof(tsReal));
+	if (!samples) TS_RETURN_0(status, TS_MALLOC, "out of memory");
+	ts_bspline_uniform_knot_seq(spline, num_samples, samples);
+	lengths = samples + num_samples;
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_bspline_chord_lengths(
+		        spline, samples, num_samples, lengths, status))
+		TS_CALL(try, err, ts_chord_lengths_equidistant_knot_seq(
+		        samples, lengths, num_samples, num, knots, status))
+	TS_FINALLY
+		free(samples); /* cannot be NULL */
+		/* free(lengths); NO! */
+	TS_END_TRY_RETURN(err)
+}
+/*! @} */
+
+
+
+/*! @name Transformation Functions
+ *
+ * @{
+ */
+tsError
+ts_int_bspline_resize(const tsBSpline *spline,
+                      int n,
+                      int back,
+                      tsBSpline *resized,
+                      tsStatus *status)
+{
+	const size_t deg = ts_bspline_degree(spline);
+	const size_t dim = ts_bspline_dimension(spline);
+	const size_t sof_real = sizeof(tsReal);
+
+	const size_t num_ctrlp = ts_bspline_num_control_points(spline);
+	const size_t num_knots = ts_bspline_num_knots(spline);
+	const size_t nnum_ctrlp = num_ctrlp + n; /**< New length of ctrlp. */
+	const size_t nnum_knots = num_knots + n; /**< New length of knots. */
+	const size_t min_num_ctrlp_vec = n < 0 ? nnum_ctrlp : num_ctrlp;
+	const size_t min_num_knots_vec = n < 0 ? nnum_knots : num_knots;
+
+	const size_t sof_min_num_ctrlp = min_num_ctrlp_vec * dim * sof_real;
+	const size_t sof_min_num_knots = min_num_knots_vec * sof_real;
+
+	tsBSpline tmp; /**< Temporarily stores the result. */
+	const tsReal* from_ctrlp = ts_int_bspline_access_ctrlp(spline);
+	const tsReal* from_knots = ts_int_bspline_access_knots(spline);
+	tsReal* to_ctrlp = NULL; /**< Pointer to the control points of tmp. */
+	tsReal* to_knots = NULL; /**< Pointer to the knots of tmp. */
+
+	tsError err;
+
+	if (n == 0) return ts_bspline_copy(spline, resized, status);
+
+	INIT_OUT_BSPLINE(spline, resized)
+	TS_CALL_ROE(err, ts_bspline_new(
+	            nnum_ctrlp, dim, deg, TS_OPENED,
+	            &tmp, status))
+	to_ctrlp = ts_int_bspline_access_ctrlp(&tmp);
+	to_knots = ts_int_bspline_access_knots(&tmp);
+
+	/* Copy control points and knots. */
+	if (!back && n < 0) {
+		memcpy(to_ctrlp, from_ctrlp - n*dim, sof_min_num_ctrlp);
+		memcpy(to_knots, from_knots - n    , sof_min_num_knots);
+	} else if (!back && n > 0) {
+		memcpy(to_ctrlp + n*dim, from_ctrlp, sof_min_num_ctrlp);
+		memcpy(to_knots + n    , from_knots, sof_min_num_knots);
+	} else {
+		/* n != 0 implies back == true */
+		memcpy(to_ctrlp, from_ctrlp, sof_min_num_ctrlp);
+		memcpy(to_knots, from_knots, sof_min_num_knots);
+	}
+
+	if (spline == resized)
+		ts_bspline_free(resized);
+	ts_bspline_move(&tmp, resized);
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_bspline_derive(const tsBSpline *spline,
+                  size_t n,
+                  tsReal epsilon,
+                  tsBSpline *deriv,
+                  tsStatus *status)
+{
+	const size_t sof_real = sizeof(tsReal);
+	const size_t dim = ts_bspline_dimension(spline);
+	const size_t sof_ctrlp = dim * sof_real;
+	size_t deg = ts_bspline_degree(spline);
+	size_t num_ctrlp = ts_bspline_num_control_points(spline);
+	size_t num_knots = ts_bspline_num_knots(spline);
+
+	tsBSpline worker; /**< Stores the intermediate result. */
+	tsReal* ctrlp;    /**< Pointer to the control points of worker. */
+	tsReal* knots;    /**< Pointer to the knots of worker. */
+
+	size_t m, i, j, k, l; /**< Used in for loops. */
+	tsReal *fst, *snd; /**< Pointer to first and second control point. */
+	tsReal dist; /**< Distance between fst and snd. */
+	tsReal kid1, ki1; /**< Knots at i+deg+1 and i+1. */
+	tsReal span; /**< Distance between kid1 and ki1. */
+
+	tsBSpline swap; /**< Used to swap worker and derivative. */
+	tsError err;
+
+	INIT_OUT_BSPLINE(spline, deriv)
+	TS_CALL_ROE(err, ts_bspline_copy(spline, &worker, status))
+	ctrlp = ts_int_bspline_access_ctrlp(&worker);
+	knots = ts_int_bspline_access_knots(&worker);
+
+	TS_TRY(try, err, status)
+		for (m = 1; m <= n; m++) { /* from 1st to n'th derivative */
+			if (deg == 0) {
+				ts_arr_fill(ctrlp, dim, 0.f);
+				ts_bspline_domain(spline,
+				                  &knots[0],
+				                  &knots[1]);
+				num_ctrlp = 1;
+				num_knots = 2;
+				break;
+			}
+			/* Check and, if possible, fix discontinuity. */
+			for (i = 2*deg + 1; i < num_knots - (deg+1); i++) {
+				if (!ts_knots_equal(knots[i], knots[i-deg]))
+					continue;
+				fst = ctrlp + (i - (deg+1)) * dim;
+				snd = fst + dim;
+				dist = ts_distance(fst, snd, dim);
+				if (epsilon >= 0.f && dist > epsilon) {
+					TS_THROW_1(try, err, status,
+					           TS_UNDERIVABLE,
+					           "discontinuity at knot: %f",
+					           knots[i])
+				}
+				memmove(snd,
+				        snd + dim,
+				        (num_ctrlp - (i+1-deg)) * sof_ctrlp);
+				memmove(&knots[i],
+				        &knots[i+1],
+				        (num_knots - (i+1)) * sof_real);
+				num_ctrlp--;
+				num_knots--;
+				i += deg-1;
+			}
+			/* Derive continuous worker. */
+			for (i = 0; i < num_ctrlp-1; i++) {
+				for (j = 0; j < dim; j++) {
+					k = i    *dim + j;
+					l = (i+1)*dim + j;
+					ctrlp[k] = ctrlp[l] - ctrlp[k];
+					kid1 = knots[i+deg+1];
+					ki1  = knots[i+1];
+					span = kid1 - ki1;
+					if (span < TS_KNOT_EPSILON)
+						span = TS_KNOT_EPSILON;
+					ctrlp[k] *= deg;
+					ctrlp[k] /= span;
+				}
+			}
+			deg       -= 1;
+			num_ctrlp -= 1;
+			num_knots -= 2;
+			knots     += 1;
+		}
+		TS_CALL(try, err, ts_bspline_new(
+		        num_ctrlp, dim, deg, TS_OPENED,
+		        &swap, status))
+		memcpy(ts_int_bspline_access_ctrlp(&swap),
+		       ctrlp,
+		       num_ctrlp * sof_ctrlp);
+		memcpy(ts_int_bspline_access_knots(&swap),
+		       knots,
+		       num_knots * sof_real);
+		if (spline == deriv)
+			ts_bspline_free(deriv);
+		ts_bspline_move(&swap, deriv);
+	TS_FINALLY
+		ts_bspline_free(&worker);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_int_bspline_insert_knot(const tsBSpline *spline,
+                           const tsDeBoorNet *net,
+                           size_t n,
+                           tsBSpline *result,
+                           tsStatus *status)
+{
+	const size_t deg = ts_bspline_degree(spline);
+	const size_t order = ts_bspline_order(spline);
+	const size_t dim = ts_bspline_dimension(spline);
+	const tsReal knot = ts_deboornet_knot(net);
+	const size_t k = ts_deboornet_index(net);
+	const size_t mult = ts_deboornet_multiplicity(net);
+	const size_t sof_real = sizeof(tsReal);
+	const size_t sof_ctrlp = dim * sof_real;
+
+	size_t N;     /**< Number of affected control points. */
+	tsReal* from; /**< Pointer to copy the values from. */
+	tsReal* to;   /**< Pointer to copy the values to. */
+	int stride;   /**< Stride of the next pointer to copy. */
+	size_t i;     /**< Used in for loops. */
+
+	tsReal *ctrlp_spline, *ctrlp_result;
+	tsReal *knots_spline, *knots_result;
+	size_t num_ctrlp_result;
+	size_t num_knots_result;
+
+	tsError err;
+
+	INIT_OUT_BSPLINE(spline, result)
+	if (n == 0)
+		return ts_bspline_copy(spline, result, status);
+	if (mult + n > order) {
+		TS_RETURN_4(status, TS_MULTIPLICITY,
+		            "multiplicity(%f) (%lu) + %lu > order (%lu)",
+		            knot, (unsigned long) mult, (unsigned long) n,
+		            (unsigned long) order)
+	}
+
+	TS_CALL_ROE(err, ts_int_bspline_resize(
+	            spline, (int)n, 1, result, status))
+	ctrlp_spline = ts_int_bspline_access_ctrlp(spline);
+	knots_spline = ts_int_bspline_access_knots(spline);
+	ctrlp_result = ts_int_bspline_access_ctrlp(result);
+	knots_result = ts_int_bspline_access_knots(result);
+	num_ctrlp_result = ts_bspline_num_control_points(result);
+	num_knots_result = ts_bspline_num_knots(result);
+
+	/* mult + n <= deg + 1 (order) with n >= 1
+	 * =>  mult <= deg
+	 * =>  regular evaluation
+	 * =>  N = h + 1 is valid */
+	N = ts_deboornet_num_insertions(net) + 1;
+
+	/* 1. Copy the relevant control points and knots from `spline'.
+	 * 2. Copy the relevant control points and knots from `net'. */
+
+	/* 1.
+	 *
+	 * a) Copy left hand side control points from `spline'.
+	 * b) Copy right hand side control points from `spline'.
+	 * c) Copy left hand side knots from `spline'.
+	 * d) Copy right hand side knots form `spline'. */
+	/*** Copy Control Points ***/
+	memmove(ctrlp_result, ctrlp_spline, (k-deg) * sof_ctrlp);      /* a) */
+	from = (tsReal *) ctrlp_spline + dim*(k-deg+N);
+	/* n >= 0 implies to >= from */
+	to = ctrlp_result + dim*(k-deg+N+n);
+	memmove(to, from, (num_ctrlp_result-n-(k-deg+N)) * sof_ctrlp); /* b) */
+	/*** Copy Knots ***/
+	memmove(knots_result, knots_spline, (k+1) * sof_real);         /* c) */
+	from = (tsReal *) knots_spline + k+1;
+	/* n >= 0 implies to >= from */
+	to = knots_result + k+1+n;
+	memmove(to, from, (num_knots_result-n-(k+1)) * sof_real);      /* d) */
+
+	/* 2.
+	 *
+	 * a) Copy left hand side control points from `net'.
+	 * b) Copy middle part control points from `net'.
+	 * c) Copy right hand side control points from `net'.
+	 * d) Copy knot from `net' (`knot'). */
+	from = ts_int_deboornet_access_points(net);
+	to = ctrlp_result + (k-deg)*dim;
+	stride = (int)(N*dim);
+
+	/*** Copy Control Points ***/
+	for (i = 0; i < n; i++) {                                      /* a) */
+		memcpy(to, from, sof_ctrlp);
+		from += stride;
+		to += dim;
+		stride -= (int)dim;
+	}
+	memcpy(to, from, (N-n) * sof_ctrlp);                           /* b) */
+
+	from -= dim;
+	to += (N-n)*dim;
+	/* N = h+1 with h = deg-mult (ts_int_bspline_eval)
+	 * =>  N = deg-mult+1 = order-mult.
+	 *
+	 * n <= order-mult
+	 * =>  N-n+1 >= order-mult - order-mult + 1 = 1
+	 * =>  -(int)(N-n+1) <= -1. */
+	stride = -(int)(N-n+1) * (int)dim;
+
+	for (i = 0; i < n; i++) {                                      /* c) */
+		memcpy(to, from, sof_ctrlp);
+		from += stride;
+		stride -= (int)dim;
+		to += dim;
+	}
+	/*** Copy Knot ***/
+	to = knots_result + k+1;
+	for (i = 0; i < n; i++) {                                      /* d) */
+		*to = knot;
+		to++;
+	}
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_bspline_insert_knot(const tsBSpline *spline,
+                       tsReal knot,
+                       size_t num,
+                       tsBSpline *result,
+                       size_t* k,
+                       tsStatus *status)
+{
+	tsDeBoorNet net;
+	tsError err;
+	INIT_OUT_BSPLINE(spline, result)
+	ts_int_deboornet_init(&net);
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_bspline_eval(
+		        spline, knot, &net, status))
+		TS_CALL(try, err, ts_int_bspline_insert_knot(
+		        spline, &net, num, result, status))
+		ts_deboornet_free(&net);
+		TS_CALL(try, err, ts_bspline_eval(
+		        result, knot, &net, status))
+		*k = ts_deboornet_index(&net);
+	TS_CATCH(err)
+		*k = 0;
+	TS_FINALLY
+		ts_deboornet_free(&net);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_split(const tsBSpline *spline,
+                 tsReal knot,
+                 tsBSpline *split,
+                 size_t* k,
+                 tsStatus *status)
+{
+	tsDeBoorNet net;
+	tsError err;
+	INIT_OUT_BSPLINE(spline, split)
+	ts_int_deboornet_init(&net);
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_bspline_eval(
+		        spline, knot, &net, status))
+		if (ts_deboornet_multiplicity(&net)
+		    == ts_bspline_order(spline)) {
+			TS_CALL(try, err, ts_bspline_copy(
+			        spline, split, status))
+			*k = ts_deboornet_index(&net);
+		} else {
+			TS_CALL(try, err, ts_int_bspline_insert_knot(
+			        spline, &net,
+			        ts_deboornet_num_insertions(&net) + 1,
+			        split, status))
+			*k = ts_deboornet_index(&net) +
+			     ts_deboornet_num_insertions(&net) + 1;
+		}
+	TS_CATCH(err)
+		*k = 0;
+	TS_FINALLY
+		ts_deboornet_free(&net);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_tension(const tsBSpline *spline,
+                   tsReal beta,
+                   tsBSpline *out,
+                   tsStatus *status)
+{
+	const size_t dim = ts_bspline_dimension(spline);
+	const size_t N = ts_bspline_num_control_points(spline);
+	const tsReal* p0 = ts_int_bspline_access_ctrlp(spline);
+	const tsReal* pn_1 = p0 + (N-1)*dim;
+
+	tsReal s; /**< The straightening factor. */
+	tsReal *ctrlp; /**< Pointer to the control points of `out'. */
+	size_t i, d; /**< Used in for loops. */
+	tsReal vec; /**< Straightening vector. */
+	tsError err;
+
+	TS_CALL_ROE(err, ts_bspline_copy(spline, out, status))
+	ctrlp = ts_int_bspline_access_ctrlp(out);
+	if (beta < (tsReal) 0.0) beta = (tsReal) 0.0;
+	if (beta > (tsReal) 1.0) beta = (tsReal) 1.0;
+	s = 1.f - beta;
+
+	for (i = 0; i < N; i++) {
+		for (d = 0; d < dim; d++) {
+			vec = ((tsReal)i / (N-1)) * (pn_1[d] - p0[d]);
+			ctrlp[i*dim + d] = beta * ctrlp[i*dim + d] +
+			                   s * (p0[d] + vec);
+		}
+	}
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_bspline_to_beziers(const tsBSpline *spline,
+                      tsBSpline *beziers,
+                      tsStatus *status)
+{
+	const size_t deg = ts_bspline_degree(spline);
+	const size_t order = ts_bspline_order(spline);
+
+	int resize;    /**< Number of control points to add/remove. */
+	size_t k;      /**< Index of the split knot value. */
+	tsReal u_min;  /**< Minimum of the knot values. */
+	tsReal u_max;  /**< Maximum of the knot values. */
+
+	tsBSpline tmp;    /**< Temporarily stores the result. */
+	tsReal *knots;    /**< Pointer to the knots of tmp. */
+	size_t num_knots; /**< Number of knots in tmp. */
+
+	tsError err;
+
+	INIT_OUT_BSPLINE(spline, beziers)
+	TS_CALL_ROE(err, ts_bspline_copy(spline, &tmp, status))
+	knots = ts_int_bspline_access_knots(&tmp);
+	num_knots = ts_bspline_num_knots(&tmp);
+
+	TS_TRY(try, err, status)
+		/* DO NOT FORGET TO UPDATE knots AND num_knots AFTER EACH
+		 * TRANSFORMATION OF tmp! */
+
+		/* Fix first control point if necessary. */
+		u_min = knots[deg];
+		if (!ts_knots_equal(knots[0], u_min)) {
+			TS_CALL(try, err, ts_bspline_split(
+			        &tmp, u_min, &tmp, &k, status))
+			resize = (int)(-1*deg + (deg*2 - k));
+			TS_CALL(try, err, ts_int_bspline_resize(
+			        &tmp, resize, 0, &tmp, status))
+			knots = ts_int_bspline_access_knots(&tmp);
+			num_knots = ts_bspline_num_knots(&tmp);
+		}
+
+		/* Fix last control point if necessary. */
+		u_max = knots[num_knots - order];
+		if (!ts_knots_equal(knots[num_knots - 1], u_max)) {
+			TS_CALL(try, err, ts_bspline_split(
+			        &tmp, u_max, &tmp, &k, status))
+			num_knots = ts_bspline_num_knots(&tmp);
+			resize = (int)(-1*deg + (k - (num_knots - order)));
+			TS_CALL(try, err, ts_int_bspline_resize(
+			        &tmp, resize, 1, &tmp, status))
+			knots = ts_int_bspline_access_knots(&tmp);
+			num_knots = ts_bspline_num_knots(&tmp);
+		}
+
+		/* Split internal knots. */
+		k = order;
+		while (k < num_knots - order) {
+			TS_CALL(try, err, ts_bspline_split(
+			        &tmp, knots[k], &tmp, &k, status))
+			knots = ts_int_bspline_access_knots(&tmp);
+			num_knots = ts_bspline_num_knots(&tmp);
+			k++;
+		}
+
+		if (spline == beziers)
+			ts_bspline_free(beziers);
+		ts_bspline_move(&tmp, beziers);
+	TS_FINALLY
+		ts_bspline_free(&tmp);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_elevate_degree(const tsBSpline *spline,
+                          size_t amount,
+                          tsReal epsilon,
+                          tsBSpline *elevated,
+                          tsStatus * status)
+{
+	tsBSpline worker;
+	size_t dim, order;
+	tsReal *ctrlp, *knots;
+	size_t num_beziers, i, a, c, d, offset, idx;
+	tsReal f, f_hat, *first, *last;
+	tsError err;
+
+	/* Trivial case. */
+	if (amount == 0)
+		return ts_bspline_copy(spline, elevated, status);
+
+	/* An overview of this algorithm can be found at:
+	 * https://pages.mtu.edu/~shene/COURSES/cs3621/LAB/curve/elevation.html */
+	INIT_OUT_BSPLINE(spline, elevated);
+	worker = ts_bspline_init();
+	TS_TRY(try, err, status)
+		/* Decompose `spline' into a sequence of bezier curves and make
+		 * space for the additional control points and knots that are
+		 * to be inserted. Results are stored in `worker'. */
+		TS_CALL(try, err, ts_bspline_to_beziers(
+		        spline, &worker, status));
+		num_beziers = ts_bspline_num_control_points(&worker) /
+		              ts_bspline_order(&worker);
+		TS_CALL(try, err, ts_int_bspline_resize(
+		        /* Resize by the number of knots to be inserted. Note
+		         * that this creates too many control points (due to
+		         * increasing the degree), which are removed at the end
+		         * of this function. */
+		        &worker, (int) ((num_beziers+1) * amount), 1, &worker,
+		        status));
+		dim = ts_bspline_dimension(&worker);
+		order = ts_bspline_order(&worker);
+		ctrlp = ts_int_bspline_access_ctrlp(&worker);
+		knots = ts_int_bspline_access_knots(&worker);
+
+		/* Move all but the first bezier curve to their new location in
+		 * the control point array so that the additional control
+		 * points can be inserted without overwriting the others. Note
+		 * that iteration must run backwards. Otherwise, the moved
+		 * values overwrite each other. */
+		for (i = num_beziers - 1; i > 0; i--) {
+			/* `i' can be interpreted as the number of bezier
+			 * curves before the current bezier curve. */
+
+			/* Location of current bezier curve. */
+			offset = i * order * dim;
+			/* Each elevation inserts an additional control point
+			 * into every bezier curve. `i * amount' is the total
+			 * number of control points to be inserted before the
+			 * current bezier curve. */
+			memmove(ctrlp + offset + (i * amount * dim),
+			        ctrlp + offset,
+			        dim * order * sizeof(tsReal));
+		}
+
+		/* Move all but the first group of knots to their new location
+		 * in the knot vector so that the additional knots can be
+		 * inserted without overwriting the others. Note that iteration
+		 * must run backwards. Otherwise, the moved values overwrite
+		 * each other. */
+		for (i = num_beziers; i > 0; i--) {
+			/* Note that the number of knot groups is one more than
+			 * the number of bezier curves. `i' can be interpreted
+			 * as the number of knot groups before the current
+			 * group. */
+
+			/* Location of current knot group. */
+			offset = i * order;
+			/* Each elevation inserts an additional knot into every
+			 * group of knots. `i * amount' is the total number of
+			 * knots to be inserted before the current knot
+			 * group. */
+			memmove(knots + offset + (i * amount),
+			        knots + offset,
+			        order * sizeof(tsReal));
+		}
+
+		/* `worker' is now fully set up.
+		 * The following formulas are based on:
+		 * https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/Bezier/bezier-elev.html */
+		for (a = 0; a < amount; a++) {
+			/* For each bezier curve... */
+			for (i = 0; i < num_beziers; i++) {
+				/* ... 1) Insert and update control points. */
+
+				/* Location of current bezier curve. Each
+				 * elevation (`a') inserts an additional
+				 * control point into every bezier curve and
+				 * increases the degree (`order') by one. The
+				 * location is thus made up of two parts:
+				 *
+				 * i) `i * order', which is the location taking
+				 * into account the increasing order but
+				 * neglecting the control points that are to be
+				 * inserted before the current bezier curve. It
+				 * can be seen as some sort of base location:
+				 * Where would the bezier curve be (with
+				 * respect to the current value of `order') if
+				 * no additional control points had to be
+				 * inserted?
+				 *
+				 * ii) `i * (amount - a)', which is the total
+				 * number of control points to be inserted
+				 * before the current bezier curve
+				 * (`i * amount') taking into account the
+				 * increasing order (`order' and `a' are
+				 * increased equally, thus, `a' compensates for
+				 * the increasing value of `order'). This part
+				 * adds the necessary offset to the base
+				 * location (`i * order'). */
+				offset = (i * order + i * (amount - a)) * dim;
+				/* Duplicate last control point to the new end
+				 * position (next control point). */
+				memmove(ctrlp + offset + ((order) * dim),
+				        ctrlp + offset + ((order-1) * dim),
+				        dim * sizeof(tsReal));
+				/* All but the outer control points must be
+				 * recalculated (domain: [1, order - 1]). By
+				 * traversing backwards, control points can be
+				 * modified in-place. */
+				for (c = order - 1; c > 0; c--) {
+					/* Location of current control point
+					 * within current bezier curve. */
+					idx = offset + c * dim;
+					f = (tsReal) c / (tsReal) (order);
+					f_hat = 1 - f;
+					for (d = 0; d < dim; d++) {
+						/* For the sake of space, we
+						 * increment idx by d and
+						 * decrement it at the end of
+						 * this loop. */
+						idx += d;
+						ctrlp[idx] =
+							f * ctrlp[idx - dim] +
+							f_hat * ctrlp[idx];
+						/* Reset idx. */
+						idx -= d;
+					}
+				}
+
+				/* ...2) Increase the multiplicity of the
+				 * second knot group (maximum of the domain of
+				 * the current bezier curve) by one. Note that
+				 * this loop misses the last knot group (the
+				 * group of the last bezier curve) as there is
+				 * one more knot group than bezier curves to
+				 * process. Thus, the last group must be
+				 * increased separately after this loop. */
+
+				/* Location of current knot group. Each
+				 * elevation (`a') inserts an additional
+				 * knot into the knot vector of every bezier
+				 * curve and increases the degree (`order') by
+				 * one. The location is thus made up of two
+				 * parts:
+				 *
+				 * i) `i * order', which is the location taking
+				 * into account the increasing order but
+				 * neglecting the knots that are to be inserted
+				 * before the current knot group. It can be
+				 * seen as some sort of base location: Where
+				 * would the knot group be (with respect to the
+				 * current value of `order') if no additional
+				 * knots had to be inserted?
+				 *
+				 * ii) `i * (amount - a)', which is the total
+				 * number of knots to be inserted before the
+				 * current knot group (`i * amount') taking
+				 * into account the increasing order (`order'
+				 * and `a' are increased equally, thus, `a'
+				 * compensates for the increasing value of
+				 * `order'). This part adds the necessary
+				 * offset to the base location
+				 * (`i * order'). */
+				offset = i * order + i * (amount - a);
+				/* Duplicate knot. */
+				knots[offset + order] = knots[offset];
+			}
+
+			/* Increase the multiplicity of the very last knot
+			 * group (the second group of the last bezier curve)
+			 * by one. For more details, see knot duplication in
+			 * previous loop. */
+			offset = num_beziers * order +
+			         num_beziers * (amount - a);
+			knots[offset + order] = knots[offset];
+
+			/* Elevated by one. */
+			order++;
+		}
+
+		/* Combine bezier curves. */
+		d = 0; /* Number of removed knots/control points. */
+		for (i = 0; i < num_beziers - 1; i++) {
+			/* Is the last control point of bezier curve `i' equal
+			 * to the first control point of bezier curve `i+1'? */
+			last = ctrlp + (
+				i * order /* base location of `i' */
+				- d /* minus the number of removed values */
+				+ (order - 1) /* jump to last control point */
+				) * dim;
+			first = last + dim; /* next control point */
+			if (ts_distance(last, first, dim) <= epsilon) {
+				/* Move control points. */
+				memmove(last, first, (num_beziers - 1 - i) *
+				        order * dim * sizeof(tsReal));
+
+				/* Move knots. `last' is the last knot of the
+				 * second knot group of bezier curve `i'.
+				 * `first' is the first knot of the first knot
+				 * group of bezier curve `i+1'. The
+				 * calculations are quite similar to those for
+				 * the control points `last' and `first' (see
+				 * above). */
+				last = knots + i * order - d + (2 * order - 1);
+				first = last + 1;
+				memmove(last, first, (num_beziers - 1 - i) *
+				        order * sizeof(tsReal));
+
+				/* Removed one knot/control point. */
+				d++;
+			}
+		}
+
+		/* Repair internal state. */
+		worker.pImpl->deg = order - 1;
+		worker.pImpl->n_knots -= d;
+		worker.pImpl->n_ctrlp = ts_bspline_num_knots(&worker) - order;
+		memmove(ts_int_bspline_access_knots(&worker),
+		        knots, ts_bspline_sof_knots(&worker));
+		worker.pImpl = realloc(worker.pImpl,
+		                       ts_int_bspline_sof_state(&worker));
+		if (worker.pImpl == NULL) {
+			TS_THROW_0(try, err, status, TS_MALLOC,
+			           "out of memory")
+		}
+
+		/* Move `worker' to output parameter. */
+		if (spline == elevated)
+			ts_bspline_free(elevated);
+		ts_bspline_move(&worker, elevated);
+	TS_FINALLY
+		ts_bspline_free(&worker);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_align(const tsBSpline *s1,
+                 const tsBSpline *s2,
+                 tsReal epsilon,
+                 tsBSpline *s1_out,
+                 tsBSpline *s2_out,
+                 tsStatus *status)
+{
+	tsBSpline s1_worker, s2_worker, *smaller, *larger;
+	tsDeBoorNet net; /* the net of `smaller'. */
+	size_t i, missing, remaining;
+	tsReal min, max, shift, nextKnot;
+	tsError err;
+
+	INIT_OUT_BSPLINE(s1, s1_out)
+	INIT_OUT_BSPLINE(s2, s2_out)
+	s1_worker = ts_bspline_init();
+	s2_worker = ts_bspline_init();
+	smaller = larger = NULL;
+	TS_TRY(try, err, status)
+		/* Set up `s1_worker' and `s2_worker'. After this
+		 * if-elseif-else-block, `s1_worker' and `s2_worker' have same
+		 * degree. */
+		if (ts_bspline_degree(s1) < ts_bspline_degree(s2)) {
+			TS_CALL(try, err, ts_bspline_elevate_degree(s1,
+			        ts_bspline_degree(s2) - ts_bspline_degree(s1),
+			        epsilon, &s1_worker, status))
+			TS_CALL(try, err, ts_bspline_copy(
+			        s2, &s2_worker, status))
+		} else if (ts_bspline_degree(s2) < ts_bspline_degree(s1)) {
+			TS_CALL(try, err, ts_bspline_elevate_degree(s2,
+			        ts_bspline_degree(s1) - ts_bspline_degree(s2),
+			        epsilon, &s2_worker, status))
+			TS_CALL(try, err, ts_bspline_copy(
+			        s1, &s1_worker, status))
+		} else {
+			TS_CALL(try, err, ts_bspline_copy(
+			        s1, &s1_worker, status))
+			TS_CALL(try, err, ts_bspline_copy(
+			        s2, &s2_worker, status))
+		}
+
+		/* Set up `smaller', `larger', and `net'. */
+		if (ts_bspline_num_knots(&s1_worker) <
+		    ts_bspline_num_knots(&s2_worker)) {
+			smaller = &s1_worker;
+			larger  = &s2_worker;
+		} else {
+			smaller = &s2_worker;
+			larger  = &s1_worker;
+		}
+		TS_CALL(try, err, ts_int_deboornet_new(
+		        smaller, &net, status))
+
+		/* Insert knots into `smaller' until it has the same number of
+		 * knots (and therefore the same number of control points) as
+		 * `larger'. */
+		ts_bspline_domain(smaller, &min, &max);
+		missing = remaining = ts_bspline_num_knots(larger) -
+			              ts_bspline_num_knots(smaller);
+		shift = (tsReal) 0.0;
+		if (missing > 0)
+			shift = ( (tsReal) 1.0 / missing ) * (tsReal) 0.5;
+		for (i = 0; remaining > 0; i++, remaining--) {
+			nextKnot = (max - min) * ((tsReal)i / missing) + min;
+			nextKnot += shift;
+			TS_CALL(try, err, ts_int_bspline_eval_woa(
+			        smaller, nextKnot, &net, status))
+			while (!ts_deboornet_num_insertions(&net)) {
+				/* Linear exploration for next knot. */
+				nextKnot += 5 * TS_KNOT_EPSILON;
+				if (nextKnot > max) {
+					TS_THROW_0(try, err, status,
+					           TS_NO_RESULT,
+					          "no more knots for insertion")
+				}
+				TS_CALL(try, err, ts_int_bspline_eval_woa(
+				        smaller, nextKnot, &net, status))
+			}
+			TS_CALL(try, err, ts_int_bspline_insert_knot(
+			        smaller, &net, 1, smaller, status))
+		}
+
+		if (s1 == s1_out)
+			ts_bspline_free(s1_out);
+		if (s2 == s2_out)
+			ts_bspline_free(s2_out);
+		ts_bspline_move(&s1_worker, s1_out);
+		/* if `s1_out' == `s2_out', `s2_worker' must not be moved
+		 * because otherwise the memory of `s1_worker' is leaked
+		 * (`s2_worker' overrides `s1_worker'). */
+		if (s1_out != s2_out)
+			ts_bspline_move(&s2_worker, s2_out);
+	TS_FINALLY
+		ts_bspline_free(&s1_worker);
+		ts_bspline_free(&s2_worker);
+		ts_deboornet_free(&net);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_morph(const tsBSpline *origin,
+                 const tsBSpline *target,
+                 tsReal t,
+                 tsReal epsilon,
+                 tsBSpline *out,
+                 tsStatus *status)
+{
+	tsBSpline origin_al, target_al; /* aligned origin and target */
+	tsReal *origin_al_c, *origin_al_k; /* control points and knots */
+	tsReal *target_al_c, *target_al_k; /* control points and knots */
+
+	/* Properties of `out'. */
+	size_t deg, dim, num_ctrlp, num_knots;
+	tsReal *ctrlp, *knots;
+	tsBSpline tmp; /* temporary buffer if `out' must be resized */
+
+	tsReal t_hat;
+	size_t i, offset, d;
+	tsError err;
+
+	origin_al = ts_bspline_init();
+	target_al = ts_bspline_init();
+	TS_TRY(try, err, status)
+		/* Clamp `t' to domain [0, 1] and set up `t_hat'. */
+		if (t < (tsReal) 0.0) t = (tsReal) 0.0;
+		if (t > (tsReal) 1.0) t = (tsReal) 1.0;
+		t_hat = (tsReal) 1.0 - t;
+
+		/* Set up `origin_al' and `target_al'. */
+		/* Degree must be elevated... */
+		if (ts_bspline_degree(origin) != ts_bspline_degree(target) ||
+		 /* .. or knots (and thus control points) must be inserted. */
+		 ts_bspline_num_knots(origin) != ts_bspline_num_knots(target)) {
+			TS_CALL(try, err, ts_bspline_align(
+			        origin, target, epsilon, &origin_al, &target_al,
+			        status));
+		} else {
+			/* Flat copy. */
+			origin_al = *origin;
+			target_al = *target;
+		}
+		origin_al_c = ts_int_bspline_access_ctrlp(&origin_al);
+		origin_al_k = ts_int_bspline_access_knots(&origin_al);
+		target_al_c = ts_int_bspline_access_ctrlp(&target_al);
+		target_al_k = ts_int_bspline_access_knots(&target_al);
+
+		/* Set up `out'. */
+		deg = ts_bspline_degree(&origin_al);
+		num_ctrlp = ts_bspline_num_control_points(&origin_al);
+		dim = ts_bspline_dimension(&origin_al);
+		if (ts_bspline_dimension(&target_al) < dim)
+			dim = ts_bspline_dimension(&target_al);
+		if (out->pImpl == NULL) {
+			TS_CALL(try, err, ts_bspline_new(num_ctrlp, dim, deg,
+			        TS_OPENED /* doesn't matter */, out, status))
+		} else if (ts_bspline_degree(out) != deg ||
+		           ts_bspline_num_control_points(out) != num_ctrlp ||
+		           ts_bspline_dimension(out) != dim) {
+			TS_CALL(try, err, ts_bspline_new(num_ctrlp, dim, deg,
+			        TS_OPENED /* doesn't matter */, &tmp, status))
+			ts_bspline_free(out);
+			ts_bspline_move(&tmp, out);
+		}
+		num_knots = ts_bspline_num_knots(out);
+		ctrlp = ts_int_bspline_access_ctrlp(out);
+		knots = ts_int_bspline_access_knots(out);
+
+		/* Interpolate control points. */
+		for (i = 0; i < num_ctrlp; i++) {
+			for (d = 0; d < dim; d++) {
+				offset = i * dim + d;
+				ctrlp[offset] = t * target_al_c[offset] +
+				                t_hat * origin_al_c[offset];
+			}
+		}
+
+		/* Interpolate knots. */
+		for (i = 0; i < num_knots; i++) {
+			knots[i] = t * target_al_k[i] +
+			           t_hat * origin_al_k[i];
+		}
+	TS_FINALLY
+		if (origin->pImpl != origin_al.pImpl)
+			ts_bspline_free(&origin_al);
+		if (target->pImpl != target_al.pImpl)
+			ts_bspline_free(&target_al);
+	TS_END_TRY_RETURN(err)
+}
+/*! @} */
+
+
+
+/*! @name Serialization and Persistence
+ *
+ * @{
+ */
+tsError
+ts_int_bspline_to_json(const tsBSpline *spline,
+                       JSON_Value **value,
+                       tsStatus *status)
+{
+	const size_t deg = ts_bspline_degree(spline);
+	const size_t dim = ts_bspline_dimension(spline);
+	const size_t len_ctrlp = ts_bspline_len_control_points(spline);
+	const size_t len_knots = ts_bspline_num_knots(spline);
+	const tsReal *ctrlp = ts_int_bspline_access_ctrlp(spline);
+	const tsReal *knots = ts_int_bspline_access_knots(spline);
+
+	size_t i; /**< Used in loops */
+	tsError err;
+
+	JSON_Value  *ctrlp_value;
+	JSON_Value  *knots_value;
+	JSON_Object *spline_object;
+	JSON_Array  *ctrlp_array;
+	JSON_Array  *knots_array;
+
+	*value = ctrlp_value = knots_value = NULL;
+	TS_TRY(values, err, status)
+		/* Init memory. */
+		*value = json_value_init_object();
+		if (!*value) {
+			TS_THROW_0(values, err, status, TS_MALLOC,
+			           "out of memory")
+		}
+		ctrlp_value = json_value_init_array();
+		if (!ctrlp_value) {
+			TS_THROW_0(values, err, status, TS_MALLOC,
+			           "out of memory")
+		}
+		knots_value = json_value_init_array();
+		if (!knots_value) {
+			TS_THROW_0(values, err, status, TS_MALLOC,
+			           "out of memory")
+		}
+
+		/* Although the following functions cannot fail, that is, they
+		 * won't return NULL or JSONFailure, we nevertheless handle
+		 * unexpected return values. */
+
+		/* Init output. */
+		spline_object = json_value_get_object(*value);
+		if (!spline_object) {
+			TS_THROW_0(values, err, status, TS_MALLOC,
+			           "out of memory")
+		}
+
+		/* Set degree and dimension. */
+		if (JSONSuccess != json_object_set_number(spline_object,
+		                                          "degree",
+		                                          (double) deg)) {
+			TS_THROW_0(values, err, status, TS_MALLOC,
+			           "out of memory")
+		}
+		if (JSONSuccess != json_object_set_number(spline_object,
+		                                          "dimension",
+		                                          (double) dim)) {
+			TS_THROW_0(values, err, status, TS_MALLOC,
+			           "out of memory")
+		}
+
+		/* Set control points. */
+		if (JSONSuccess != json_object_set_value(spline_object,
+		                                         "control_points",
+		                                         ctrlp_value)) {
+			TS_THROW_0(values, err, status, TS_MALLOC,
+			           "out of memory")
+		}
+		ctrlp_array = json_array(ctrlp_value);
+		if (!ctrlp_array) {
+			TS_THROW_0(values, err, status, TS_MALLOC,
+			           "out of memory")
+		}
+		for (i = 0; i < len_ctrlp; i++) {
+			if (JSONSuccess != json_array_append_number(
+				    ctrlp_array, (double) ctrlp[i])) {
+				TS_THROW_0(values, err, status, TS_MALLOC,
+				           "out of memory")
+			}
+		}
+
+		/* Set knots. */
+		if (JSONSuccess != json_object_set_value(spline_object,
+		                                         "knots",
+		                                         knots_value)) {
+			TS_THROW_0(values, err, status, TS_MALLOC,
+			           "out of memory")
+		}
+		knots_array = json_array(knots_value);
+		if (!knots_array) {
+			TS_THROW_0(values, err, status, TS_MALLOC,
+				"out of memory")
+		}
+		for (i = 0; i < len_knots; i++) {
+			if (JSONSuccess != json_array_append_number(
+				    knots_array, (double) knots[i])) {
+				TS_THROW_0(values, err, status, TS_MALLOC,
+				           "out of memory")
+			}
+		}
+	TS_CATCH(err)
+		if (*value)
+			json_value_free(*value);
+		if (ctrlp_value && !json_value_get_parent(ctrlp_value))
+			json_value_free(ctrlp_value);
+		if (knots_value && !json_value_get_parent(knots_value))
+			json_value_free(knots_value);
+		*value = NULL;
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_int_bspline_parse_json(const JSON_Value *spline_value,
+                          tsBSpline *spline,
+                          tsStatus *status)
+{
+	size_t deg, dim, len_ctrlp, num_knots;
+	tsReal *ctrlp, *knots;
+
+	JSON_Object *spline_object;
+	JSON_Value *deg_value;
+	JSON_Value *dim_value;
+	JSON_Value *ctrlp_value;
+	JSON_Array *ctrlp_array;
+	JSON_Value *knots_value;
+	JSON_Array *knots_array;
+	JSON_Value *real_value;
+	size_t i;
+	tsError err;
+
+	ts_int_bspline_init(spline);
+
+	/* Read spline object. */
+	if (json_value_get_type(spline_value) != JSONObject)
+		TS_RETURN_0(status, TS_PARSE_ERROR, "invalid json input")
+	spline_object = json_value_get_object(spline_value);
+	if (!spline_object)
+		TS_RETURN_0(status, TS_PARSE_ERROR, "invalid json input")
+
+	/* Read degree. */
+	deg_value = json_object_get_value(spline_object, "degree");
+	if (json_value_get_type(deg_value) != JSONNumber)
+		TS_RETURN_0(status, TS_PARSE_ERROR, "degree is not a number")
+	if (json_value_get_number(deg_value) < -0.01f) {
+		TS_RETURN_1(status, TS_PARSE_ERROR,
+		            "degree (%f) < 0",
+		            json_value_get_number(deg_value))
+	}
+	deg = (size_t) json_value_get_number(deg_value);
+
+	/* Read dimension. */
+	dim_value = json_object_get_value(spline_object, "dimension");
+	if (json_value_get_type(dim_value) != JSONNumber) {
+		TS_RETURN_0(status, TS_PARSE_ERROR,
+		            "dimension is not a number")
+	}
+	if (json_value_get_number(dim_value) < 0.99f) {
+		TS_RETURN_1(status, TS_PARSE_ERROR,
+		            "dimension (%f) < 1",
+		            json_value_get_number(deg_value))
+	}
+	dim = (size_t) json_value_get_number(dim_value);
+
+	/* Read length of control point vector. */
+	ctrlp_value = json_object_get_value(spline_object, "control_points");
+	if (json_value_get_type(ctrlp_value) != JSONArray) {
+		TS_RETURN_0(status, TS_PARSE_ERROR,
+		            "control_points is not an array")
+	}
+	ctrlp_array = json_value_get_array(ctrlp_value);
+	len_ctrlp = json_array_get_count(ctrlp_array);
+	if (len_ctrlp % dim != 0) {
+		TS_RETURN_2(status, TS_PARSE_ERROR,
+		           "len(control_points) (%lu) %% dimension (%lu) != 0",
+		            (unsigned long) len_ctrlp, (unsigned long) dim)
+	}
+
+	/* Read number of knots. */
+	knots_value = json_object_get_value(spline_object, "knots");
+	if (json_value_get_type(knots_value) != JSONArray) {
+		TS_RETURN_0(status, TS_PARSE_ERROR,
+		            "knots is not an array")
+	}
+	knots_array = json_value_get_array(knots_value);
+	num_knots = json_array_get_count(knots_array);
+
+	/* Create spline. */
+	TS_TRY(try, err, status)
+		TS_CALL(try, err, ts_bspline_new(
+		        len_ctrlp/dim, dim, deg,
+		        TS_CLAMPED, spline, status))
+		if (num_knots != ts_bspline_num_knots(spline))
+			TS_THROW_2(try, err, status, TS_NUM_KNOTS,
+			           "unexpected num(knots): (%lu) != (%lu)",
+			           (unsigned long) num_knots,
+			          (unsigned long) ts_bspline_num_knots(spline))
+
+		/* Set control points. */
+		ctrlp = ts_int_bspline_access_ctrlp(spline);
+		for (i = 0; i < len_ctrlp; i++) {
+			real_value = json_array_get_value(ctrlp_array, i);
+			if (json_value_get_type(real_value) != JSONNumber)
+				TS_THROW_1(try, err, status, TS_PARSE_ERROR,
+				"control_points: value at index %lu is not a number",
+				           (unsigned long) i)
+			ctrlp[i] = (tsReal) json_value_get_number(real_value);
+		}
+		TS_CALL(try, err, ts_bspline_set_control_points(
+		        spline, ctrlp, status))
+
+		/* Set knots. */
+		knots = ts_int_bspline_access_knots(spline);
+		for (i = 0; i < num_knots; i++) {
+			real_value = json_array_get_value(knots_array, i);
+			if (json_value_get_type(real_value) != JSONNumber)
+			TS_THROW_1(try, err, status, TS_PARSE_ERROR,
+			           "knots: value at index %lu is not a number",
+			           (unsigned long) i)
+			knots[i] = (tsReal) json_value_get_number(real_value);
+		}
+		TS_CALL(try, err, ts_bspline_set_knots(
+		        spline, knots, status))
+	TS_CATCH(err)
+		ts_bspline_free(spline);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_to_json(const tsBSpline *spline,
+                   char **json,
+                   tsStatus *status)
+{
+	tsError err;
+	JSON_Value *value = NULL;
+	*json = NULL;
+	TS_CALL_ROE(err, ts_int_bspline_to_json(spline, &value, status))
+	*json = json_serialize_to_string_pretty(value);
+	json_value_free(value);
+	if (!*json)
+		TS_RETURN_0(status, TS_MALLOC, "out of memory")
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_bspline_parse_json(const char *json,
+                      tsBSpline *spline,
+                      tsStatus *status)
+{
+	tsError err;
+	JSON_Value *value = NULL;
+	ts_int_bspline_init(spline);
+	TS_TRY(try, err, status)
+		value = json_parse_string(json);
+		if (!value) {
+			TS_RETURN_0(status, TS_PARSE_ERROR,
+			            "invalid json input")
+		}
+		TS_CALL(try, err, ts_int_bspline_parse_json(
+		        value, spline, status))
+	TS_FINALLY
+		if (value)
+			json_value_free(value);
+	TS_END_TRY_RETURN(err)
+}
+
+tsError
+ts_bspline_save(const tsBSpline *spline,
+                const char *path,
+                tsStatus *status)
+{
+	tsError err;
+	JSON_Status json_status;
+	JSON_Value *value = NULL;
+	TS_CALL_ROE(err, ts_int_bspline_to_json(spline, &value, status))
+	json_status = json_serialize_to_file_pretty(value, path);
+	json_value_free(value);
+	if (json_status != JSONSuccess)
+		TS_RETURN_0(status, TS_IO_ERROR, "unexpected io error")
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_bspline_load(const char *path,
+                tsBSpline *spline,
+                tsStatus *status)
+{
+	tsError err;
+	FILE *file = NULL;
+	JSON_Value *value = NULL;
+	ts_int_bspline_init(spline);
+	TS_TRY(try, err, status)
+		file = fopen(path, "r");
+		if (!file) {
+			TS_THROW_0(try, err, status, TS_IO_ERROR,
+			           "unable to open file")
+		}
+		value = json_parse_file(path);
+		if (!value) {
+			TS_THROW_0(try, err, status, TS_PARSE_ERROR,
+			           "invalid json input")
+		}
+		TS_CALL(try, err, ts_int_bspline_parse_json(
+		        value, spline, status))
+	TS_FINALLY
+		if (file) fclose(file);
+		if (value) json_value_free(value);
+	TS_CATCH(err)
+		ts_bspline_free(spline);
+	TS_END_TRY_RETURN(err)
+}
+/*! @} */
+
+
+
+/*! @name Vector Math
+ * @{
+ */
+void
+ts_vec2_init(tsReal *out,
+             tsReal x,
+             tsReal y)
+{
+	out[0] = x;
+	out[1] = y;
+}
+
+void
+ts_vec3_init(tsReal *out,
+             tsReal x,
+             tsReal y,
+             tsReal z)
+{
+	out[0] = x;
+	out[1] = y;
+	out[2] = z;
+}
+
+void
+ts_vec4_init(tsReal *out,
+             tsReal x,
+             tsReal y,
+             tsReal z,
+             tsReal w)
+{
+	out[0] = x;
+	out[1] = y;
+	out[2] = z;
+	out[3] = w;
+}
+
+void
+ts_vec2_set(tsReal *out,
+            const tsReal *x,
+            size_t dim)
+{
+	const size_t n = dim > 2 ? 2 : dim;
+	memmove(out, x, n * sizeof(tsReal));
+	if (dim < 2)
+		ts_arr_fill(out + dim, 2 - dim, (tsReal) 0.0);
+}
+
+void
+ts_vec3_set(tsReal *out,
+            const tsReal *x,
+            size_t dim)
+{
+	const size_t n = dim > 3 ? 3 : dim;
+	memmove(out, x, n * sizeof(tsReal));
+	if (dim < 3)
+		ts_arr_fill(out + dim, 3 - dim, (tsReal) 0.0);
+}
+
+void
+ts_vec4_set(tsReal *out,
+            const tsReal *x,
+            size_t dim)
+{
+	const size_t n = dim > 4 ? 4 : dim;
+	memmove(out, x, n * sizeof(tsReal));
+	if (dim < 4)
+		ts_arr_fill(out + dim, 4 - dim, (tsReal) 0.0);
+}
+
+void
+ts_vec_add(const tsReal *x,
+           const tsReal *y,
+           size_t dim,
+           tsReal *out)
+{
+	size_t i;
+	for (i = 0; i < dim; i++)
+		out[i] = x[i] + y[i];
+}
+
+void
+ts_vec_sub(const tsReal *x,
+           const tsReal *y,
+           size_t dim,
+           tsReal *out)
+{
+	size_t i;
+	if (x == y) {
+		/* More stable version. */
+		ts_arr_fill(out, dim, (tsReal) 0.0);
+		return;
+	}
+	for (i = 0; i < dim; i++)
+		out[i] = x[i] - y[i];
+}
+
+tsReal
+ts_vec_dot(const tsReal *x,
+           const tsReal *y,
+           size_t dim)
+{
+	size_t i;
+	tsReal dot = 0;
+	for (i = 0; i < dim; i++)
+		dot += x[i] * y[i];
+	return dot;
+}
+
+tsReal
+ts_vec_angle(const tsReal *x,
+             const tsReal *y,
+             tsReal *buf,
+             size_t dim)
+{
+	const tsReal *x_norm, *y_norm;
+	if (buf) {
+		ts_vec_norm(x, dim, buf);
+		ts_vec_norm(y, dim, buf + dim);
+		x_norm = buf;
+		y_norm = buf + dim;
+	} else {
+		x_norm = x;
+		y_norm = y;
+	}
+	return (tsReal) (
+		/* Use doubles as long as possible. */
+		acos(ts_vec_dot(x_norm,
+		                y_norm,
+		                dim))
+		* (180.0 / TS_PI) /* radiant to degree */
+		);
+}
+
+void
+ts_vec3_cross(const tsReal *x,
+              const tsReal *y,
+              tsReal *out)
+{
+	tsReal a, b, c;
+	a = x[1] * y[2] - x[2] * y[1];
+	b = x[2] * y[0] - x[0] * y[2];
+	c = x[0] * y[1] - x[1] * y[0];
+	out[0] = a;
+	out[1] = b;
+	out[2] = c;
+}
+
+void
+ts_vec_norm(const tsReal *x,
+            size_t dim,
+            tsReal *out)
+{
+	size_t i;
+	const tsReal m = ts_vec_mag(x, dim);
+	if (m < TS_LENGTH_ZERO) {
+		ts_arr_fill(out, dim, (tsReal) 0.0);
+		return;
+	}
+	for (i = 0; i < dim; i++)
+		out[i] = x[i] / m;
+}
+
+tsReal
+ts_vec_mag(const tsReal *x,
+           size_t dim)
+{
+	size_t i;
+	tsReal sum = 0;
+	for (i = 0; i < dim; i++)
+		sum += (x[i] * x[i]);
+	return (tsReal) sqrt(sum);
+}
+
+void
+ts_vec_mul(const tsReal *x,
+           size_t dim,
+           tsReal val,
+           tsReal *out)
+{
+	size_t i;
+	for (i = 0; i < dim; i++)
+		out[i] = x[i] * val;
+}
+/*! @} */
+
+
+
+/*! @name Chord Length Method
+ *
+ * @{
+ */
+tsError
+ts_chord_lengths_length_to_knot(const tsReal *knots,
+                                const tsReal *lengths,
+                                size_t num,
+                                tsReal len,
+                                tsReal *knot,
+                                tsStatus *status)
+{
+	tsReal numer, denom, r, r_hat;
+	size_t idx, low, high;
+
+	/* Handle spacial cases. */
+	if (num == 0) { /* well... */
+		TS_RETURN_0(status, TS_NO_RESULT, "empty chord lengths")
+	}
+	if (num == 1) { /* no computation needed */
+		*knot = knots[0];
+		TS_RETURN_SUCCESS(status)
+	}
+	if (lengths[num - 1] < TS_LENGTH_ZERO) { /* spline is too short */
+		*knot = knots[0];
+		TS_RETURN_SUCCESS(status)
+	}
+	if (len <= lengths[0]) { /* clamp `len' to lower bound */
+		*knot = knots[0];
+		TS_RETURN_SUCCESS(status)
+	}
+	if (len >= lengths[num - 1]) { /* clamp `len' to upper bound */
+		*knot = knots[num - 1];
+		TS_RETURN_SUCCESS(status)
+	}
+
+	/* From now on: i) `len' is less than the last chord length in
+	   `lengths' and ii) `lengths' contains at least two values. Hence, the
+	   index (`idx') determined by the following binary search cannot be
+	   the last index in `knots' and `lengths', respectively (i.e., `idx <=
+	   num - 2`). It is therefore safe to access `knots' and `lengths' at
+	   index `idx + 1`. */
+
+	/* Binary search. Similar to how locating a knot within a knot vector
+	   is implemented in ::ts_int_bspline_find_knot. */
+	low = 0;
+	high = num - 1;
+	idx = (low+high) / 2;
+	while (len < lengths[idx] || len >= lengths[(idx) + 1]) {
+		if (len < lengths[idx]) high = idx;
+		else                     low  = idx;
+		idx = (low+high) / 2;
+	}
+
+	/* Determine `knot' by linear interpolation. */
+	denom = lengths[(idx) + 1] - lengths[idx];
+	if (denom < TS_LENGTH_ZERO) { /* segment is too short */
+		*knot = knots[idx];
+		TS_RETURN_SUCCESS(status)
+	}
+	numer = len - lengths[idx];
+	r = numer / denom; /* denom >= TS_LENGTH_ZERO */
+	r_hat = (tsReal) 1.0 - r;
+	*knot = r * knots[(idx) + 1] + r_hat * knots[idx];
+	TS_RETURN_SUCCESS(status)
+}
+
+tsError
+ts_chord_lengths_t_to_knot(const tsReal *knots,
+                           const tsReal *lengths,
+                           size_t num,
+                           tsReal t,
+                           tsReal *knot,
+                           tsStatus *status)
+{
+	/* Delegate error handling.  If `num' is `0`,
+	  `ts_chord_lengths_length_to_knot' doesn't read `len' at all. */
+	tsReal len = num == 0 ? 0 : t * lengths[num - 1];
+	return ts_chord_lengths_length_to_knot(knots,
+	                                       lengths,
+	                                       num,
+	                                       len,
+	                                       knot,
+	                                       status);
+}
+
+tsError
+ts_chord_lengths_equidistant_knot_seq(const tsReal *knots,
+                                      const tsReal *lengths,
+                                      size_t num,
+                                      size_t num_knot_seq,
+                                      tsReal *knot_seq,
+                                      tsStatus *status)
+{
+	tsError err;
+	size_t i;
+	tsReal t, knot;
+	if (num_knot_seq == 0) TS_RETURN_SUCCESS(status)
+	TS_TRY(try, err, status)
+		for (i = 0; i < num_knot_seq; i++) {
+			t = (tsReal) i / (num_knot_seq - 1);
+			TS_CALL(try, err, ts_chord_lengths_t_to_knot(
+			        knots, lengths, num, t, &knot, status))
+			knot_seq[i] = knot;
+		}
+		/* Set `knot_seq[0]` after `knot_seq[num_knot_seq - 1]` to
+		   ensure that `knot_seq[0] = min` if `num_knot_seq` is
+		   `1'. Note that `num_knot_seq` and `num` can't be `0'. */
+		knot_seq[num_knot_seq - 1] = knots[num - 1];
+		knot_seq[0] = knots[0];
+	TS_END_TRY_RETURN(err)
+}
+/*! @} */
+
+
+
+/*! @name Utility Functions
+ *
+ * @{
+ */
+int ts_knots_equal(tsReal x,
+                   tsReal y)
+{
+	return fabs(x-y) < TS_KNOT_EPSILON ? 1 : 0;
+}
+
+void ts_arr_fill(tsReal *arr,
+                 size_t num,
+                 tsReal val)
+{
+	size_t i;
+	for (i = 0; i < num; i++)
+		arr[i] = val;
+}
+
+tsReal ts_distance(const tsReal *x,
+                   const tsReal *y,
+                   size_t dim)
+{
+	size_t i;
+	tsReal sum = 0;
+	for (i = 0; i < dim; i++)
+		sum += (x[i] - y[i]) * (x[i] - y[i]);
+	return (tsReal) sqrt(sum);
+}
+/*! @} */
+
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif
diff --git a/projects/Roads/utilities/src/thirdparty/tinysplinecxx.cxx b/projects/Roads/utilities/src/thirdparty/tinysplinecxx.cxx
new file mode 100644
index 0000000000..b06d1e5fde
--- /dev/null
+++ b/projects/Roads/utilities/src/thirdparty/tinysplinecxx.cxx
@@ -0,0 +1,1744 @@
+#define TINYSPLINE_EXPORT
+#include "roads/thirdparty/tinysplinecxx.h"
+
+#include <stdlib.h>
+#include <cstring>
+#include <stdexcept>
+#include <cstdio>
+#include <sstream>
+
+/* Suppress some useless MSVC warnings. */
+#ifdef _MSC_VER
+#pragma warning(push)
+/* address of dllimport */
+#pragma warning(disable:4232)
+/* binding rvalues to non-const references */
+#pragma warning(disable:4350)
+/* unreferenced inline function */
+#pragma warning(disable:4514)
+/* function not inlined */
+#pragma warning(disable:4710)
+/* byte padding */
+#pragma warning(disable:4820)
+/* meaningless deprecation */
+#pragma warning(disable:4996)
+/* Spectre mitigation */
+#pragma warning(disable:5045)
+#endif
+
+
+
+/*! @name Swig Type Mapping
+ *
+ * See tinysplinecxx.h for more details.
+ *
+ * @{
+ */
+#ifdef SWIG
+#define std_real_vector_init(var) \
+	std_real_vector_out var = new std::vector<tinyspline::real>
+#define std_real_vector_read(var) var->
+#else
+#define std_real_vector_init(var) \
+	std_real_vector_out var
+#define std_real_vector_read(var) var.
+#endif
+/*! @} */
+
+
+
+/*! @name Vec2
+ *
+ * @{
+ */
+tinyspline::Vec2::Vec2()
+{
+	const real v = (real) 0.0;
+	ts_vec2_init(m_vals, v, v);
+}
+
+tinyspline::Vec2::Vec2(real x,
+                       real y)
+{
+	ts_vec2_init(m_vals, x, y);
+}
+
+tinyspline::Vec2
+tinyspline::Vec2::operator+(const Vec2 &other)
+{
+	return add(other);
+}
+
+tinyspline::Vec2
+tinyspline::Vec2::operator-(const Vec2 &other)
+{
+	return subtract(other);
+}
+
+tinyspline::Vec2
+tinyspline::Vec2::operator*(real scalar)
+{
+	return multiply(scalar);
+}
+
+tinyspline::real
+tinyspline::Vec2::x() const
+{
+	return m_vals[0];
+}
+
+void
+tinyspline::Vec2::setX(real val)
+{
+	m_vals[0] = val;
+}
+
+tinyspline::real
+tinyspline::Vec2::y() const
+{
+	return m_vals[1];
+}
+
+void
+tinyspline::Vec2::setY(real val)
+{
+	m_vals[1] = val;
+}
+
+std::vector<tinyspline::real>
+tinyspline::Vec2::values() const
+{
+	return std::vector<real>({ x(), y() });
+}
+
+tinyspline::Vec2
+tinyspline::Vec2::add(const Vec2 &other) const
+{
+	Vec2 vec;
+	ts_vec_add(m_vals, other.m_vals, 2, vec.m_vals);
+	return vec;
+}
+
+tinyspline::Vec2
+tinyspline::Vec2::subtract(const Vec2 &other) const
+{
+	Vec2 vec;
+	ts_vec_sub(m_vals, other.m_vals, 2, vec.m_vals);
+	return vec;
+}
+
+tinyspline::Vec2
+tinyspline::Vec2::multiply(real scalar) const
+{
+	Vec2 vec;
+	ts_vec_mul(m_vals, 2, scalar, vec.m_vals);
+	return vec;
+}
+
+tinyspline::Vec2
+tinyspline::Vec2::normalize() const
+{
+	Vec2 vec;
+	ts_vec_norm(m_vals, 2, vec.m_vals);
+	return vec;
+}
+
+tinyspline::real
+tinyspline::Vec2::magnitude() const
+{
+	return ts_vec_mag(m_vals, 2);
+}
+
+tinyspline::real
+tinyspline::Vec2::dot(const Vec2 &other) const
+{
+	return ts_vec_dot(m_vals, other.m_vals, 2);
+}
+
+tinyspline::real
+tinyspline::Vec2::angle(const Vec2 &other) const
+{
+	real buf[4];
+	return ts_vec_angle(m_vals, other.m_vals, buf, 2);
+}
+
+tinyspline::real
+tinyspline::Vec2::distance(const Vec2 &other) const
+{
+	return ts_distance(m_vals, other.m_vals, 2);
+}
+
+std::string
+tinyspline::Vec2::toString() const
+{
+	std::ostringstream oss;
+	oss << "Vec2{"
+	    << "x: " << x()
+	    << ", y: " << y()
+	    << "}";
+	return oss.str();
+}
+/*! @} */
+
+
+
+/*! @name Vec3
+ *
+ * @{
+ */
+tinyspline::Vec3::Vec3()
+{
+	const real v = (real) 0.0;
+	ts_vec3_init(m_vals, v, v, v);
+}
+
+tinyspline::Vec3::Vec3(real x,
+                       real y,
+                       real z)
+{
+	ts_vec3_init(m_vals, x, y, z);
+}
+
+tinyspline::Vec3
+tinyspline::Vec3::operator+(const Vec3 &other)
+{
+	return add(other);
+}
+
+tinyspline::Vec3
+tinyspline::Vec3::operator-(const Vec3 &other)
+{
+	return subtract(other);
+}
+
+tinyspline::Vec3
+tinyspline::Vec3::operator*(real scalar)
+{
+	return multiply(scalar);
+}
+
+tinyspline::real
+tinyspline::Vec3::x() const
+{
+	return m_vals[0];
+}
+
+void
+tinyspline::Vec3::setX(real val)
+{
+	m_vals[0] = val;
+}
+
+tinyspline::real
+tinyspline::Vec3::y() const
+{
+	return m_vals[1];
+}
+
+void
+tinyspline::Vec3::setY(real val)
+{
+	m_vals[1] = val;
+}
+
+tinyspline::real
+tinyspline::Vec3::z() const
+{
+	return m_vals[2];
+}
+
+void
+tinyspline::Vec3::setZ(real val)
+{
+	m_vals[2] = val;
+}
+
+std::vector<tinyspline::real>
+tinyspline::Vec3::values() const
+{
+	return std::vector<real>({ x(), y(), z() });
+}
+
+tinyspline::Vec3
+tinyspline::Vec3::add(const Vec3 &other) const
+{
+	Vec3 vec;
+	ts_vec_add(m_vals, other.m_vals, 3, vec.m_vals);
+	return vec;
+}
+
+tinyspline::Vec3
+tinyspline::Vec3::subtract(const Vec3 &other) const
+{
+	Vec3 vec;
+	ts_vec_sub(m_vals, other.m_vals, 3, vec.m_vals);
+	return vec;
+}
+
+tinyspline::Vec3
+tinyspline::Vec3::multiply(real scalar) const
+{
+	Vec3 vec;
+	ts_vec_mul(m_vals, 3, scalar, vec.m_vals);
+	return vec;
+}
+
+tinyspline::Vec3
+tinyspline::Vec3::cross(const Vec3 &other) const
+{
+	Vec3 vec;
+	ts_vec3_cross(m_vals, other.m_vals, vec.m_vals);
+	return vec;
+}
+
+tinyspline::Vec3
+tinyspline::Vec3::normalize() const
+{
+	Vec3 vec;
+	ts_vec_norm(m_vals, 3, vec.m_vals);
+	return vec;
+}
+
+tinyspline::real
+tinyspline::Vec3::magnitude() const
+{
+	return ts_vec_mag(m_vals, 3);
+}
+
+tinyspline::real
+tinyspline::Vec3::dot(const Vec3 &other) const
+{
+	return ts_vec_dot(m_vals, other.m_vals, 3);
+}
+
+tinyspline::real
+tinyspline::Vec3::angle(const Vec3 &other) const
+{
+	real buf[6];
+	return ts_vec_angle(m_vals, other.m_vals, buf, 3);
+}
+
+tinyspline::real
+tinyspline::Vec3::distance(const Vec3 &other) const
+{
+	return ts_distance(m_vals, other.m_vals, 3);
+}
+
+std::string
+tinyspline::Vec3::toString() const
+{
+	std::ostringstream oss;
+	oss << "Vec3{"
+	    << "x: " << x()
+	    << ", y: " << y()
+	    << ", z: " << z()
+	    << "}";
+	return oss.str();
+}
+/*! @} */
+
+
+
+/*! @name Vec4
+ *
+ * @{
+ */
+tinyspline::Vec4::Vec4()
+{
+	const real v = (real) 0.0;
+	ts_vec4_init(m_vals, v, v, v, v);
+}
+
+tinyspline::Vec4::Vec4(real x,
+                       real y,
+                       real z,
+                       real w)
+{
+	ts_vec4_init(m_vals, x, y, z, w);
+}
+
+tinyspline::Vec4
+tinyspline::Vec4::operator+(const Vec4 &other)
+{
+	return add(other);
+}
+
+tinyspline::Vec4
+tinyspline::Vec4::operator-(const Vec4 &other)
+{
+	return subtract(other);
+}
+
+tinyspline::Vec4
+tinyspline::Vec4::operator*(real scalar)
+{
+	return multiply(scalar);
+}
+
+tinyspline::real
+tinyspline::Vec4::x() const
+{
+	return m_vals[0];
+}
+
+void
+tinyspline::Vec4::setX(real val)
+{
+	m_vals[0] = val;
+}
+
+tinyspline::real
+tinyspline::Vec4::y() const
+{
+	return m_vals[1];
+}
+
+void
+tinyspline::Vec4::setY(real val)
+{
+	m_vals[1] = val;
+}
+
+tinyspline::real
+tinyspline::Vec4::z() const
+{
+	return m_vals[2];
+}
+
+void
+tinyspline::Vec4::setZ(real val)
+{
+	m_vals[2] = val;
+}
+
+tinyspline::real
+tinyspline::Vec4::w() const
+{
+	return m_vals[3];
+}
+
+void
+tinyspline::Vec4::setW(real val)
+{
+	m_vals[3] = val;
+}
+
+std::vector<tinyspline::real>
+tinyspline::Vec4::values() const
+{
+	return std::vector<real>({ x(), y(), z(), w() });
+}
+
+tinyspline::Vec4
+tinyspline::Vec4::add(const Vec4 &other) const
+{
+	Vec4 vec;
+	ts_vec_add(m_vals, other.m_vals, 4, vec.m_vals);
+	return vec;
+}
+
+tinyspline::Vec4
+tinyspline::Vec4::subtract(const Vec4 &other) const
+{
+	Vec4 vec;
+	ts_vec_sub(m_vals, other.m_vals, 4, vec.m_vals);
+	return vec;
+}
+
+tinyspline::Vec4
+tinyspline::Vec4::multiply(real scalar) const
+{
+	Vec4 vec;
+	ts_vec_mul(m_vals, 4, scalar, vec.m_vals);
+	return vec;
+}
+
+tinyspline::Vec4
+tinyspline::Vec4::normalize() const
+{
+	Vec4 vec;
+	ts_vec_norm(m_vals, 4, vec.m_vals);
+	return vec;
+}
+
+tinyspline::real
+tinyspline::Vec4::magnitude() const
+{
+	return ts_vec_mag(m_vals, 4);
+}
+
+tinyspline::real
+tinyspline::Vec4::dot(const Vec4 &other) const
+{
+	return ts_vec_dot(m_vals, other.m_vals, 4);
+}
+
+tinyspline::real
+tinyspline::Vec4::angle(const Vec4 &other) const
+{
+	real buf[8];
+	return ts_vec_angle(m_vals, other.m_vals, buf, 4);
+}
+
+tinyspline::real
+tinyspline::Vec4::distance(const Vec4 &other) const
+{
+	return ts_distance(m_vals, other.m_vals, 4);
+}
+
+std::string
+tinyspline::Vec4::toString() const
+{
+	std::ostringstream oss;
+	oss << "Vec4{"
+	    << "x: " << x()
+	    << ", y: " << y()
+	    << ", z: " << z()
+	    << ", w: " << w()
+	    << "}";
+	return oss.str();
+}
+/*! @} */
+
+
+
+/*! @name Frame
+ *
+ * @{
+ */
+tinyspline::Frame::Frame(Vec3 &position,
+                         Vec3 &tangent,
+                         Vec3 &normal,
+                         Vec3 &binormal)
+: m_position(position),
+  m_tangent(tangent),
+  m_normal(normal),
+  m_binormal(binormal)
+{}
+
+tinyspline::Vec3
+tinyspline::Frame::position() const
+{
+	return m_position;
+}
+
+tinyspline::Vec3
+tinyspline::Frame::tangent() const
+{
+	return m_tangent;
+}
+
+tinyspline::Vec3
+tinyspline::Frame::normal() const
+{
+	return m_normal;
+}
+
+tinyspline::Vec3
+tinyspline::Frame::binormal() const
+{
+	return m_binormal;
+}
+
+std::string
+tinyspline::Frame::toString() const
+{
+	std::ostringstream oss;
+	oss << "Frame{"
+	    << "position: " << position().toString()
+	    << ", tangent: " << tangent().toString()
+	    << ", normal: " << normal().toString()
+	    << ", binormal: " << binormal().toString()
+	    << "}";
+	return oss.str();
+}
+/*! @} */
+
+
+
+/*! @name FrameSeq
+ *
+ * @{
+ */
+tinyspline::FrameSeq::FrameSeq()
+: m_frames(nullptr), m_size(0)
+{}
+
+tinyspline::FrameSeq::FrameSeq(tsFrame *frames,
+                               size_t len)
+: m_frames(frames), m_size(len)
+{}
+
+tinyspline::FrameSeq::FrameSeq(const FrameSeq &other)
+: m_frames(nullptr), m_size(other.m_size)
+{
+	m_frames = new tsFrame[m_size];
+	std::copy(other.m_frames,
+	          other.m_frames + m_size,
+	          m_frames);
+}
+
+tinyspline::FrameSeq::FrameSeq(FrameSeq &&other)
+: m_frames(nullptr), m_size(other.m_size)
+{
+	m_frames = other.m_frames;
+	other.m_frames = nullptr;
+	other.m_size = 0;
+}
+
+tinyspline::FrameSeq::~FrameSeq()
+{
+	delete [] m_frames;
+	m_size = 0;
+}
+
+tinyspline::FrameSeq &
+tinyspline::FrameSeq::operator=(const FrameSeq &other)
+{
+	if (&other != this) {
+		tsFrame *data = new tsFrame[other.m_size];
+		std::copy(other.m_frames,
+		          other.m_frames + other.m_size,
+		          data);
+		delete [] m_frames;
+		m_frames = data;
+		m_size = other.m_size;
+	}
+	return *this;
+}
+
+tinyspline::FrameSeq &
+tinyspline::FrameSeq::operator=(FrameSeq &&other)
+{
+	if (&other != this) {
+		delete [] m_frames;
+		m_frames = other.m_frames;
+		m_size = other.m_size;
+		other.m_frames = nullptr;
+		other.m_size = 0;
+	}
+	return *this;
+}
+
+size_t
+tinyspline::FrameSeq::size() const
+{
+	return m_size;
+}
+
+tinyspline::Frame
+tinyspline::FrameSeq::at(size_t idx) const
+{
+	if (idx >= m_size)
+		throw std::out_of_range( "idx >= size");
+	tsFrame frame = m_frames[idx];
+	Vec3 position = Vec3(frame.position[0],
+	                     frame.position[1],
+	                     frame.position[2]);
+	Vec3 tangent = Vec3(frame.tangent[0],
+	                    frame.tangent[1],
+	                    frame.tangent[2]);
+	Vec3 normal = Vec3(frame.normal[0],
+	                   frame.normal[1],
+	                   frame.normal[2]);
+	Vec3 binormal = Vec3(frame.binormal[0],
+	                     frame.binormal[1],
+	                     frame.binormal[2]);
+	return Frame(position, tangent, normal, binormal);
+}
+
+std::string
+tinyspline::FrameSeq::toString() const
+{
+	std::ostringstream oss;
+	oss << "FrameSeq{"
+	    << "frames: " << size()
+	    << "}";
+	return oss.str();
+}
+/*! @} */
+
+
+
+/*! @name Domain
+ *
+ * @{
+ */
+tinyspline::Domain::Domain(real min,
+                           real max)
+: m_min(min), m_max(max)
+{}
+
+tinyspline::real
+tinyspline::Domain::min() const
+{
+	return m_min;
+}
+
+tinyspline::real
+tinyspline::Domain::max() const
+{
+	return m_max;
+}
+
+std::string
+tinyspline::Domain::toString() const
+{
+	std::ostringstream oss;
+        oss << "Domain{"
+            << "min: " << min()
+            << ", max: " << max()
+            << "}";
+	return oss.str();
+}
+/*! @} */
+
+
+
+/*! @name DeBoorNet
+ *
+ * @{
+ */
+tinyspline::DeBoorNet::DeBoorNet(tsDeBoorNet &data)
+: m_net(ts_deboornet_init())
+{
+	ts_deboornet_move(&data, &m_net);
+}
+
+tinyspline::DeBoorNet::DeBoorNet(const DeBoorNet &other)
+: m_net(ts_deboornet_init())
+{
+	tsStatus status;
+	if (ts_deboornet_copy(&other.m_net, &m_net, &status))
+		throw std::runtime_error(status.message);
+}
+
+tinyspline::DeBoorNet::DeBoorNet(DeBoorNet &&other)
+: m_net(ts_deboornet_init())
+{
+	ts_deboornet_move(&other.m_net, &m_net);
+}
+
+tinyspline::DeBoorNet::~DeBoorNet()
+{
+	ts_deboornet_free(&m_net);
+}
+
+tinyspline::DeBoorNet &
+tinyspline::DeBoorNet::operator=(const DeBoorNet &other)
+{
+	if (&other != this) {
+		tsDeBoorNet data = ts_deboornet_init();
+		tsStatus status;
+		if (ts_deboornet_copy(&other.m_net, &data, &status))
+			throw std::runtime_error(status.message);
+		ts_deboornet_free(&m_net);
+		ts_deboornet_move(&data, &m_net);
+	}
+	return *this;
+}
+
+tinyspline::DeBoorNet &
+tinyspline::DeBoorNet::operator=(DeBoorNet && other)
+{
+	if (&other != this) {
+		ts_deboornet_free(&m_net);
+		ts_deboornet_move(&other.m_net, &m_net);
+	}
+	return *this;
+}
+
+tinyspline::real
+tinyspline::DeBoorNet::knot() const
+{
+	return ts_deboornet_knot(&m_net);
+}
+
+size_t
+tinyspline::DeBoorNet::index() const
+{
+	return ts_deboornet_index(&m_net);
+}
+
+size_t
+tinyspline::DeBoorNet::multiplicity() const
+{
+	return ts_deboornet_multiplicity(&m_net);
+}
+
+size_t
+tinyspline::DeBoorNet::numInsertions() const
+{
+	return ts_deboornet_num_insertions(&m_net);
+}
+
+size_t
+tinyspline::DeBoorNet::dimension() const
+{
+	return ts_deboornet_dimension(&m_net);
+}
+
+std::vector<tinyspline::real>
+tinyspline::DeBoorNet::points() const
+{
+	const real *points = ts_deboornet_points_ptr(&m_net);
+	size_t len = ts_deboornet_len_points(&m_net);
+	return std::vector<real>(points, points + len);
+}
+
+std::vector<tinyspline::real>
+tinyspline::DeBoorNet::result() const
+{
+	const real *result = ts_deboornet_result_ptr(&m_net);
+	size_t len = ts_deboornet_len_result(&m_net);
+	return std::vector<real>(result, result + len);
+}
+
+tinyspline::Vec2
+tinyspline::DeBoorNet::resultVec2(size_t idx) const
+{
+	Vec4 vec4 = resultVec4(idx);
+	return Vec2(vec4.x(), vec4.y());
+}
+
+tinyspline::Vec3
+tinyspline::DeBoorNet::resultVec3(size_t idx) const
+{
+	Vec4 vec4 = resultVec4(idx);
+	return Vec3(vec4.x(), vec4.y(), vec4.z());
+}
+
+tinyspline::Vec4
+tinyspline::DeBoorNet::resultVec4(size_t idx) const
+{
+	if (idx >= ts_deboornet_num_result(&m_net))
+		throw std::out_of_range( "idx >= num(result)");
+	const real *result = ts_deboornet_result_ptr(&m_net);
+	real vals[4];
+	ts_vec4_set(vals, result + idx * dimension(), dimension());
+	return Vec4(vals[0], vals[1], vals[2], vals[3]);
+}
+
+std::string
+tinyspline::DeBoorNet::toString() const
+{
+	std::ostringstream oss;
+	oss << "DeBoorNet{"
+	    << "knot: " << knot()
+	    << ", index: " << index()
+	    << ", multiplicity: " << multiplicity()
+	    << ", insertions: " << numInsertions()
+	    << ", dimension: " << dimension()
+	    << ", points: " << ts_deboornet_num_points(&m_net)
+	    << "}";
+	return oss.str();
+}
+/*! @} */
+
+
+
+/*! @name BSpline
+ *
+ * @{
+ */
+tinyspline::BSpline::BSpline(tsBSpline &data)
+: m_spline(ts_bspline_init())
+{
+	ts_bspline_move(&data, &m_spline);
+}
+
+tinyspline::BSpline::BSpline()
+: m_spline(ts_bspline_init())
+{
+	tsStatus status;
+	if (ts_bspline_new_with_control_points(1,
+	                                       3,
+	                                       0,
+	                                       TS_CLAMPED,
+	                                       &m_spline,
+	                                       &status,
+	                                       0.0, 0.0, 0.0))
+		throw std::runtime_error(status.message);
+}
+
+tinyspline::BSpline::BSpline(const tinyspline::BSpline &other)
+: m_spline(ts_bspline_init())
+{
+	tsStatus status;
+	if (ts_bspline_copy(&other.m_spline, &m_spline, &status))
+		throw std::runtime_error(status.message);
+}
+
+tinyspline::BSpline::BSpline(BSpline &&other)
+: m_spline(ts_bspline_init())
+{
+	ts_bspline_move(&other.m_spline, &m_spline);
+}
+
+tinyspline::BSpline::BSpline(size_t numControlPoints,
+                             size_t dimension,
+                             size_t degree,
+                             Type type)
+: m_spline(ts_bspline_init())
+{
+	tsBSplineType c_type = TS_CLAMPED;
+	switch (type) {
+	case Opened:
+		c_type = TS_OPENED;
+		break;
+	case Clamped:
+		c_type = TS_CLAMPED;
+		break;
+	case Beziers:
+		c_type = TS_BEZIERS;
+		break;
+	default:
+		throw std::runtime_error("unknown type");
+	}
+	tsStatus status;
+	if (ts_bspline_new(numControlPoints,
+	                   dimension,
+	                   degree,
+	                   c_type,
+	                   &m_spline,
+	                   &status))
+		throw std::runtime_error(status.message);
+}
+
+tinyspline::BSpline::~BSpline()
+{
+	ts_bspline_free(&m_spline);
+}
+
+tinyspline::BSpline
+tinyspline::BSpline::interpolateCubicNatural(std_real_vector_in points,
+                                             size_t dimension)
+{
+	if (dimension == 0)
+		throw std::runtime_error("unsupported dimension: 0");
+	if (std_real_vector_read(points)size() % dimension != 0)
+		throw std::runtime_error("#points % dimension != 0");
+	tsBSpline data = ts_bspline_init();
+	tsStatus status;
+	if (ts_bspline_interpolate_cubic_natural(
+			std_real_vector_read(points)data(),
+			std_real_vector_read(points)size()/dimension,
+			dimension, &data, &status))
+		throw std::runtime_error(status.message);
+	return BSpline(data);
+}
+
+tinyspline::BSpline
+tinyspline::BSpline::interpolateCatmullRom(std_real_vector_in points,
+                                           size_t dimension,
+                                           real alpha,
+                                           std::vector<real> *first,
+                                           std::vector<real> *last,
+                                           real epsilon)
+{
+	if (dimension == 0)
+		throw std::runtime_error("unsupported dimension: 0");
+	if (std_real_vector_read(points)size() % dimension != 0)
+		throw std::runtime_error("#points % dimension != 0");
+	real *fst = nullptr;
+	if (first && first->size() >= dimension)
+		fst = first->data();
+	real *lst = nullptr;
+	if (last && last->size() >= dimension)
+		lst = last->data();
+	tsBSpline data = ts_bspline_init();
+	tsStatus status;
+	if (ts_bspline_interpolate_catmull_rom(
+			std_real_vector_read(points)data(),
+			std_real_vector_read(points)size()/dimension,
+			dimension, alpha, fst, lst, epsilon, &data,
+			&status))
+		throw std::runtime_error(status.message);
+	return BSpline(data);
+}
+
+tinyspline::BSpline
+tinyspline::BSpline::parseJson(std::string json)
+{
+	tsBSpline data = ts_bspline_init();
+	tsStatus status;
+	if (ts_bspline_parse_json(json.c_str(), &data, &status))
+		throw std::runtime_error(status.message);
+	return BSpline(data);
+}
+
+tinyspline::BSpline
+tinyspline::BSpline::load(std::string path)
+{
+	tsBSpline data = ts_bspline_init();
+	tsStatus status;
+	if (ts_bspline_load(path.c_str(), &data, &status))
+		throw std::runtime_error(status.message);
+	return BSpline(data);
+}
+
+bool
+tinyspline::BSpline::knotsEqual(real x, real y)
+{
+	return ts_knots_equal(x, y);
+}
+
+tinyspline::BSpline &
+tinyspline::BSpline::operator=(const BSpline &other)
+{
+	if (&other != this) {
+		tsBSpline data = ts_bspline_init();
+		tsStatus status;
+		if (ts_bspline_copy(&other.m_spline, &data, &status))
+			throw std::runtime_error(status.message);
+		ts_bspline_free(&m_spline);
+		ts_bspline_move(&data, &m_spline);
+	}
+	return *this;
+}
+
+tinyspline::BSpline &
+tinyspline::BSpline::operator=(BSpline &&other)
+{
+	if (&other != this) {
+		ts_bspline_free(&m_spline);
+		ts_bspline_move(&other.m_spline, &m_spline);
+	}
+	return *this;
+}
+
+tinyspline::DeBoorNet
+tinyspline::BSpline::operator()(real knot) const
+{
+	return eval(knot);
+}
+
+size_t
+tinyspline::BSpline::degree() const
+{
+	return ts_bspline_degree(&m_spline);
+}
+
+size_t
+tinyspline::BSpline::order() const
+{
+	return ts_bspline_order(&m_spline);
+}
+
+size_t
+tinyspline::BSpline::dimension() const
+{
+	return ts_bspline_dimension(&m_spline);
+}
+
+std::vector<tinyspline::real>
+tinyspline::BSpline::controlPoints() const
+{
+	const real *ctrlps = ts_bspline_control_points_ptr(&m_spline);
+	const size_t len = ts_bspline_len_control_points(&m_spline);
+	return std::vector<real>(ctrlps, ctrlps + len);
+}
+
+tinyspline::Vec2
+tinyspline::BSpline::controlPointVec2At(size_t idx) const
+{
+	const Vec4 vec4 = controlPointVec4At(idx);
+	return Vec2(vec4.x(), vec4.y());
+}
+
+tinyspline::Vec3
+tinyspline::BSpline::controlPointVec3At(size_t idx) const
+{
+	const Vec4 vec4 = controlPointVec4At(idx);
+	return Vec3(vec4.x(), vec4.y(), vec4.z());
+}
+
+tinyspline::Vec4
+tinyspline::BSpline::controlPointVec4At(size_t idx) const
+{
+	const real *ctrlp;
+	tsStatus status;
+	if (ts_bspline_control_point_at_ptr(&m_spline,
+	                                    idx,
+	                                    &ctrlp,
+	                                    &status))
+		throw std::runtime_error(status.message);
+	real vals[4];
+	ts_vec4_set(vals, ctrlp, dimension());
+	return Vec4(vals[0], vals[1], vals[2], vals[3]);
+}
+
+std::vector<tinyspline::real>
+tinyspline::BSpline::knots() const
+{
+	const real *knots = ts_bspline_knots_ptr(&m_spline);
+	size_t num = ts_bspline_num_knots(&m_spline);
+	return std::vector<real>(knots, knots + num);
+}
+
+tinyspline::real
+tinyspline::BSpline::knotAt(size_t idx) const
+{
+	real knot;
+	tsStatus status;
+	if (ts_bspline_knot_at(&m_spline, idx, &knot, &status))
+		throw std::runtime_error(status.message);
+	return knot;
+}
+
+size_t
+tinyspline::BSpline::numControlPoints() const
+{
+	return ts_bspline_num_control_points(&m_spline);
+}
+
+tinyspline::DeBoorNet
+tinyspline::BSpline::eval(real knot) const
+{
+	tsDeBoorNet net = ts_deboornet_init();
+	tsStatus status;
+	if (ts_bspline_eval(&m_spline, knot, &net, &status))
+		throw std::runtime_error(status.message);
+	return tinyspline::DeBoorNet(net);
+}
+
+tinyspline::std_real_vector_out
+tinyspline::BSpline::evalAll(std_real_vector_in knots) const
+{
+	const size_t num_knots = std_real_vector_read(knots)size();
+	const real *knots_ptr = std_real_vector_read(knots)data();
+	tinyspline::real *points;
+	tsStatus status;
+	if (ts_bspline_eval_all(&m_spline,
+	                        knots_ptr,
+	                        num_knots,
+	                        &points,
+	                        &status)) {
+		throw std::runtime_error(status.message);
+	}
+	real *first = points;
+	real *last = first + num_knots * dimension();
+	std_real_vector_init(vec)(first, last);
+	std::free(points);
+	return vec;
+}
+
+tinyspline::std_real_vector_out
+tinyspline::BSpline::sample(size_t num) const
+{
+	tinyspline::real *points;
+	size_t actualNum;
+	tsStatus status;
+	if (ts_bspline_sample(&m_spline,
+	                      num,
+	                      &points,
+	                      &actualNum,
+	                      &status)) {
+		throw std::runtime_error(status.message);
+	}
+	real *first = points;
+	real *last = first + actualNum * dimension();
+	std_real_vector_init(vec)(first, last);
+	std::free(points);
+	return vec;
+}
+
+tinyspline::DeBoorNet
+tinyspline::BSpline::bisect(real value,
+                            real epsilon,
+                            bool persnickety,
+                            size_t index,
+                            bool ascending,
+                            size_t maxIter) const
+{
+	tsDeBoorNet net = ts_deboornet_init();
+	tsStatus status;
+	if (ts_bspline_bisect(&m_spline,
+	                      value,
+	                      epsilon,
+	                      persnickety,
+	                      index,
+	                      ascending,
+	                      maxIter,
+	                      &net,
+	                      &status))
+		throw std::runtime_error(status.message);
+	return DeBoorNet(net);
+}
+
+tinyspline::Domain
+tinyspline::BSpline::domain() const
+{
+	real min, max;
+	ts_bspline_domain(&m_spline, &min, &max);
+	return Domain(min, max);
+}
+
+bool
+tinyspline::BSpline::isClosed(real epsilon) const
+{
+	int closed = 0;
+	tsStatus status;
+	if (ts_bspline_is_closed(&m_spline, epsilon, &closed, &status))
+		throw std::runtime_error(status.message);
+	return closed == 1;
+}
+
+tinyspline::FrameSeq
+tinyspline::BSpline::computeRMF(std_real_vector_in knots,
+                                tinyspline::Vec3 *firstNormal) const
+{
+	tsStatus status;
+	size_t num = std_real_vector_read(knots)size();
+	const real *knots_ptr = std_real_vector_read(knots)data();
+	tsFrame *frames = new tsFrame[num];
+	if (firstNormal && num > 0) {
+		ts_vec3_init(frames[0].normal,
+		             firstNormal->x(),
+		             firstNormal->y(),
+		             firstNormal->z());
+	}
+	if (ts_bspline_compute_rmf(&m_spline,
+	                           knots_ptr,
+	                           num,
+	                           firstNormal != nullptr,
+	                           frames,
+	                           &status))
+		throw std::runtime_error(status.message);
+	FrameSeq seq = FrameSeq(frames, num);
+	return seq;
+}
+
+
+tinyspline::BSpline
+tinyspline::BSpline::subSpline(real knot0, real knot1) const
+{
+	tsBSpline data = ts_bspline_init();
+	tsStatus status;
+	if (ts_bspline_sub_spline(&m_spline,
+	                          knot0,
+	                          knot1,
+	                          &data,
+	                          &status))
+		throw std::runtime_error(status.message);
+	return BSpline(data);
+}
+
+tinyspline::std_real_vector_out
+tinyspline::BSpline::uniformKnotSeq(size_t num) const
+{
+	std_real_vector_init(knots)(num);
+	real *knots_ptr = std_real_vector_read(knots)data();
+	ts_bspline_uniform_knot_seq(&m_spline, num, knots_ptr);
+	return knots;
+}
+
+tinyspline::std_real_vector_out
+tinyspline::BSpline::equidistantKnotSeq(size_t num,
+                                        size_t numSamples) const
+{
+	tsStatus status;
+	std_real_vector_init(knots)(num);
+	real *knots_ptr = std_real_vector_read(knots)data();
+	if (ts_bspline_equidistant_knot_seq(&m_spline,
+	                                    num,
+	                                    knots_ptr,
+	                                    numSamples,
+	                                    &status)) {
+#ifdef SWIG
+		delete knots;
+#endif
+		throw std::runtime_error(status.message);
+	}
+	return knots;
+}
+
+tinyspline::ChordLengths
+tinyspline::BSpline::chordLengths(std_real_vector_in knots) const
+{
+	tsStatus status;
+	size_t num = std_real_vector_read(knots)size();
+	real *knotsArr = new real[num];
+	real *lengths = new real[num];
+	std::copy(std_real_vector_read(knots)begin(),
+	          std_real_vector_read(knots)end(),
+	          knotsArr);
+	if (ts_bspline_chord_lengths(&m_spline,
+	                             knotsArr,
+	                             num,
+	                             lengths,
+	                             &status))
+		throw std::runtime_error(status.message);
+	return ChordLengths(*this, knotsArr, lengths, num);
+}
+
+tinyspline::ChordLengths
+tinyspline::BSpline::chordLengths(size_t numSamples) const
+{
+	return chordLengths(uniformKnotSeq(numSamples));
+}
+
+std::string
+tinyspline::BSpline::toJson() const
+{
+	char *json;
+	tsStatus status;
+	if (ts_bspline_to_json(&m_spline, &json, &status))
+		throw std::runtime_error(status.message);
+	std::string string(json);
+	std::free(json);
+	return string;
+}
+
+void
+tinyspline::BSpline::save(std::string path) const
+{
+	tsStatus status;
+	if (ts_bspline_save(&m_spline, path.c_str(), &status))
+		throw std::runtime_error(status.message);
+}
+
+void
+tinyspline::BSpline::setControlPoints(
+	const std::vector<tinyspline::real> &ctrlp)
+{
+	size_t expected = ts_bspline_len_control_points(&m_spline);
+	size_t actual = ctrlp.size();
+	if (expected != actual) {
+		std::ostringstream oss;
+		oss << "Expected size: " << expected
+		    << ", Actual size: " << actual;
+		throw std::runtime_error(oss.str());
+	}
+	tsStatus status;
+	if (ts_bspline_set_control_points(&m_spline, ctrlp.data(), &status))
+		throw std::runtime_error(status.message);
+}
+
+void
+tinyspline::BSpline::setControlPointVec2At(size_t idx, Vec2 &cp)
+{
+	Vec4 vec4(cp.x(), cp.y(), (real) 0.0, (real) 0.0);
+	setControlPointVec4At(idx, vec4);
+}
+
+void
+tinyspline::BSpline::setControlPointVec3At(size_t idx, Vec3 &cp)
+{
+	Vec4 vec4(cp.x(), cp.y(), cp.z(), (real) 0.0);
+	setControlPointVec4At(idx, vec4);
+}
+
+void
+tinyspline::BSpline::setControlPointVec4At(size_t idx, Vec4 &cp)
+{
+	std::vector<real> vals(dimension());
+	for (size_t i = 0; i < vals.size(); i++)
+		vals[i] = (real) 0.0;
+	if (vals.size() >= 4) vals[3] = cp.w();
+	if (vals.size() >= 3) vals[2] = cp.z();
+	if (vals.size() >= 2) vals[1] = cp.y();
+	if (vals.size() >= 1) vals[0] = cp.x();
+	tsStatus status;
+	if (ts_bspline_set_control_point_at(&m_spline,
+	                                    idx,
+	                                    vals.data(),
+	                                    &status))
+		throw std::runtime_error(status.message);
+}
+
+void
+tinyspline::BSpline::setKnots(const std::vector<real> &knots)
+{
+	size_t expected = ts_bspline_num_knots(&m_spline);
+	size_t actual = knots.size();
+	if (expected != actual) {
+		std::ostringstream oss;
+		oss << "Expected size: " << expected
+		    << ", Actual size: " << actual;
+		throw std::runtime_error(oss.str());
+	}
+	tsStatus status;
+	if (ts_bspline_set_knots(&m_spline,
+	                         knots.data(),
+	                         &status))
+		throw std::runtime_error(status.message);
+}
+
+void
+tinyspline::BSpline::setKnotAt(size_t idx, real knot)
+{
+	tsStatus status;
+	if (ts_bspline_set_knot_at(&m_spline, idx, knot, &status))
+		throw std::runtime_error(status.message);
+}
+
+tinyspline::BSpline
+tinyspline::BSpline::insertKnot(real knot, size_t num) const
+{
+	tsBSpline data = ts_bspline_init();
+	size_t k;
+	tsStatus status;
+	if (ts_bspline_insert_knot(&m_spline, knot, num, &data, &k, &status))
+		throw std::runtime_error(status.message);
+	return BSpline(data);
+}
+
+tinyspline::BSpline
+tinyspline::BSpline::split(real knot) const
+{
+	tsBSpline data = ts_bspline_init();
+	size_t k;
+	tsStatus status;
+	if (ts_bspline_split(&m_spline, knot, &data, &k, &status))
+		throw std::runtime_error(status.message);
+	return BSpline(data);
+}
+
+tinyspline::BSpline
+tinyspline::BSpline::tension(real beta) const
+{
+	tsBSpline data = ts_bspline_init();
+	tsStatus status;
+	if (ts_bspline_tension(&m_spline, beta, &data, &status))
+		throw std::runtime_error(status.message);
+	return BSpline(data);
+}
+
+tinyspline::BSpline
+tinyspline::BSpline::toBeziers() const
+{
+	tsBSpline data = ts_bspline_init();
+	tsStatus status;
+	if (ts_bspline_to_beziers(&m_spline, &data, &status))
+		throw std::runtime_error(status.message);
+	return BSpline(data);
+}
+
+tinyspline::BSpline
+tinyspline::BSpline::derive(size_t num,
+                            real eps) const
+{
+	tsBSpline data = ts_bspline_init();
+	tsStatus status;
+	if (ts_bspline_derive(&m_spline, num, eps, &data, &status))
+		throw std::runtime_error(status.message);
+	return BSpline(data);
+}
+
+tinyspline::BSpline
+tinyspline::BSpline::elevateDegree(size_t amount,
+                                   real eps) const
+{
+	tsBSpline data = ts_bspline_init();
+	tsStatus status;
+	if (ts_bspline_elevate_degree(&m_spline, amount, eps, &data, &status))
+		throw std::runtime_error(status.message);
+	return BSpline(data);
+}
+
+tinyspline::BSpline
+tinyspline::BSpline::alignWith(const BSpline &other,
+                               BSpline &otherAligned,
+                               real eps) const
+{
+	tsBSpline data = ts_bspline_init();
+	tsBSpline deleteIf_Other_And_OtherAligned_AreDifferent =
+		otherAligned.m_spline;
+	tsStatus status;
+	if (ts_bspline_align(&m_spline,
+	                     &other.m_spline,
+	                     eps,
+	                     &data,
+	                     &otherAligned.m_spline, &status))
+		throw std::runtime_error(status.message);
+	if (&other != &otherAligned)
+		ts_bspline_free(&deleteIf_Other_And_OtherAligned_AreDifferent);
+	return BSpline(data);
+}
+
+tinyspline::Morphism
+tinyspline::BSpline::morphTo(const BSpline &other,
+                             real eps) const
+{
+	return Morphism(*this, other, eps);
+}
+
+std::string tinyspline::BSpline::toString() const
+{
+	Domain d = domain();
+	std::ostringstream oss;
+	oss << "BSpline{"
+	    << "dimension: " << dimension()
+	    << ", degree: " << degree()
+	    << ", domain: [" << d.min() << ", " << d.max() << "]"
+	    << ", control points: " << numControlPoints()
+	    << ", knots: " << ts_bspline_num_knots(&m_spline)
+	    << "}";
+	return oss.str();
+}
+/*! @} */
+
+
+
+/*! @name Morphism
+ *
+ * @{
+ */
+tinyspline::Morphism::Morphism(const BSpline &origin,
+			       const BSpline &target,
+			       real epsilon)
+: m_origin(origin), m_target(target), m_epsilon(epsilon)
+{
+	m_originAligned = origin.alignWith(target, m_targetAligned, epsilon);
+	// Make buffer compatible by copying one of the aligned splines.
+	m_buffer = m_originAligned;
+}
+
+tinyspline::BSpline
+tinyspline::Morphism::eval(real t)
+{
+	tsStatus status;
+	if (t <= 0) return m_origin;
+	if (t >= 1) return m_target;
+	if (ts_bspline_morph(&m_originAligned.m_spline,
+			     &m_targetAligned.m_spline,
+			     t, m_epsilon,
+			     &m_buffer.m_spline, &status)) {
+		throw std::runtime_error(status.message);
+	}
+	return m_buffer;
+}
+
+tinyspline::BSpline
+tinyspline::Morphism::origin() const
+{
+	return m_origin;
+}
+
+tinyspline::BSpline
+tinyspline::Morphism::target() const
+{
+	return m_target;
+}
+
+tinyspline::real
+tinyspline::Morphism::epsilon() const
+{
+	return m_epsilon;
+}
+
+tinyspline::BSpline
+tinyspline::Morphism::operator()(real t)
+{
+	return eval(t);
+}
+
+std::string tinyspline::Morphism::toString() const
+{
+	std::ostringstream oss;
+	oss << "Morphism{"
+	    << "buffer: " << m_buffer.toString()
+	    << ", epsilon: " << epsilon()
+	    << "}";
+	return oss.str();
+}
+/*! @} */
+
+
+
+/*! @name ChordLenghts
+ * @{
+ */
+tinyspline::ChordLengths::ChordLengths()
+: m_spline(),
+  m_knots(nullptr),
+  m_lengths(nullptr),
+  m_size(0)
+{}
+
+tinyspline::ChordLengths::ChordLengths(const BSpline &spline,
+                                       real *knots,
+                                       real *lengths,
+                                       size_t size)
+: m_spline(spline),
+  m_knots(knots),
+  m_lengths(lengths),
+  m_size(size)
+{}
+
+tinyspline::ChordLengths::ChordLengths(const ChordLengths &other)
+: m_spline(other.m_spline),
+  m_knots(nullptr),
+  m_lengths(nullptr),
+  m_size(other.m_size)
+{
+	m_knots = new real[m_size];
+	std::copy(other.m_knots,
+	          other.m_knots + m_size,
+	          m_knots);
+	m_lengths = new real[m_size];
+	std::copy(other.m_lengths,
+	          other.m_lengths + m_size,
+	          m_lengths);
+}
+
+tinyspline::ChordLengths::ChordLengths(ChordLengths &&other)
+: m_spline(),
+  m_knots(nullptr),
+  m_lengths(nullptr),
+  m_size(0)
+{
+	*this = std::move(other);
+}
+
+tinyspline::ChordLengths::~ChordLengths()
+{
+	delete [] m_knots;
+	delete [] m_lengths;
+	m_size = 0;
+}
+
+tinyspline::ChordLengths &
+tinyspline::ChordLengths::operator=(const ChordLengths &other)
+{
+	if (&other != this) {
+		real *knots = new real[other.m_size];
+		std::copy(other.m_knots,
+		          other.m_knots + other.m_size,
+		          knots);
+		real *lengths = new real[other.m_size];
+		std::copy(other.m_lengths,
+		          other.m_lengths + other.m_size,
+		          lengths);
+		delete [] m_knots;
+		delete [] m_lengths;
+		m_spline = other.m_spline;
+		m_knots = knots;
+		m_lengths = lengths;
+		m_size = other.m_size;
+	}
+	return *this;
+}
+
+tinyspline::ChordLengths &
+tinyspline::ChordLengths::operator=(ChordLengths &&other)
+{
+	if (&other != this) {
+		delete [] m_knots;
+		delete [] m_lengths;
+		m_spline = other.m_spline;
+		m_knots = other.m_knots;
+		m_lengths = other.m_lengths;
+		m_size = other.m_size;
+		other.m_spline = BSpline();
+		other.m_knots = nullptr;
+		other.m_lengths = nullptr;
+		other.m_size = 0;
+	}
+	return *this;
+}
+
+tinyspline::BSpline
+tinyspline::ChordLengths::spline() const
+{
+	return m_spline;
+}
+
+std::vector<tinyspline::real>
+tinyspline::ChordLengths::knots() const
+{
+	return std::vector<real>(m_knots,
+	                         m_knots + m_size);
+}
+
+std::vector<tinyspline::real>
+tinyspline::ChordLengths::lengths() const
+{
+	return std::vector<real>(m_lengths,
+	                         m_lengths + m_size);
+}
+
+std::vector<tinyspline::real>
+tinyspline::ChordLengths::values() const
+{
+	return lengths();
+}
+
+size_t
+tinyspline::ChordLengths::size() const
+{
+	return m_size;
+}
+
+tinyspline::real
+tinyspline::ChordLengths::arcLength() const
+{
+	return m_size == 0 ? 0 : m_lengths[m_size - 1];
+}
+
+tinyspline::real
+tinyspline::ChordLengths::lengthToKnot(real len) const
+{
+	tsStatus status;
+	real knot;
+	if (ts_chord_lengths_length_to_knot(m_knots,
+	                               m_lengths,
+	                               m_size,
+	                               len,
+	                               &knot,
+	                               &status))
+		throw std::runtime_error(status.message);
+	return knot;
+}
+
+tinyspline::real
+tinyspline::ChordLengths::tToKnot(real t) const
+{
+	tsStatus status;
+	real knot;
+	if (ts_chord_lengths_t_to_knot(m_knots,
+	                               m_lengths,
+	                               m_size,
+	                               t,
+	                               &knot,
+	                               &status))
+		throw std::runtime_error(status.message);
+	return knot;
+}
+
+tinyspline::std_real_vector_out
+tinyspline::ChordLengths::equidistantKnotSeq(size_t num) const
+{
+	tsStatus status;
+	std_real_vector_init(knots)(num);
+	real *knots_ptr = std_real_vector_read(knots)data();
+	if (ts_chord_lengths_equidistant_knot_seq(m_knots,
+	                                          m_lengths,
+	                                          m_size,
+	                                          num,
+	                                          knots_ptr,
+	                                          &status)) {
+#ifdef SWIG
+		delete knots;
+#endif
+		throw std::runtime_error(status.message);
+	}
+	return knots;
+}
+
+std::string
+tinyspline::ChordLengths::toString() const
+{
+	std::ostringstream oss;
+	oss << "ChordLengths{"
+	    << "spline: " << m_spline.toString()
+	    << ", values: " << m_size
+	    << "}";
+	return oss.str();
+}
+/*! @} */
+
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif

From 57f78bc264116b2c304b8ad0e41e0539c5039046 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Tue, 29 Aug 2023 16:59:08 +0800
Subject: [PATCH 30/50] feat: using catmull-rom spline

---
 projects/Roads/utilities/src/grid.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index d4ebbbfc96..9dd56600f3 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -136,7 +136,7 @@ tinyspline::BSpline spline::GenerateBSplineFromSegment(const ArrayList<std::arra
     }
     Points.insert(std::end(Points), { float(InPoints[Segments[Segments.size() - 1][1]][0]), float(InPoints[Segments[Segments.size() - 1][1]][1]), float(InPoints[Segments[Segments.size() - 1][1]][2]) });
 
-    return BSpline::interpolateCubicNatural(Points, 3);
+    return BSpline::interpolateCatmullRom(Points, 3);
 }
 
 ArrayList<Eigen::Vector3f> spline::GenerateAndSamplePointsFromSegments(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments, int32_t SamplePoints) {
@@ -144,11 +144,12 @@ ArrayList<Eigen::Vector3f> spline::GenerateAndSamplePointsFromSegments(const Arr
     float Step = 1.0f / float(SamplePoints);
 
     ArrayList<Eigen::Vector3f> Result;
-    Result.reserve(SamplePoints);
+    Result.resize(SamplePoints);
 
+#pragma omp parallel for
     for (int32_t i = 0; i < SamplePoints; i++) {
         auto Point = Spline.eval(float(i) * Step).resultVec3();
-        Result.push_back( Eigen::Vector3f { Point.x(), Point.y(), Point.z() } );
+        Result[i] = Eigen::Vector3f { Point.x(), Point.y(), Point.z() };
     }
 
     return Result;

From 23d8e35e88515842884c8493a921739c770721ce Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Tue, 29 Aug 2023 18:31:14 +0800
Subject: [PATCH 31/50] feat: add refine node

---
 projects/Roads/nodes/src/cost.cpp             | 34 +++++++++++++++-
 projects/Roads/utilities/include/roads/grid.h |  6 ++-
 projects/Roads/utilities/src/grid.cpp         | 40 ++++++++++++++-----
 3 files changed, 66 insertions(+), 14 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 1446356855..d2950c8952 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -13,6 +13,8 @@
 #include <queue>
 #include <stack>
 
+#include "roads/thirdparty/tinysplinecxx.h"
+
 template<typename... Args>
 inline void RoadsAssert(const bool Expr, const std::string &InMsg = "[Roads] Assert Failed", Args... args) {
     if (!Expr) {
@@ -56,6 +58,16 @@ namespace zeno::reflect {
 
 }// namespace zeno::reflect
 
+namespace zeno {
+    struct RoadBSplineObject : public IObject {
+
+        explicit RoadBSplineObject(const tinyspline::BSpline& Lhs): Spline(Lhs) {}
+        explicit RoadBSplineObject(tinyspline::BSpline&& LhsToMove) : Spline(std::forward<tinyspline::BSpline&&>(LhsToMove)) {}
+
+        tinyspline::BSpline Spline;
+    };
+}// namespace zeno
+
 namespace {
     using namespace zeno;
     using namespace roads;
@@ -423,6 +435,9 @@ namespace {
         int SampleNum = 1;
         ZENO_DECLARE_INPUT_FIELD(SampleNum, "Sample Points", false, "", "1000");
 
+        std::shared_ptr<zeno::RoadBSplineObject> Spline;
+        ZENO_DECLARE_OUTPUT_FIELD(Spline, "Spline");
+
         void apply() override {
             auto& Prim = AutoParameter->Primitive;
 
@@ -430,9 +445,10 @@ namespace {
             ArrayList<std::array<int, 2>> Lines(Prim->lines.begin(), Prim->lines.end());
 
             ROADS_TIMING_PRE_GENERATED;
-            ROADS_TIMING_BLOCK("Resample segments", auto Result = spline::SmoothAndResampleSegments(Vertices, Lines, AutoParameter->SampleNum));
+            ROADS_TIMING_BLOCK("Spline Creation", auto Spline = spline::GenerateBSplineFromSegment(Vertices, Lines));
+            ROADS_TIMING_BLOCK("Resample segments", auto Result = spline::GenerateAndSamplePointsFromSegments(Spline, AutoParameter->SampleNum));
 
-            //auto Lines = ArrayList<Eigen::Vector3f>{};
+            AutoParameter->Spline = std::make_shared<zeno::RoadBSplineObject>(Spline);
 
             Prim = std::make_shared<zeno::PrimitiveObject>();
 
@@ -450,6 +466,20 @@ namespace {
         }
     };
 
+    struct ZENO_CRTP(PrimRefineWithSpline, zeno::reflect::IParameterAutoNode) {
+        ZENO_GENERATE_NODE_BODY(PrimRefineWithSpline);
+
+        std::shared_ptr<zeno::PrimitiveObject> Primitive;
+        ZENO_DECLARE_INPUT_FIELD(Primitive, "Prim");
+        ZENO_DECLARE_OUTPUT_FIELD(Primitive, "Prim");
+
+        std::shared_ptr<zeno::RoadBSplineObject> Spline;
+        ZENO_DECLARE_INPUT_FIELD(Spline, "Spline");
+
+        void apply() override {
+        }
+    };
+
 #if _MSC_VER
 #include "Windows.h"
     // Windows Debug
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index 65e49b7391..8362e16a48 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -232,9 +232,11 @@ namespace roads {
         class tinyspline::BSpline GenerateBSplineFromSegment(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments);
 
         //template<typename PointContainerType = ArrayList<std::array<float, 3>>, typename SegmentContainerType = ArrayList<std::array<int, 2>>>
-        ArrayList<Eigen::Vector3f> GenerateAndSamplePointsFromSegments(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments, int32_t SamplePoints = 1000);
+        ArrayList<Eigen::Vector3f> GenerateAndSamplePointsFromSegments(const class tinyspline::BSpline& Spline, int32_t SamplePoints = 1000);
 
-        ArrayList<Eigen::Vector3f> SmoothAndResampleSegments(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments, int32_t SamplePoints = 1000);
+        float Distance(const Eigen::Vector3d &point, const tinyspline::BSpline &bSpline, float t);
+
+        float FindNearestPoint(const Eigen::Vector3d &point, const tinyspline::BSpline &bSpline, float& t, float step = 0.01, float tolerance = 1e-6);
     }// namespace spline
 
 }// namespace roads
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index 9dd56600f3..51e726b843 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -122,25 +122,20 @@ double roads::energy::CalculateStepSize(const Point2D &Pt, const Point2D &P, con
     return 0.0;
 }
 
-ArrayList<Eigen::Vector3f> spline::SmoothAndResampleSegments(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments, int32_t SamplePoints) {
-    return GenerateAndSamplePointsFromSegments(InPoints, Segments, SamplePoints);
-}
-
 tinyspline::BSpline spline::GenerateBSplineFromSegment(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments) {
     using namespace tinyspline;
 
     ArrayList<float> Points;
 
-    for (const auto& Seg : Segments) {
-        Points.insert(std::end(Points), { float(InPoints[Seg[0]][0]), float(InPoints[Seg[0]][1]), float(InPoints[Seg[0]][2]) });
+    for (const auto &Seg: Segments) {
+        Points.insert(std::end(Points), {float(InPoints[Seg[0]][0]), float(InPoints[Seg[0]][1]), float(InPoints[Seg[0]][2])});
     }
-    Points.insert(std::end(Points), { float(InPoints[Segments[Segments.size() - 1][1]][0]), float(InPoints[Segments[Segments.size() - 1][1]][1]), float(InPoints[Segments[Segments.size() - 1][1]][2]) });
+    Points.insert(std::end(Points), {float(InPoints[Segments[Segments.size() - 1][1]][0]), float(InPoints[Segments[Segments.size() - 1][1]][1]), float(InPoints[Segments[Segments.size() - 1][1]][2])});
 
     return BSpline::interpolateCatmullRom(Points, 3);
 }
 
-ArrayList<Eigen::Vector3f> spline::GenerateAndSamplePointsFromSegments(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments, int32_t SamplePoints) {
-    auto Spline = GenerateBSplineFromSegment(InPoints, Segments);
+ArrayList<Eigen::Vector3f> spline::GenerateAndSamplePointsFromSegments(const class tinyspline::BSpline& Spline, int32_t SamplePoints) {
     float Step = 1.0f / float(SamplePoints);
 
     ArrayList<Eigen::Vector3f> Result;
@@ -149,8 +144,33 @@ ArrayList<Eigen::Vector3f> spline::GenerateAndSamplePointsFromSegments(const Arr
 #pragma omp parallel for
     for (int32_t i = 0; i < SamplePoints; i++) {
         auto Point = Spline.eval(float(i) * Step).resultVec3();
-        Result[i] = Eigen::Vector3f { Point.x(), Point.y(), Point.z() };
+        Result[i] = Eigen::Vector3f{Point.x(), Point.y(), Point.z()};
     }
 
     return Result;
 }
+
+float spline::Distance(const Eigen::Vector3d &point, const tinyspline::BSpline &bSpline, float t) {
+    tinyspline::DeBoorNet net = bSpline.eval(t);
+    auto Result = net.resultVec3();
+
+    Eigen::Vector3d splinePoint(Result.x(), Result.y(), Result.z());
+    return float((splinePoint - point).norm());
+}
+
+float spline::FindNearestPoint(const Eigen::Vector3d &point, const tinyspline::BSpline &bSpline, float& t, float step, float tolerance) {
+    // initial guess
+    t = 0.0;
+    while (step > tolerance) {
+        float f_current = Distance(point, bSpline, t);
+        float f_next = Distance(point, bSpline, t + step);
+
+        if (f_next < f_current) {
+            t += step;
+        } else {
+            step *= 0.5;
+        }
+    }
+
+    return t;
+}

From cbf0822b05c9aea3caa01e320751ce8ab439bf0b Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Wed, 30 Aug 2023 16:04:17 +0800
Subject: [PATCH 32/50] feat: add KDTree

---
 projects/Roads/nodes/src/cost.cpp             |  4 +-
 .../Roads/utilities/include/roads/kdtree.h    | 38 ++++++++
 .../Roads/utilities/include/roads/roads.h     |  1 +
 projects/Roads/utilities/src/kdtree.cpp       | 91 +++++++++++++++++++
 4 files changed, 132 insertions(+), 2 deletions(-)
 create mode 100644 projects/Roads/utilities/include/roads/kdtree.h
 create mode 100644 projects/Roads/utilities/src/kdtree.cpp

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index d2950c8952..06dbce4f73 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -466,8 +466,8 @@ namespace {
         }
     };
 
-    struct ZENO_CRTP(PrimRefineWithSpline, zeno::reflect::IParameterAutoNode) {
-        ZENO_GENERATE_NODE_BODY(PrimRefineWithSpline);
+    struct ZENO_CRTP(RoadsPrimRefineWithSpline, zeno::reflect::IParameterAutoNode) {
+        ZENO_GENERATE_NODE_BODY(RoadsPrimRefineWithSpline);
 
         std::shared_ptr<zeno::PrimitiveObject> Primitive;
         ZENO_DECLARE_INPUT_FIELD(Primitive, "Prim");
diff --git a/projects/Roads/utilities/include/roads/kdtree.h b/projects/Roads/utilities/include/roads/kdtree.h
new file mode 100644
index 0000000000..3c49dce40f
--- /dev/null
+++ b/projects/Roads/utilities/include/roads/kdtree.h
@@ -0,0 +1,38 @@
+#pragma once
+
+#include "Eigen/Dense"
+#include "pch.h"
+#include <algorithm>
+#include <memory>
+#include <type_traits>
+#include <utility>
+
+namespace roads {
+    using namespace Eigen;
+
+    struct KDTreeNode {
+        VectorXf Point;
+        std::shared_ptr<KDTreeNode> Left;
+        std::shared_ptr<KDTreeNode> Right;
+
+        explicit KDTreeNode(VectorXf Value);
+    };
+
+    class KDTree {
+        std::shared_ptr<KDTreeNode> Root;
+
+        std::shared_ptr<KDTreeNode> BuildKdTree_Impl(ArrayList<VectorXf> &Data, uint32_t Lower, uint32_t Upper, uint32_t Depth);
+
+        std::shared_ptr<KDTreeNode> Insert_Impl(const std::shared_ptr<KDTreeNode> &Node, VectorXf Point, uint32_t Depth);
+
+        void SearchNode(KDTreeNode* Node, VectorXf Point, float Radius, ArrayList<VectorXf>& OutPoints, uint32_t Depth = 0);
+
+    public:
+        void Insert(VectorXf Point);
+
+        ArrayList<VectorXf> SearchRadius(const VectorXf& Point, float Radius);
+
+        static std::shared_ptr<KDTree> BuildKdTree(ArrayList<VectorXf> Data);
+    };
+
+}// namespace roads
diff --git a/projects/Roads/utilities/include/roads/roads.h b/projects/Roads/utilities/include/roads/roads.h
index 5f1d2ab3ee..2e554477be 100644
--- a/projects/Roads/utilities/include/roads/roads.h
+++ b/projects/Roads/utilities/include/roads/roads.h
@@ -2,3 +2,4 @@
 
 #include "pch.h"
 #include "grid.h"
+#include "kdtree.h"
diff --git a/projects/Roads/utilities/src/kdtree.cpp b/projects/Roads/utilities/src/kdtree.cpp
new file mode 100644
index 0000000000..9653c9c09f
--- /dev/null
+++ b/projects/Roads/utilities/src/kdtree.cpp
@@ -0,0 +1,91 @@
+
+#include "roads/kdtree.h"
+
+using namespace roads;
+using namespace Eigen;
+
+KDTreeNode::KDTreeNode(VectorXf Value) : Point(std::move(Value)), Left(nullptr), Right(nullptr) {}
+
+std::shared_ptr<KDTreeNode> KDTree::BuildKdTree_Impl(ArrayList<VectorXf> &Data, uint32_t Lower, uint32_t Upper, uint32_t Depth) {
+    if (Lower >= Upper || Data.empty()) {
+        return nullptr;
+    }
+
+    // check all data, they must have same dimension.
+    const uint32_t Dim = Data[0].rows();
+    if (Depth == 0 && std::any_of(std::begin(Data), std::end(Data), [Dim] (const VectorXf& Value) { return Value.rows() != Dim; })) {
+        return nullptr;
+    }
+
+    uint32_t Axis = Depth % Data[0].rows();
+
+    std::sort(std::begin(Data) + Lower, std::begin(Data) + Upper, [Axis] (const VectorXf &a, const VectorXf &b) {
+        return a[Axis] < b[Axis];
+    });
+
+    uint32_t MedianIndex = (Lower + Upper) / 2;
+    auto Node = std::make_shared<KDTreeNode>(Data[MedianIndex]);
+
+    Node->Left = BuildKdTree_Impl(Data, Lower, MedianIndex, Depth + 1);
+    Node->Right = BuildKdTree_Impl(Data, MedianIndex + 1, Upper, Depth + 1);
+
+    return Node;
+}
+
+std::shared_ptr<KDTree> KDTree::BuildKdTree(ArrayList<VectorXf> Data) {
+    std::shared_ptr<KDTree> NewTree = std::make_shared<KDTree>();
+    NewTree->Root = NewTree->BuildKdTree_Impl(Data, 0, Data.size(), 0);
+
+    if (!NewTree->Root) {
+        throw std::invalid_argument("[Roads] KdTree built with invalid arguments.");
+    }
+
+    return NewTree;
+}
+
+void KDTree::Insert(VectorXf Point) {
+    Root = Insert_Impl(Root, std::move(Point), 0);
+}
+
+std::shared_ptr<KDTreeNode> KDTree::Insert_Impl(const std::shared_ptr<KDTreeNode> &Node, VectorXf Point, uint32_t Depth) {
+    if (!Node) {
+        return std::make_shared<KDTreeNode>(Point);
+    }
+
+    uint32_t Axis = Depth % Point.rows();
+
+    if (Point[Axis] < Node->Point[Axis]) {
+        Node->Left = Insert_Impl(Node->Left, Point, Depth + 1);
+    } else {
+        Node->Right = Insert_Impl(Node->Right, Point, Depth + 1);
+    }
+
+    return Node;
+}
+
+void KDTree::SearchNode(KDTreeNode *Node, VectorXf Point, float Radius, ArrayList<VectorXf> &OutPoints, uint32_t Depth) {
+    if (nullptr == Node) return;
+
+    uint32_t Axis = Depth % Point.rows();
+
+    float Distance = (Node->Point - Point).norm();
+
+    if (Distance <= Radius) {
+        OutPoints.push_back(Node->Point);
+    }
+
+    if (Node->Point[Axis] >= Point[Axis] - Radius) {
+        SearchNode(Node->Left.get(), Point, Radius, OutPoints, Depth + 1);
+    }
+    if (Node->Point[Axis] <= Point[Axis] + Radius) {
+        SearchNode(Node->Right.get(), Point, Radius, OutPoints, Depth + 1);
+    }
+}
+
+ArrayList<VectorXf> KDTree::SearchRadius(const VectorXf &Point, float Radius) {
+    ArrayList<VectorXf> Result;
+
+    SearchNode(Root.get(), Point, Radius, Result);
+
+    return Result;
+}

From 6ffd6d3bc81922362fa30bb05cd336537830ba78 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Thu, 31 Aug 2023 13:01:19 +0800
Subject: [PATCH 33/50] fix: abi compatibility...

---
 projects/Roads/nodes/src/cost.cpp             | 32 ++++++--
 projects/Roads/utilities/include/roads/grid.h | 34 ---------
 .../Roads/utilities/include/roads/kdtree.h    | 10 ++-
 .../Roads/utilities/include/roads/roads.h     |  1 -
 projects/Roads/utilities/src/grid.cpp         | 74 -------------------
 projects/Roads/utilities/src/kdtree.cpp       | 42 +++++------
 6 files changed, 51 insertions(+), 142 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 06dbce4f73..b855b91789 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -7,6 +7,7 @@
 #include "zeno/utils/PropertyVisitor.h"
 #include "zeno/utils/logger.h"
 #include "zeno/zeno.h"
+#include <Eigen/Core>
 #include <iostream>
 #include <limits>
 #include <numeric>
@@ -466,17 +467,34 @@ namespace {
         }
     };
 
-    struct ZENO_CRTP(RoadsPrimRefineWithSpline, zeno::reflect::IParameterAutoNode) {
-        ZENO_GENERATE_NODE_BODY(RoadsPrimRefineWithSpline);
+    struct ZENO_CRTP(RoadsPrimRefineWithLine, zeno::reflect::IParameterAutoNode) {
+        ZENO_GENERATE_NODE_BODY(RoadsPrimRefineWithLine);
 
-        std::shared_ptr<zeno::PrimitiveObject> Primitive;
-        ZENO_DECLARE_INPUT_FIELD(Primitive, "Prim");
-        ZENO_DECLARE_OUTPUT_FIELD(Primitive, "Prim");
+        std::shared_ptr<zeno::PrimitiveObject> Mesh;
+        ZENO_DECLARE_INPUT_FIELD(Mesh, "MeshPrim");
+        ZENO_DECLARE_OUTPUT_FIELD(Mesh, "MeshPrim");
 
-        std::shared_ptr<zeno::RoadBSplineObject> Spline;
-        ZENO_DECLARE_INPUT_FIELD(Spline, "Spline");
+        std::shared_ptr<zeno::PrimitiveObject> Lines;
+        ZENO_DECLARE_INPUT_FIELD(Lines, "LinePrim");
 
         void apply() override {
+            ArrayList<Eigen::VectorXf> Result;
+
+            auto& Points = AutoParameter->Mesh->verts;
+            auto& LineVertices = AutoParameter->Lines->verts;
+            auto& Lines = AutoParameter->Lines->lines;
+
+            Result.reserve(Points.size());
+            for (auto& p : Points) {
+                Eigen::VectorXf Point(3);
+
+                Point(0) = p[0];
+                Point(1) = p[1];
+                Point(2) = p[2];
+
+                Result.push_back(Point);
+            }
+
         }
     };
 
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index 8362e16a48..64c9b41171 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -47,7 +47,6 @@ namespace roads {
 
     using EdgeWeightProperty = boost::property<boost::edge_weight_t, double>;
     using WeightedGridUndirectedGraph = boost::adjacency_list<boost::listS, boost::vecS, boost::directedS, boost::no_property, EdgeWeightProperty>;
-    using WeightedGridUndirectedGraphIterator = boost::graph_traits<WeightedGridUndirectedGraph>::edge_iterator;
     using Edge = std::pair<size_t, size_t>;
 
     ROADS_API ROADS_INLINE double EuclideanDistance(const Point &Point1, const Point &Point2);
@@ -56,40 +55,7 @@ namespace roads {
 
     ROADS_API DynamicGrid<SlopePoint> CalculateSlope(const DynamicGrid<HeightPoint> &InHeightField);
 
-    ROADS_API WeightedGridUndirectedGraph CreateWeightGraphFromCostGrid(
-        const DynamicGrid<CostPoint> &InCostGrid, ConnectiveType Type, const std::function<double(double)> &HeightMappingFunc = [](double v) { return v; }, const std::function<double(double)> &GradientMappingFunc = [](double v) { return v; }, const std::function<double(double)> &CurvatureMappingFunc = [](double v) { return v; });
-
-    ROADS_API ArrayList<ArrayList<double>> FloydWarshallShortestPath(WeightedGridUndirectedGraph &InGraph);
-
-    template<typename T>
-    ROADS_API void AverageSmooth(ArrayList<T> &InOutContainer, size_t Iteration = 1) {
-        auto num_vertices = InOutContainer.size();
-        ArrayList<float> NewHeights(num_vertices);
-
-        for (unsigned int iteration = 0; iteration < 4; ++iteration) {
-#pragma omp parallel for
-            for (size_t i = 0; i < num_vertices; ++i) {
-                // Include the vertex itself and its neighbors.
-                ArrayList<T *> Points = InOutContainer[i].Neighbors;
-                Points.push_back(&InOutContainer[i]);
-
-                // Calculate the average height.
-                NewHeights[i] = std::accumulate(begin(Points), end(Points), 0.0f,
-                                                [](float sum, const T *v) { return sum + v->Height; }) /
-                                Points.size();
-            }
-
-            // Update the heights for the next iteration or the final result.
-#pragma omp parallel for
-            for (size_t i = 0; i < num_vertices; ++i) {
-                InOutContainer[i].Height = NewHeights[i];
-            }
-        }
-    }
-
     namespace energy {
-        ROADS_API double CalculateStepSize(const Point2D &Pt, const Point2D &P, const Point2D &PrevX, const Point2D &CurrX);
-
         template<typename IntLikeT>
         inline IntLikeT GreatestCommonDivisor(IntLikeT i, IntLikeT j) {
             if (0 == j) {
diff --git a/projects/Roads/utilities/include/roads/kdtree.h b/projects/Roads/utilities/include/roads/kdtree.h
index 3c49dce40f..0de1fd0c07 100644
--- a/projects/Roads/utilities/include/roads/kdtree.h
+++ b/projects/Roads/utilities/include/roads/kdtree.h
@@ -10,18 +10,20 @@
 namespace roads {
     using namespace Eigen;
 
-    struct KDTreeNode {
+    struct KDTreeNode : std::enable_shared_from_this<KDTreeNode> {
         VectorXf Point;
         std::shared_ptr<KDTreeNode> Left;
         std::shared_ptr<KDTreeNode> Right;
 
         explicit KDTreeNode(VectorXf Value);
+
+        virtual ~KDTreeNode() = default;
     };
 
     class KDTree {
-        std::shared_ptr<KDTreeNode> Root;
+        std::shared_ptr<KDTreeNode> Root = nullptr;
 
-        std::shared_ptr<KDTreeNode> BuildKdTree_Impl(ArrayList<VectorXf> &Data, uint32_t Lower, uint32_t Upper, uint32_t Depth);
+        static KDTreeNode* BuildKdTree_Impl(ArrayList<VectorXf> Data, int64_t Lower, int64_t Upper, uint32_t Depth);
 
         std::shared_ptr<KDTreeNode> Insert_Impl(const std::shared_ptr<KDTreeNode> &Node, VectorXf Point, uint32_t Depth);
 
@@ -32,7 +34,7 @@ namespace roads {
 
         ArrayList<VectorXf> SearchRadius(const VectorXf& Point, float Radius);
 
-        static std::shared_ptr<KDTree> BuildKdTree(ArrayList<VectorXf> Data);
+        static KDTree* BuildKdTree(const ArrayList<VectorXf>& Data);
     };
 
 }// namespace roads
diff --git a/projects/Roads/utilities/include/roads/roads.h b/projects/Roads/utilities/include/roads/roads.h
index 2e554477be..5f1d2ab3ee 100644
--- a/projects/Roads/utilities/include/roads/roads.h
+++ b/projects/Roads/utilities/include/roads/roads.h
@@ -2,4 +2,3 @@
 
 #include "pch.h"
 #include "grid.h"
-#include "kdtree.h"
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index 51e726b843..5be1126612 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -48,80 +48,6 @@ DynamicGrid<SlopePoint> roads::CalculateSlope(const DynamicGrid<HeightPoint> &In
     return Result;
 }
 
-IntPoint2D InterpolatePoints(const IntPoint2D &A, const IntPoint2D &B, float t) {
-    IntPoint2D result;
-    result[0] = long(std::round(float(A[0]) + t * float(B[0] - A[0])));
-    result[1] = long(std::round(float(A[1]) + t * float(B[1] - A[1])));
-    return result;
-}
-
-WeightedGridUndirectedGraph roads::CreateWeightGraphFromCostGrid(const DynamicGrid<CostPoint> &InCostGrid, const ConnectiveType Type, const std::function<double(double)> &HeightMappingFunc, const std::function<double(double)> &GradientMappingFunc, const std::function<double(double)> &CurvatureMappingFunc) {
-    ArrayList<IntPoint2D> Directions = {{0, -1}, {0, 1}, {-1, 0}, {1, 0}};
-    if (Type >= ConnectiveType::EIGHT) {
-        Directions.insert(Directions.end(), {{-1, -1}, {1, -1}, {-1, 1}, {1, 1}});
-    }
-    if (Type >= ConnectiveType::SIXTEEN) {
-        Directions.insert(Directions.end(), {{-1, -2}, {1, -2}, {-2, -1}, {2, -1}, {-2, 1}, {2, 1}, {-1, 2}, {1, 2}});
-    }
-    if (Type >= ConnectiveType::FOURTY) {
-        // Directions.insert(Directions.end(), { { -2, -2 }, { -2, -1 }, { -2, 0 }, { -2, 1 }, { -2, 2 }, { -1, -2 }, { -1, 2 }, { 0, -2 }, { 0, 2 }, { 1, -2 }, { 1, 2 }, { 2, -2 }, { 2, -1 }, { 2, 0 }, { 2, 1 }, { 2, 2 } });
-        size_t original_size = Directions.size();
-        for (size_t i = 0; i + 1 < original_size; ++i) {
-            Directions.push_back(InterpolatePoints(Directions[i], Directions[i + 1], 1.0 / 3.0));
-            Directions.push_back(InterpolatePoints(Directions[i], Directions[i + 1], 2.0 / 3.0));
-        }
-    }
-
-    WeightedGridUndirectedGraph NewGraph{InCostGrid.size()};
-
-    // boost graph library seem not provide thread safe
-#pragma omp parallel for
-    for (int32_t y = 0; y < InCostGrid.Ny; ++y) {
-        for (int32_t x = 0; x < InCostGrid.Nx; ++x) {
-            const size_t OriginIdx = y * InCostGrid.Nx + x;
-            boost::property_map<WeightedGridUndirectedGraph, boost::edge_weight_t>::type WeightMap = boost::get(boost::edge_weight, NewGraph);
-            for (auto &Direction: Directions) {
-                const size_t ix = x + Direction[0];
-                const size_t iy = y + Direction[1];
-                if (ix >= InCostGrid.Nx || iy >= InCostGrid.Ny) continue;
-                const size_t TargetIdx = iy * InCostGrid.Nx + ix;
-                using EdgeDescriptor = boost::graph_traits<WeightedGridUndirectedGraph>::edge_descriptor;
-                auto [edge1, _] = boost::add_edge(OriginIdx, TargetIdx, NewGraph);
-                auto [edge2, _2] = boost::add_edge(TargetIdx, OriginIdx, NewGraph);
-                //                WeightMap[edge1] = std::pow(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] > 0 ? std::min(InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 20.0, InCostGrid[TargetIdx] - InCostGrid[OriginIdx] - 10.0) : InCostGrid[TargetIdx] - InCostGrid[OriginIdx], 2);
-                //                WeightMap[edge2] = std::pow(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] > 0 ? std::min(InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 20.0, InCostGrid[OriginIdx] - InCostGrid[TargetIdx] - 10.0) : InCostGrid[OriginIdx] - InCostGrid[TargetIdx], 2);
-                WeightMap[edge1] =
-                    (HeightMappingFunc(InCostGrid[TargetIdx].Height - InCostGrid[OriginIdx].Height + 1.0) + GradientMappingFunc(InCostGrid[TargetIdx].Gradient - InCostGrid[OriginIdx].Gradient + 1.0)
-                     //                    + CurvatureMappingFunc(InCostGrid[TargetIdx].Curvature - InCostGrid[OriginIdx].Curvature + 1.0)
-                    );
-                WeightMap[edge2] =
-                    (HeightMappingFunc(InCostGrid[OriginIdx].Height - InCostGrid[TargetIdx].Height + 1.0) + GradientMappingFunc(InCostGrid[OriginIdx].Gradient - InCostGrid[TargetIdx].Gradient + 1.0)
-                     //                    + CurvatureMappingFunc(InCostGrid[OriginIdx].Curvature - InCostGrid[TargetIdx].Curvature + 1.0)
-                    );
-            }
-        }
-    }
-
-    return NewGraph;
-}
-
-ArrayList<ArrayList<double>> roads::FloydWarshallShortestPath(WeightedGridUndirectedGraph &InGraph) {
-    ArrayList<ArrayList<double>> D{InGraph.m_vertices.size()};
-    ArrayList<double> d(InGraph.m_vertices.size(), (std::numeric_limits<double>::max)());
-    printf("%llu", InGraph.m_vertices.size());
-    boost::floyd_warshall_all_pairs_shortest_paths(InGraph, D, boost::distance_map(&d[0]));
-    return D;
-}
-
-double roads::energy::CalculateStepSize(const Point2D &Pt, const Point2D &P, const Point2D &PrevX, const Point2D &CurrX) {
-    const double LengthPtToXc = EuclideanDistance(Pt, CurrX);
-    const double LengthPtToP = EuclideanDistance(Pt, P);
-    const double LengthPrevXToP = EuclideanDistance(PrevX, P);
-    const double LengthPrevXToCurrX = EuclideanDistance(PrevX, CurrX);
-
-    return 0.0;
-}
-
 tinyspline::BSpline spline::GenerateBSplineFromSegment(const ArrayList<std::array<float, 3>> &InPoints, const ArrayList<std::array<int, 2>> &Segments) {
     using namespace tinyspline;
 
diff --git a/projects/Roads/utilities/src/kdtree.cpp b/projects/Roads/utilities/src/kdtree.cpp
index 9653c9c09f..73d4b01a39 100644
--- a/projects/Roads/utilities/src/kdtree.cpp
+++ b/projects/Roads/utilities/src/kdtree.cpp
@@ -1,40 +1,38 @@
 
 #include "roads/kdtree.h"
+#include <stack>
+#include <utility>
 
 using namespace roads;
 using namespace Eigen;
 
-KDTreeNode::KDTreeNode(VectorXf Value) : Point(std::move(Value)), Left(nullptr), Right(nullptr) {}
+KDTreeNode::KDTreeNode(VectorXf Value) : Point(std::move(Value)) {}
 
-std::shared_ptr<KDTreeNode> KDTree::BuildKdTree_Impl(ArrayList<VectorXf> &Data, uint32_t Lower, uint32_t Upper, uint32_t Depth) {
-    if (Lower >= Upper || Data.empty()) {
+KDTreeNode* KDTree::BuildKdTree_Impl(ArrayList<VectorXf> Data, int64_t Lower, int64_t Upper, uint32_t Depth) {
+    if (Lower >= Upper) {
         return nullptr;
     }
 
-    // check all data, they must have same dimension.
-    const uint32_t Dim = Data[0].rows();
-    if (Depth == 0 && std::any_of(std::begin(Data), std::end(Data), [Dim] (const VectorXf& Value) { return Value.rows() != Dim; })) {
-        return nullptr;
-    }
-
-    uint32_t Axis = Depth % Data[0].rows();
-
-    std::sort(std::begin(Data) + Lower, std::begin(Data) + Upper, [Axis] (const VectorXf &a, const VectorXf &b) {
-        return a[Axis] < b[Axis];
-    });
-
-    uint32_t MedianIndex = (Lower + Upper) / 2;
-    auto Node = std::make_shared<KDTreeNode>(Data[MedianIndex]);
+    int64_t Axis = Depth % Data[0].size();
+    auto MiddleIter = Data.begin() + Lower + (Upper - Lower) / 2;
+    std::nth_element(
+        Data.begin() + Lower, MiddleIter, Data.begin() + Upper,
+        [Axis](const Eigen::VectorXf &a, const Eigen::VectorXf &b) {
+            return a[Axis] < b[Axis];
+        });
+    MiddleIter = Data.begin() + Lower + (Upper - Lower) / 2;
 
-    Node->Left = BuildKdTree_Impl(Data, Lower, MedianIndex, Depth + 1);
-    Node->Right = BuildKdTree_Impl(Data, MedianIndex + 1, Upper, Depth + 1);
+    auto* Node = new KDTreeNode(*MiddleIter);
+    Node->Left = std::shared_ptr<KDTreeNode>(BuildKdTree_Impl(Data, Lower, MiddleIter - Data.begin(), Depth + 1));
+    Node->Right = std::shared_ptr<KDTreeNode>(BuildKdTree_Impl(Data, MiddleIter - Data.begin() + 1, Upper, Depth + 1));
 
     return Node;
 }
 
-std::shared_ptr<KDTree> KDTree::BuildKdTree(ArrayList<VectorXf> Data) {
-    std::shared_ptr<KDTree> NewTree = std::make_shared<KDTree>();
-    NewTree->Root = NewTree->BuildKdTree_Impl(Data, 0, Data.size(), 0);
+KDTree* KDTree::BuildKdTree(const ArrayList<VectorXf>& Data) {
+    auto* NewTree = new KDTree;
+    auto* Root = (BuildKdTree_Impl(Data, 0, int64_t(Data.size() - 1), 0));
+    NewTree->Root = std::shared_ptr<KDTreeNode>(Root);
 
     if (!NewTree->Root) {
         throw std::invalid_argument("[Roads] KdTree built with invalid arguments.");

From 584d509a287178d84e8b614bea7d67be2261c639 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Thu, 31 Aug 2023 16:44:35 +0800
Subject: [PATCH 34/50] feat: add road mask node

---
 projects/Roads/nodes/src/cost.cpp             | 101 +++++++++++++----
 projects/Roads/utilities/include/roads/grid.h |   2 +
 .../Roads/utilities/include/roads/kdtree.h    |  35 ++++++
 .../Roads/utilities/include/roads/roads.h     |   1 +
 projects/Roads/utilities/src/grid.cpp         |  36 +++++-
 projects/Roads/utilities/src/kdtree.cpp       | 103 ++++++++++++++++++
 6 files changed, 255 insertions(+), 23 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index b855b91789..1742a92822 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -1,5 +1,6 @@
+#include "boost/geometry.hpp"
+#include "boost/geometry/index/rtree.hpp"
 #include "boost/graph/astar_search.hpp"
-#include "boost/graph/dijkstra_shortest_paths.hpp"
 #include "roads/roads.h"
 #include "zeno/PrimitiveObject.h"
 #include "zeno/types/CurveObject.h"
@@ -59,11 +60,37 @@ namespace zeno::reflect {
 
 }// namespace zeno::reflect
 
+namespace boost::geometry {
+    template<>
+    struct point_type<zeno::vec3f> {
+        using type = zeno::vec3f;
+    };
+}// namespace boost::geometry
+
+namespace boost::geometry::index {
+    template<typename Ptr>
+    struct indexable<Ptr *> {
+
+        typedef Ptr *V;
+        typedef Ptr const &result_type;
+
+        result_type operator()(V const &v) const { return *v; }
+    };
+
+    template<typename Box>
+    struct indexable<std::shared_ptr<Box>> {
+        typedef std::shared_ptr<Box> V;
+
+        typedef Box const &result_type;
+        result_type operator()(V const &v) const { return *v; }
+    };
+}// namespace boost::geometry::index
+
 namespace zeno {
     struct RoadBSplineObject : public IObject {
 
-        explicit RoadBSplineObject(const tinyspline::BSpline& Lhs): Spline(Lhs) {}
-        explicit RoadBSplineObject(tinyspline::BSpline&& LhsToMove) : Spline(std::forward<tinyspline::BSpline&&>(LhsToMove)) {}
+        explicit RoadBSplineObject(const tinyspline::BSpline &Lhs) : Spline(Lhs) {}
+        explicit RoadBSplineObject(tinyspline::BSpline &&LhsToMove) : Spline(std::forward<tinyspline::BSpline &&>(LhsToMove)) {}
 
         tinyspline::BSpline Spline;
     };
@@ -440,7 +467,7 @@ namespace {
         ZENO_DECLARE_OUTPUT_FIELD(Spline, "Spline");
 
         void apply() override {
-            auto& Prim = AutoParameter->Primitive;
+            auto &Prim = AutoParameter->Primitive;
 
             ArrayList<std::array<float, 3>> Vertices(Prim->verts.begin(), Prim->verts.end());
             ArrayList<std::array<int, 2>> Lines(Prim->lines.begin(), Prim->lines.end());
@@ -454,13 +481,13 @@ namespace {
             Prim = std::make_shared<zeno::PrimitiveObject>();
 
             Prim->verts.reserve(Result.size());
-            for (const auto& line : Result) {
+            for (const auto &line: Result) {
                 Prim->verts.emplace_back(line.x(), line.y(), line.z());
             }
 
             Prim->lines.resize(Result.size() - 1);
             for (int i = 0; i < Result.size() - 1; i++) {
-                Prim->lines[i] = zeno::vec2i { i, i + 1 };
+                Prim->lines[i] = zeno::vec2i{i, i + 1};
             }
 
             zeno::log_info("[Roads] Vertices Num: {}, Lines Num: {}", Prim->verts.size(), Prim->lines.size());
@@ -471,30 +498,60 @@ namespace {
         ZENO_GENERATE_NODE_BODY(RoadsPrimRefineWithLine);
 
         std::shared_ptr<zeno::PrimitiveObject> Mesh;
-        ZENO_DECLARE_INPUT_FIELD(Mesh, "MeshPrim");
-        ZENO_DECLARE_OUTPUT_FIELD(Mesh, "MeshPrim");
+        ZENO_DECLARE_INPUT_FIELD(Mesh, "Mesh Prim");
+        ZENO_DECLARE_OUTPUT_FIELD(Mesh, "Mesh Prim");
 
-        std::shared_ptr<zeno::PrimitiveObject> Lines;
-        ZENO_DECLARE_INPUT_FIELD(Lines, "LinePrim");
+        //std::shared_ptr<zeno::PrimitiveObject> Lines;
+        //ZENO_DECLARE_INPUT_FIELD(Lines, "Line Prim");
+
+        std::shared_ptr<zeno::RoadBSplineObject> Spline;
+        ZENO_DECLARE_INPUT_FIELD(Spline, "Spline");
+
+        int32_t RoadWidth = 3;
+        ZENO_DECLARE_INPUT_FIELD(RoadWidth, "Road Radius", false, "", "5");
+
+        std::string SizeXChannel;
+        ZENO_DECLARE_INPUT_FIELD(SizeXChannel, "Nx Channel (UserData)", false, "", "nx");
+
+        int Nx = 0;
+        ZENO_BINDING_PRIMITIVE_USERDATA(Mesh, Nx, SizeXChannel, false);
 
         void apply() override {
-            ArrayList<Eigen::VectorXf> Result;
+            using namespace boost::geometry;
 
-            auto& Points = AutoParameter->Mesh->verts;
-            auto& LineVertices = AutoParameter->Lines->verts;
-            auto& Lines = AutoParameter->Lines->lines;
+            //            using PointType = model::point<float, 3, cs::cartesian>;
+            //            using BoxType = model::box<PointType>;
+            //            using BoxPtr = std::shared_ptr<BoxType>;
 
-            Result.reserve(Points.size());
-            for (auto& p : Points) {
-                Eigen::VectorXf Point(3);
+            //            index::rtree<BoxPtr, index::linear<128, 4>> RTree;
 
-                Point(0) = p[0];
-                Point(1) = p[1];
-                Point(2) = p[2];
+            //auto& LineVertices = AutoParameter->Lines->verts;
+            //auto& Lines = AutoParameter->Lines->lines;
 
-                Result.push_back(Point);
-            }
+            //            for (const auto& p : Points) {
+            //                PointType Point { p[0], p[1], p[2] };
+            //                BoxPtr b = std::make_shared<BoxType>( Point, Point );
+            //                RTree.insert(b);
+            //            }
+            //
+            //            for (const auto& Seg : Lines) {
+            //                PointType a { LineVertices[Seg[0]][0], LineVertices[Seg[0]][1], LineVertices[Seg[0]][2] };
+            //                std::vector<BoxPtr> n;
+            //                RTree.query(index::nearest(PointType(0, 0, 0), 5), std::back_inserter(n));
+            //                for (const auto& a : n) {
+            //                    std::cout << a->min_corner().get<0>() << ", " << a->min_corner().get<1>() << ", " << a->min_corner().get<2>() << std::endl;
+            //                }
+            //            }
+
+            auto &Points = AutoParameter->Mesh->verts;
+
+            std::vector<std::array<float, 3>> New(Points.begin(), Points.end());
+
+            tinyspline::BSpline& SplineQwQ = AutoParameter->Spline->Spline;
+            auto DistanceAttr = spline::CalcRoadMask(New, SplineQwQ, AutoParameter->RoadWidth, AutoParameter->Nx);
 
+            auto& DisAttr = AutoParameter->Mesh->verts.add_attr<float>("roadDis");
+            DisAttr.swap(DistanceAttr);
         }
     };
 
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index 64c9b41171..c4d4cfaf73 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -203,6 +203,8 @@ namespace roads {
         float Distance(const Eigen::Vector3d &point, const tinyspline::BSpline &bSpline, float t);
 
         float FindNearestPoint(const Eigen::Vector3d &point, const tinyspline::BSpline &bSpline, float& t, float step = 0.01, float tolerance = 1e-6);
+
+        ArrayList<float> CalcRoadMask(const std::vector<std::array<float, 3>>& Points, const tinyspline::BSpline& SplineQwQ, int32_t Width, int32_t Nx);
     }// namespace spline
 
 }// namespace roads
diff --git a/projects/Roads/utilities/include/roads/kdtree.h b/projects/Roads/utilities/include/roads/kdtree.h
index 0de1fd0c07..9dd8e8190d 100644
--- a/projects/Roads/utilities/include/roads/kdtree.h
+++ b/projects/Roads/utilities/include/roads/kdtree.h
@@ -37,4 +37,39 @@ namespace roads {
         static KDTree* BuildKdTree(const ArrayList<VectorXf>& Data);
     };
 
+
+    class Octree {
+    public:
+        using Point3D = Eigen::Vector3f;
+
+    private:
+        class Node {
+        public:
+            Point3D* point;
+            std::array<float, 6> bounds; // minX, maxX, minY, maxY, minZ, maxZ
+            std::array<std::unique_ptr<Node>, 8> children;
+
+            explicit Node(std::array<float, 6>& bounds): bounds(bounds), point(nullptr), children({nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr}) {}
+        };
+
+        std::unique_ptr<Node> root;
+        unsigned int maxDepth;
+
+    public:
+        Octree(float minX, float maxX, float minY, float maxY, float minZ, float maxZ, unsigned int maxDepth);
+
+        void addPoint(Point3D* point);
+
+        std::vector<Point3D*> findNearestNeighbours(float x, float y, float z, unsigned int k);
+
+        std::vector<Point3D*> findPointsInRadius(float x, float y, float z, float radius);
+
+    private:
+        void findNearestNeighbours(Node* node, Point3D* target, std::vector<Point3D*>& nearestNeighbours, unsigned int depth);
+
+        void addPoint(Node* node, Point3D* point, unsigned int depth);
+
+        void findPointsInRadius(Node* node, Point3D* center, float radius, std::vector<Point3D*>& pointsInRadius);
+    };
+
 }// namespace roads
diff --git a/projects/Roads/utilities/include/roads/roads.h b/projects/Roads/utilities/include/roads/roads.h
index 5f1d2ab3ee..2e554477be 100644
--- a/projects/Roads/utilities/include/roads/roads.h
+++ b/projects/Roads/utilities/include/roads/roads.h
@@ -2,3 +2,4 @@
 
 #include "pch.h"
 #include "grid.h"
+#include "kdtree.h"
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index 5be1126612..538400b7bd 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -98,5 +98,39 @@ float spline::FindNearestPoint(const Eigen::Vector3d &point, const tinyspline::B
         }
     }
 
-    return t;
+    return Distance(point, bSpline, t);
+}
+
+ArrayList<float> spline::CalcRoadMask(const std::vector<std::array<float, 3>> &Points, const tinyspline::BSpline &SplineQwQ, int32_t Width, int32_t Nx) {
+    ArrayList<float> Result;
+    Result.resize(Points.size(), std::numeric_limits<float>::max() - 1);
+
+    int32_t Ny = int32_t(Points.size()) / Nx;
+
+#pragma omp parallel for
+    for (int32_t i = 0; i < Points.size(); ++i) {
+        float t = 0;
+        const std::array<float, 3>& zp = Points[i];
+        Eigen::Vector3d ep(zp[0], zp[1], zp[2]);
+        float Distance = spline::FindNearestPoint(ep, SplineQwQ, t);
+        //auto sp = SplineQwQ.eval(t).resultVec3();
+
+        for (int32_t dx = -Width; dx <= Width; ++dx) {
+            for (int32_t dy = -Width; dy <= Width; ++dy) {
+                int32_t ix = (i % Nx) + dx;
+                int32_t iy = (i / Nx) + dy;
+                int32_t idx = ix + iy * Nx;
+                if (ix >= 0 && ix < Nx && iy >= 0 && iy < Ny && Result.IsValidIndex(idx)) {
+                    Result[idx] = std::min(Distance, Result[idx]);
+                }
+            }
+        }
+//        if (Distance >= LittleR && Distance <= BigR) {
+//            zp[1] = float((1 - OriginHeightInterpretationRatio) * sp.y() + OriginHeightInterpretationRatio * zp[1]) * 0.5f;
+//        } else if (Distance < LittleR) {
+//            zp[1] = float(sp.y());
+//        }
+    }
+
+    return Result;
 }
diff --git a/projects/Roads/utilities/src/kdtree.cpp b/projects/Roads/utilities/src/kdtree.cpp
index 73d4b01a39..3b5aae3abd 100644
--- a/projects/Roads/utilities/src/kdtree.cpp
+++ b/projects/Roads/utilities/src/kdtree.cpp
@@ -87,3 +87,106 @@ ArrayList<VectorXf> KDTree::SearchRadius(const VectorXf &Point, float Radius) {
 
     return Result;
 }
+
+Octree::Octree(float minX, float maxX, float minY, float maxY, float minZ, float maxZ, unsigned int maxDepth) :
+    maxDepth(maxDepth) {
+    std::array<float, 6> bounds = {minX, maxX, minY, maxY, minZ, maxZ};
+    root = std::make_unique<Node>(bounds);
+}
+
+void Octree::addPoint(Octree::Point3D *point) {
+    addPoint(root.get(), point, 1);
+}
+
+std::vector<Octree::Point3D *> Octree::findNearestNeighbours(float x, float y, float z, unsigned int k) {
+    Point3D point(x, y, z);
+    std::vector<Point3D*> nearestNeighbours(k);
+    findNearestNeighbours(root.get(), &point, nearestNeighbours, 1);
+    return nearestNeighbours;
+}
+
+void Octree::findNearestNeighbours(Octree::Node *node, Octree::Point3D *target, std::vector<Point3D *> &nearestNeighbours, unsigned int depth) {
+    if(node == nullptr){
+        return;
+    }
+
+    if(node->point){
+        Point3D& current = *node->point;
+        float dist = (current - *target).squaredNorm();
+
+        for(auto& neighbour : nearestNeighbours){
+            // 首先，确保即将添加的点不是已经添加到 `nearestNeighbours` 中的点。
+            if(neighbour == node->point){
+                return;
+            }
+
+            if(neighbour == nullptr || dist < (*neighbour - *target).squaredNorm()){
+                neighbour = node->point;
+                dist = (*neighbour - *target).squaredNorm();
+            }
+        }
+    }
+
+    if(depth < maxDepth){
+        for(auto& child : node->children){
+            findNearestNeighbours(child.get(), target, nearestNeighbours, depth + 1);
+        }
+    }
+}
+
+void Octree::addPoint(Octree::Node *node, Octree::Point3D *point, unsigned int depth) {
+    if(depth >= maxDepth){
+        node->point = point;
+    } else {
+        unsigned index = 0;
+        index |= (*point)[0] > node->bounds[0] ? 1 : 0;
+        index |= (*point)[1] > node->bounds[2] ? 2 : 0;
+        index |= (*point)[2] > node->bounds[4] ? 4 : 0;
+
+        if(node->children[index] == nullptr){
+            std::array<float, 6> bounds{};
+            bounds[0] = (index & 1 ? node->bounds[0] : (*point)[0]);
+            bounds[1] = (index & 1 ? (*point)[0] : node->bounds[1]);
+            bounds[2] = (index & 2 ? node->bounds[2] : (*point)[1]);
+            bounds[3] = (index & 2 ? (*point)[1] : node->bounds[3]);
+            bounds[4] = (index & 4 ? node->bounds[4] : (*point)[2]);
+            bounds[5] = (index & 4 ? (*point)[2] : node->bounds[5]);
+
+            node->children[index] = std::make_unique<Node>(bounds);
+        }
+
+        addPoint(node->children[index].get(), point, depth + 1);
+    }
+}
+
+std::vector<Octree::Point3D *> Octree::findPointsInRadius(float x, float y, float z, float radius) {
+    Point3D center(x, y, z);
+    std::vector<Point3D*> pointsInRadius;
+    findPointsInRadius(root.get(), &center, radius, pointsInRadius);
+    return pointsInRadius;
+}
+
+void Octree::findPointsInRadius(Octree::Node *node, Octree::Point3D *center, float radius, std::vector<Point3D *> &pointsInRadius) {
+    if(node == nullptr){
+        return;
+    }
+
+    if(node->point){
+        Point3D& current = *node->point;
+        float dist = (*center - current).squaredNorm();
+
+        if(dist <= radius * radius){
+            pointsInRadius.push_back(node->point);
+        }
+    }
+
+    float deltaX = std::max(node->bounds[0] - center->x(), center->x() - node->bounds[1]);
+    float deltaY = std::max(node->bounds[2] - center->y(), center->y() - node->bounds[3]);
+    float deltaZ = std::max(node->bounds[4] - center->z(), center->z() - node->bounds[5]);
+
+    if (deltaX*deltaX + deltaY*deltaY + deltaZ*deltaZ < radius * radius) {
+        for(auto& child : node->children){
+            findPointsInRadius(child.get(), center, radius, pointsInRadius);
+        }
+    }
+}

From a4d633dd233fddff9be3851e23b410dc88a424c1 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Thu, 31 Aug 2023 17:11:13 +0800
Subject: [PATCH 35/50] feat: using spline radius

---
 projects/Roads/nodes/src/cost.cpp             | 23 +++++++------
 projects/Roads/utilities/include/roads/grid.h |  2 +-
 projects/Roads/utilities/src/grid.cpp         | 32 +++++++------------
 3 files changed, 25 insertions(+), 32 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 1742a92822..57394d5da0 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -494,8 +494,8 @@ namespace {
         }
     };
 
-    struct ZENO_CRTP(RoadsPrimRefineWithLine, zeno::reflect::IParameterAutoNode) {
-        ZENO_GENERATE_NODE_BODY(RoadsPrimRefineWithLine);
+    struct ZENO_CRTP(RoadCalcMask, zeno::reflect::IParameterAutoNode) {
+        ZENO_GENERATE_NODE_BODY(RoadCalcMask);
 
         std::shared_ptr<zeno::PrimitiveObject> Mesh;
         ZENO_DECLARE_INPUT_FIELD(Mesh, "Mesh Prim");
@@ -507,14 +507,17 @@ namespace {
         std::shared_ptr<zeno::RoadBSplineObject> Spline;
         ZENO_DECLARE_INPUT_FIELD(Spline, "Spline");
 
-        int32_t RoadWidth = 3;
-        ZENO_DECLARE_INPUT_FIELD(RoadWidth, "Road Radius", false, "", "5");
+        std::string OutputChannel;
+        ZENO_DECLARE_INPUT_FIELD(OutputChannel, "Road Distance Channel (Vert)", false, "", "roadDis");
 
-        std::string SizeXChannel;
-        ZENO_DECLARE_INPUT_FIELD(SizeXChannel, "Nx Channel (UserData)", false, "", "nx");
+        float MaxDistance = 3;
+        ZENO_DECLARE_INPUT_FIELD(MaxDistance, "Road Radius", false, "", "5");
 
-        int Nx = 0;
-        ZENO_BINDING_PRIMITIVE_USERDATA(Mesh, Nx, SizeXChannel, false);
+        //std::string SizeXChannel;
+        //ZENO_DECLARE_INPUT_FIELD(SizeXChannel, "Nx Channel (UserData)", false, "", "nx");
+
+        //int Nx = 0;
+        //ZENO_BINDING_PRIMITIVE_USERDATA(Mesh, Nx, SizeXChannel, false);
 
         void apply() override {
             using namespace boost::geometry;
@@ -548,9 +551,9 @@ namespace {
             std::vector<std::array<float, 3>> New(Points.begin(), Points.end());
 
             tinyspline::BSpline& SplineQwQ = AutoParameter->Spline->Spline;
-            auto DistanceAttr = spline::CalcRoadMask(New, SplineQwQ, AutoParameter->RoadWidth, AutoParameter->Nx);
+            auto DistanceAttr = spline::CalcRoadMask(New, SplineQwQ, AutoParameter->MaxDistance);
 
-            auto& DisAttr = AutoParameter->Mesh->verts.add_attr<float>("roadDis");
+            auto& DisAttr = AutoParameter->Mesh->verts.add_attr<float>(AutoParameter->OutputChannel);
             DisAttr.swap(DistanceAttr);
         }
     };
diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index c4d4cfaf73..0fe3c73393 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -204,7 +204,7 @@ namespace roads {
 
         float FindNearestPoint(const Eigen::Vector3d &point, const tinyspline::BSpline &bSpline, float& t, float step = 0.01, float tolerance = 1e-6);
 
-        ArrayList<float> CalcRoadMask(const std::vector<std::array<float, 3>>& Points, const tinyspline::BSpline& SplineQwQ, int32_t Width, int32_t Nx);
+        ArrayList<float> CalcRoadMask(const std::vector<std::array<float, 3>>& Points, const tinyspline::BSpline& SplineQwQ, float MaxDistance);
     }// namespace spline
 
 }// namespace roads
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index 538400b7bd..2435fec810 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -87,12 +87,17 @@ float spline::Distance(const Eigen::Vector3d &point, const tinyspline::BSpline &
 float spline::FindNearestPoint(const Eigen::Vector3d &point, const tinyspline::BSpline &bSpline, float& t, float step, float tolerance) {
     // initial guess
     t = 0.0;
+
     while (step > tolerance) {
         float f_current = Distance(point, bSpline, t);
-        float f_next = Distance(point, bSpline, t + step);
+        float nextT = t + step;
+        if (nextT > 1) {
+            nextT = 1;
+        }
+        float f_next = Distance(point, bSpline, nextT);
 
         if (f_next < f_current) {
-            t += step;
+            t = nextT;
         } else {
             step *= 0.5;
         }
@@ -101,35 +106,20 @@ float spline::FindNearestPoint(const Eigen::Vector3d &point, const tinyspline::B
     return Distance(point, bSpline, t);
 }
 
-ArrayList<float> spline::CalcRoadMask(const std::vector<std::array<float, 3>> &Points, const tinyspline::BSpline &SplineQwQ, int32_t Width, int32_t Nx) {
+ArrayList<float> spline::CalcRoadMask(const std::vector<std::array<float, 3>> &Points, const tinyspline::BSpline &SplineQwQ, float MaxDistance) {
     ArrayList<float> Result;
     Result.resize(Points.size(), std::numeric_limits<float>::max() - 1);
 
-    int32_t Ny = int32_t(Points.size()) / Nx;
-
 #pragma omp parallel for
     for (int32_t i = 0; i < Points.size(); ++i) {
         float t = 0;
         const std::array<float, 3>& zp = Points[i];
         Eigen::Vector3d ep(zp[0], zp[1], zp[2]);
         float Distance = spline::FindNearestPoint(ep, SplineQwQ, t);
-        //auto sp = SplineQwQ.eval(t).resultVec3();
-
-        for (int32_t dx = -Width; dx <= Width; ++dx) {
-            for (int32_t dy = -Width; dy <= Width; ++dy) {
-                int32_t ix = (i % Nx) + dx;
-                int32_t iy = (i / Nx) + dy;
-                int32_t idx = ix + iy * Nx;
-                if (ix >= 0 && ix < Nx && iy >= 0 && iy < Ny && Result.IsValidIndex(idx)) {
-                    Result[idx] = std::min(Distance, Result[idx]);
-                }
-            }
+
+        if (std::abs<float>(Distance) < MaxDistance) {
+            Result[i] = t;
         }
-//        if (Distance >= LittleR && Distance <= BigR) {
-//            zp[1] = float((1 - OriginHeightInterpretationRatio) * sp.y() + OriginHeightInterpretationRatio * zp[1]) * 0.5f;
-//        } else if (Distance < LittleR) {
-//            zp[1] = float(sp.y());
-//        }
     }
 
     return Result;

From b58609a4d4d19303ef290346bd55bddf332b2aab Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Mon, 4 Sep 2023 14:00:00 +0800
Subject: [PATCH 36/50] feat: using simulated annealing instead of gradient
 descent

---
 projects/Roads/utilities/include/roads/grid.h |  2 ++
 projects/Roads/utilities/src/grid.cpp         | 34 ++++++++++++++++++-
 2 files changed, 35 insertions(+), 1 deletion(-)

diff --git a/projects/Roads/utilities/include/roads/grid.h b/projects/Roads/utilities/include/roads/grid.h
index 0fe3c73393..3d52336b3c 100644
--- a/projects/Roads/utilities/include/roads/grid.h
+++ b/projects/Roads/utilities/include/roads/grid.h
@@ -204,6 +204,8 @@ namespace roads {
 
         float FindNearestPoint(const Eigen::Vector3d &point, const tinyspline::BSpline &bSpline, float& t, float step = 0.01, float tolerance = 1e-6);
 
+        float FindNearestPointSA(const Eigen::Vector3d &Point, const tinyspline::BSpline &Spline);
+
         ArrayList<float> CalcRoadMask(const std::vector<std::array<float, 3>>& Points, const tinyspline::BSpline& SplineQwQ, float MaxDistance);
     }// namespace spline
 
diff --git a/projects/Roads/utilities/src/grid.cpp b/projects/Roads/utilities/src/grid.cpp
index 2435fec810..b45c2a1ef1 100644
--- a/projects/Roads/utilities/src/grid.cpp
+++ b/projects/Roads/utilities/src/grid.cpp
@@ -3,6 +3,7 @@
 #include "boost/graph/floyd_warshall_shortest.hpp"
 
 #include "roads/thirdparty/tinysplinecxx.h"
+#include <random>
 
 using namespace roads;
 
@@ -115,7 +116,8 @@ ArrayList<float> spline::CalcRoadMask(const std::vector<std::array<float, 3>> &P
         float t = 0;
         const std::array<float, 3>& zp = Points[i];
         Eigen::Vector3d ep(zp[0], zp[1], zp[2]);
-        float Distance = spline::FindNearestPoint(ep, SplineQwQ, t);
+        //float Distance = spline::FindNearestPoint(ep, SplineQwQ, t);
+        float Distance = spline::FindNearestPointSA(ep, SplineQwQ);
 
         if (std::abs<float>(Distance) < MaxDistance) {
             Result[i] = t;
@@ -124,3 +126,33 @@ ArrayList<float> spline::CalcRoadMask(const std::vector<std::array<float, 3>> &P
 
     return Result;
 }
+
+float spline::FindNearestPointSA(const Eigen::Vector3d &Point, const tinyspline::BSpline &Spline) {
+    std::mt19937 gen(std::random_device{}());
+    std::uniform_real_distribution<float> distr(0.0, 1.0);
+
+    float T = 1.0; // initial temperature
+    float T_min = 0.001; // minial temperature
+    float CoolingRate = 0.99; // sink rate
+
+    float t = distr(gen); // select a initial value
+    float BestT = t; // best value
+
+    while (T > T_min) {
+        float NewT = distr(gen); // generate new value
+        float CurrentDistance = Distance(Point, Spline, t); // distance to current value
+        float NewDistance = Distance(Point, Spline, NewT); // distance to new value
+        float dE = NewDistance - CurrentDistance; // delta value
+
+        // Randomly accept new value even if become worse
+        if (dE < 0 || distr(gen) < std::exp(-dE / T)) {
+            t = NewT;
+            if (NewDistance < Distance(Point, Spline, BestT)) {
+                BestT = NewT; // update best value
+            }
+        }
+        T *= CoolingRate; // decrease temperature
+    }
+
+    return Distance(Point, Spline, BestT);
+}

From 5b9a51f3de482113e50ac140db0d31f37232e04c Mon Sep 17 00:00:00 2001
From: zhouhang95 <765229842@qq.com>
Date: Tue, 5 Sep 2023 18:20:23 +0800
Subject: [PATCH 37/50] improve

---
 zenovis/xinxinoptix/ChiefDesignerEXR.h  | 30 ++++++++++++++++---------
 zenovis/xinxinoptix/optixPathTracer.cpp |  6 +++--
 2 files changed, 24 insertions(+), 12 deletions(-)

diff --git a/zenovis/xinxinoptix/ChiefDesignerEXR.h b/zenovis/xinxinoptix/ChiefDesignerEXR.h
index 1cd09203ce..01602411d2 100644
--- a/zenovis/xinxinoptix/ChiefDesignerEXR.h
+++ b/zenovis/xinxinoptix/ChiefDesignerEXR.h
@@ -55,7 +55,6 @@ inline void FreeEXRErrorMessage(const char *err) {
 
 inline int SaveEXR(float *pixels, int width, int height, int channels,
                    int asfp16, const char *filepath, const char **err) {
-    if (channels != 4) throw std::runtime_error("SaveEXR only support RGBA for now");
     if (asfp16 != 1) throw std::runtime_error("SaveEXR only support FP16 for now");
     try {
         using namespace Imf;
@@ -69,23 +68,34 @@ inline int SaveEXR(float *pixels, int width, int height, int channels,
         header.displayWindow() = displayWindow;
 
         // Create the frame buffer and add the R, G, B, A channels
-        Rgba* pixelsBuffer = new Rgba[width * height];
-        for (int i = 0; i < width * height; i++) {
-            pixelsBuffer[i].r = pixels[4 * i];
-            pixelsBuffer[i].g = pixels[4 * i + 1];
-            pixelsBuffer[i].b = pixels[4 * i + 2];
-            pixelsBuffer[i].a = pixels[4 * i + 3];
+        std::vector<Rgba> pixelsBuffer(width * height);q
+        if (channels == 4) {
+            for (int i = 0; i < width * height; i++) {
+                pixelsBuffer[i].r = pixels[4 * i];
+                pixelsBuffer[i].g = pixels[4 * i + 1];
+                pixelsBuffer[i].b = pixels[4 * i + 2];
+                pixelsBuffer[i].a = pixels[4 * i + 3];
+            }
+        }
+        else if (channels == 3) {
+            for (int i = 0; i < width * height; i++) {
+                pixelsBuffer[i].r = pixels[3 * i];
+                pixelsBuffer[i].g = pixels[3 * i + 1];
+                pixelsBuffer[i].b = pixels[3 * i + 2];
+                pixelsBuffer[i].a = 1;
+            }
+        }
+        else {
+            throw std::runtime_error("SaveEXR only support RGBA and RGB for now");
         }
 
         // Create the output file
         RgbaOutputFile file(filepath, header);
 
         // Write the pixels to the file
-        file.setFrameBuffer(pixelsBuffer, 1, width);
+        file.setFrameBuffer(pixelsBuffer.data(), 1, width);
         file.writePixels(height);
 
-        // Clean up
-        delete[] pixelsBuffer;
         return 0;
     } catch (const std::exception& e) {
         *err = strdup(e.what());
diff --git a/zenovis/xinxinoptix/optixPathTracer.cpp b/zenovis/xinxinoptix/optixPathTracer.cpp
index 1efdeaa2ae..49f3ae46c3 100644
--- a/zenovis/xinxinoptix/optixPathTracer.cpp
+++ b/zenovis/xinxinoptix/optixPathTracer.cpp
@@ -63,7 +63,9 @@
 #include <glm/gtx/transform.hpp>
 #include <glm/gtc/type_ptr.hpp>
 
-#include "tinyexr.h"
+#include "ChiefDesignerEXR.h"
+using namespace zeno::ChiefDesignerEXR;
+
 #include "zeno/utils/image_proc.h"
 
 #ifndef M_PI
@@ -3426,7 +3428,7 @@ static void save_exr(float3* ptr, int w, int h, std::string path) {
     zeno::image_flip_vertical(data.data(), w, h);
     const char *err = nullptr;
     int ret = SaveEXR((float *) data.data(), w, h, 3, 1, path.c_str(), &err);
-    if (ret != TINYEXR_SUCCESS) {
+    if (ret != 0) {
         if (err) {
             zeno::log_error("failed to perform SaveEXR to {}: {}", path, err);
             FreeEXRErrorMessage(err);

From 94fbb5e0b249f95e917b3b707afb258e52e40848 Mon Sep 17 00:00:00 2001
From: zhouhang95 <765229842@qq.com>
Date: Tue, 5 Sep 2023 18:20:56 +0800
Subject: [PATCH 38/50] fix

---
 zenovis/xinxinoptix/ChiefDesignerEXR.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/zenovis/xinxinoptix/ChiefDesignerEXR.h b/zenovis/xinxinoptix/ChiefDesignerEXR.h
index 01602411d2..b510b86dd5 100644
--- a/zenovis/xinxinoptix/ChiefDesignerEXR.h
+++ b/zenovis/xinxinoptix/ChiefDesignerEXR.h
@@ -68,7 +68,7 @@ inline int SaveEXR(float *pixels, int width, int height, int channels,
         header.displayWindow() = displayWindow;
 
         // Create the frame buffer and add the R, G, B, A channels
-        std::vector<Rgba> pixelsBuffer(width * height);q
+        std::vector<Rgba> pixelsBuffer(width * height);
         if (channels == 4) {
             for (int i = 0; i < width * height; i++) {
                 pixelsBuffer[i].r = pixels[4 * i];

From 47e263e80cd1663d1c797c7a37ee0f343a562272 Mon Sep 17 00:00:00 2001
From: zhouhang95 <765229842@qq.com>
Date: Wed, 6 Sep 2023 16:31:16 +0800
Subject: [PATCH 39/50] multipart exr

---
 zenovis/xinxinoptix/ChiefDesignerEXR.h  | 49 +++++++++++++++++++++++++
 zenovis/xinxinoptix/optixPathTracer.cpp | 35 +++++++++++++-----
 2 files changed, 75 insertions(+), 9 deletions(-)

diff --git a/zenovis/xinxinoptix/ChiefDesignerEXR.h b/zenovis/xinxinoptix/ChiefDesignerEXR.h
index b510b86dd5..b82c6dc3c0 100644
--- a/zenovis/xinxinoptix/ChiefDesignerEXR.h
+++ b/zenovis/xinxinoptix/ChiefDesignerEXR.h
@@ -1,5 +1,8 @@
 #pragma once
 
+#include <ImfMultiPartOutputFile.h>
+#include <ImfOutputPart.h>
+#include <ImfChannelList.h>
 #include <ImfHeader.h>
 #include <ImfRgbaFile.h>
 #include <ImfArray.h>
@@ -102,5 +105,51 @@ inline int SaveEXR(float *pixels, int width, int height, int channels,
         return 1;
     }
 }
+inline void SaveMultiLayerEXR(
+    std::vector<float*> pixels, int width, int height, std::vector<std::string> names,
+    const char *filepath
+ ) {
+    int layer_count = names.size();
+    using namespace Imf;
+    using namespace Imath;
+    std::vector<Header> headers(layer_count);
+    for (auto l = 0; l < layer_count; l++) {
+
+        // Create the header with the image size
+        headers[l] = Header(width, height);
+        headers[l].setName(names[l]);
+
+        // Set the display window (region of the image that should be displayed)
+        Box2i displayWindow(V2i(0, 0), V2i(width - 1, height - 1));
+        headers[l].displayWindow() = displayWindow;
+    }
+    MultiPartOutputFile multiPartFile(filepath, headers.data(), layer_count);
+
+    // Create the frame buffer and add the R, G, B, A channels
+    for (auto l = 0; l < layer_count; l++) {
+        OutputPart outputPart(multiPartFile, l);
+
+        std::vector<Rgba> pixelsBuffer(width * height);
+        for (int i = 0; i < width * height; i++) {
+            pixelsBuffer[i].r = pixels[l][3 * i];
+            pixelsBuffer[i].g = pixels[l][3 * i + 1];
+            pixelsBuffer[i].b = pixels[l][3 * i + 2];
+            pixelsBuffer[i].a = 1;
+        }
+
+        size_t xs = 1 * sizeof (Rgba);
+        size_t ys = width * sizeof (Rgba);
+
+        FrameBuffer fb;
+
+        fb.insert ("R", Slice (HALF, (char*) &pixelsBuffer[0].r, xs, ys));
+        fb.insert ("G", Slice (HALF, (char*) &pixelsBuffer[0].g, xs, ys));
+        fb.insert ("B", Slice (HALF, (char*) &pixelsBuffer[0].b, xs, ys));
+        fb.insert ("A", Slice (HALF, (char*) &pixelsBuffer[0].a, xs, ys));
+
+        outputPart.setFrameBuffer(fb);
+        outputPart.writePixels(height);
+    }
+}
 
 }
diff --git a/zenovis/xinxinoptix/optixPathTracer.cpp b/zenovis/xinxinoptix/optixPathTracer.cpp
index 49f3ae46c3..fa31f0ed01 100644
--- a/zenovis/xinxinoptix/optixPathTracer.cpp
+++ b/zenovis/xinxinoptix/optixPathTracer.cpp
@@ -3471,7 +3471,32 @@ void optixrender(int fbo, int samples, bool denoise, bool simpleRender) {
         stbi_flip_vertically_on_write(true);
         if (zeno::getSession().userData().get2<bool>("output_exr", true)) {
             auto exr_path = path.substr(0, path.size() - 4) + ".exr";
-            save_exr((float3 *)optixgetimg_extra("color"), w, h, exr_path);
+
+            // AOV
+            if (zeno::getSession().userData().get2<bool>("output_aov", true)) {
+                SaveMultiLayerEXR(
+                        {
+                            (float*)optixgetimg_extra("color"),
+                            (float*)optixgetimg_extra("diffuse"),
+                            (float*)optixgetimg_extra("specular"),
+                            (float*)optixgetimg_extra("transmit"),
+                            (float*)optixgetimg_extra("background"),
+                        },
+                        w,
+                        h,
+                        {
+                            "C",
+                            "diffuse",
+                            "specular",
+                            "transmit",
+                            "background",
+                        },
+                        exr_path.c_str()
+                );
+            }
+            else {
+                save_exr((float3 *)optixgetimg_extra("color"), w, h, exr_path);
+            }
         }
         else {
             stbi_write_jpg(path.c_str(), w, h, 4, p, 100);
@@ -3490,14 +3515,6 @@ void optixrender(int fbo, int samples, bool denoise, bool simpleRender) {
         zeno::log_info("optix: saving screenshot {}x{} to {}", w, h, path);
         ud.erase("optix_image_path");
 
-        // AOV
-        if (zeno::getSession().userData().get2<bool>("output_aov", true)) {
-            path = path.substr(0, path.size() - 4);
-            save_exr((float3 *)optixgetimg_extra("diffuse"), w, h, path + ".diffuse.exr");
-            save_exr((float3 *)optixgetimg_extra("specular"), w, h, path + ".specular.exr");
-            save_exr((float3 *)optixgetimg_extra("transmit"), w, h, path + ".transmit.exr");
-            save_exr((float3 *)optixgetimg_extra("background"), w, h, path + ".background.exr");
-        }
         imageRendered = true;
     }
 }

From d693b1dc630fd2ad467baaae7c7c3193ed206d71 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Mon, 11 Sep 2023 15:27:26 +0800
Subject: [PATCH 40/50] feat: add BDozerSimulate node

---
 projects/Roads/nodes/src/cost.cpp | 111 ++++++++++++++++++++++--------
 1 file changed, 81 insertions(+), 30 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 57394d5da0..0851e67ae9 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -513,39 +513,9 @@ namespace {
         float MaxDistance = 3;
         ZENO_DECLARE_INPUT_FIELD(MaxDistance, "Road Radius", false, "", "5");
 
-        //std::string SizeXChannel;
-        //ZENO_DECLARE_INPUT_FIELD(SizeXChannel, "Nx Channel (UserData)", false, "", "nx");
-
-        //int Nx = 0;
-        //ZENO_BINDING_PRIMITIVE_USERDATA(Mesh, Nx, SizeXChannel, false);
-
         void apply() override {
             using namespace boost::geometry;
 
-            //            using PointType = model::point<float, 3, cs::cartesian>;
-            //            using BoxType = model::box<PointType>;
-            //            using BoxPtr = std::shared_ptr<BoxType>;
-
-            //            index::rtree<BoxPtr, index::linear<128, 4>> RTree;
-
-            //auto& LineVertices = AutoParameter->Lines->verts;
-            //auto& Lines = AutoParameter->Lines->lines;
-
-            //            for (const auto& p : Points) {
-            //                PointType Point { p[0], p[1], p[2] };
-            //                BoxPtr b = std::make_shared<BoxType>( Point, Point );
-            //                RTree.insert(b);
-            //            }
-            //
-            //            for (const auto& Seg : Lines) {
-            //                PointType a { LineVertices[Seg[0]][0], LineVertices[Seg[0]][1], LineVertices[Seg[0]][2] };
-            //                std::vector<BoxPtr> n;
-            //                RTree.query(index::nearest(PointType(0, 0, 0), 5), std::back_inserter(n));
-            //                for (const auto& a : n) {
-            //                    std::cout << a->min_corner().get<0>() << ", " << a->min_corner().get<1>() << ", " << a->min_corner().get<2>() << std::endl;
-            //                }
-            //            }
-
             auto &Points = AutoParameter->Mesh->verts;
 
             std::vector<std::array<float, 3>> New(Points.begin(), Points.end());
@@ -558,6 +528,87 @@ namespace {
         }
     };
 
+    struct ZENO_CRTP(RoadBDozerSimulate, zeno::reflect::IParameterAutoNode) {
+        ZENO_GENERATE_NODE_BODY(RoadBDozerSimulate);
+
+        std::shared_ptr<zeno::PrimitiveObject> Mesh;
+        ZENO_DECLARE_INPUT_FIELD(Mesh, "Mesh Prim");
+        ZENO_DECLARE_OUTPUT_FIELD(Mesh, "Mesh Prim");
+
+        int SmoothRadius = 3;
+        ZENO_DECLARE_INPUT_FIELD(SmoothRadius, "Smooth Radius", false, "", "3");
+
+        int Epochs = 5;
+        ZENO_DECLARE_INPUT_FIELD(Epochs, "Epochs", false, "", "5");
+
+        std::string RoadChannel;
+        ZENO_DECLARE_INPUT_FIELD(RoadChannel, "Road Distance Channel (Vert)", false, "", "roadMask");
+
+        std::string SizeXChannel;
+        ZENO_DECLARE_INPUT_FIELD(SizeXChannel, "Nx Channel (UserData)", false, "", "nx");
+
+        std::string SizeYChannel;
+        ZENO_DECLARE_INPUT_FIELD(SizeYChannel, "Ny Channel (UserData)", false, "", "ny");
+
+        int Nx = 0;
+        ZENO_BINDING_PRIMITIVE_USERDATA(Mesh, Nx, SizeXChannel, false);
+
+        int Ny = 0;
+        ZENO_BINDING_PRIMITIVE_USERDATA(Mesh, Ny, SizeYChannel, false);
+
+        zeno::AttrVector<float> RoadMask{};
+        ZENO_BINDING_PRIMITIVE_ATTRIBUTE(Mesh, RoadMask, RoadChannel, zeno::reflect::EZenoPrimitiveAttr::VERT);
+
+        void apply() override {
+            Mesh = AutoParameter->Mesh;
+            SmoothRadius = AutoParameter->SmoothRadius;
+            Epochs = AutoParameter->Epochs;
+            RoadChannel = AutoParameter->RoadChannel;
+            Nx = AutoParameter->Nx;
+            Ny = AutoParameter->Ny;
+
+            zeno::AttrVector<zeno::vec3f>& PositionAttr = Mesh->verts;
+            zeno::AttrVector<float>& RoadMaskk = AutoParameter->RoadMask;
+
+            std::vector<float> UpdatedHeightField(PositionAttr.size());
+
+            for (int32_t Epoch = 0; Epoch < Epochs; ++Epoch) {
+#pragma omp parallel for
+                for (int32_t y = 0; y < Ny; ++y) {
+                    for (int32_t x = 0; x < Nx; ++x) {
+                        int32_t Idx = y * Nx + x;
+                        if (0 != RoadMaskk[Idx]) {
+                            float Summary = 0.0f;
+                            int32_t Count = 0;
+
+                            for (int32_t dx = -SmoothRadius; dx <= SmoothRadius; ++dx) {
+                                for (int32_t dy = -SmoothRadius; dy <= SmoothRadius; ++dy) {
+                                    int32_t nx = x + dx;
+                                    int32_t ny = y + dy;
+
+                                    if (nx >= 0 && ny >= 0 && nx < Nx && ny < Ny) {
+                                        Summary += PositionAttr[ny * Nx + nx].at(1);
+                                        ++Count;
+                                    }
+                                }
+                            }
+
+                            if (Count > 0) {
+                                UpdatedHeightField[Idx] = Summary / float(Count);
+                            }
+                        }
+                    }
+                }
+            }
+
+#pragma omp parallel for
+            for (int32_t i = 0; i < UpdatedHeightField.size(); ++i) {
+                if (0 != RoadMaskk[i])
+                    PositionAttr[i][1] = UpdatedHeightField[i];
+            }
+        }
+    };
+
 #if _MSC_VER
 #include "Windows.h"
     // Windows Debug

From cc17662a69a92d22f6f2476b13190602eb1b9040 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Mon, 11 Sep 2023 15:39:26 +0800
Subject: [PATCH 41/50] feat: weighted BDozerSimulate

---
 projects/Roads/nodes/src/cost.cpp | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 0851e67ae9..02432c0613 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -578,8 +578,8 @@ namespace {
                     for (int32_t x = 0; x < Nx; ++x) {
                         int32_t Idx = y * Nx + x;
                         if (0 != RoadMaskk[Idx]) {
-                            float Summary = 0.0f;
-                            int32_t Count = 0;
+                            float HeightSummary = 0.0f;
+                            float WeightSummary = 0;
 
                             for (int32_t dx = -SmoothRadius; dx <= SmoothRadius; ++dx) {
                                 for (int32_t dy = -SmoothRadius; dy <= SmoothRadius; ++dy) {
@@ -587,14 +587,15 @@ namespace {
                                     int32_t ny = y + dy;
 
                                     if (nx >= 0 && ny >= 0 && nx < Nx && ny < Ny) {
-                                        Summary += PositionAttr[ny * Nx + nx].at(1);
-                                        ++Count;
+                                        float Weight = std::exp(-(dx*dx + dy*dy) / (2.0f * SmoothRadius * SmoothRadius));
+                                        HeightSummary += PositionAttr[ny * Nx + nx].at(1) * Weight;
+                                        WeightSummary += Weight;
                                     }
                                 }
                             }
 
-                            if (Count > 0) {
-                                UpdatedHeightField[Idx] = Summary / float(Count);
+                            if (WeightSummary > 0) {
+                                UpdatedHeightField[Idx] = HeightSummary / WeightSummary;
                             }
                         }
                     }

From 5ba9eb2b42cc4320117cd401382218c1bd62934b Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Mon, 11 Sep 2023 15:58:52 +0800
Subject: [PATCH 42/50] feat: add road slope threshold

---
 projects/Roads/nodes/src/cost.cpp | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/projects/Roads/nodes/src/cost.cpp b/projects/Roads/nodes/src/cost.cpp
index 02432c0613..bf1337b3b6 100644
--- a/projects/Roads/nodes/src/cost.cpp
+++ b/projects/Roads/nodes/src/cost.cpp
@@ -541,6 +541,12 @@ namespace {
         int Epochs = 5;
         ZENO_DECLARE_INPUT_FIELD(Epochs, "Epochs", false, "", "5");
 
+        float SlopeThreshold = 0.5f;
+        ZENO_DECLARE_INPUT_FIELD(SlopeThreshold, "Slope Threshold", false, "", "0.5");
+
+        float OverThresholdWeightRatio = 0.1f;
+        ZENO_DECLARE_INPUT_FIELD(OverThresholdWeightRatio, "Over Threshold Weight Ratio", false, "", "0.1");
+
         std::string RoadChannel;
         ZENO_DECLARE_INPUT_FIELD(RoadChannel, "Road Distance Channel (Vert)", false, "", "roadMask");
 
@@ -566,6 +572,8 @@ namespace {
             RoadChannel = AutoParameter->RoadChannel;
             Nx = AutoParameter->Nx;
             Ny = AutoParameter->Ny;
+            SlopeThreshold = AutoParameter->SlopeThreshold;
+            OverThresholdWeightRatio = AutoParameter->OverThresholdWeightRatio;
 
             zeno::AttrVector<zeno::vec3f>& PositionAttr = Mesh->verts;
             zeno::AttrVector<float>& RoadMaskk = AutoParameter->RoadMask;
@@ -587,7 +595,19 @@ namespace {
                                     int32_t ny = y + dy;
 
                                     if (nx >= 0 && ny >= 0 && nx < Nx && ny < Ny) {
+                                        float Distance = std::sqrt(dx * dx + dy * dy);
+                                        float Slope = 0.0f;
+
+                                        if (Distance > 1e-3) {
+                                            Slope = std::abs<float>(PositionAttr[y*Nx + x][1] - PositionAttr[ny*Nx + nx][1]) / Distance;
+                                        }
+
                                         float Weight = std::exp(-(dx*dx + dy*dy) / (2.0f * SmoothRadius * SmoothRadius));
+
+                                        if (Slope > SlopeThreshold) {
+                                            Weight *= OverThresholdWeightRatio;
+                                        }
+
                                         HeightSummary += PositionAttr[ny * Nx + nx].at(1) * Weight;
                                         WeightSummary += Weight;
                                     }

From 0d8f14a6890d8afc1a13dbdbcc9925acad34a00e Mon Sep 17 00:00:00 2001
From: YingQ <hqq.2020@foxmail.com>
Date: Tue, 12 Sep 2023 14:29:17 +0800
Subject: [PATCH 43/50] Update PrimitiveReduction.cpp

---
 zeno/src/nodes/prim/PrimitiveReduction.cpp | 30 +++++++++++++++++++++-
 1 file changed, 29 insertions(+), 1 deletion(-)

diff --git a/zeno/src/nodes/prim/PrimitiveReduction.cpp b/zeno/src/nodes/prim/PrimitiveReduction.cpp
index f5c54950c1..e077c08034 100644
--- a/zeno/src/nodes/prim/PrimitiveReduction.cpp
+++ b/zeno/src/nodes/prim/PrimitiveReduction.cpp
@@ -64,9 +64,37 @@ ZENDEFNODE(PrimitiveReduction,
     {"string", "attr", "pos"},
     {"enum avg max min absmax", "op", "avg"},
     }, /* category: */ {
-    "primitive",
+    "deprecated",
     }});
 
+struct PrimReduction : zeno::INode {
+    virtual void apply() override{
+        auto prim = get_input<PrimitiveObject>("prim");
+        auto attrToReduce = get_input2<std::string>(("attrName"));
+        auto op = get_input2<std::string>(("op"));
+        zeno::NumericValue result;
+        if (prim->attr_is<zeno::vec3f>(attrToReduce))
+            result = prim_reduce<zeno::vec3f>(prim.get(), attrToReduce, op);
+        else 
+            result = prim_reduce<float>(prim.get(), attrToReduce, op);
+        auto out = std::make_shared<zeno::NumericObject>();
+        out->set(result);
+        set_output("result", std::move(out));
+    }
+};
+ZENDEFNODE(PrimReduction,{
+    {
+        {"prim"},
+        {"string", "attrName", "pos"},
+        {"enum avg max min absmax", "op", "avg"},
+    },
+    {
+        {"result"},
+    },
+    {},
+    {"primitive"},
+});
+
 struct PrimitiveBoundingBox : zeno::INode {
     virtual void apply() override{
         auto prim = get_input<PrimitiveObject>("prim");

From b55c937ecba6e69b04e3cfa641deb2a19d4b5fa2 Mon Sep 17 00:00:00 2001
From: zhouhang95 <765229842@qq.com>
Date: Tue, 12 Sep 2023 15:12:09 +0800
Subject: [PATCH 44/50] fix-cpp-wirte

---
 zeno/src/nodes/neo/WriteObjPrim.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/zeno/src/nodes/neo/WriteObjPrim.cpp b/zeno/src/nodes/neo/WriteObjPrim.cpp
index fe96255b1e..e297e621f4 100644
--- a/zeno/src/nodes/neo/WriteObjPrim.cpp
+++ b/zeno/src/nodes/neo/WriteObjPrim.cpp
@@ -13,12 +13,14 @@
 #include <cassert>
 #include <cstdio>
 #include <fstream>
+#include <iomanip>
 
 namespace zeno {
 namespace {
 
 void dump_obj(PrimitiveObject *prim, std::ostream &fout) {
     fout << "# https://github.com/zenustech/zeno\n";
+    fout << std::setiosflags(std::ios::fixed) << std::setprecision(6);
     for (auto const &[x, y, z]: prim->verts) {
         fout << "v " << x << ' ' << y << ' ' << z << '\n';
     }

From 26ea7d2ea87d5497444cd737ca182f8559227a28 Mon Sep 17 00:00:00 2001
From: zhouhang95 <765229842@qq.com>
Date: Tue, 12 Sep 2023 15:31:12 +0800
Subject: [PATCH 45/50] fix-cpp-wirte

---
 zeno/src/nodes/neo/WriteObjPrim.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/zeno/src/nodes/neo/WriteObjPrim.cpp b/zeno/src/nodes/neo/WriteObjPrim.cpp
index e297e621f4..91bdb0c567 100644
--- a/zeno/src/nodes/neo/WriteObjPrim.cpp
+++ b/zeno/src/nodes/neo/WriteObjPrim.cpp
@@ -20,7 +20,7 @@ namespace {
 
 void dump_obj(PrimitiveObject *prim, std::ostream &fout) {
     fout << "# https://github.com/zenustech/zeno\n";
-    fout << std::setiosflags(std::ios::fixed) << std::setprecision(6);
+    fout << std::setprecision(8);
     for (auto const &[x, y, z]: prim->verts) {
         fout << "v " << x << ' ' << y << ' ' << z << '\n';
     }

From 9345adb3f8c20e73120eb1ac343ea6f6e3b157b6 Mon Sep 17 00:00:00 2001
From: zhouhang95 <765229842@qq.com>
Date: Fri, 15 Sep 2023 18:49:04 +0800
Subject: [PATCH 46/50] work

---
 zenovis/xinxinoptix/ChiefDesignerEXR.h  | 81 ++++++++++++++-----------
 zenovis/xinxinoptix/optixPathTracer.cpp | 10 +--
 2 files changed, 50 insertions(+), 41 deletions(-)

diff --git a/zenovis/xinxinoptix/ChiefDesignerEXR.h b/zenovis/xinxinoptix/ChiefDesignerEXR.h
index b82c6dc3c0..d7c1835176 100644
--- a/zenovis/xinxinoptix/ChiefDesignerEXR.h
+++ b/zenovis/xinxinoptix/ChiefDesignerEXR.h
@@ -1,7 +1,7 @@
 #pragma once
 
 #include <ImfMultiPartOutputFile.h>
-#include <ImfOutputPart.h>
+#include <ImfOutputFile.h>
 #include <ImfChannelList.h>
 #include <ImfHeader.h>
 #include <ImfRgbaFile.h>
@@ -11,6 +11,7 @@
 #include <cstring>
 #include <vector>
 #include <string>
+#include "zeno/utils/image_proc.h"
 
 namespace zeno::ChiefDesignerEXR {
 
@@ -106,50 +107,58 @@ inline int SaveEXR(float *pixels, int width, int height, int channels,
     }
 }
 inline void SaveMultiLayerEXR(
-    std::vector<float*> pixels, int width, int height, std::vector<std::string> names,
-    const char *filepath
- ) {
-    int layer_count = names.size();
-    using namespace Imf;
+    std::vector<float*> pixels
+    , int width
+    , int height
+    , std::vector<std::string> channels
+    , const char* exrFilePath
+) {
     using namespace Imath;
-    std::vector<Header> headers(layer_count);
-    for (auto l = 0; l < layer_count; l++) {
-
-        // Create the header with the image size
-        headers[l] = Header(width, height);
-        headers[l].setName(names[l]);
-
-        // Set the display window (region of the image that should be displayed)
-        Box2i displayWindow(V2i(0, 0), V2i(width - 1, height - 1));
-        headers[l].displayWindow() = displayWindow;
-    }
-    MultiPartOutputFile multiPartFile(filepath, headers.data(), layer_count);
+    using namespace Imf;
 
-    // Create the frame buffer and add the R, G, B, A channels
-    for (auto l = 0; l < layer_count; l++) {
-        OutputPart outputPart(multiPartFile, l);
+    Header header(width, height);
+    ChannelList channelList;
 
-        std::vector<Rgba> pixelsBuffer(width * height);
-        for (int i = 0; i < width * height; i++) {
-            pixelsBuffer[i].r = pixels[l][3 * i];
-            pixelsBuffer[i].g = pixels[l][3 * i + 1];
-            pixelsBuffer[i].b = pixels[l][3 * i + 2];
-            pixelsBuffer[i].a = 1;
+    const char *std_suffix = "RGB";
+    for (auto channel: channels) {
+        for (int i = 0; i < 3; i++) {
+            std::string name = zeno::format("{}{}", channel, std_suffix[i]);
+            channelList.insert(name, Channel(HALF));
         }
+    }
 
-        size_t xs = 1 * sizeof (Rgba);
-        size_t ys = width * sizeof (Rgba);
+    header.channels() = channelList;
 
-        FrameBuffer fb;
+    OutputFile file (exrFilePath, header);
+    FrameBuffer frameBuffer;
 
-        fb.insert ("R", Slice (HALF, (char*) &pixelsBuffer[0].r, xs, ys));
-        fb.insert ("G", Slice (HALF, (char*) &pixelsBuffer[0].g, xs, ys));
-        fb.insert ("B", Slice (HALF, (char*) &pixelsBuffer[0].b, xs, ys));
-        fb.insert ("A", Slice (HALF, (char*) &pixelsBuffer[0].a, xs, ys));
+    std::vector<std::vector<half>> data;
+    for (float *rgb: pixels) {
+        std::vector<half> r(width * height);
+        std::vector<half> g(width * height);
+        std::vector<half> b(width * height);
+        for (auto i = 0; i < width * height; i++) {
+            r[i] = rgb[3 * i + 0];
+            g[i] = rgb[3 * i + 1];
+            b[i] = rgb[3 * i + 2];
+        }
+        zeno::image_flip_vertical(r.data(), width, height);
+        zeno::image_flip_vertical(g.data(), width, height);
+        zeno::image_flip_vertical(b.data(), width, height);
+        data.push_back(std::move(r));
+        data.push_back(std::move(g));
+        data.push_back(std::move(b));
+    }
 
-        outputPart.setFrameBuffer(fb);
-        outputPart.writePixels(height);
+    for (auto i = 0; i < channels.size(); i++) {
+        for (auto j = 0; j < 3; j++) {
+            std::string name = zeno::format("{}{}", channels[i], std_suffix[j]);
+            frameBuffer.insert (name, Slice ( HALF, (char*) data[i * 3 + j].data(), sizeof (half) * 1, sizeof (half) * width));
+        }
     }
+
+    file.setFrameBuffer (frameBuffer);
+    file.writePixels (height);
 }
 
 }
diff --git a/zenovis/xinxinoptix/optixPathTracer.cpp b/zenovis/xinxinoptix/optixPathTracer.cpp
index fa31f0ed01..cde1448f5f 100644
--- a/zenovis/xinxinoptix/optixPathTracer.cpp
+++ b/zenovis/xinxinoptix/optixPathTracer.cpp
@@ -3485,11 +3485,11 @@ void optixrender(int fbo, int samples, bool denoise, bool simpleRender) {
                         w,
                         h,
                         {
-                            "C",
-                            "diffuse",
-                            "specular",
-                            "transmit",
-                            "background",
+                            "",
+                            "diffuse.",
+                            "specular.",
+                            "transmit.",
+                            "background.",
                         },
                         exr_path.c_str()
                 );

From 233e5c94f16c894beaf5ec2b3ab49dbf1c5ad071 Mon Sep 17 00:00:00 2001
From: YingQ <hqq.2020@foxmail.com>
Date: Tue, 19 Sep 2023 18:48:39 +0800
Subject: [PATCH 47/50] ImageAlphacheckboard

---
 ui/zenoedit/panel/zenoimagepanel.cpp | 21 ++++++++++++++++++++-
 ui/zenoedit/panel/zenoimagepanel.h   |  1 +
 2 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/ui/zenoedit/panel/zenoimagepanel.cpp b/ui/zenoedit/panel/zenoimagepanel.cpp
index 3d0fc32392..9ec9ea65ba 100644
--- a/ui/zenoedit/panel/zenoimagepanel.cpp
+++ b/ui/zenoedit/panel/zenoimagepanel.cpp
@@ -111,6 +111,7 @@ void ZenoImagePanel::setPrim(std::string primid) {
         return;
 
     bool enableGamma = pGamma->checkState() == Qt::Checked;
+    bool enableAlpha = pAlpha->checkState() == Qt::Checked;
     bool found = false;
     for (auto const &[key, ptr]: scene->objectsMan->pairs()) {
         if ((key.substr(0, key.find(":"))) != primid) {
@@ -127,7 +128,7 @@ void ZenoImagePanel::setPrim(std::string primid) {
             if (image_view) {
                 QImage img(width, height, QImage::Format_RGB32);
                 int gridSize = 50;
-                if (obj->verts.has_attr("alpha")) {
+                if (obj->verts.has_attr("alpha")&&enableAlpha) {
                     auto &alpha = obj->verts.attr<float>("alpha");
                     for (auto i = 0; i < obj->verts.size(); i++) {
                         int h = i / width;
@@ -207,6 +208,10 @@ ZenoImagePanel::ZenoImagePanel(QWidget *parent) : QWidget(parent) {
     pGamma->setCheckState(Qt::Checked);
     pTitleLayout->addWidget(pGamma);
 
+    pAlpha->setStyleSheet("color: white;");
+    pAlpha->setCheckState(Qt::Unchecked);
+    pTitleLayout->addWidget(pAlpha);
+
     pFit->setProperty("cssClass", "grayButton");
     pTitleLayout->addWidget(pFit);
 
@@ -260,6 +265,20 @@ ZenoImagePanel::ZenoImagePanel(QWidget *parent) : QWidget(parent) {
             }
         }
     });
+    connect(pAlpha, &QCheckBox::stateChanged, this, [=](int state) {
+        std::string prim_name = pPrimName->text().toStdString();
+        Zenovis* zenovis = wids[0]->getZenoVis();
+        ZASSERT_EXIT(zenovis);
+        auto session = zenovis->getSession();
+        ZASSERT_EXIT(session);
+        auto scene = session->get_scene();
+        ZASSERT_EXIT(scene);
+        for (auto const &[key, ptr]: scene->objectsMan->pairs()) {
+            if (key.find(prim_name) == 0) {
+                setPrim(key);
+            }
+        }
+    });
 
     connect(pFit, &QPushButton::clicked, this, [=](bool _) {
         image_view->fitMode = true;
diff --git a/ui/zenoedit/panel/zenoimagepanel.h b/ui/zenoedit/panel/zenoimagepanel.h
index 2e97311353..a9e18b71c2 100644
--- a/ui/zenoedit/panel/zenoimagepanel.h
+++ b/ui/zenoedit/panel/zenoimagepanel.h
@@ -14,6 +14,7 @@ class ZenoImagePanel : public QWidget {
     QLabel* pStatusBar = new QLabel();
     QLabel* pPrimName = new QLabel();
     QCheckBox *pGamma = new QCheckBox("Gamma");
+    QCheckBox *pAlpha = new QCheckBox("Checkerboard");
     QPushButton *pFit = new QPushButton("Fit");
     ZenoImageView *image_view = nullptr;
 

From 036d43a15396aca602e18b7c5e93cf2f0d015d87 Mon Sep 17 00:00:00 2001
From: YingQ <hqq.2020@foxmail.com>
Date: Tue, 19 Sep 2023 18:52:54 +0800
Subject: [PATCH 48/50] ImagealphaBlend

---
 projects/ImgCV/ImageComposite.cpp  | 499 ++++++++++-------------------
 projects/ImgCV/ImageProcessing.cpp |  18 +-
 2 files changed, 173 insertions(+), 344 deletions(-)

diff --git a/projects/ImgCV/ImageComposite.cpp b/projects/ImgCV/ImageComposite.cpp
index 594c847b58..1ca26589ba 100644
--- a/projects/ImgCV/ImageComposite.cpp
+++ b/projects/ImgCV/ImageComposite.cpp
@@ -568,257 +568,185 @@ ZENDEFNODE(Composite, {
         {"image"}
     },
     {},
-    { "comp" },
+    { "deprecated" },
 });
 
-struct Blend: INode {
-    virtual void apply() override {
-        auto blend = get_input<PrimitiveObject>("Foreground");
-        auto base = get_input<PrimitiveObject>("Background");
-        auto maskopacity = get_input2<float>("Mask Opacity");
 
-        auto compmode = get_input2<std::string>("Blending Mode");
-        auto opacity1 = get_input2<float>("Foreground Opacity");
-        auto opacity2 = get_input2<float>("Background Opacity");
-
-        auto &ud1 = blend->userData();
-        int w1 = ud1.get2<int>("w");
-        int h1 = ud1.get2<int>("h");
-        auto mask = std::make_shared<PrimitiveObject>();
-        if(has_input("Mask")) {
-            mask = get_input<PrimitiveObject>("Mask");
 
+template <class T>
+static T BlendMode(const float &alpha1, const float &alpha2, const T& rgb1, const T& rgb2, const vec3f opacity, std::string compmode)
+{
+        if(compmode == std::string("Copy")) {//copy and over is different!!!
+                T value = rgb1 * opacity[0] + rgb2 * (1 - opacity[0]);
+                return value;
         }
-        else {
-            mask->verts.resize(w1*h1);
-            mask->userData().set2("isImage", 1);
-            mask->userData().set2("w", w1);
-            mask->userData().set2("h", h1);
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    mask->verts[i * w1 + j] = {maskopacity,maskopacity,maskopacity};
-                }
-            }
+        else if(compmode == std::string("Over")) {
+                T value = (rgb1 + rgb2 * (1 - alpha1)) * opacity[0] + rgb2 * (1 - opacity[0]);
+                return value;
         }
-
-//就地修改比较快！
-
-//todo： image1和image2大小不同的情况
-
-        if(compmode == "Normal") {
-#pragma omp parallel for
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    vec3f rgb1 = blend->verts[i * w1 + j] * opacity1;
-                    vec3f rgb2 = base->verts[i * w1 + j] * opacity2;
-                    vec3f opacity = mask->verts[i * w1 + j] * maskopacity;
-                    vec3f c = rgb1 * opacity + rgb2 * (1 - opacity);
-                    blend->verts[i * w1 + j] = zeno::clamp(c, 0, 1);
-                }
-            }
+        else if(compmode == std::string("Under")) {
+                T value = (rgb2 + rgb1 * (1 - alpha2)) * opacity[0] + rgb2 * (1 - opacity[0]);
+                return value;
         }
-        
-        else if(compmode == "Add") {
-#pragma omp parallel for
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    vec3f rgb1 = blend->verts[i * w1 + j] * opacity1;
-                    vec3f rgb2 = base->verts[i * w1 + j] * opacity2;
-                    vec3f opacity = mask->verts[i * w1 + j] * maskopacity;
-                    vec3f c = zeno::min(rgb1 + rgb2, vec3f(1.0f))*opacity + rgb2 * (1 - opacity);
-                    blend->verts[i * w1 + j] = zeno::clamp(c, 0, 1);
-                }
-            }
-        }                
-
-        else if(compmode == "Subtract") {
-#pragma omp parallel for
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    vec3f rgb1 = blend->verts[i * w1 + j] * opacity1;
-                    vec3f rgb2 = base->verts[i * w1 + j] * opacity2;
-                    vec3f opacity = mask->verts[i * w1 + j] * maskopacity;
-                    vec3f c = zeno::max(rgb2 - rgb1, vec3f(0.0f))*opacity + rgb2 * (1 - opacity);
-                    blend->verts[i * w1 + j] = zeno::clamp(c, 0, 1);
-                }
-            }
+        else if(compmode == std::string("Atop")) {
+                T value = (rgb1 * alpha2 + rgb2 * (1 - alpha1)) * opacity[0] + rgb2 * (1 - opacity[0]);
+                return value;
         }
-
-        else if(compmode == "Multiply") {
-#pragma omp parallel for
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    vec3f rgb1 = blend->verts[i * w1 + j] * opacity1;
-                    vec3f rgb2 = base->verts[i * w1 + j] * opacity2;
-                    vec3f opacity = mask->verts[i * w1 + j] * maskopacity;
-                    vec3f c = rgb1 * rgb2 * opacity + rgb2 * (1 - opacity);
-                    blend->verts[i * w1 + j] = zeno::clamp(c, 0, 1);
-                }
-            }
+        else if(compmode == std::string("In")) {
+                T value = rgb1 * alpha2 * opacity[0] + rgb2 * (1 - opacity[0]);
+                return value;
         }
-
-        else if(compmode == "Max(Lighten)") {
-#pragma omp parallel for
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    vec3f rgb1 = blend->verts[i * w1 + j] * opacity1;
-                    vec3f rgb2 = base->verts[i * w1 + j] * opacity2;
-                    vec3f opacity = mask->verts[i * w1 + j] * maskopacity;
-                    vec3f c = zeno::max(rgb1, rgb2) * opacity + rgb2 * (1 - opacity);
-                    blend->verts[i * w1 + j] = zeno::clamp(c, 0, 1);
-                }
-            }
+        else if(compmode == std::string("Out")) {
+                T value = (rgb1 * (1 - alpha2)) * opacity[0] + rgb2 * (1 - opacity[0]);
+                return value;
         }
-
-        else if(compmode == "Min(Darken)") {
-#pragma omp parallel for
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    vec3f rgb1 = blend->verts[i * w1 + j] * opacity1;
-                    vec3f rgb2 = base->verts[i * w1 + j] * opacity2;
-                    vec3f opacity = mask->verts[i * w1 + j] * maskopacity;
-                    vec3f c = zeno::min(rgb1, rgb2) * opacity + rgb2 * (1 - opacity);
-                    blend->verts[i * w1 + j] = zeno::clamp(c, 0, 1);
-                }
-            }
+        else if(compmode == std::string("Xor")) {
+                T value = (rgb1 * (1 - alpha2) + rgb2 * (1 - alpha1)) * opacity[0] + rgb2 * (1 - opacity[0]);
+                return value;
         }
-/*
-        else if(compmode == "AddSub") {
-#pragma omp parallel for
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    vec3f &rgb1 = blend->verts[i * w1 + j] * opacity1;
-                    rgb1 = pow(rgb1, 1.0/2.2);
-                    vec3f &rgb2 = base->verts[i * w1 + j] * opacity2;
-                    rgb2 = pow(rgb2, 1.0/2.2);
-                    vec3f &opacity = mask->verts[i * w1 + j] * maskopacity;
-                    vec3f c;
+        else if(compmode == std::string("Add")) {
+                T value = zeno::min(rgb1 + rgb2, T(1.0f))*opacity[0] + rgb2 * (1 - opacity[0]);//clamp?
+                return value;
+        }
+        else if(compmode == std::string("Subtract")) {
+                T value = zeno::max(rgb2 - rgb1, T(0.0f))*opacity[0] + rgb2 * (1 - opacity[0]);
+                return value;
+        }
+        else if(compmode == std::string("Multiply")) {
+                T value = rgb1 * rgb2 * opacity[0] + rgb2 * (1 - opacity[0]);
+                return value;
+        }
+        else if(compmode == std::string("Max(Lighten)")) {
+                T value = zeno::max(rgb1, rgb2) * opacity[0] + rgb2 * (1 - opacity[0]);
+                return value;
+        }
+        else if(compmode == std::string("Min(Darken)")) {
+                T value = zeno::min(rgb1, rgb2) * opacity[0] + rgb2 * (1 - opacity[0]);
+                return value;
+        }
+        else if(compmode == std::string("Screen")) {//A+B-AB if A and B between 0-1, else A if A>B else B
+                    T value = (1 - (1 - rgb2) * (1 - rgb1)) * opacity[0] + rgb2 * (1 - opacity[0]);//only care 0-1!
+                    return value;
+        }
+        else if(compmode == std::string("Difference")) {
+                    T value = zeno::abs(rgb1 - rgb2) * opacity[0] + rgb2 * (1 - opacity[0]);
+                    return value;
+        }
+        else if(compmode == std::string("Average")) {
+                    T value = (rgb1 + rgb2) / 2 * opacity[0] + rgb2 * (1 - opacity[0]);
+                    return value;
+        }
+        return T(0);
+}
+
+static zeno::vec3f BlendModeV(const float &alpha1, const float &alpha2, const vec3f& rgb1, const vec3f& rgb2, const vec3f opacity, std::string compmode)
+{
+        if(compmode == std::string("Overlay")) {
+                    vec3f value;
                     for (int k = 0; k < 3; k++) {
-                        if (rgb1[k] > 0.5) {
-                            c[k] = rgb1[k] + rgb2[k];
+                        if (rgb2[k] < 0.5) {
+                            value[k] = 2 * rgb1[k] * rgb2[k];
                         } else {
-                            c[k] = rgb2[k] - rgb1[k];
+                            value[k] = 1 - 2 * (1 - rgb1[k]) * (1 - rgb2[k]);//screen
                         }
                     }
-                    c = pow(c, 2.2) * opacity + pow(rgb2, 2.2) * (1 - opacity);
-                    //c = c * opacity + rgb2 * (1 - opacity);
-                    //c = pow(c, 2.2);
-                    blend->verts[i * w1 + j] = zeno::clamp(c, 0, 1);
-                }
-            }
-        
-*/
-        else if(compmode == "Overlay") {
-#pragma omp parallel for
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    vec3f rgb1 = blend->verts[i * w1 + j] * opacity1;
-                    //rgb1 = pow(rgb1, 1.0/2.2);
-                    vec3f rgb2 = base->verts[i * w1 + j] * opacity2;
-                    //rgb2 = pow(rgb2, 1.0/2.2);
-                    vec3f opacity = mask->verts[i * w1 + j] * maskopacity;
-                    vec3f c;
+                    value = value * opacity[0] + rgb2 * (1 - opacity[0]);
+                    return value;
+        }
+        else if(compmode == std::string("SoftLight")) {
+                    vec3f value;
                     for (int k = 0; k < 3; k++) {
-                        if ( rgb2[k] < 0.5) {
-                            c[k] = 2 * rgb1[k] * rgb2[k];
+                        if (rgb1[k] < 0.5) {
+                            value[k] = 2 * rgb1[k] * rgb2[k] + rgb2[k] * rgb2[k] * (1 - 2 * rgb1[k]);
                         } else {
-                            c[k] = 1 - 2 * (1 - rgb1[k]) * (1 - rgb2[k]);
+                            value[k] = 2 * rgb2[k] * (1 - rgb1[k]) + sqrt(rgb2[k]) * (2 * rgb1[k] - 1);
                         }
                     }
-                    c = c * opacity + rgb2 * (1 - opacity);
-                    //c = pow(c, 2.2);
-                    blend->verts[i * w1 + j] = zeno::clamp(c, 0, 1);
-                }
-            }
-        }
-
-        else if(compmode == "Screen") {
-#pragma omp parallel for
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    vec3f rgb1 = blend->verts[i * w1 + j] * opacity1;
-                    vec3f rgb2 = base->verts[i * w1 + j] * opacity2;
-                    vec3f opacity = mask->verts[i * w1 + j] * maskopacity;
-                    vec3f c = (1 - (1 - rgb2) * (1 - rgb1)) * opacity + rgb2 * (1 - opacity);
-                    blend->verts[i * w1 + j] = zeno::clamp(c, 0, 1);
-                }
-            }
+                    value = value * opacity[0] + rgb2 * (1 - opacity[0]);
+                    return value;
         }
-
-        else if(compmode == "SoftLight") {
-#pragma omp parallel for
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    vec3f rgb1 = blend->verts[i * w1 + j] * opacity1;
-                    //rgb1 = pow(rgb1, 1.0/2.2);
-                    vec3f rgb2 = base->verts[i * w1 + j] * opacity2;
-                    //rgb2 = pow(rgb2, 1.0/2.2);
-                    vec3f opacity = mask->verts[i * w1 + j] * maskopacity;
-                    vec3f c;
+        else if(compmode == std::string("Divide")) {
+                    vec3f value;
                     for (int k = 0; k < 3; k++) {
-                        if (rgb1[k] < 0.5) {
-                            c[k] = 2 * rgb1[k] * rgb2[k] + rgb2[k] * rgb2[k] * (1 - 2 * rgb1[k]);
+                        if (rgb1[k] == 0) {
+                            value[k] = 1;
                         } else {
-                            c[k] = 2 * rgb2[k] * (1 - rgb1[k]) + sqrt(rgb2[k]) * (2 * rgb1[k] - 1);
+                            value[k] = rgb2[k] / rgb1[k];
                         }
                     }
-                    c = c * opacity + rgb2 * (1 - opacity);
-                    //c = pow(c, 2.2);
-                    blend->verts[i * w1 + j] = zeno::clamp(c, 0, 1);
-                }
-            }
+                    value = value * opacity[0] + rgb2 * (1 - opacity[0]);
+                    return value;
         }
+        return zeno::vec3f(0);
+}
 
-        else if(compmode == "Difference") {
-#pragma omp parallel for
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    vec3f rgb1 = blend->verts[i * w1 + j] * opacity1;
-                    vec3f rgb2 = base->verts[i * w1 + j] * opacity2;
-                    vec3f opacity = mask->verts[i * w1 + j] * maskopacity;
-                    vec3f c = zeno::abs(rgb1 - rgb2) * opacity + rgb2 * (1 - opacity);
-                    blend->verts[i * w1 + j] = zeno::clamp(c, 0, 1);
-                }
+struct Blend: INode {
+    virtual void apply() override {//todo:: add blend scope RGBA and Premultiplied / Alpha Blending(https://github.com/jamieowen/glsl-blend/issues/6)
+        auto blend = get_input<PrimitiveObject>("Foreground");
+        auto base = get_input<PrimitiveObject>("Background");
+        auto maskopacity = get_input2<float>("Mask Opacity");
+
+        auto compmode = get_input2<std::string>("Blending Mode");
+        //auto alphablend = get_input2<std::string>("Alpha Blending");
+        auto alphamode = get_input2<std::string>("Alpha Mode");
+        auto opacity1 = get_input2<float>("Foreground Opacity");
+        auto opacity2 = get_input2<float>("Background Opacity");
+
+        auto &ud1 = blend->userData();
+        int w1 = ud1.get2<int>("w");
+        int h1 = ud1.get2<int>("h");
+        int imagesize = w1 * h1;
+        auto mask = std::make_shared<PrimitiveObject>();
+        if(has_input("Mask")) {
+            mask = get_input<PrimitiveObject>("Mask");
+
+        }
+        else {
+            mask->verts.resize(w1*h1);
+            mask->userData().set2("isImage", 1);
+            mask->userData().set2("w", w1);
+            mask->userData().set2("h", h1);
+            for (int i = 0; i < imagesize; i++) {
+                    mask->verts[i] = {maskopacity,maskopacity,maskopacity};
             }
         }
-    
-        else if(compmode == "Divide") {
-#pragma omp parallel for
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    vec3f rgb1 = blend->verts[i * w1 + j] * opacity1;
-                    vec3f rgb2 = base->verts[i * w1 + j] * opacity2;
-                    vec3f opacity = mask->verts[i * w1 + j] * maskopacity;
-                    vec3f c;
-                    for (int k = 0; k < 3; k++) {
-                        if (rgb1[k] == 0) {
-                            c[k] = 1;
-                        } else {
+        auto image2 = std::make_shared<PrimitiveObject>();
+        image2->userData().set2("isImage", 1);
+        image2->userData().set2("w", w1);
+        image2->userData().set2("h", h1);
+        image2->verts.resize(imagesize);
+        bool alphaoutput =  blend->has_attr("alpha")||base->has_attr("alpha");
+        auto &image2alpha = image2->add_attr<float>("alpha");
+        auto &blendalpha = blend->has_attr("alpha")?blend->attr<float>("alpha"):std::vector<float>(imagesize, 1.0f);
+        auto &basealpha = base->has_attr("alpha")?base->attr<float>("alpha"):std::vector<float>(imagesize, 1.0f);
 
-                            c[k] = rgb2[k] / rgb1[k];
-                        }
-                    }
-                    c = c * opacity + rgb2 * (1 - opacity);
-                    blend->verts[i * w1 + j] = zeno::clamp(c, 0, 1);
+//todo： rgb1和rgb2大小不同的情况
+#pragma omp parallel for
+            for (int i = 0; i < imagesize; i++) {
+                vec3f rgb1 = zeno::clamp(blend->verts[i], 0, 1) * opacity1;
+                vec3f rgb2 = zeno::clamp(base->verts[i], 0, 1) * opacity2;
+                vec3f opacity = zeno::clamp(mask->verts[i], 0, 1) * maskopacity;
+                if(compmode == "Overlay"||compmode == "Softlight"||compmode == "Divide"){
+                    vec3f c = BlendModeV(blendalpha[i], basealpha[i], rgb1, rgb2, opacity, compmode);
+                    image2->verts[i] = zeno::clamp(c, 0, 1);
+                }
+                else{
+                    vec3f c = BlendMode<zeno::vec3f>(blendalpha[i], basealpha[i], rgb1, rgb2, opacity, compmode);
+                    image2->verts[i] = zeno::clamp(c, 0, 1);
                 }
             }
-        }
-
-        else if(compmode == "Average") {
+            if(alphaoutput) {//如果两个输入 其中一个没有alpha  对于rgb和alpha  alpha的默认值不一样 前者为1 后者为0？
+                auto &blendalpha = blend->has_attr("alpha")?blend->attr<float>("alpha"):blend->add_attr<float>("alpha");//只有blendbase都没有alpha 结果才没有
+                auto &basealpha = base->has_attr("alpha")?base->attr<float>("alpha"):base->add_attr<float>("alpha");
 #pragma omp parallel for
-            for (int i = 0; i < h1; i++) {
-                for (int j = 0; j < w1; j++) {
-                    vec3f rgb1 = blend->verts[i * w1 + j] * opacity1;
-                    vec3f rgb2 = base->verts[i * w1 + j] * opacity2;
-                    vec3f opacity = mask->verts[i * w1 + j] * maskopacity;
-                    vec3f c = (rgb1 + rgb2) / 2 * opacity + rgb2 * (1 - opacity);
-                    blend->verts[i * w1 + j] = zeno::clamp(c, 0, 1);
+                for (int i = 0; i < imagesize; i++) {
+                vec3f opacity = zeno::clamp(mask->verts[i], 0, 1) * maskopacity;
+                float alpha = BlendMode<float>(blendalpha[i], basealpha[i], blendalpha[i], basealpha[i], opacity, alphamode);
+                image2alpha[i] = zeno::clamp(alpha, 0, 1);
                 }
             }
-        }
-        
 
-        set_output("image", blend);
+        set_output("image", image2);
     }
 };
 
@@ -827,7 +755,9 @@ ZENDEFNODE(Blend, {
         {"Foreground"},
         {"Background"},
         {"Mask"},
-        {"enum Normal Add Subtract Multiply Max(Lighten) Min(Darken) Overlay Screen SoftLight Difference Divide Average", "Blending Mode", "Normal"},
+        {"enum Over Copy Under Atop In Out Screen Add Subtract Multiply Max(Lighten) Min(Darken) Average Difference Overlay SoftLight Divide Xor", "Blending Mode", "Normal"},
+        //{"enum IgnoreAlpha SourceAlpha", "Alpha Blending", "Ignore Alpha"}, SUBSTANCE DESIGNER ALPHA MODE
+        {"enum Over Under Atop In Out Screen Add Subtract Multiply Max(Lighten) Min(Darken) Average Difference Xor", "Alpha Mode", "Over"},
         {"float", "Mask Opacity", "1"},
         {"float", "Foreground Opacity", "1"},
         {"float", "Background Opacity", "1"},
@@ -839,89 +769,6 @@ ZENDEFNODE(Blend, {
     { "comp" },
 });
 
-
-/*
-struct CompositeCV: INode {
-    virtual void apply() override {
-        auto image1 = get_input<PrimitiveObject>("Foreground");
-        auto image2 = get_input<PrimitiveObject>("Background");
-        auto mode = get_input2<std::string>("mode");
-        auto Alpha1 = get_input2<float>("Alpha1");
-        auto Alpha2 = get_input2<float>("Alpha2");
-        auto &a1 = image1->verts.attr<float>("alpha");
-        auto &a2 = image2->verts.attr<float>("alpha");
-
-        auto &ud1 = image1->userData();
-        int w1 = ud1.get2<int>("w");
-        int h1 = ud1.get2<int>("h");
-        auto &ud2 = image2->userData();
-        int w2 = ud2.get2<int>("w");
-        int h2 = ud2.get2<int>("h");
-
-        cv::Mat imagecvin1(h1, w1, CV_32FC3);
-        cv::Mat imagecvin2(h2, w2, CV_32FC3);
-        cv::Mat imagecvadd(h1, w1, CV_32FC3);
-        cv::Mat imagecvsub(h1, w1, CV_32FC3);
-        cv::Mat imagecvout(h1, w1, CV_32FC3);
-
-        for (int i = 0; i < h1; i++) {
-            for (int j = 0; j < w1; j++) {
-                vec3f rgb = image1->verts[i * w1 + j];
-                imagecvin1.at<cv::Vec3f>(i, j) = {rgb[0], rgb[1], rgb[2]};
-            }
-        }
-        for (int i = 0; i < h2; i++) {
-            for (int j = 0; j < w2; j++) {
-                vec3f rgb = image2->verts[i * w2 + j];
-                imagecvin2.at<cv::Vec3f>(i, j) = {rgb[0], rgb[1], rgb[2]};
-            }
-        }
-        cv::resize(imagecvin2, imagecvin2,imagecvin1.size());
-        if(mode == "Add"){
-            cv::addWeighted(imagecvin1, Alpha1, imagecvin2, Alpha2, 0, imagecvout);
-        }
-        if(mode == "Subtract"){
-            cv::subtract(imagecvin1*Alpha1, imagecvin2*Alpha2, imagecvout);
-        }
-        if(mode == "Multiply"){
-            cv::multiply(imagecvin1*Alpha1, imagecvin2*Alpha2, imagecvout);
-        }
-        if(mode == "Divide"){
-            cv::divide(imagecvin1*Alpha1, imagecvin2*Alpha2, imagecvout, 1,  -1);
-        }
-        if(mode == "Diff"){
-            cv::absdiff(imagecvin1*Alpha1, imagecvin2*Alpha2, imagecvout);
-        }
-
-        for (int i = 0; i < h1; i++) {
-            for (int j = 0; j < w1; j++) {
-                cv::Vec3f rgb = imagecvout.at<cv::Vec3f>(i, j);
-                image1->verts[i * w1 + j] = {rgb[0], rgb[1], rgb[2]};
-            }
-        }
-        set_output("image", image1);
-    }
-};
-
-ZENDEFNODE(CompositeCV, {
-    {
-        {"Foreground"},
-        {"Background"},
-        {"enum Add(Linear Dodge) Subtract Multiply Add Sub Diff", "mode", "Add"},
-        {"float", "Alpha1", "1"},
-        {"float", "Alpha2", "1"},
-    },
-    {
-        {"image"}
-    },
-    {},
-    { "comp" },
-});
-
-*/
-
-
-
 // 自定义卷积核
 std::vector<std::vector<float>> createKernel(float blurValue,
                                              float l_blurValue, float r_blurValue,
@@ -1010,7 +857,7 @@ ZENDEFNODE(CompBlur, {
     { "comp" },
 });
 
-struct CompExtractChanel_gray: INode {
+/*struct CompExtractChanel_gray: INode {
     virtual void apply() override {
         auto image = get_input<PrimitiveObject>("image");
         auto RGB = get_input2<bool>("RGB");
@@ -1087,9 +934,9 @@ ZENDEFNODE(CompExtractChanel_gray, {
     },
     {},
     { "deprecated" },
-});
+});*/
 
-struct CompExtractChanel : INode {
+struct ImageExtractChannel : INode {//why so slow...
     virtual void apply() override {
         auto image = get_input<PrimitiveObject>("image");
         auto channel = get_input2<std::string>("channel");
@@ -1103,50 +950,34 @@ struct CompExtractChanel : INode {
         image2->verts.resize(image->size());
         if(channel == "R") {
             for (auto i = 0; i < image->verts.size(); i++) {
-                image2->verts[i][0] = image->verts[i][0];
-                image2->verts[i][1] = image->verts[i][0];
-                image2->verts[i][2] = image->verts[i][0];
+                image2->verts[i] = vec3f(image->verts[i][0]);//为了速度 verts也要提前吗？ 试一下
             }
         }
-        if(channel == "G") {
+        else if(channel == "G") {
             for (auto i = 0; i < image->verts.size(); i++) {
-                image2->verts[i][0] = image->verts[i][1];
-                image2->verts[i][1] = image->verts[i][1];
-                image2->verts[i][2] = image->verts[i][1];
+                image2->verts[i] = vec3f(image->verts[i][1]);
             }
         }
-        if(channel == "B") {
+        else if(channel == "B") {
             for (auto i = 0; i < image->verts.size(); i++) {
-                image2->verts[i][0] = image->verts[i][2];
-                image2->verts[i][1] = image->verts[i][2];
-                image2->verts[i][2] = image->verts[i][2];
+                image2->verts[i] = vec3f(image->verts[i][2]);
             }
         }
-        if(channel == "A") {
+        else if(channel == "A") {
             if (image->verts.has_attr("alpha")) {
-                auto &Alpha = image->verts.attr<float>("alpha");
-                image2->verts.add_attr<float>("alpha");
-                image2->verts.attr<float>("alpha")=image->verts.attr<float>("alpha");
-                for(int i = 0;i < w * h;i++){
-                    image2->verts[i][0] = image->verts.attr<float>("alpha")[i];
-                    image2->verts[i][1] = image->verts.attr<float>("alpha")[i];
-                    image2->verts[i][2] = image->verts.attr<float>("alpha")[i];
+                auto &attr = image->verts.attr<float>("alpha");
+                for(int i = 0; i < w * h; i++){
+                    image2->verts[i] = {attr[i], attr[i], attr[i]};
                 }
             }
             else{
-                image2->verts.add_attr<float>("alpha");
-                for(int i = 0;i < w * h;i++){
-                    image2->verts.attr<float>("alpha")[i] = 1.0;
-                    image2->verts[i][0] = image->verts.attr<float>("alpha")[i];
-                    image2->verts[i][1] = image->verts.attr<float>("alpha")[i];
-                    image2->verts[i][2] = image->verts.attr<float>("alpha")[i];
-                }
+                throw zeno::makeError("image have no alpha channel");
             }
         }
         set_output("image", image2);
     }
 };
-ZENDEFNODE(CompExtractChanel, {
+ZENDEFNODE(ImageExtractChannel, {
     {
         {"image"},
         {"enum R G B A", "channel", "R"},
@@ -1167,8 +998,8 @@ struct CompImport : INode {
     virtual void apply() override {
         auto prim = get_input<PrimitiveObject>("prim");
         auto &ud = prim->userData();
-        int nx = ud.get2<int>("nx");
-        int ny = ud.get2<int>("ny");
+        int nx = ud.has("nx")?ud.get2<int>("nx"):ud.get2<int>("w");
+        int ny = ud.has("ny")?ud.get2<int>("ny"):ud.get2<int>("h");
         auto attrName = get_input2<std::string>("attrName");
         auto remapRange = get_input2<vec2f>("RemapRange");
         auto remap = get_input2<bool>("Remap");
@@ -1177,12 +1008,10 @@ struct CompImport : INode {
         image->userData().set2("isImage", 1);
         image->userData().set2("w", nx);
         image->userData().set2("h", ny);
-        //zeno::PrimitiveObject *prim = prim.get();
 
         if (prim->verts.attr_is<float>(attrName)) {
             auto &attr = prim->verts.attr<float>(attrName);
             //calculate max and min attr value and remap it to 0-1
-            //minresult = prim_reduce<float>(prim.get(), attr);
             float minresult = zeno::parallel_reduce_array<float>(attr.size(), attr[0], [&] (size_t i) -> float { return attr[i]; },
             [&] (float i, float j) -> float { return zeno::min(i, j); });
             float maxresult = zeno::parallel_reduce_array<float>(attr.size(), attr[0], [&] (size_t i) -> float { return attr[i]; },
@@ -1202,7 +1031,7 @@ struct CompImport : INode {
                 }
             }
         }
-        else if (prim->verts.attr_is<vec3f>(attrName)) {
+        else if (prim->verts.attr_is<vec3f>(attrName)) {//todo::add remap
             auto &attr = prim->attr<vec3f>(attrName);
             for (auto i = 0; i < nx * ny; i++) {
                 image->verts[i] = attr[i];
diff --git a/projects/ImgCV/ImageProcessing.cpp b/projects/ImgCV/ImageProcessing.cpp
index 8d69b30a85..e7672cb7c2 100644
--- a/projects/ImgCV/ImageProcessing.cpp
+++ b/projects/ImgCV/ImageProcessing.cpp
@@ -1733,20 +1733,20 @@ ZENDEFNODE(ImageErode, {
 struct ImageColor : INode {
     virtual void apply() override {
         auto image = std::make_shared<PrimitiveObject>();
-        auto color = get_input2<vec3f>("Color");
+        auto color = get_input2<vec4f>("Color");
         auto size = get_input2<vec2i>("Size");
-        image->verts.resize(size[0] * size[1]);
+        auto vertsize = size[0] * size[1];
+        image->verts.resize(vertsize);
         image->userData().set2("isImage", 1);
         image->userData().set2("w", size[0]);
         image->userData().set2("h", size[1]);
+        image->verts.add_attr<float>("alpha");
 
 #pragma omp parallel
-        for (int i = 0; i < size[1]; i++) {
-            for (int j = 0; j < size[0]; j++) {
-                image->verts[i * size[0] + j] = {color[0], color[1], color[2]};
-            }
+        for (int i = 0; i < vertsize ; i++) {
+                image->verts[i ] = {color[0], color[1], color[2]};
+                image->verts.attr<float>("alpha")[i] = color[3];
         }
-
         set_output("image", image);
         
     }
@@ -1754,7 +1754,7 @@ struct ImageColor : INode {
 
 ZENDEFNODE(ImageColor, {
     {
-        {"vec3f", "Color", "1,1,1"},
+        {"vec4f", "Color", "1,1,1,1"},
         {"vec2i", "Size", "1024,1024"},
     },
     {
@@ -2447,7 +2447,7 @@ ZENDEFNODE(ImageShape, {
     {"deprecated"},
 });
 
-struct ImageLevels: INode {
+struct ImageLevels: INode {//todo :latger than 1 ?
     void apply() override {
         std::shared_ptr<PrimitiveObject> image = get_input<PrimitiveObject>("image");
         auto inputLevels = get_input2<vec2f>("Input Levels");

From da7eff42d9a5892ae0e75af5559f94b66eb948be Mon Sep 17 00:00:00 2001
From: YingQ <hqq.2020@foxmail.com>
Date: Wed, 20 Sep 2023 17:41:14 +0800
Subject: [PATCH 49/50] levelAlpha

---
 projects/ImgCV/ImageComposite.cpp  |  19 +-
 projects/ImgCV/ImageProcessing.cpp | 304 +++++------------------------
 2 files changed, 67 insertions(+), 256 deletions(-)

diff --git a/projects/ImgCV/ImageComposite.cpp b/projects/ImgCV/ImageComposite.cpp
index 1ca26589ba..5667ee8feb 100644
--- a/projects/ImgCV/ImageComposite.cpp
+++ b/projects/ImgCV/ImageComposite.cpp
@@ -605,11 +605,11 @@ static T BlendMode(const float &alpha1, const float &alpha2, const T& rgb1, cons
                 return value;
         }
         else if(compmode == std::string("Add")) {
-                T value = zeno::min(rgb1 + rgb2, T(1.0f))*opacity[0] + rgb2 * (1 - opacity[0]);//clamp?
+                T value = (rgb1 + rgb2) * opacity[0] + rgb2 * (1 - opacity[0]);//clamp?
                 return value;
         }
         else if(compmode == std::string("Subtract")) {
-                T value = zeno::max(rgb2 - rgb1, T(0.0f))*opacity[0] + rgb2 * (1 - opacity[0]);
+                T value = (rgb2 - rgb1) * opacity[0] + rgb2 * (1 - opacity[0]);
                 return value;
         }
         else if(compmode == std::string("Multiply")) {
@@ -662,6 +662,13 @@ static zeno::vec3f BlendModeV(const float &alpha1, const float &alpha2, const ve
                             value[k] = 2 * rgb2[k] * (1 - rgb1[k]) + sqrt(rgb2[k]) * (2 * rgb1[k] - 1);
                         }
                     }
+                    /*for (int k = 0; k < 3; k++) {     Nuke method
+                        if (rgb1[k] * rgb2[k] < 1) {
+                            value[k] = rgb2[k] * (2 * rgb1[k] + (rgb2[k] * (1-rgb1[k] * rgb2[k])));
+                        } else {
+                            value[k] = 2 * rgb2[k] * rgb1[k];
+                        }
+                    }*/
                     value = value * opacity[0] + rgb2 * (1 - opacity[0]);
                     return value;
         }
@@ -726,7 +733,7 @@ struct Blend: INode {
                 vec3f rgb1 = zeno::clamp(blend->verts[i], 0, 1) * opacity1;
                 vec3f rgb2 = zeno::clamp(base->verts[i], 0, 1) * opacity2;
                 vec3f opacity = zeno::clamp(mask->verts[i], 0, 1) * maskopacity;
-                if(compmode == "Overlay"||compmode == "Softlight"||compmode == "Divide"){
+                if(compmode == "Overlay" || compmode == "SoftLight" || compmode == "Divide"){
                     vec3f c = BlendModeV(blendalpha[i], basealpha[i], rgb1, rgb2, opacity, compmode);
                     image2->verts[i] = zeno::clamp(c, 0, 1);
                 }
@@ -738,6 +745,7 @@ struct Blend: INode {
             if(alphaoutput) {//如果两个输入 其中一个没有alpha  对于rgb和alpha  alpha的默认值不一样 前者为1 后者为0？
                 auto &blendalpha = blend->has_attr("alpha")?blend->attr<float>("alpha"):blend->add_attr<float>("alpha");//只有blendbase都没有alpha 结果才没有
                 auto &basealpha = base->has_attr("alpha")?base->attr<float>("alpha"):base->add_attr<float>("alpha");
+                //std::string alphablendmode = alphamode == "SameWithBlend" ? compmode : alphamode;
 #pragma omp parallel for
                 for (int i = 0; i < imagesize; i++) {
                 vec3f opacity = zeno::clamp(mask->verts[i], 0, 1) * maskopacity;
@@ -757,6 +765,7 @@ ZENDEFNODE(Blend, {
         {"Mask"},
         {"enum Over Copy Under Atop In Out Screen Add Subtract Multiply Max(Lighten) Min(Darken) Average Difference Overlay SoftLight Divide Xor", "Blending Mode", "Normal"},
         //{"enum IgnoreAlpha SourceAlpha", "Alpha Blending", "Ignore Alpha"}, SUBSTANCE DESIGNER ALPHA MODE
+        //{"enum SameWithBlend Over Under Atop In Out Screen Add Subtract Multiply Max(Lighten) Min(Darken) Average Difference Xor", "Alpha Mode", "SameWithBlend"},
         {"enum Over Under Atop In Out Screen Add Subtract Multiply Max(Lighten) Min(Darken) Average Difference Xor", "Alpha Mode", "Over"},
         {"float", "Mask Opacity", "1"},
         {"float", "Foreground Opacity", "1"},
@@ -936,7 +945,7 @@ ZENDEFNODE(CompExtractChanel_gray, {
     { "deprecated" },
 });*/
 
-struct ImageExtractChannel : INode {//why so slow...
+struct ImageExtractChannel : INode {
     virtual void apply() override {
         auto image = get_input<PrimitiveObject>("image");
         auto channel = get_input2<std::string>("channel");
@@ -950,7 +959,7 @@ struct ImageExtractChannel : INode {//why so slow...
         image2->verts.resize(image->size());
         if(channel == "R") {
             for (auto i = 0; i < image->verts.size(); i++) {
-                image2->verts[i] = vec3f(image->verts[i][0]);//为了速度 verts也要提前吗？ 试一下
+                image2->verts[i] = vec3f(image->verts[i][0]);
             }
         }
         else if(channel == "G") {
diff --git a/projects/ImgCV/ImageProcessing.cpp b/projects/ImgCV/ImageProcessing.cpp
index e7672cb7c2..e52cf97a3d 100644
--- a/projects/ImgCV/ImageProcessing.cpp
+++ b/projects/ImgCV/ImageProcessing.cpp
@@ -1225,36 +1225,6 @@ ZENDEFNODE(ImageBilateralBlur, {
     { "image" },
 });
 
-struct CreateImage : INode {
-    virtual void apply() override {
-        auto RGB = get_input2<vec3f>("RGB");
-        auto x = get_input2<int>("width");
-        auto y = get_input2<int>("height");
-
-        auto image = std::make_shared<PrimitiveObject>();
-        image->verts.resize(x * y);
-        image->userData().set2("h", y);
-        image->userData().set2("w", x);
-        image->userData().set2("isImage", 1);
-        for (int i = 0; i < x * y; i++){
-            image->verts[i] = {zeno::clamp(RGB[0]/255, 0, 255),zeno::clamp(RGB[1]/255, 0, 255),zeno::clamp(RGB[2]/255, 0, 255)};
-        }
-        set_output("image", image);
-    }
-};
-
-ZENDEFNODE(CreateImage, {
-    {
-        {"vec3f", "RGB", "255,255,255"},
-        {"int", "width", "1024"},
-        {"int", "height", "1024"},
-    },
-    {
-        {"image"}
-    },
-    {},
-    { "create" },
-});
 
 struct ImageEditContrast : INode {
     virtual void apply() override {
@@ -1408,7 +1378,7 @@ ZENDEFNODE(ImageToNormalMap, {
     { "image" },
 });
 
-struct ImageGray : INode {
+struct ImageGray : INode {//todo
     void apply() override {
         auto image = get_input<PrimitiveObject>("image");
         auto mode = get_input2<std::string>("mode");
@@ -1441,28 +1411,6 @@ ZENDEFNODE(ImageGray, {
     { "image" },
 });
 
-struct ImageGetSize : INode {
-    void apply() override {
-        auto image = get_input<PrimitiveObject>("image");
-        auto &ud = image->userData();
-        int w = ud.get2<int>("w");
-        int h = ud.get2<int>("h");
-        set_output2("width", w);
-        set_output2("height", h);
-    }
-};
-ZENDEFNODE(ImageGetSize, {
-    {
-        {"image"},
-    },
-    {
-        {"int", "width"},
-        {"int", "height"},
-    },
-    {},
-    {"image"},
-});
-
 static std::shared_ptr<PrimitiveObject> normal_tiling(std::shared_ptr<PrimitiveObject> &image1, std::shared_ptr<PrimitiveObject> &image2, int rows, int cols) {
     int width = image1->userData().get2<int>("w");
     int height = image1->userData().get2<int>("h");
@@ -1735,17 +1683,23 @@ struct ImageColor : INode {
         auto image = std::make_shared<PrimitiveObject>();
         auto color = get_input2<vec4f>("Color");
         auto size = get_input2<vec2i>("Size");
+        auto balpha = get_input2<bool>("alpha");
         auto vertsize = size[0] * size[1];
         image->verts.resize(vertsize);
         image->userData().set2("isImage", 1);
         image->userData().set2("w", size[0]);
         image->userData().set2("h", size[1]);
-        image->verts.add_attr<float>("alpha");
-
-#pragma omp parallel
-        for (int i = 0; i < vertsize ; i++) {
-                image->verts[i ] = {color[0], color[1], color[2]};
-                image->verts.attr<float>("alpha")[i] = color[3];
+        if(balpha){
+            auto &alphaAttr = image->verts.add_attr<float>("alpha");
+            for (int i = 0; i < vertsize ; i++) {
+                image->verts[i] = {zeno::clamp(color[0], 0.0f, 1.0f), zeno::clamp(color[1], 0.0f, 1.0f), zeno::clamp(color[2], 0.0f, 1.0f)};
+                alphaAttr[i] = zeno::clamp(color[3], 0.0f, 1.0f);
+            }
+        }
+        else{
+            for (int i = 0; i < vertsize ; i++) {
+                image->verts[i] = {zeno::clamp(color[0], 0.0f, 1.0f), zeno::clamp(color[1], 0.0f, 1.0f), zeno::clamp(color[2], 0.0f, 1.0f)};
+            }
         }
         set_output("image", image);
         
@@ -1756,6 +1710,7 @@ ZENDEFNODE(ImageColor, {
     {
         {"vec4f", "Color", "1,1,1,1"},
         {"vec2i", "Size", "1024,1024"},
+        {"bool", "alpha", "1"},
     },
     {
         {"image"},
@@ -2247,155 +2202,6 @@ ZENDEFNODE(ImageMatting, {
     { "image" },
 });
 
-struct ImageDelAlpha: INode {
-    virtual void apply() override {
-        auto image = get_input<PrimitiveObject>("image");
-        auto &ud = image->userData();
-        int w = ud.get2<int>("w");
-        int h = ud.get2<int>("h");
-
-        if(image->verts.has_attr("alpha")){
-            image->verts.erase_attr("alpha");
-        }
-        set_output("image", image);
-    }
-};
-
-ZENDEFNODE(ImageDelAlpha, {
-    {
-        {"image"},
-    },
-    {
-        {"image"}
-    },
-    {},
-    { "image" },
-});
-
-struct ImageAddAlpha: INode {
-    virtual void apply() override {
-        auto image = get_input<PrimitiveObject>("image");
-        auto &ud = image->userData();
-        int w = ud.get2<int>("w");
-        int h = ud.get2<int>("h");
-        auto maskmode = get_input2<std::string>("maskmode");
-        image->verts.add_attr<float>("alpha");
-        for(int i = 0;i < image->size();i++){
-            image->verts.attr<float>("alpha")[i] = 1;
-        }
-        if (has_input("mask")) {
-            auto gimage = get_input2<PrimitiveObject>("mask");
-            auto &gud = gimage->userData();
-            int wg = gud.get2<int>("w");
-            int hg = gud.get2<int>("h");
-            if (wg == w && hg == h) {
-                 if (maskmode == "gray_black") {
-                    if (gimage->verts.has_attr("alpha")) {
-                        for (int i = 0; i < h; i++) {
-                            for (int j = 0; j < w; j++) {
-                                if (gimage->verts.attr<float>("alpha")[i * w + j] != 0 &&
-                                    image->verts.attr<float>("alpha")[i * w + j] != 0) {
-                                    image->verts.attr<float>("alpha")[i * w + j] = 1 - gimage->verts[i * w + j][0];
-                                } else {
-                                    image->verts.attr<float>("alpha")[i * w + j] = 0;
-                                }
-                            }
-                        }
-                    } else {
-                        for (int i = 0; i < h; i++) {
-                            for (int j = 0; j < w; j++) {
-                                if (image->verts.attr<float>("alpha")[i * w + j] != 0) {
-                                    image->verts.attr<float>("alpha")[i * w + j] = 1 - gimage->verts[i * w + j][0];
-                                }
-                            }
-                        }
-                    }
-                } else if (maskmode == "gray_white") {
-                    if (gimage->verts.has_attr("alpha")) {
-                        for (int i = 0; i < h; i++) {
-                            for (int j = 0; j < w; j++) {
-                                if (gimage->verts.attr<float>("alpha")[i * w + j] != 0 &&
-                                    image->verts.attr<float>("alpha")[i * w + j] != 0) {
-                                    image->verts.attr<float>("alpha")[i * w + j] = gimage->verts[i * w + j][0];
-                                } else {
-                                    image->verts.attr<float>("alpha")[i * w + j] = 0;
-                                }
-                            }
-                        }
-                    } else {
-                        for (int i = 0; i < h; i++) {
-                            for (int j = 0; j < w; j++) {
-                                if (image->verts.attr<float>("alpha")[i * w + j] != 0) {
-                                    image->verts.attr<float>("alpha")[i * w + j] = gimage->verts[i * w + j][0];
-                                }
-                            }
-                        }
-                    }
-                }
-                else if (maskmode == "alpha") {
-                    if (gimage->verts.has_attr("alpha")) {
-                        image->verts.attr<float>("alpha") = gimage->verts.attr<float>("alpha");
-                    } else {
-                        for (int i = 0; i < h; i++) {
-                            for (int j = 0; j < w; j++) {
-                                if (image->verts.attr<float>("alpha")[i * w + j] != 0) {
-                                    image->verts.attr<float>("alpha")[i * w + j] = 1;
-                                }
-                            }
-                        }
-                    }
-                }
-            }
-        }
-        set_output("image", image);
-    }
-};
-ZENDEFNODE(ImageAddAlpha, {
-    {
-        {"image"},
-        {"mask"},
-        {"enum alpha gray_black gray_white", "maskmode", "alpha"},
-    },
-    {
-        {"image"}
-    },
-    {},
-    { "image" },
-});
-
-struct ImageCut: INode {
-    void apply() override {
-        std::shared_ptr<PrimitiveObject> image = get_input<PrimitiveObject>("image");
-        auto tilemode = get_input2<std::string>("tilemode");
-        auto rows = get_input2<int>("rows");
-        auto cols = get_input2<int>("cols");
-        UserData &ud = image->userData();
-        int w = ud.get2<int>("w");
-        int h = ud.get2<int>("h");
-        int w1 = w * cols;
-        int h1 = h * rows;
-        auto image2 = std::make_shared<PrimitiveObject>();
-        image2->resize(w1 * h1);
-        image2->userData().set2("isImage", 1);
-        image2->userData().set2("w", w1);
-        image2->userData().set2("h", h1);
-        set_output("image", image2);
-    }
-};
-ZENDEFNODE(ImageCut, {
-    {
-        {"image"},
-        {"enum normal mirror", "tilemode", "normal"},
-        {"int", "rows", "2"},
-        {"int", "cols", "2"},
-    },
-    {
-        {"deprecated"},
-    },
-    {},
-    {"image"},
-});
-
 //根据灰度进行上色
 struct MaskEdit: INode {
     void apply() override {
@@ -2423,37 +2229,15 @@ ZENDEFNODE(MaskEdit, {
     {"deprecated"},
 });
 
-struct ImageShape: INode {
-    void apply() override {
-        std::shared_ptr<PrimitiveObject> image = get_input<PrimitiveObject>("image");
-        auto rows = get_input2<int>("rows");
-        auto cols = get_input2<int>("cols");
-        UserData &ud = image->userData();
-        int w = ud.get2<int>("w");
-        int h = ud.get2<int>("h");
-        set_output("image", image);
-    }
-};
-ZENDEFNODE(ImageShape, {
-    {
-        {"image"},
-        {"int", "rows", "2"},
-        {"int", "cols", "2"},
-    },
-    {
-        {"image"},
-    },
-    {},
-    {"deprecated"},
-});
 
-struct ImageLevels: INode {//todo :latger than 1 ?
+struct ImageLevels: INode {
     void apply() override {
         std::shared_ptr<PrimitiveObject> image = get_input<PrimitiveObject>("image");
         auto inputLevels = get_input2<vec2f>("Input Levels");
         auto outputLevels = get_input2<vec2f>("Output Levels");
         auto gamma = get_input2<float>("gamma");//range  0.01 - 9.99
         auto channel = get_input2<std::string>("channel");
+        auto clamp = get_input2<bool>("Clamp Output");
         UserData &ud = image->userData();
         int w = ud.get2<int>("w");
         int h = ud.get2<int>("h");
@@ -2463,60 +2247,77 @@ struct ImageLevels: INode {//todo :latger than 1 ?
         float outputMin = outputLevels[0];
         float gammaCorrection = 1.0f / gamma;
 
-        if (channel == "RGB") {
+        if (channel == "All") {
 #pragma omp parallel for
-        for (int i = 0; i < w; i++) {
-            for (int j = 0; j < h; j++) {
-                vec3f &v = image->verts[i * w + j];
+            for (int i = 0; i < w * h; i++) {
+                vec3f &v = image->verts[i];
                 v[0] = (v[0] < inputMin) ? inputMin : v[0];
                 v[1] = (v[1] < inputMin) ? inputMin : v[1];
                 v[2] = (v[2] < inputMin) ? inputMin : v[2];
                 v = (v - inputMin) / inputRange; 
                 v = pow(v, gammaCorrection);
-                v = v * outputRange + outputMin;
+                v = clamp ? zeno::clamp((v * outputRange + outputMin), 0, 1) : (v * outputRange + outputMin);
+            }
+            if(image->has_attr("alpha")){
+                auto &alphaAttr = image->verts.attr<float>("alpha");
+#pragma omp parallel for
+                for (int i = 0; i < w * h; i++) {
+                    alphaAttr[i] = (alphaAttr[i] < inputMin) ? inputMin : alphaAttr[i];
+                    alphaAttr[i] = (alphaAttr[i] - inputMin) / inputRange;
+                    alphaAttr[i] = pow(alphaAttr[i], gammaCorrection) * outputRange + outputMin;
+                    alphaAttr[i] = clamp ? zeno::clamp(alphaAttr[i], 0, 1) : alphaAttr[i];
+                }
             }
-        }
         }
 
         else if (channel == "R") {
 #pragma omp parallel for
         for (int i = 0; i < w; i++) {
-            for (int j = 0; j < h; j++) { 
-                float &v = image->verts[i * w + j][0];
+                float &v = image->verts[i][0];
                 if (v < inputMin) v = inputMin;
                 v = (v - inputMin) / inputRange;
                 v = pow(v, gammaCorrection);
-                v = v * outputRange + outputMin;
+                v = clamp ? zeno::clamp((v * outputRange + outputMin), 0, 1) : (v * outputRange + outputMin);
             }
         }
-        }
 
         else if (channel == "G") {
 #pragma omp parallel for
         for (int i = 0; i < w; i++) {
-            for (int j = 0; j < h; j++) { 
-                float &v = image->verts[i * w + j][1];
+                float &v = image->verts[i][1];
                 if (v < inputMin) v = inputMin;
                 v = (v - inputMin) / inputRange;
                 v = pow(v, gammaCorrection);
-                v = v * outputRange + outputMin;
+                v = clamp ? zeno::clamp((v * outputRange + outputMin), 0, 1) : (v * outputRange + outputMin);
             }
         }
-        }
         
         else if (channel == "B") {
 #pragma omp parallel for
         for (int i = 0; i < w; i++) {
-            for (int j = 0; j < h; j++) { 
-                float &v = image->verts[i * w + j][2];
+                float &v = image->verts[i][2];
                 if (v < inputMin) v = inputMin;
                 v = (v - inputMin) / inputRange;
                 v = pow(v, gammaCorrection);
-                v = v * outputRange + outputMin;
+                v = clamp ? zeno::clamp((v * outputRange + outputMin), 0, 1) : (v * outputRange + outputMin);
             }
         }
+        
+        else if (channel == "A") {
+        if(image->has_attr("alpha")){
+            auto &alphaAttr = image->verts.attr<float>("alpha");
+#pragma omp parallel for
+            for (int i = 0; i < w * h; i++) {
+                alphaAttr[i] = (alphaAttr[i] < inputMin) ? inputMin : alphaAttr[i];
+                alphaAttr[i] = (alphaAttr[i] - inputMin) / inputRange;
+                alphaAttr[i] = pow(alphaAttr[i], gammaCorrection) * outputRange + outputMin;
+                alphaAttr[i] = clamp ? zeno::clamp(alphaAttr[i], 0, 1) : alphaAttr[i];
+                }
+            }
+            else{
+                zeno::log_error("no alpha channel");
+            }
         }
-
         set_output("image", image);
     }
 };
@@ -2527,7 +2328,8 @@ ZENDEFNODE(ImageLevels, {
         {"float", "gamma", "1"},
         {"vec2f", "Output Levels", "0, 1"},
         //{"bool", "auto level", "false"}, //auto level
-        {"enum RGB R G B", "channel", "RGB"},
+        {"enum All R G B A", "channel", "RGB"},
+        {"bool", "Clamp Output", "1"},
     },
     {
         {"image"},

From 9c20ea960f593ce24e35c39cfab102a9a8b3bd04 Mon Sep 17 00:00:00 2001
From: DarcJC <me@darc.pro>
Date: Thu, 21 Sep 2023 18:12:35 +0800
Subject: [PATCH 50/50] fix: typo

---
 zeno/include/zeno/utils/PropertyVisitor.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/zeno/include/zeno/utils/PropertyVisitor.h b/zeno/include/zeno/utils/PropertyVisitor.h
index c8d747ed1d..2ea9a8b080 100644
--- a/zeno/include/zeno/utils/PropertyVisitor.h
+++ b/zeno/include/zeno/utils/PropertyVisitor.h
@@ -13,7 +13,7 @@
 #include <zeno/core/Descriptor.h>
 #include <zeno/types/AttrVector.h>
 #include <zeno/types/UserData.h>
-#include <zeno/types/CUrveObject.h>
+#include <zeno/types/CurveObject.h>
 #include <zeno/utils/logger.h>
 #include <zeno/zeno.h>