Initial wasm simulator checkpoint

core/src/rtcp_kinematics.cpp

@@ -0,0 +1,72 @@
+#include "rtcp_kinematics.h"
+
+#include <cmath>
+
+namespace {
+
+constexpr double kPi = 3.141592653589793238462643383279502884;
+
+double radians(double degrees) {
+    return degrees * kPi / 180.0;
+}
+
+RtcpVector rotate_x(RtcpVector vector, double angle) {
+    const double c = std::cos(angle);
+    const double s = std::sin(angle);
+    return {
+        vector.x,
+        vector.y * c - vector.z * s,
+        vector.y * s + vector.z * c,
+    };
+}
+
+RtcpVector rotate_y(RtcpVector vector, double angle) {
+    const double c = std::cos(angle);
+    const double s = std::sin(angle);
+    return {
+        vector.x * c + vector.z * s,
+        vector.y,
+        -vector.x * s + vector.z * c,
+    };
+}
+
+RtcpVector rotate_z(RtcpVector vector, double angle) {
+    const double c = std::cos(angle);
+    const double s = std::sin(angle);
+    return {
+        vector.x * c - vector.y * s,
+        vector.x * s + vector.y * c,
+        vector.z,
+    };
+}
+
+} // namespace
+
+RtcpVector rtcp_rotate_abc_degrees(RtcpVector vector, double a_deg, double b_deg, double c_deg) {
+    vector = rotate_x(vector, radians(a_deg));
+    vector = rotate_y(vector, radians(b_deg));
+    vector = rotate_z(vector, radians(c_deg));
+    return vector;
+}
+
+RtcpVector rtcp_tool_vector_from_pose(const CncSimPose &pose, double tool_length) {
+    return rtcp_rotate_abc_degrees({0.0, 0.0, -tool_length}, pose.a, pose.b, pose.c);
+}
+
+CncSimPose rtcp_pivot_from_tool_tip(const CncSimPose &tool_tip, double tool_length) {
+    const RtcpVector tool = rtcp_tool_vector_from_pose(tool_tip, tool_length);
+    CncSimPose pivot = tool_tip;
+    pivot.x = tool_tip.x - tool.x;
+    pivot.y = tool_tip.y - tool.y;
+    pivot.z = tool_tip.z - tool.z;
+    return pivot;
+}
+
+CncSimPose rtcp_tool_tip_from_pivot(const CncSimPose &pivot, double tool_length) {
+    const RtcpVector tool = rtcp_tool_vector_from_pose(pivot, tool_length);
+    CncSimPose tool_tip = pivot;
+    tool_tip.x = pivot.x + tool.x;
+    tool_tip.y = pivot.y + tool.y;
+    tool_tip.z = pivot.z + tool.z;
+    return tool_tip;
+}