feat: move 3d cursor by yaw/roll/pitch control

MotionCore/MotionCore.vcxproj

@@ -151,6 +151,7 @@
     <ClCompile Include="Matrix3x3.cpp" />
     <ClCompile Include="Motor.cpp" />
     <ClCompile Include="OSerialBus.cpp" />
+    <ClCompile Include="Scene_CCDIK.cpp" />
     <ClCompile Include="Scene_JointSpaceControl.cpp" />
     <ClCompile Include="StateMachine.cpp" />
     <ClCompile Include="Vector3d.cpp" />
@@ -164,6 +165,7 @@
     <ClInclude Include="Matrix3x3.h" />
     <ClInclude Include="Motor.h" />
     <ClInclude Include="OSerialBus.h" />
+    <ClInclude Include="Scene_CCDIK.h" />
     <ClInclude Include="Scene_JointSpaceControl.h" />
     <ClInclude Include="StateMachine.h" />
     <ClInclude Include="Vector3d.h" />

MotionCore/MotionCore.vcxproj.filters

@@ -51,6 +51,9 @@
     <ClCompile Include="Scene_JointSpaceControl.cpp">
       <Filter>Quelldateien</Filter>
     </ClCompile>
+    <ClCompile Include="Scene_CCDIK.cpp">
+      <Filter>Quelldateien</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="MainLoop.h">
@@ -89,5 +92,8 @@
     <ClInclude Include="Scene_JointSpaceControl.h">
       <Filter>Headerdateien</Filter>
     </ClInclude>
+    <ClInclude Include="Scene_CCDIK.h">
+      <Filter>Headerdateien</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>

MotionCore/Scene_CCDIK.cpp

@@ -12,8 +12,9 @@ Scene_CCDIK::Scene_CCDIK(Arm& arm) :
             Vector3d(0, -160, 0),
             Vector3d(20000, 100, 20000)
         )
-        }),
+    }),
-    leftVKB(L" VKBsim Gladiator EVO  L  ")
+    leftVKB(L" VKBsim Gladiator EVO  L  "),
+    cursor3d(0,0,0)
 {
 
     if (!arm.assumeHomePoseImmediate()) {
@@ -29,8 +30,22 @@ Scene_CCDIK::Scene_CCDIK(Arm& arm) :
 
 void Scene_CCDIK::update(double frametime)
 {
+    vkbi = leftVKB.get();
+
     constexpr double speed = 5000;
     VkbInputs vkbi = leftVKB.get();
 
+    update3dCursor(frametime);
+
     arm.update(frametime);
 }
+
+void Scene_CCDIK::update3dCursor(double frametime)
+{
+    constexpr double cursorSpeed = 1.0;
+    cursor3d += Vector3d(
+        vkbi.roll * cursorSpeed * frametime,
+        vkbi.yaw * cursorSpeed * frametime,
+        -vkbi.pitch * cursorSpeed * frametime
+    );
+}

MotionCore/Scene_CCDIK.h

@@ -2,6 +2,7 @@
 #include "Arm.h"
 #include "AxisAlignedBoundingBox.h"
 #include "vkb_controller.h"
+#include "Vector3d.h"
 
 class Scene_CCDIK
 {
@@ -11,16 +12,14 @@ public:
 	void update(double frametime);
 
 private:
+	void update3dCursor(double frametime);
+
 	Arm& arm;
 	std::array<AxisAlignedBoundingBox, 2> deadzones;
 	VKBSimController leftVKB;
 
 
-	double j0Pos = 0;
+	Vector3d cursor3d;
-	double j1Pos = 0;
+	VkbInputs vkbi;
-	double j2Pos = 0;
-	double j3Pos = 0;
-	double j4Pos = 0;
-	double j5Pos = 0;
 };