diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index edf0cae4b265e67848f64104183e4b8c67412ab8..30d76079707d844c7ddce3d81f20ce6d4f8e55d6 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -7,6 +7,7 @@
 #include <VirtualRobot/Obstacle.h>
 #include <VirtualRobot/CollisionDetection/CollisionModel.h>
 #include <VirtualRobot/Visualization/TriMeshModel.h>
+#include <VirtualRobot/Visualization/VisualizationNode.h>
 #include <VirtualRobot/Primitive.h>
 
 
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
index 10e20c571b1bf20f6668bf0083e59c4adc7608df..e48c8ebaee956b2aacbeb756ce72e2fa6d8ca75d 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.h
@@ -24,6 +24,7 @@
 
 #include "../DynamicsObject.h"
 #include "SimoxMotionState.h"
+#include <VirtualRobot/Primitive.h>
 
 #include "btBulletDynamicsCommon.h"
 
diff --git a/VirtualRobot/BoundingBox.h b/VirtualRobot/BoundingBox.h
index ddb8088627c62785f97ba600051e730742d6904d..65d00b99d41eb94ace8fa62137013c39da33f226 100644
--- a/VirtualRobot/BoundingBox.h
+++ b/VirtualRobot/BoundingBox.h
@@ -22,13 +22,18 @@
 */
 #pragma once
 
-#include "MathTools.h"
+#include <VirtualRobot/VirtualRobotImportExport.h>
 
 #include <Eigen/Core>
 #include <vector>
 
 namespace VirtualRobot
 {
+    namespace MathTools
+    {
+        struct Plane;
+    }
+
     class CollisionChecker;
     /*!
         An axis oriented bounding box.
diff --git a/VirtualRobot/Dynamics/tests/DynamicsRBDLTest.cpp b/VirtualRobot/Dynamics/tests/DynamicsRBDLTest.cpp
index 040ef6caeb83b9a83c418651d6d65ba8e81ef38a..6362809a7af795fd8fedbcb1a21aefe8c88fc74a 100644
--- a/VirtualRobot/Dynamics/tests/DynamicsRBDLTest.cpp
+++ b/VirtualRobot/Dynamics/tests/DynamicsRBDLTest.cpp
@@ -12,6 +12,7 @@
 #include <VirtualRobot/XML/RobotIO.h>
 #include <VirtualRobot/Robot.h>
 #include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/MathTools.h>
 
 #include <Eigen/Core>
 
diff --git a/VirtualRobot/Grasping/Grasp.cpp b/VirtualRobot/Grasping/Grasp.cpp
index e016a37133c9f8aa57e43db8e8ca6a53b2c235ad..2566b9b4de9e8b2fde6aedef1d20d9078248be88 100644
--- a/VirtualRobot/Grasping/Grasp.cpp
+++ b/VirtualRobot/Grasping/Grasp.cpp
@@ -3,6 +3,7 @@
 #include "../RobotConfig.h"
 #include "..//Robot.h"
 #include "../VirtualRobotException.h"
+#include <VirtualRobot/MathTools.h>
 
 #include <Eigen/Dense>
 
diff --git a/VirtualRobot/IK/CoMIK.cpp b/VirtualRobot/IK/CoMIK.cpp
index 49305a979d218d94ee568fefa0a0a50c76172e68..85d80e7f7319e82d8a8ebe00ef2083faabbcccb3 100644
--- a/VirtualRobot/IK/CoMIK.cpp
+++ b/VirtualRobot/IK/CoMIK.cpp
@@ -4,6 +4,7 @@
 #include "../Nodes/RobotNodePrismatic.h"
 #include "../VirtualRobotException.h"
 #include "../Robot.h"
+#include <VirtualRobot/MathTools.h>
 
 #include <Eigen/Geometry>
 
diff --git a/VirtualRobot/IK/HierarchicalIK.cpp b/VirtualRobot/IK/HierarchicalIK.cpp
index 233c0ba70d265e1e7819f9c1da903e9f07d02000..2f7405b515afa01d7eef5cada393d7a869f58360 100644
--- a/VirtualRobot/IK/HierarchicalIK.cpp
+++ b/VirtualRobot/IK/HierarchicalIK.cpp
@@ -1,4 +1,7 @@
 #include "HierarchicalIK.h"
+
+#include <VirtualRobot/MathTools.h>
+
 using namespace VirtualRobot;
 using namespace std;
 
diff --git a/VirtualRobot/IK/HierarchicalIKSolver.cpp b/VirtualRobot/IK/HierarchicalIKSolver.cpp
index 3dfec0767983e71f1f7a8934ee00b901a1a1f1dd..b9864223980eaa83b366e9cd248e7fa6ed29b690 100644
--- a/VirtualRobot/IK/HierarchicalIKSolver.cpp
+++ b/VirtualRobot/IK/HierarchicalIKSolver.cpp
@@ -1,4 +1,7 @@
 #include "HierarchicalIKSolver.h"
+
+#include <VirtualRobot/MathTools.h>
+
 //#define DEBUG
 
 HierarchicalIKSolver::HierarchicalIKSolver(RobotNodeSetPtr allRobotNodes) : HierarchicalIK(allRobotNodes)
diff --git a/VirtualRobot/IK/JacobiProvider.cpp b/VirtualRobot/IK/JacobiProvider.cpp
index c8be128c7531976be64d46ba46cd77e64891c616..ec464538d48b322a9ae64a09675f31baf0ca8a9c 100644
--- a/VirtualRobot/IK/JacobiProvider.cpp
+++ b/VirtualRobot/IK/JacobiProvider.cpp
@@ -1,6 +1,8 @@
 #include <Eigen/Geometry>
 #include "JacobiProvider.h"
 
+#include <VirtualRobot/MathTools.h>
+
 #include <Eigen/Dense>
 
 #include <algorithm>
diff --git a/VirtualRobot/IK/PoseQualityExtendedManipulability.cpp b/VirtualRobot/IK/PoseQualityExtendedManipulability.cpp
index 0b230abc8d8b747efc1f40adddf68ae6c7824cf7..0ecb2e85a3ae5c008cbfe5d3e13564762acb174b 100644
--- a/VirtualRobot/IK/PoseQualityExtendedManipulability.cpp
+++ b/VirtualRobot/IK/PoseQualityExtendedManipulability.cpp
@@ -1,6 +1,8 @@
 
 #include "PoseQualityExtendedManipulability.h"
 
+#include <VirtualRobot/MathTools.h>
+
 #include <boost/math/special_functions/fpclassify.hpp>
 
 #include <Eigen/Geometry>
diff --git a/VirtualRobot/IK/StackedIK.cpp b/VirtualRobot/IK/StackedIK.cpp
index 2ab7969f17eff6c638cbd16774a6362f0da57978..f6c8549a076dc441cb4cefb16793a16c5aa51d20 100644
--- a/VirtualRobot/IK/StackedIK.cpp
+++ b/VirtualRobot/IK/StackedIK.cpp
@@ -23,6 +23,8 @@
 
 #include "StackedIK.h"
 
+#include <VirtualRobot/MathTools.h>
+
 using namespace VirtualRobot;
 
 namespace VirtualRobot
diff --git a/VirtualRobot/Nodes/CameraSensor.cpp b/VirtualRobot/Nodes/CameraSensor.cpp
index 4d5132d2a71d8880eb6f1cf12049ebb6ce0a6e73..41138ee69f1d32ee3c1c7d35c13c8c14d7c833c6 100644
--- a/VirtualRobot/Nodes/CameraSensor.cpp
+++ b/VirtualRobot/Nodes/CameraSensor.cpp
@@ -2,6 +2,8 @@
 #include "CameraSensor.h"
 #include "CameraSensorFactory.h"
 
+#include <VirtualRobot/Visualization/VisualizationNode.h>
+
 namespace VirtualRobot
 {
 
diff --git a/VirtualRobot/Nodes/PositionSensor.cpp b/VirtualRobot/Nodes/PositionSensor.cpp
index bb9e21656a1acfede1b9e7fda506368e36e8f83c..1773e94f601f1d21b8942816decb6b48c02cb48d 100644
--- a/VirtualRobot/Nodes/PositionSensor.cpp
+++ b/VirtualRobot/Nodes/PositionSensor.cpp
@@ -3,6 +3,8 @@
 #include "PositionSensorFactory.h"
 #include "../XML/BaseIO.h"
 
+#include <VirtualRobot/Visualization/VisualizationNode.h>
+
 namespace VirtualRobot
 {
 
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index e54c52a2d16d96551e49e0f7d64202690a8a719c..94c42229a77f03fba828c801c666a51eaee35fef 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -15,6 +15,7 @@
 #include <algorithm>
 #include <filesystem>
 #include "../math/Helpers.h"
+#include "../Visualization/VisualizationNode.h"
 
 #include <Eigen/Core>
 
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index 88f6acf96e718a4075615a2ab4f36f46f390f5b3..83e1835be0d026ac26e764fcd2bb65aecf691a27 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -26,13 +26,13 @@
 #include "../VirtualRobotException.h"
 #include "../SceneObject.h"
 #include "../Transformation/DHParameter.h"
-#include "../Visualization/VisualizationNode.h"
 #include "Sensor.h"
 
 #include <Eigen/Core>
 
 #include <string>
 #include <vector>
+#include <map>
 
 
 namespace VirtualRobot
diff --git a/VirtualRobot/Nodes/RobotNodePrismatic.cpp b/VirtualRobot/Nodes/RobotNodePrismatic.cpp
index 88c95894895715076b4d95c4f366da787646a5b0..c1cd484ec2a27316076aa7c69f940cc44b6f4498 100644
--- a/VirtualRobot/Nodes/RobotNodePrismatic.cpp
+++ b/VirtualRobot/Nodes/RobotNodePrismatic.cpp
@@ -3,6 +3,7 @@
 #include "../Robot.h"
 #include "../VirtualRobotException.h"
 #include "../CollisionDetection/CollisionModel.h"
+#include "../Visualization/VisualizationNode.h"
 
 #include <cmath>
 #include <algorithm>
diff --git a/VirtualRobot/Nodes/Sensor.cpp b/VirtualRobot/Nodes/Sensor.cpp
index 43fba92837cbaddaeb2863064218b9d10733d865..249f93ac46c839028a5aa38c260b6f3c24fb31e0 100644
--- a/VirtualRobot/Nodes/Sensor.cpp
+++ b/VirtualRobot/Nodes/Sensor.cpp
@@ -5,6 +5,7 @@
 #include "../XML/BaseIO.h"
 #include "../Visualization/TriMeshModel.h"
 #include "RobotNode.h"
+#include <VirtualRobot/Visualization/VisualizationNode.h>
 
 #include <Eigen/Core>
 
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index 7c550309c5f9ca16f7aafac57681db3d2541b1e5..b6ab8d9cc46f732996aa3a5544d8f962d52c64e4 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -4,6 +4,7 @@
 #include "Trajectory.h"
 #include "VirtualRobotException.h"
 #include "Nodes/Sensor.h"
+#include "Visualization/VisualizationNode.h"
 #include "CollisionDetection/CollisionChecker.h"
 #include "CollisionDetection/CollisionModel.h"
 #include "EndEffector/EndEffector.h"
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index b494bfe93aef3cce2203a2e7574545be07eb7401..1a860c12bd351f073e5ba0912e3093931421afbc 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -26,6 +26,7 @@
 #include "SceneObject.h"
 #include "Nodes/RobotNode.h"
 #include "Nodes/ConditionedLock.h"
+#include "BoundingBox.h"
 
 #include <type_traits>
 #include <string>
diff --git a/VirtualRobot/RobotFactory.cpp b/VirtualRobot/RobotFactory.cpp
index 175c815cc899aef21756b13cec734d3eb6b81f37..34e03e14ebe965edabc2a2f761c52b3921ec5f44 100644
--- a/VirtualRobot/RobotFactory.cpp
+++ b/VirtualRobot/RobotFactory.cpp
@@ -9,6 +9,7 @@
 #include "Nodes/RobotNodeFixedFactory.h"
 #include "CollisionDetection/CollisionModel.h"
 #include "EndEffector/EndEffector.h"
+#include "Visualization/VisualizationNode.h"
 #include "Visualization//VisualizationFactory.h"
 #include "VirtualRobotException.h"
 
diff --git a/VirtualRobot/SceneObject.cpp b/VirtualRobot/SceneObject.cpp
index f7386decb0278339b92e9d6bdbec986215c0c722..16c2fe396001df30ce34d844e4a073c19b3f64db 100644
--- a/VirtualRobot/SceneObject.cpp
+++ b/VirtualRobot/SceneObject.cpp
@@ -4,6 +4,7 @@
 #include "CollisionDetection/CollisionChecker.h"
 #include "Visualization/TriMeshModel.h"
 #include "Visualization/VisualizationFactory.h"
+#include "Visualization/VisualizationNode.h"
 #include "Visualization/Visualization.h"
 #include "VirtualRobotException.h"
 #include "Robot.h"
diff --git a/VirtualRobot/Tools/Gravity.cpp b/VirtualRobot/Tools/Gravity.cpp
index 448b6afa7ec4f7d58ee9eb5e067fd51b52955639..4c43755f4751f4661cd2b8614b699f922968fc2d 100644
--- a/VirtualRobot/Tools/Gravity.cpp
+++ b/VirtualRobot/Tools/Gravity.cpp
@@ -4,6 +4,7 @@
 #include "../Nodes/RobotNodeRevolute.h"
 #include "../RobotNodeSet.h"
 #include "../Robot.h"
+#include <VirtualRobot/MathTools.h>
 
 #include <Eigen/Geometry>
 
diff --git a/VirtualRobot/Trajectory.cpp b/VirtualRobot/Trajectory.cpp
index 8cf4dfc5908c6e648a1a56cea308e2653fd2ca5f..8ff0825c37931d98f8bfdd40eae8af1773869e34 100644
--- a/VirtualRobot/Trajectory.cpp
+++ b/VirtualRobot/Trajectory.cpp
@@ -1,6 +1,7 @@
 #include "Trajectory.h"
 #include "Robot.h"
 #include "VirtualRobotException.h"
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
 #include <iostream>
 #include <sstream>
 
diff --git a/VirtualRobot/XML/BaseIO.cpp b/VirtualRobot/XML/BaseIO.cpp
index b01e929d56fea58322c1f4306caf07f7d3ab1d4c..2c80d079a871b8d4fd311971b89ab0458670b468 100644
--- a/VirtualRobot/XML/BaseIO.cpp
+++ b/VirtualRobot/XML/BaseIO.cpp
@@ -13,6 +13,7 @@
 #include "../Nodes/RobotNodeFixedFactory.h"
 #include "../Transformation/DHParameter.h"
 #include "../Visualization/VisualizationFactory.h"
+#include "../Visualization/VisualizationNode.h"
 #include "rapidxml.hpp"
 
 #include <filesystem>
diff --git a/VirtualRobot/XML/BaseIO.h b/VirtualRobot/XML/BaseIO.h
index 91a67de5a98ab04d658f83c94c221c6280f37dd1..601eed2239da8a7ece7de1a53d5523e1b17ddad6 100644
--- a/VirtualRobot/XML/BaseIO.h
+++ b/VirtualRobot/XML/BaseIO.h
@@ -26,6 +26,7 @@
 #include "../Units.h"
 #include "../RobotConfig.h"
 #include "../Nodes/RobotNode.h"
+#include "../Primitive.h"
 
 #include <string>
 #include <vector>
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index 37cf9a81b4f8571a8282bcfd9abd264aaa8e41f2..ffd601195f654e083fbd5ad341829e9719b8c2fb 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -12,6 +12,7 @@
 #include "../Nodes/RobotNodePrismatic.h"
 #include "../Transformation/DHParameter.h"
 #include "../Visualization/VisualizationFactory.h"
+#include "../Visualization/VisualizationNode.h"
 #include "../Visualization/TriMeshModel.h"
 #include "../RobotConfig.h"
 #include "../RuntimeEnvironment.h"
diff --git a/VirtualRobot/XML/mujoco/RobotMjcf.cpp b/VirtualRobot/XML/mujoco/RobotMjcf.cpp
index e25aee3e37140862716b08aed01bba0fc8eb1499..039a9e493aba1ff27d4546a4206610133a2a0e93 100644
--- a/VirtualRobot/XML/mujoco/RobotMjcf.cpp
+++ b/VirtualRobot/XML/mujoco/RobotMjcf.cpp
@@ -6,6 +6,7 @@
 
 #include <VirtualRobot/Nodes/RobotNodePrismatic.h>
 #include <VirtualRobot/Nodes/RobotNodeRevolute.h>
+#include <VirtualRobot/Visualization/VisualizationNode.h>
 
 
 #include "MeshConverter.h"