diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj b/FalconLibraryCPP/FalconLibraryCPP.vcxproj
index c0ad85c..e136499 100644
--- a/FalconLibraryCPP/FalconLibraryCPP.vcxproj
+++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj
@@ -124,14 +124,14 @@
   </ItemDefinitionGroup>
   <ItemGroup>
     <ClInclude Include="src\FalconLibrary.h" />
-    <ClInclude Include="src\Interpolatable.h" />
-    <ClInclude Include="src\Pose2d.h" />
-    <ClInclude Include="src\Pose2dWithCurvature.h" />
-    <ClInclude Include="src\Rotation2d.h" />
-    <ClInclude Include="src\Translation2d.h" />
-    <ClInclude Include="src\Twist2d.h" />
+    <ClInclude Include="src\types\Interpolatable.h" />
+    <ClInclude Include="src\mathematics\geometry\Pose2d.h" />
+    <ClInclude Include="src\mathematics\geometry\Pose2dWithCurvature.h" />
+    <ClInclude Include="src\mathematics\geometry\Rotation2d.h" />
+    <ClInclude Include="src\mathematics\geometry\Translation2d.h" />
+    <ClInclude Include="src\mathematics\geometry\Twist2d.h" />
     <ClInclude Include="src\Utilities.h" />
-    <ClInclude Include="src\VaryInterpolatable.h" />
+    <ClInclude Include="src\types\VaryInterpolatable.h" />
   </ItemGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
   <ImportGroup Label="ExtensionTargets">
diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
index 4f0ba1f..af4a841 100644
--- a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
+++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
@@ -15,32 +15,32 @@
     </Filter>
   </ItemGroup>
   <ItemGroup>
-    <ClInclude Include="src\VaryInterpolatable.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
-    <ClInclude Include="src\Interpolatable.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
-    <ClInclude Include="src\Pose2d.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
-    <ClInclude Include="src\Pose2dWithCurvature.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
-    <ClInclude Include="src\Rotation2d.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
-    <ClInclude Include="src\Translation2d.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
-    <ClInclude Include="src\Twist2d.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
     <ClInclude Include="src\Utilities.h">
       <Filter>Header Files</Filter>
     </ClInclude>
     <ClInclude Include="src\FalconLibrary.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="src\types\Interpolatable.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\types\VaryInterpolatable.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\geometry\Pose2d.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\geometry\Pose2dWithCurvature.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\geometry\Rotation2d.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\geometry\Translation2d.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="src\mathematics\geometry\Twist2d.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>
\ No newline at end of file
diff --git a/FalconLibraryCPP/src/FalconLibrary.h b/FalconLibraryCPP/src/FalconLibrary.h
index 5c3cdef..04398ae 100644
--- a/FalconLibraryCPP/src/FalconLibrary.h
+++ b/FalconLibraryCPP/src/FalconLibrary.h
@@ -1,12 +1,14 @@
 #pragma once
 
-#include "Interpolatable.h"
-#include "Pose2d.h"
-#include "Pose2dWithCurvature.h"
-#include "Rotation2d.h"
-#include "Translation2d.h"
-#include "Twist2d.h"
+#include "types/Interpolatable.h"
+#include "types/VaryInterpolatable.h"
+
+#include "mathematics/geometry/Pose2d.h"
+#include "mathematics/geometry/Pose2dWithCurvature.h"
+#include "mathematics/geometry/Rotation2d.h"
+#include "mathematics/geometry/Translation2d.h"
+#include "mathematics/geometry/Twist2d.h"
+
 #include "Utilities.h"
-#include "VaryInterpolatable.h"
 
 namespace frc5190 {}
\ No newline at end of file
diff --git a/FalconLibraryCPP/src/Pose2d.h b/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h
similarity index 98%
rename from FalconLibraryCPP/src/Pose2d.h
rename to FalconLibraryCPP/src/mathematics/geometry/Pose2d.h
index 381b173..1c35bed 100644
--- a/FalconLibraryCPP/src/Pose2d.h
+++ b/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h
@@ -1,6 +1,6 @@
 #pragma once
 
-#include "VaryInterpolatable.h"
+#include "types/VaryInterpolatable.h"
 #include "Translation2d.h"
 #include "Rotation2d.h"
 #include "Utilities.h"
diff --git a/FalconLibraryCPP/src/Pose2dWithCurvature.h b/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h
similarity index 95%
rename from FalconLibraryCPP/src/Pose2dWithCurvature.h
rename to FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h
index 3575f14..3de5aa1 100644
--- a/FalconLibraryCPP/src/Pose2dWithCurvature.h
+++ b/FalconLibraryCPP/src/mathematics/geometry/Pose2dWithCurvature.h
@@ -1,9 +1,8 @@
 #pragma once
 
 #include <utility>
-#include "VaryInterpolatable.h"
+#include "types/VaryInterpolatable.h"
 #include "Pose2d.h"
-#include "Interpolatable.h"
 
 namespace frc5190 {
 class Pose2dWithCurvature final
diff --git a/FalconLibraryCPP/src/Rotation2d.h b/FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h
similarity index 100%
rename from FalconLibraryCPP/src/Rotation2d.h
rename to FalconLibraryCPP/src/mathematics/geometry/Rotation2d.h
diff --git a/FalconLibraryCPP/src/Translation2d.h b/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h
similarity index 96%
rename from FalconLibraryCPP/src/Translation2d.h
rename to FalconLibraryCPP/src/mathematics/geometry/Translation2d.h
index 3dd4028..6248f25 100644
--- a/FalconLibraryCPP/src/Translation2d.h
+++ b/FalconLibraryCPP/src/mathematics/geometry/Translation2d.h
@@ -2,8 +2,8 @@
 
 #include <cmath>
 
-#include "Interpolatable.h"
-#include "VaryInterpolatable.h"
+#include "types/Interpolatable.h"
+#include "types/VaryInterpolatable.h"
 #include "Rotation2d.h"
 
 namespace frc5190 {
diff --git a/FalconLibraryCPP/src/Twist2d.h b/FalconLibraryCPP/src/mathematics/geometry/Twist2d.h
similarity index 100%
rename from FalconLibraryCPP/src/Twist2d.h
rename to FalconLibraryCPP/src/mathematics/geometry/Twist2d.h
diff --git a/FalconLibraryCPP/src/Interpolatable.h b/FalconLibraryCPP/src/types/Interpolatable.h
similarity index 100%
rename from FalconLibraryCPP/src/Interpolatable.h
rename to FalconLibraryCPP/src/types/Interpolatable.h
diff --git a/FalconLibraryCPP/src/VaryInterpolatable.h b/FalconLibraryCPP/src/types/VaryInterpolatable.h
similarity index 100%
rename from FalconLibraryCPP/src/VaryInterpolatable.h
rename to FalconLibraryCPP/src/types/VaryInterpolatable.h