rbdlsim/tests/sconvcolTests.cc

#include <iostream>

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

using namespace std;

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("ManualCubeHull", "[sconvcol]") {}

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);
      }
    }
  }
}