diff --git a/CMakeLists.txt b/CMakeLists.txt
index 26bb5bf..13a1c60 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,56 +1,49 @@
 cmake_minimum_required(VERSION 3.8)
 project(hydrodynamics)
 
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-  set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++ 17
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp
-  ament_cmake
-  eigen3_cmake_module
   Eigen3
 )
 
+find_package(ament_cmake REQUIRED)
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
-include_directories(
-  include
-)
-
-add_library(${PROJECT_NAME} SHARED
+add_library(hydrodynamics SHARED
   src/hydrodynamics.cpp
 )
 
-target_link_libraries(${PROJECT_NAME}
-  ${rclcpp_LIBRARIES}
+target_include_directories(hydrodynamics
+  PUBLIC
+    $<INSTALL_INTERFACE:include>
+    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+    ${EIGEN3_INCLUDE_DIR}
 )
-target_include_directories(${PROJECT_NAME} PUBLIC
-  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+target_compile_features(hydrodynamics PUBLIC cxx_std_17)
+target_link_libraries(hydrodynamics
+  PUBLIC
+    ${rclcpp_LIBRARIES}
+)
+
+ament_target_dependencies(hydrodynamics
+  PUBLIC
+    rclcpp
 )
-ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 install(
   DIRECTORY include/
-  DESTINATION include/${PROJECT_NAME}
+  DESTINATION include/hydrodynamics
 )
 
 install(
-  TARGETS ${PROJECT_NAME}
-  EXPORT "export_${PROJECT_NAME}"
+  TARGETS hydrodynamics
+  EXPORT "export_hydrodynamics"
   RUNTIME DESTINATION bin
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
@@ -58,7 +51,6 @@ install(
 )
 
 if(BUILD_TESTING)
-  find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
 
   # Run linters found in package.xml except those below
@@ -68,7 +60,7 @@ if(BUILD_TESTING)
   set(ament_cmake_flake8_FOUND TRUE)
 endif()
 
-ament_export_targets("export_${PROJECT_NAME}" HAS_LIBRARY_TARGET)
+ament_export_targets("export_hydrodynamics" HAS_LIBRARY_TARGET)
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 ament_package()
diff --git a/include/hydrodynamics.hpp b/include/hydrodynamics.hpp
index fe2e4b1..b8f3308 100644
--- a/include/hydrodynamics.hpp
+++ b/include/hydrodynamics.hpp
@@ -21,7 +21,6 @@
 #pragma once
 
 #include <Eigen/Dense>
-#include <optional>
 
 #include "eigen.hpp"
 
diff --git a/package.xml b/package.xml
index 38c7684..2ada76c 100644
--- a/package.xml
+++ b/package.xml
@@ -15,12 +15,9 @@
   <author>Evan Palmer</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
   <depend>eigen</depend>
 
-  <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
-
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
diff --git a/src/hydrodynamics.cpp b/src/hydrodynamics.cpp
index b3ede62..f22e5e7 100644
--- a/src/hydrodynamics.cpp
+++ b/src/hydrodynamics.cpp
@@ -148,4 +148,19 @@ RestoringForces::RestoringForces(double weight, double buoyancy, Eigen::Vector3d
 {
 }
 
+Eigen::Vector6d RestoringForces::calculateRestoringForcesVector(const Eigen::Matrix3d & rot) const
+{
+  const Eigen::Vector3d fg(0, 0, weight_);
+  const Eigen::Vector3d fb(0, 0, -buoyancy_);
+
+  Eigen::Vector6d g_rb;
+
+  g_rb.topRows(3) = rot * (fg + fb);
+  g_rb.bottomRows(3) = center_of_gravity_.cross(rot * fg) + center_of_buoyancy_.cross(rot * fb);
+
+  g_rb *= -1;
+
+  return g_rb;
+}
+
 }  // namespace hydrodynamics