rbdlsim/tests/sconvcolTests.cc

#include <iostream>

#include "catch.hpp"
#include "sconvcol.h"

using namespace std;

TEST_CASE("simd4x4f_trans_inf", "[sconvcol]") {
  simd4x4f trans;
  simd4x4f_lookat(&trans, simd4f_create (1.f, 0.f, 1.f, 1.f), simd4f_create (0.f, 0.f, -1.f, 1.f), simd4f_create (0.f, 1.f, 0.f, 1.f));

  simd4x4f inv;
  sch_simd4x4f_transform_inv(&trans, &inv);

  simd4x4f mul_res;
  simd4x4f_matrix_mul(&inv, &trans, &mul_res);

  simd4x4f identity;
  simd4x4f_identity(&identity);

  REQUIRE(sch_simd4f_close(identity.x, mul_res.x, 1.0e-5));
  REQUIRE(sch_simd4f_close(identity.y, mul_res.y, 1.0e-5));
  REQUIRE(sch_simd4f_close(identity.z, mul_res.z, 1.0e-5));
  REQUIRE(sch_simd4f_close(identity.w, mul_res.w, 1.0e-5));
}

TEST_CASE("Plane Distance", "[sconvcol]") {
  sch_plane plane = {
      simd4f_create(0.f, 0.f, 0.f, 0.f),
      simd4f_create(0.f, 1.f, 0.f, 0.f)};

  simd4f point = simd4f_create(0.f, 0.23f, 0.f, 0.f);

  float dist = sch_plane_distance(&plane, &point);

  REQUIRE(dist == 0.23f);
}

TEST_CASE("Edge Get Direction", "[sconvcol]") {
  simd4f vertices[4] = {
      simd4f_create(-1.f, -1.f, 0.f, 0.f),
      simd4f_create(1.f, -1.f, 0.f, 0.f),
      simd4f_create(1.f, 1.f, 0.f, 0.f),
      simd4f_create(-1.f, 1.f, 0.f, 0.f)};

  sch_face face = {nullptr};
  sch_create_face(4, vertices, &face);

  simd4f dir;
  sch_edge_get_dir(face.edge, &dir);

  simd4f dir_ref = simd4f_create(1.f, 0.f, 0.f, 0.f);
  REQUIRE(1.f == simd4f_dot3_scalar(dir, dir_ref));
}

TEST_CASE("HullBuilder", "[sconvcol]") {
  sch_hull_builder builder;

  builder.num_vertices = 3;

  GIVEN("A builder") {
    WHEN("is reset") {
      sch_builder_reset(&builder);

      THEN("number of vertices and faces is set to 0") {
        REQUIRE(builder.num_vertices == 0);
        REQUIRE(builder.num_faces == 0);
      }

      WHEN("adding a vertex") {
        int num_vertices = builder.num_vertices;

        simd4f vertex = simd4f_create(1.f, 0.f, 0.f, 1.f);
        sch_builder_face_begin(&builder);
        sch_builder_face_vertex(&builder, vertex);
        THEN("number of vertices increased") {
          REQUIRE(builder.num_vertices > num_vertices);
        }
      }
    }
  }

  GIVEN("An empty builder") {
    sch_builder_reset(&builder);

    simd4f vertex0 = simd4f_create(0.f, 0.f, 0.f, 1.f);
    simd4f vertex1 = simd4f_create(1.f, 0.f, 0.f, 1.f);
    simd4f vertex2 = simd4f_create(0.5f, 1.f, 0.f, 1.f);

    WHEN("adding two vertices ") {
      sch_builder_face_begin(&builder);
      sch_builder_face_vertex(&builder, vertex0);
      sch_builder_face_vertex(&builder, vertex1);

      WHEN("ending the face") {
        sch_builder_face_end(&builder);
        THEN("face idx range goes from 0 to 1") {
          REQUIRE(builder.num_faces == 1);
          REQUIRE(builder.num_vertices == 2);
          REQUIRE(builder.face_vert_idx_start[0] == 0);
          REQUIRE(builder.face_vert_idx_end[0] == 1);
        }
      }

      WHEN("creating a triangle") {
        sch_builder_face_vertex(&builder, vertex2);
        sch_builder_face_end(&builder);

        THEN("face idx range goes from 0 to 2") {
          REQUIRE(builder.num_faces == 1);
          REQUIRE(builder.num_vertices == 3);
          REQUIRE(builder.face_vert_idx_start[0] == 0);
          REQUIRE(builder.face_vert_idx_end[0] == 2);
        }

        WHEN("creating a hull") {
          sch_hull hull;
          SchHullResult result = sch_builder_create_hull(&builder, &hull);

          THEN("verify triangle hull") {
            REQUIRE(result == SchHullResultOpenHull);
            REQUIRE(hull.num_faces == 1);
            sch_face* face = &hull.faces[0];

            sch_vert* hull_vert0 = face->edge->vert;
            sch_vert* hull_vert1 = face->edge->next->vert;
            sch_vert* hull_vert2 = face->edge->next->next->vert;

            REQUIRE(sch_simd4f_equal(hull_vert0->p, vertex0));
            REQUIRE(sch_simd4f_equal(hull_vert1->p, vertex1));
            REQUIRE(sch_simd4f_equal(hull_vert2->p, vertex2));

            REQUIRE(face->edge->next->next->next == face->edge);
          }

          sch_hull_free_memory(&hull);
        }

        WHEN("adding concave face") {
          sch_hull hull;
          SchHullResult result = sch_builder_create_hull(&builder, &hull);

          simd4f vertex3 = simd4f_create(0.5f, 0.25f, 0.1337f, 1.f);

          sch_builder_face_begin(&builder);
          sch_builder_face_vertex(&builder, vertex0);
          sch_builder_face_vertex(&builder, vertex1);
          sch_builder_face_vertex(&builder, vertex3);
          sch_builder_face_end(&builder);

          THEN("check face and vertex definitions") {
            REQUIRE(builder.num_faces == 2);
            REQUIRE(builder.num_vertices == 4);
            REQUIRE(builder.face_vert_idx_start[0] == 0);
            REQUIRE(builder.face_vert_idx_end[0] == 2);
            REQUIRE(builder.face_vert_idx_start[1] == 3);
            REQUIRE(builder.face_vert_idx_end[1] == 5);
          }

          THEN("hull creation fails") {
            sch_hull concave_hull;
            SchHullResult result = sch_builder_create_hull(&builder, &concave_hull);

            REQUIRE(result == SchHullResultConcaveVertex);

            sch_hull_free_memory(&concave_hull);
          }

          sch_hull_free_memory(&hull);
        }

        WHEN("adding adjacent clockwise winded face") {
          simd4f vertex4 = simd4f_create(0.5f, -0.25f, 0, 1.f);

          sch_builder_face_begin(&builder);
          sch_builder_face_vertex(&builder, vertex0);
          sch_builder_face_vertex(&builder, vertex1);
          sch_builder_face_vertex(&builder, vertex4);
          sch_builder_face_end(&builder);

          THEN("hull creation fails") {
            sch_hull hull;
            SchHullResult result = sch_builder_create_hull(&builder, &hull);

            REQUIRE(result == SchHullResultWrongWinding);

            sch_hull_free_memory(&hull);
          }
        }

        WHEN("adding adjacent counter clockwise winded face") {
          simd4f vertex4 = simd4f_create(0.5f, -0.25f, 0, 1.f);

          sch_builder_face_begin(&builder);
          sch_builder_face_vertex(&builder, vertex0);
          sch_builder_face_vertex(&builder, vertex4);
          sch_builder_face_vertex(&builder, vertex1);
          sch_builder_face_end(&builder);

          THEN("edge v0-v1 has twin edge v1-v0") {
            sch_hull hull;
            SchHullResult result = sch_builder_create_hull(&builder, &hull);

            REQUIRE(result == SchHullResultOpenHull);

            sch_edge* edge_v0_v1 = sch_hull_find_edge(&hull, vertex0, vertex1);
            sch_edge* edge_v1_v0 = sch_hull_find_edge(&hull, vertex1, vertex0);

            REQUIRE (edge_v0_v1 != NULL);
            REQUIRE (edge_v1_v0->twin == edge_v0_v1);

            sch_hull_free_memory(&hull);
          }
        }
      }
    }

    WHEN("creating a tetrahedron ") {
      simd4f vertex3 = simd4f_create (0.5f, 0.f, -0.5f, 1.f);

      // +z facing triangle
      sch_builder_face_begin(&builder);
      sch_builder_face_vertex(&builder, vertex0);
      sch_builder_face_vertex(&builder, vertex1);
      sch_builder_face_vertex(&builder, vertex2);
      sch_builder_face_end(&builder);

      // +x,-z facing triangle
      sch_builder_face_begin(&builder);
      sch_builder_face_vertex(&builder, vertex1);
      sch_builder_face_vertex(&builder, vertex3);
      sch_builder_face_vertex(&builder, vertex2);
      sch_builder_face_end(&builder);

      // -x,-z facing triangle
      sch_builder_face_begin(&builder);
      sch_builder_face_vertex(&builder, vertex0);
      sch_builder_face_vertex(&builder, vertex2);
      sch_builder_face_vertex(&builder, vertex3);
      sch_builder_face_end(&builder);

      // -y facing triangle
      sch_builder_face_begin(&builder);
      sch_builder_face_vertex(&builder, vertex0);
      sch_builder_face_vertex(&builder, vertex3);
      sch_builder_face_vertex(&builder, vertex1);
      sch_builder_face_end(&builder);

      THEN("results in a closed hull") {
        sch_hull hull;
        SchHullResult result = sch_builder_create_hull(&builder, &hull);
        REQUIRE (result == SchHullResultOK);
        sch_hull_free_memory(&hull);
      }
    }
  }
}

TEST_CASE("UnitCubeSupport", "[sconvcol]") {
  GIVEN("A unit cube hull") {
    sch_hull hull;
    sch_create_unitbox(&hull);

    WHEN("Querying support for 2, 2, 2") {
      simd4f support;
      simd4f normal = simd4f_create(2.f, 2.f, 2.f, 1.f);
      sch_hull_get_support(&hull, normal, &support);
      THEN ("support is 0.5, 0.5, 0.5") {
        simd4f reference = simd4f_create (0.5f, 0.5f, 0.5f, 1.f);
        REQUIRE (sch_simd4f_equal(reference, support));
      }
    }

    WHEN("Querying support for -2, -2, -2") {
      simd4f support;
      simd4f normal = simd4f_create(-2.f, -2.f, -2.f, 1.f);
      sch_hull_get_support(&hull, normal, &support);
      THEN ("support is -0.5, -0.5, -0.5") {
        simd4f reference = simd4f_create (-0.5f, -0.5f, -0.5f, 1.f);
        REQUIRE (sch_simd4f_equal(reference, support));
      }
    }

    WHEN("Querying support for 0, 1, 0") {
      simd4f support;
      simd4f normal = simd4f_create(0.f, 1.f, 0.f, 1.f);
      sch_hull_get_support(&hull, normal, &support);
      THEN ("y component of support is 0.5") {
        REQUIRE (simd4f_get_y(support) == 0.5);
      }
    }

    sch_hull_free_memory(&hull);
  }
}

TEST_CASE("UnitCubeTransform", "[sconvcol]") {
  GIVEN("A unit cube hull") {
    sch_hull hull;
    sch_create_unitbox(&hull);

    WHEN ("not transforming cube") {
      THEN ("center is at 0,0,0") {
        REQUIRE (sch_simd4f_equal(simd4f_create(0.f, 0.f, 0.f, 1.f), hull.center));
      }
    }

    WHEN ("translating by 1, 2, 3") {
      sch_hull_translate(&hull, 1.f, 2.f, 3.f);
      THEN ("center is at 1, 2, 3") {
        REQUIRE (sch_simd4f_equal(simd4f_create(1.f, 2.f, 3.f, 1.f), hull.center));
      }

      THEN ("is equivalent to transform by translation matrix") {
        sch_hull hull_trans_mat;
        sch_create_unitbox(&hull_trans_mat);
        simd4x4f trans_mat;
        simd4x4f_translation(&trans_mat, 1.f, 2.f, 3.f);
        sch_hull_transform(&hull_trans_mat, trans_mat);

        REQUIRE (sch_simd4f_equal(hull.center, hull_trans_mat.center));

        sch_hull_free_memory(&hull_trans_mat);
      }
    }

    WHEN ("rotating around y by 90 degrees") {
      simd4f vert0 = simd4f_create (0.5f, -0.5f, 0.5f, 1.f);
      REQUIRE (sch_simd4f_equal(vert0, hull.vertices[0].p));
      simd4f vert0_rot = simd4f_create (0.5f, -0.5f, -0.5f, 1.f);
      sch_hull_rotate(&hull, M_PI * 0.5, simd4f_create (1.f, 0.f, 0.f, 1.f));
      THEN ("center is at 0, 0, 0") {
        REQUIRE (sch_simd4f_equal(simd4f_create(0.f, 0.f, 0.f, 1.f), hull.center));
        REQUIRE (sch_simd4f_close(vert0_rot, hull.vertices[0].p, 1.0e-5f));
      }
    }

    sch_hull_free_memory(&hull);
  }
}

TEST_CASE ("UnitCubeSAT", "[sconvcol]") {
  sch_hull hull_A;
  sch_create_unitbox(&hull_A);

  sch_hull hull_B;
  sch_create_unitbox(&hull_B);

  sch_manifold manifold;
  sch_manifold_alloc(&manifold, SCH_BUILDER_MAX_NUM_FACE_VERTICES);

  GIVEN ("Two unit boxes separated by 1.1 along x axis") {
    simd4f r = simd4f_create(0.5f + 0.4f * sqrtf(2.), 0.f, 0.f, 1.f);
    simd4x4f translation;
    simd4x4f_translation(
        &translation,
        simd4f_get_x(r),
        simd4f_get_y(r),
        simd4f_get_z(r));

    THEN("Boxes separated") {
      sch_hull_transform(&hull_B, translation);
      simd4x4f identity;
      simd4x4f_identity(&identity);
      bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
      REQUIRE(separated);
    }

    THEN("Boxes separated when supplying trans_B") {
      simd4x4f identity;
      simd4x4f_identity(&identity);
      bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &translation, &manifold);
      REQUIRE(separated);
    }

    THEN("Boxes separated when supplying trans_A") {
      simd4f r = simd4f_create(-0.5f  -0.4f * sqrtf(2.), 0.f, 0.f, 1.f);
      simd4x4f_translation(
          &translation,
          simd4f_get_x(r),
          simd4f_get_y(r),
          simd4f_get_z(r));

      simd4x4f identity;
      simd4x4f_identity(&identity);
      bool separated = sch_hull_sat(&hull_A, &translation, &hull_B, &identity, &manifold);
      REQUIRE(separated);
    }
  }

  GIVEN("Box B rotated by 45 degrees around y and translated along x by 1.1") {
    sch_hull_rotate(&hull_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 1.f, 0.f, 1.f));
    sch_hull_translate(&hull_B, 1.1f, 0.f, 0.f);

    THEN("Boxes overlap") {
      simd4x4f identity;
      simd4x4f_identity(&identity);
      bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
      REQUIRE(!separated);
    }
  }

  GIVEN("Box A rotated by 45 deg around z, Box B rotated by 45 degrees around y and translated along x by sqrt(2)") {
    sch_hull_rotate(&hull_A, M_PI / 180.0f * 45.f, simd4f_create(0.f, 0.f, 1.f, 1.f));
    sch_hull_rotate(&hull_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 1.f, 0.f, 1.f));
    sch_hull_translate(&hull_B, sqrt(2.f), 0.f, 0.f);

    THEN("Boxes overlap") {
      simd4x4f identity;
      simd4x4f_identity(&identity);
      bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
      REQUIRE(!separated);
    }

    WHEN("Shifted by another 0.001 along x") {
      sch_hull_translate(&hull_B, 0.001f, 0.f, 0.f);

      THEN("Boxes are separated") {
        simd4x4f identity;
        simd4x4f_identity(&identity);
        bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
        REQUIRE(separated);
      }
    }
  }

  GIVEN("Box B translated by 1z and rotated 45 degrees around z") {
    sch_hull_rotate(&hull_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 0.f, 1.f, 1.f));
    sch_hull_translate(&hull_B, 0., 0.f, 0.97f);

//    THEN("Boxes overlap") {
//      simd4x4f identity;
//      simd4x4f_identity(&identity);
//      bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
//      REQUIRE(!separated);
//      REQUIRE(manifold.num_points == 8);
//    }
  }

  GIVEN("Box B translated by 1x, 1.001z") {
    sch_hull_translate(&hull_B, 1., 0.f, 1.00f);

//    THEN("Boxes overlap") {
//      simd4x4f identity;
//      simd4x4f_identity(&identity);
//      bool separated = sch_hull_sat(&hull_A, &identity, &hull_B, &identity, &manifold);
//      REQUIRE(!separated);
//      REQUIRE(manifold.num_points == 8);
//    }
  }

  sch_manifold_free_memory(&manifold);
  sch_hull_free_memory(&hull_A);
  sch_hull_free_memory(&hull_B);
}

TEST_CASE("PlaneTests", "[sconvcol]") {
  GIVEN("Z=0 Plane") {
    sch_plane plane;
    plane.n = simd4f_create(0.f, 0.f, 1.f, 1.f);
    plane.p = simd4f_create(1.f, 1.2354f, 0.f, 1.f);

    WHEN("Querying point with z = -0.1") {
      simd4f p = simd4f_create (1231.3f, 552.2f, -0.1f, 1.f);
      float dist = sch_plane_distance (&plane, &p);
      THEN ("distance is -0.1") {
        REQUIRE (dist == -0.1f);
      }
    }

    WHEN ("Intersecting with line (5., 3., 2.) - (12., 332., SCH_EPS") {
      simd4f p0 = simd4f_create (5.f, 3.f, 1.f, 1.f);
      simd4f p1 = simd4f_create (12.f, 332.f, SCH_EPS, 1.f);
      simd4f result;
      bool is_intersecting = sch_plane_intersection(&plane, &p0, &p1, &result);
      THEN ("have intersection") {
        REQUIRE (is_intersecting);
      }

      WHEN ("Reversing the query") {
        simd4f result;
        bool is_intersecting = sch_plane_intersection(&plane, &p1, &p0, &result);
        THEN ("have intersection") {
          REQUIRE (is_intersecting);
        }
      }
    }

    WHEN ("Intersecting with line (5., 3., 2.) - (12., 332., SCH_EPS * 2") {
      simd4f p0 = simd4f_create (5.f, 3.f, 1.f, 1.f);
      simd4f p1 = simd4f_create (12.f, 332.f, SCH_EPS * 2.f, 1.f);
      simd4f result;
      bool is_intersecting = sch_plane_intersection(&plane, &p0, &p1, &result);
      THEN ("have no intersection") {
        REQUIRE (is_intersecting);
      }

      WHEN ("Reversing the query") {
        simd4f result;
        bool is_intersecting = sch_plane_intersection(&plane, &p1, &p0, &result);
        THEN ("have no intersection") {
          REQUIRE (is_intersecting);
        }
      }
    }

    WHEN("Projecting point (123.4, 332., 212.) onto plane") {
      simd4f p = simd4f_create (123.4f, 332.f, 212.f, 1.f);
      sch_plane_point_project(&plane, &p);
      THEN ("z component is 0.") {
        REQUIRE (simd4f_get_z(p) == 0.f);
      }

      WHEN("Using z = -212.0") {
        p = simd4f_create(123.4f, 332.f, -212.f, 1.f);
        sch_plane_point_project(&plane, &p);
        THEN("projected z component is 0.") {
          REQUIRE(simd4f_get_z(p) == 0.f);
        }
      }
    }
  }
}

TEST_CASE ("ClipFaces", "[sconvcol]") {
  sch_hull_builder builder;
  sch_hull triangle_hull;
  sch_hull quad_hull;

  // Build triangle
  sch_builder_reset(&builder);

  // top
  sch_builder_face_begin(&builder);
  sch_builder_face_vertex(&builder, simd4f_create (0.5f, -0.5f, 0.f, 1.f));
  sch_builder_face_vertex(&builder, simd4f_create ( 0.0f, 0.5f, 0.f, 1.f));
  sch_builder_face_vertex(&builder, simd4f_create ( -0.5f, -0.5f,  0.f, 1.f));

  sch_builder_face_end(&builder);

  // bottom
  sch_builder_face_begin(&builder);
  sch_builder_face_vertex(&builder, simd4f_create (0.5f, -0.5f, 0.f, 1.f));
  sch_builder_face_vertex(&builder, simd4f_create ( -0.5f, -0.5f,  0.f, 1.f));
  sch_builder_face_vertex(&builder, simd4f_create ( 0.0f, 0.5f, 0.f, 1.f));
  sch_builder_face_end(&builder);

  int hull_result = sch_builder_create_hull(&builder, &triangle_hull);
  triangle_hull.center = simd4f_create(0.f, 0.f, 0.f, 1.f);
  REQUIRE (hull_result == SchHullResultOK);

  // Build quad
  sch_builder_reset(&builder);

  // top
  sch_builder_face_begin(&builder);
  sch_builder_face_vertex(&builder, simd4f_create (-0.5f, -0.5f, 0.f, 1.f));
  sch_builder_face_vertex(&builder, simd4f_create ( 0.5f, -0.5f, 0.f, 1.f));
  sch_builder_face_vertex(&builder, simd4f_create ( 0.5f, 0.5f,  0.f, 1.f));
  sch_builder_face_vertex(&builder, simd4f_create (-0.5f, 0.5f,  0.f, 1.f));
  sch_builder_face_end(&builder);

  // bottom
  sch_builder_face_begin(&builder);
  sch_builder_face_vertex(&builder, simd4f_create(0.5f, -0.5f, 0.f, 1.f));
  sch_builder_face_vertex(&builder, simd4f_create(-0.5f, -0.5f, 0.f, 1.f));
  sch_builder_face_vertex(&builder, simd4f_create(-0.5f, 0.5f, 0.f, 1.f));
  sch_builder_face_vertex(&builder, simd4f_create(0.5f, 0.5f, 0.f, 1.f));
  sch_builder_face_end(&builder);

  hull_result = sch_builder_create_hull(&builder, &quad_hull);
  quad_hull.center = simd4f_create(0.f, 0.f, 0.f, 1.f);
  REQUIRE (hull_result == SchHullResultOK);

  GIVEN ("A Triangle and a Quad") {
    WHEN ("Translating the Quad along +Y") {
      sch_hull_translate(&quad_hull, 0.f, 0.5f, 0.f);
      sch_manifold manifold;
      sch_manifold_alloc(&manifold, SCH_BUILDER_MAX_NUM_FACE_VERTICES);
      sch_manifold_reset(&manifold);

      sch_clip_faces(&triangle_hull.faces[0], &quad_hull.faces[1], &manifold);
      REQUIRE (manifold.num_points == 3);
      sch_manifold_free_memory(&manifold);
    }
  }
}