added files

firmware/MotionControllerRP/src/kinemtaic_models/kinematic_model_delta3d.h

@@ -0,0 +1,42 @@
+#pragma once
+
+//*** INCLUDE ***************************************************************************
+
+#include "kinematic_model_base.h"
+#include "utilities/math3d.h"
+
+//*** CLASS *****************************************************************************
+
+class Pose6DF;
+
+//--- KinemtaicModel_Delta3D ------------------------------------------------------------
+
+class KinematicModel_Delta3D : public IKinemtaicModel {
+  public:
+    KinematicModel_Delta3D();
+
+    int get_joint_count();
+    bool foreward(const float* joint_positions, Pose6DF& pose) override;
+    bool inverse(const Pose6DF& pose, float* joint_positions) override;
+
+    void test();
+
+  protected:
+    Vec3F arm_attachment_point(int joint_idx, float rotor_angle);
+
+  public:
+    float arm_length;
+    float rotor_radius;
+    float rotor_angle_offset[3];   // angle offset of the neutral position from zero position in rad
+    Pose6DF base_to_actuator[3];
+    Pose6DF actuator_to_base[3];
+    Vec3F ee_attachment_points[3]; // EE arm attachment points in endeffector coordinate system
+};
+
+//*** FUNCTION **************************************************************************
+
+bool circle_sphere_intersection(double r1, const Vec3F& p, double r2, Vec3F intersections[2]);
+bool three_sphere_intersection(const Vec3F& p1, float r1,
+                               const Vec3F& p2, float r2,
+                               const Vec3F& p3, float r3,
+                               Vec3F intersections[2]);