From a96cf082128f38b9bf0195581fe8cb64ce3e340d Mon Sep 17 00:00:00 2001
From: Leonetienne <leon@etiennes.de>
Date: Tue, 10 Feb 2026 22:09:38 +0100
Subject: [PATCH] create and hook up ccdik scene

---
 MotionCore/MainLoop.cpp    |  6 +++---
 MotionCore/Scene_CCDIK.cpp | 36 ++++++++++++++++++++++++++++++++++++
 MotionCore/Scene_CCDIK.h   | 26 ++++++++++++++++++++++++++
 3 files changed, 65 insertions(+), 3 deletions(-)
 create mode 100755 MotionCore/Scene_CCDIK.cpp
 create mode 100755 MotionCore/Scene_CCDIK.h

diff --git a/MotionCore/MainLoop.cpp b/MotionCore/MainLoop.cpp
index 3f21a3e..5d2eb18 100755
--- a/MotionCore/MainLoop.cpp
+++ b/MotionCore/MainLoop.cpp
@@ -5,7 +5,7 @@
 #include <numbers>
 #include "StateMachine.h"
 #include "ArmSegment.h"
-#include "Scene_JointSpaceControl.h"
+#include "Scene_CCDIK.h"
 
 MainLoop::MainLoop() noexcept :
     arm(L"COM4")
@@ -33,7 +33,7 @@ void MainLoop::run()
     static std::chrono::steady_clock::time_point lastBegin =
         std::chrono::high_resolution_clock::now();
 
-    Scene_JointSpaceControl sceneJointSpaceControl(arm);
+    Scene_CCDIK sceneCCDIK(arm);
 
     while (isRunning)
     {
@@ -52,7 +52,7 @@ void MainLoop::run()
             case StateMachine::STATE::IDLING:
             case StateMachine::STATE::MOVING: {
                 // HERE SCENE UPDATE
-                sceneJointSpaceControl.update(frametime);
+                sceneCCDIK.update(frametime);
 
                 // Unset MOVING to IDLING state if no arm is moving
                 if (StateMachine::getInstance().getState() == StateMachine::STATE::MOVING && !arm.isMoving()) {
diff --git a/MotionCore/Scene_CCDIK.cpp b/MotionCore/Scene_CCDIK.cpp
new file mode 100755
index 0000000..1adf819
--- /dev/null
+++ b/MotionCore/Scene_CCDIK.cpp
@@ -0,0 +1,36 @@
+#include "Scene_CCDIK.h"
+#include <iostream>
+
+Scene_CCDIK::Scene_CCDIK(Arm& arm) :
+    arm{ arm },
+    deadzones({
+        AxisAlignedBoundingBox(
+            Vector3d(-125, -125, -20),
+            Vector3d(65, 100, 120)
+        ),
+        AxisAlignedBoundingBox(
+            Vector3d(0, -160, 0),
+            Vector3d(20000, 100, 20000)
+        )
+        }),
+    leftVKB(L" VKBsim Gladiator EVO  L  ")
+{
+
+    if (!arm.assumeHomePoseImmediate()) {
+        std::cerr << "Unable to move arm to home!" << std::endl;
+        exit(-1);
+    }
+
+    if (!leftVKB.connect()) {
+        std::cerr << "Unable to connect to left VKB controller!" << std::endl;
+        exit(-1);
+    }
+}
+
+void Scene_CCDIK::update(double frametime)
+{
+    constexpr double speed = 5000;
+    VkbInputs vkbi = leftVKB.get();
+
+    arm.update(frametime);
+}
diff --git a/MotionCore/Scene_CCDIK.h b/MotionCore/Scene_CCDIK.h
new file mode 100755
index 0000000..22bbb28
--- /dev/null
+++ b/MotionCore/Scene_CCDIK.h
@@ -0,0 +1,26 @@
+#pragma once
+#include "Arm.h"
+#include "AxisAlignedBoundingBox.h"
+#include "vkb_controller.h"
+
+class Scene_CCDIK
+{
+public:
+	Scene_CCDIK(Arm& arm);
+
+	void update(double frametime);
+
+private:
+	Arm& arm;
+	std::array<AxisAlignedBoundingBox, 2> deadzones;
+	VKBSimController leftVKB;
+
+
+	double j0Pos = 0;
+	double j1Pos = 0;
+	double j2Pos = 0;
+	double j3Pos = 0;
+	double j4Pos = 0;
+	double j5Pos = 0;
+};
+