Types for RPMPLv2

.gitignore

@@ -4,4 +4,5 @@ log
 .vscode
 *.log
 .ccache
-vgcore*
+.cache
+Testing

Makefile

@@ -9,4 +9,4 @@ build:
 	cmake --build build --config Release
 
 run_tests:
-	if [ -d build/tests ]; then ctest --test-dir build/tests; fi
+	ctest --test-dir build/tests -C Release --output-on-failure

include/common/Vector.h

@@ -0,0 +1,159 @@
+#pragma once
+
+#include <algorithm>
+#include <cstddef>
+#include <stdexcept>
+#include <valarray>
+
+namespace base {
+
+template<typename T>
+concept Arithmetic = std::is_default_constructible_v<T> &&
+                     std::is_arithmetic_v<T>;
+
+// Forward declaration
+template <Arithmetic ValueType> struct Vector;
+
+using VectorF = Vector<float>;
+using VectorD = Vector<double>;
+
+template <Arithmetic ValueType> struct Vector {
+  using VectorType = std::valarray<ValueType>;
+  VectorType coord{};
+  static constexpr ValueType ZERO{};
+
+  // constructors
+  Vector();
+  explicit Vector(std::valarray<ValueType> valArray);
+  Vector(std::initializer_list<ValueType> il);
+  Vector(const Vector &other);
+  Vector(Vector &&other) noexcept;
+
+  // operators
+  ValueType &operator()(size_t index);
+  const ValueType &operator()(size_t index) const;
+  Vector &operator=(const Vector &other);
+  Vector &operator=(Vector &&other) noexcept;
+
+  // arithmetic operators
+  Vector operator+(const Vector &other) const;
+  Vector operator+(const ValueType &other) const;
+  ValueType dot(const Vector &other) const;
+  ValueType norm() const;
+
+  // overload compound operators
+  Vector& operator+=(const ValueType& other);
+  Vector& operator+=(const Vector& other);
+
+  // zero, one
+  static Vector zeros(size_t dim);
+  static Vector ones(size_t dim);
+
+  // methods
+  size_t size() const;
+};
+
+// IMPLEMENTATION
+template <Arithmetic ValueType>
+Vector<ValueType>::Vector() : coord(VectorType()) {}
+
+template <Arithmetic ValueType>
+Vector<ValueType>::Vector(std::valarray<ValueType> valArray)
+         : coord(valArray) {}
+
+template <Arithmetic ValueType>
+Vector<ValueType>::Vector(std::initializer_list<ValueType> il)
+    : coord(il) {}
+
+template <Arithmetic ValueType>
+Vector<ValueType>::Vector(const Vector<ValueType> &other)
+    : coord(other.coord) {}
+
+template <Arithmetic ValueType>
+Vector<ValueType>::Vector(Vector<ValueType> &&other) noexcept
+    : coord(std::move(other.coord)) {}
+
+template <Arithmetic ValueType>
+ValueType &Vector<ValueType>::operator()(size_t index) {
+  if (index >= coord.size()) {
+    throw std::out_of_range("Index out of bounds");
+  }
+  return coord[index];
+}
+
+template <Arithmetic ValueType>
+Vector<ValueType> &
+Vector<ValueType>::operator=(const Vector<ValueType> &other) {
+  if (this != &other) {
+    coord = other.coord;
+  }
+  return *this;
+}
+
+template <Arithmetic ValueType>
+Vector<ValueType>& Vector<ValueType>::operator=(Vector<ValueType> &&other) noexcept {
+  if (this != &other) {
+    coord = std::move(other.coord);
+  }
+  return *this;
+}
+
+template <Arithmetic ValueType>
+const ValueType &Vector<ValueType>::operator()(size_t index) const {
+  if (index >= coord.size()) {
+    throw std::out_of_range("Index out of bounds");
+  }
+  return coord[index];
+}
+
+template <Arithmetic ValueType> 
+size_t Vector<ValueType>::size() const {
+  return coord.size();
+}
+
+template <Arithmetic ValueType>
+Vector<ValueType> Vector<ValueType>::operator+(const Vector<ValueType> &other) const {
+  assert(coord.size() == other.coord.size());
+  return Vector{coord + other.coord};
+}
+
+template <Arithmetic ValueType>
+Vector<ValueType> Vector<ValueType>::operator+(const ValueType &other) const {
+  return Vector{coord + other};
+}
+
+template <Arithmetic ValueType>
+Vector<ValueType>& Vector<ValueType>::operator+=(const Vector<ValueType>& other) {
+  assert(coord.size() == other.coord.size());
+  this->coord += other.coord;
+  return *this;
+}
+
+template <Arithmetic ValueType>
+Vector<ValueType>& Vector<ValueType>::operator+=(const ValueType& other) {
+  this->coord += other;
+  return *this;
+}
+
+template <Arithmetic ValueType>
+ValueType Vector<ValueType>::dot(const Vector<ValueType> &other) const {
+  assert(coord.size() == other.coord.size());
+  return (coord * other.coord).sum();
+}
+
+template <Arithmetic ValueType> 
+ValueType Vector<ValueType>::norm() const {
+  return std::sqrt(this->dot(*this));
+}
+
+template <Arithmetic ValueType> 
+Vector<ValueType> Vector<ValueType>::zeros(size_t dim) {
+  return Vector({VectorType(ZERO, dim)});
+}
+
+template <Arithmetic ValueType> 
+Vector<ValueType> Vector<ValueType>::ones(size_t dim) {
+  return Vector({VectorType(ZERO+1, dim)});
+}
+
+} // namespace base

src/CMakeLists.txt

@@ -1,8 +1,9 @@
 # Note that headers are optional, and do not affect add_library, but they will not
 # show up in IDEs unless they are listed in add_library.
 
-set(INCLUDE_DIRS 
-	${PROJECT_SOURCE_DIR}/include
+set(INCLUDE_DIRS
+        ${PROJECT_SOURCE_DIR}/include
+        ${PROJECT_SOURCE_DIR}/include/common
         ${PROJECT_SOURCE_DIR}/include/state_spaces
         ${PROJECT_SOURCE_DIR}/include/state_spaces/real_vector_space
         ${PROJECT_SOURCE_DIR}/include/planners
@@ -18,6 +19,7 @@ set(INCLUDE_DIRS
         ${PROJECT_SOURCE_DIR}/include/configurations
         ${PROJECT_SOURCE_DIR}/include/benchmark
         ${PROJECT_SOURCE_DIR}/external/nanoflann/include
+
         # ${PROJECT_SOURCE_DIR}/external/QuadProgpp/src
 )
 
@@ -38,6 +40,6 @@ target_compile_features(rpmpl_library PUBLIC cxx_std_17)
 
 # IDEs should put the headers in a nice place
 source_group(
-  TREE "${PROJECT_SOURCE_DIR}/include"
-  PREFIX "Header Files"
-  FILES ${HEADER_LIST})
+        TREE "${PROJECT_SOURCE_DIR}/include"
+        PREFIX "Header Files"
+        FILES ${HEADER_LIST})

tests/CMakeLists.txt

@@ -1,17 +1,19 @@
 # Testing library
 
-
 include_directories(include)
 
 # Tests need to be added as executables first
-add_executable(main_test main_test.cpp)
+add_executable(test_realvectorspacestate test_realvectorspacestate.cpp)
+add_executable(test_vectorf test_vectorf.cpp)
 
-# I'm using C++17 in the test
-target_compile_features(main_test PRIVATE cxx_std_17)
+target_compile_features(test_realvectorspacestate PRIVATE)
+target_compile_features(test_vectorf PRIVATE)
 
 # Should be linked to the main library, as well as the Catch2 testing library
-target_link_libraries(main_test PRIVATE rpmpl_library ${PROJECT_LIBRARIES})
+target_link_libraries(test_realvectorspacestate PRIVATE rpmpl_library ${PROJECT_LIBRARIES})
+target_link_libraries(test_vectorf PRIVATE rpmpl_library ${PROJECT_LIBRARIES})
 
 # If you register a test, then ctest and make test will run it.
 # You can also run examples and check the output, as well.
-add_test(NAME main_test COMMAND main_test) # Command can be a target
+add_test(NAME test_realvectorspacestate COMMAND test_realvectorspacestate) # Command can be a target
+add_test(NAME test_vectorf COMMAND test_vectorf) # Command can be a target

tests/tests_realvectorspacestate.h → tests/test_realvectorspacestate.cpp

@@ -1,6 +1,4 @@
-//
-// Created by dinko on 28.5.21..
-//
+#include <gtest/gtest.h>
 #include "RealVectorSpaceState.h"
 #include <Eigen/Dense>
 
@@ -22,3 +20,11 @@ TEST(RealVectorSpaceStateTest, testConstructor1)
     ASSERT_EQ(q->getNumDimensions(), 6);
     ASSERT_EQ(q->getCoord(), state_coord);
 }
+
+
+
+int main(int argc, char **argv) 
+{
+	::testing::InitGoogleTest(&argc, argv);
+	return RUN_ALL_TESTS();
+}

tests/test_vectorf.cpp

@@ -0,0 +1,92 @@
+#include <gtest/gtest.h>
+#include "Vector.h"
+#include <Eigen/Dense>
+
+
+TEST(VectorFTest, testConstructor)
+{
+    base::VectorF vec{1.0f, 2.0f, 3.0f};
+
+    ASSERT_EQ(vec(0), 1.0f);
+    ASSERT_EQ(vec(1), 2.0f);
+    ASSERT_EQ(vec(2), 3.0f);
+
+    ASSERT_EQ(vec.size(), 3);
+}
+
+TEST(VectorFTest, testAddition1)
+{
+    base::VectorF vec1{1.0f, 2.0f, 3.0f};
+    base::VectorF vec2{4.0f, 5.0f, 6.0f};
+
+    base::VectorF vec = vec1 + vec2; 
+
+    ASSERT_EQ(vec(0), 5.0f);
+    ASSERT_EQ(vec(1), 7.0f);
+    ASSERT_EQ(vec(2), 9.0f);
+
+    ASSERT_EQ(vec.size(), 3);
+}
+
+TEST(VectorFTest, testZeroOne)
+{
+    constexpr size_t dim{4};
+    base::VectorF vec1 = base::VectorF::zeros(dim);
+    base::VectorF vec2 = base::VectorF::ones(dim);
+
+    ASSERT_EQ(vec1(0), 0.0f);
+    ASSERT_EQ(vec2(0), 1.0f);
+
+    ASSERT_EQ(vec1.size(), dim);
+    ASSERT_EQ(vec2.size(), dim);
+}
+
+TEST(VectorFTest, testAddition2)
+{
+    base::VectorF vec1{1.0f, 2.0f, 3.0f};
+    base::VectorF vec2{4.0f, 5.0f, 6.0f};
+
+    vec1 += vec2;
+
+    ASSERT_EQ(vec1(0), 5.0f);
+    ASSERT_EQ(vec1(1), 7.0f);
+    ASSERT_EQ(vec1(2), 9.0f);
+
+    ASSERT_EQ(vec1.size(), 3);
+}
+
+TEST(VectorFTest, testAdditionWithScalar1)
+{
+    base::VectorF vec1{1.0f, 2.0f, 3.0f};
+    constexpr const float val{1.0f};
+
+    base::VectorF vec = vec1 + val; 
+
+    ASSERT_EQ(vec(0), 2.0f);
+    ASSERT_EQ(vec(1), 3.0f);
+    ASSERT_EQ(vec(2), 4.0f);
+
+    ASSERT_EQ(vec.size(), 3);
+}
+
+TEST(VectorFTest, testAdditionWithScalar2)
+{
+    base::VectorF vec1{1.0f, 2.0f, 3.0f};
+    constexpr float val{1.0f};
+
+    vec1 += val; 
+
+    ASSERT_EQ(vec1(0), 2.0f);
+    ASSERT_EQ(vec1(1), 3.0f);
+    ASSERT_EQ(vec1(2), 4.0f);
+
+    ASSERT_EQ(vec1.size(), 3);
+}
+
+
+
+int main(int argc, char **argv) 
+{
+	::testing::InitGoogleTest(&argc, argv);
+	return RUN_ALL_TESTS();
+}