Initial wasm simulator checkpoint

core/src/rtcp_kinematics.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "cnc_sim_api.h"
+
+struct RtcpVector {
+    double x;
+    double y;
+    double z;
+};
+
+// Rotation order is intrinsic tool orientation A then B then C, represented as
+// Rz(C) * Ry(B) * Rx(A) applied to a local tool vector.
+RtcpVector rtcp_rotate_abc_degrees(RtcpVector vector, double a_deg, double b_deg, double c_deg);
+
+RtcpVector rtcp_tool_vector_from_pose(const CncSimPose &pose, double tool_length);
+CncSimPose rtcp_pivot_from_tool_tip(const CncSimPose &tool_tip, double tool_length);
+CncSimPose rtcp_tool_tip_from_pivot(const CncSimPose &pivot, double tool_length);