Add angular constraints test

Signed-off-by: Henrik Söderlund <s0001813@ucnd3012bx2>
henriksod · Sep 5, 2024 · ff92fa0 · ff92fa0
1 parent f8cee50
commit ff92fa0
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 2 deletions.
diff --git a/src/FABRIK2D.cpp b/src/FABRIK2D.cpp
@@ -207,8 +207,12 @@ uint8_t Fabrik2D::solve(float x, float y, int lengths[]) {
                     static_cast<float>((1 - lambda_i) * ny + lambda_i * jy);
 
                 if (i > 0) {
+                    Joint const& parent_joint = this->_chain->joints[i - 1];
                     _applyAngularConstraints(this->_chain->joints[i - 1], this->_chain->joints[i],
                                              this->_chain->joints[i + 1]);
+                } else {
+                    _applyAngularConstraints(this->_chain->joints[i], this->_chain->joints[i],
+                                             this->_chain->joints[i + 1]);
                 }
             }
 
@@ -438,6 +442,11 @@ void Fabrik2D::_applyAngularConstraints(Joint const& parent_joint, Joint const&
     float nx = next_joint.x - joint.x;
     float ny = next_joint.y - joint.y;
 
+    // If previous vector results in zero vector, make it a vector pointing in positive x
+    if (_distance(0.0, 0.0, px, py) == 0.0) {
+        px = 1.0;
+    }
+
     float current_angle = _angleBetween(px, py, nx, ny);
 
     // Clamp the angle between the joint's limits

diff --git a/src/FABRIK2D.h b/src/FABRIK2D.h
@@ -339,8 +339,8 @@ class Fabrik2D {
      */
     float _angleBetween(float x1, float y1, float x2, float y2);
 
-    /* _applyAngularConstraints(parent_joint, joint, next_joint)
-     * inputs: parent_joint, joint
+    /* _applyAngularConstraints(parent_x, parent_y, joint, next_joint)
+     * inputs: joints
      *
      * Applies angular constraints to a joint, based on the position of parent_joint
      */

diff --git a/test/unit_tests.cpp b/test/unit_tests.cpp
@@ -207,6 +207,28 @@ unittest(test_solve) {
     assertEqualFloat(150, fabrik2D_4_4DOF_GO.getZ(3), fabrik2D_4_4DOF_GO.getTolerance());
 }
 
+unittest(test_solve_angular_constraint) {
+    int success = 0;
+    int lengths_4_joints[] = {200, 200, 200};
+    Fabrik2D::AngularConstraint angular_constraints[] =
+        [ {-M_PI / 2.0, -M_PI / 2.0}, {-2.0 * M_PI, 2.0 * M_PI}, {-2.0 * M_PI, 2.0 * M_PI} ];
+
+    // Solve 4 joints, 3DOF, Angular Constraint
+    fprintf(stderr, "Solve 4 joints, 3DOF\n");
+    Fabrik2D fabrik2D(4, lengths_4_joints, angular_constraints, 1);
+    success = fabrik2D.solve2(100, 100, 100, lengths_4_joints);
+    assertEqual(1, success);
+
+    float base_angle = atan2(100, 100);
+
+    assertEqualFloat(base_angle, fabrik2D_4_3DOF.getBaseAngle(), 1e-3);
+
+    assertEqualFloat(100, fabrik2D_4_3DOF.getX(3), fabrik2D_4_3DOF.getTolerance());
+    assertEqualFloat(100, fabrik2D_4_3DOF.getY(3), fabrik2D_4_3DOF.getTolerance());
+    assertEqualFloat(100, fabrik2D_4_3DOF.getZ(3), fabrik2D_4_3DOF.getTolerance());
+    assertEqualFloat(-M_PI / 2.0, fabrik2D_4_3DOF.getAngle(1), M_PI / 50.0);
+}
+
 unittest(test_getters_setters) {
     int lengths[] = {200, 200};
     Fabrik2D fabrik2D(3, lengths);