rbdlsim/src/sconvcol.h

#ifndef SCONVCOL_H
#define SCONVCOL_H

#ifdef __cplusplus
extern "C" {
#endif

#include <assert.h>
#include <float.h>
#include <stdbool.h>

#include "vectorial/simd4x4f.h"

#define SCH_EPS 0.001

inline bool sch_simd4f_equal(simd4f a, simd4f b) {
  return (simd4f_get_x(simd4f_length4_squared(simd4f_sub(a, b))) == 0.f);
}

inline bool sch_simd4f_close(simd4f a, simd4f b, float tol) {
  return (simd4f_get_x(simd4f_length4_squared(simd4f_sub(a, b))) < tol * tol);
}

inline void sch_simd4x4f_transform_inv(simd4x4f* trans, simd4x4f* out) {
  *out = *trans;
  simd4f w = simd4f_create(
      -simd4f_get_x(trans->w),
      -simd4f_get_y(trans->w),
      -simd4f_get_z(trans->w),
      1.f);

  out->w = simd4f_create(0.f, 0.f, 0.f, 1.f);
  simd4x4f_transpose_inplace(out);

  simd4f out_w;
  simd4x4f_matrix_vector_mul(out, &w, &out_w);
  out->w = out_w;
}

typedef struct sch_edge sch_edge;
typedef struct sch_vert sch_vert;
typedef struct sch_face sch_face;
typedef struct sch_hull sch_hull;
typedef struct sch_plane sch_plane;
typedef struct sch_hull_builder sch_hull_builder;
typedef struct sch_face_query sch_face_query;
typedef struct sch_edge_query sch_edge_query;
typedef struct sch_manifold sch_manifold;

struct sch_edge {
  sch_vert* vert;
  sch_edge* twin;
  sch_face* face;
  sch_edge* next;
};

struct sch_vert {
  simd4f p;
  sch_edge* edge;
};

struct sch_face {
  sch_edge* edge;
};

struct sch_hull {
  sch_face* faces;
  sch_edge* edges;
  sch_vert* vertices;
  simd4f center;
  int num_faces;
  int num_edges;
  int num_vertices;
};

struct sch_plane {
  simd4f p;
  simd4f n;
};

struct sch_face_query {
  float dist;
  int face_idx;
  sch_plane plane;
  simd4f vert;
};

struct sch_edge_query {
  float dist;
  sch_plane plane;
  int edge_A_idx;
  int edge_B_idx;
};

struct sch_manifold {
  int num_points;
  int num_vertices;
  simd4f* vertices;
  simd4f normal;
};

//
// Hull Builder
//

#define SCH_BUILDER_MAX_NUM_VERTICES 1024
#define SCH_BUILDER_MAX_NUM_FACES 256
#define SCH_BUILDER_MAX_NUM_FACE_VERTICES 32

enum SchHullResult {
  SchHullResultOK = 0,
  SchHullResultConcaveVertex = -1,
  SchHullResultWrongWinding = -2,
  SchHullResultInvalidTwinEdges = -3,
  SchHullResultOpenHull = -4
};

typedef enum SchHullResult SchHullResult;

struct sch_hull_builder {
  int num_vertices;
  int num_faces;
  simd4f vertices[SCH_BUILDER_MAX_NUM_VERTICES];
  int face_vert_idx[SCH_BUILDER_MAX_NUM_FACES * SCH_BUILDER_MAX_NUM_VERTICES];
  int face_vert_idx_start[SCH_BUILDER_MAX_NUM_FACES];
  int face_vert_idx_end[SCH_BUILDER_MAX_NUM_FACES];
};

void sch_builder_reset(sch_hull_builder* builder);

void sch_builder_face_begin(sch_hull_builder* builder);

void sch_builder_face_vertex(sch_hull_builder* builder, simd4f vertex);

void sch_builder_face_end(sch_hull_builder* builder);

SchHullResult sch_builder_create_hull(sch_hull_builder* builder, sch_hull* out_hull);

//
// Helper data structures
//
void sch_manifold_alloc (sch_manifold* manifold, int num_vertices);
void sch_manifold_add_point (sch_manifold* manifold, simd4f p);
void sch_manifold_reset (sch_manifold* manifold);
void sch_manifold_free_memory (sch_manifold* manifold);

//
// Calculations
//

float sch_plane_distance(const sch_plane* plane, const simd4f* v);

bool sch_plane_intersection(const sch_plane* plane, const simd4f* p0, const simd4f* p1, simd4f* result);

void sch_plane_point_project(const sch_plane* plane, simd4f* p);

void sch_edge_get_dir(const sch_edge* edge, simd4f* out_dir);

void sch_hull_free_memory (sch_hull* hull);

void sch_hull_translate (sch_hull* hull, float x, float y, float z);

void sch_hull_rotate (sch_hull* hull, const float radians, const simd4f axis);

void sch_hull_transform (sch_hull* hull, simd4x4f mat);

void sch_hull_calc_plane(const sch_hull* hull, const int index, sch_plane* out_plane);

sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4f v1);

void sch_hull_get_support(const sch_hull* hull, simd4x4f* trans, simd4f n, simd4f* out_vert);

float sch_query_face_directions(
    const sch_hull* hull_A,
    simd4x4f* trans_AtoB,
    const sch_hull* hull_B,
    sch_face_query* result);

void sch_clip_faces (const sch_face* ref_face, const sch_face* inc_face, sch_manifold* manifold);

void sch_create_face_contact (const sch_face_query* query_A_B, const sch_hull* hull_A, const sch_face_query* query_B_A, const sch_hull* hull_B, sch_manifold *result);

void sch_create_edge_contact (const sch_edge_query* query_edge, const sch_hull* hull_A, const sch_hull* hull_B, sch_manifold* result);

bool sch_hull_sat(const sch_hull* hull_A, simd4x4f* trans_A, const sch_hull* hull_B, simd4x4f* trans_B, sch_manifold* result);

int sch_hull_is_vertex_concave(const sch_hull* hull, const simd4f p);

SchHullResult sch_hull_is_closed (const sch_hull* hull);

SchHullResult sch_hull_connect_face_edges(const sch_hull* hull, int face_index);

void sch_create_face(int num_vert, simd4f* vertices, sch_face* out_face);

void sch_create_unitbox(sch_hull* out_hull);

//
// sconvcol Implementation
//

#ifdef SCONVCOL_IMPLEMENTATION

void sch_manifold_alloc (sch_manifold* manifold, int num_vertices) {
  manifold->vertices = (simd4f*) aligned_alloc(16, sizeof(simd4f) * num_vertices);
  assert (manifold->vertices != NULL);
  manifold->num_vertices = num_vertices;
  sch_manifold_reset(manifold);
}

void sch_manifold_add_point (sch_manifold* manifold, simd4f p) {
  assert (manifold->num_vertices > manifold->num_points);
  manifold->vertices[manifold->num_points] = p;
  manifold->num_points++;
}

void sch_manifold_reset (sch_manifold* manifold) {
  manifold->num_points = 0;
}

void sch_manifold_free_memory (sch_manifold* manifold) {
  free (manifold->vertices);
  manifold->vertices = NULL;
  manifold->num_vertices = 0;
}

float sch_plane_distance(const sch_plane* plane, const simd4f* v) {
  return simd4f_dot3_scalar(simd4f_sub(*v, plane->p), plane->n);
}

bool sch_plane_intersection(const sch_plane* plane, const simd4f* p0, const simd4f* p1, simd4f* result) {
  simd4f line = simd4f_sub (*p1, *p0);
  float line_dot_n = simd4f_dot3_scalar(plane->n, line);
  if (fabs(line_dot_n) < SCH_EPS) {
    return false;
  }

  float s = -simd4f_dot3_scalar(simd4f_sub(*p0, plane->p), plane->n) / line_dot_n;
  *result = simd4f_add(*p0, simd4f_mul(line, simd4f_splat(s)));
  if (fabs (sch_plane_distance(plane, result)) <= SCH_EPS) {
    return true;
  }

  return false;
}

void sch_plane_point_project(const sch_plane* plane, simd4f* p) {
  float s = -simd4f_dot3_scalar(simd4f_sub(*p, plane->p), plane->n);
  *p = simd4f_add (*p, simd4f_mul(plane->n, simd4f_splat(s)));
}

void sch_create_face(int num_vert, simd4f* vertices, sch_face* out_face) {
  assert(out_face != NULL);
  assert(out_face->edge == NULL);

  int i = 0;
  sch_edge* f_edges = (sch_edge*) malloc(sizeof(sch_edge) * num_vert);
  sch_vert* f_verts = (sch_vert*) malloc(sizeof(sch_vert) * num_vert);

  while (i < num_vert) {
    sch_vert* vert = &f_verts[i];
    sch_edge* edge = &f_edges[i];

    edge->twin = NULL;
    edge->vert = vert;
    edge->face = out_face;
    edge->next = &f_edges[(i + 1) % num_vert];

    vert->edge = edge;
    vert->p = vertices[i];

    i++;
  }

  out_face->edge = &f_edges[0];
}

void sch_edge_get_dir(const sch_edge* edge, simd4f* out_dir) {
  *out_dir = simd4f_sub(edge->next->vert->p, edge->vert->p);
  float recip_len = 1.f / sqrtf(simd4f_dot3_scalar(*out_dir, *out_dir));
  *out_dir = simd4f_mul(*out_dir, simd4f_splat(recip_len));
}

void sch_builder_reset(sch_hull_builder* builder) {
  builder->num_vertices = 0;
  builder->num_faces = 0;
}

void sch_builder_face_begin(sch_hull_builder* builder) {
  int face_index = builder->num_faces;
  if (builder->num_faces == 0) {
    builder->face_vert_idx_start[face_index] = 0;
  } else {
    builder->face_vert_idx_start[face_index] = builder->face_vert_idx_end[face_index - 1] + 1;
  }

  builder->face_vert_idx_end[face_index] = -1;
}

void sch_builder_face_end(sch_hull_builder* builder) { builder->num_faces++; }

void sch_builder_allocate_memory (sch_hull_builder* builder, sch_hull* out_hull) {
  out_hull->faces = (sch_face*)malloc(sizeof(sch_face) * builder->num_faces);

  out_hull->num_vertices = builder->num_vertices;
  out_hull->vertices =
      (sch_vert*)malloc(sizeof(sch_vert) * builder->num_vertices);

  int num_edges = 0;
  for (int face_index = 0; face_index < builder->num_faces; face_index++) {
    int start_idx = builder->face_vert_idx_start[face_index];
    int end_idx = builder->face_vert_idx_end[face_index];
    int edge_count = end_idx - start_idx;

    num_edges += edge_count + 1;
  }

  out_hull->num_edges = num_edges;
  out_hull->edges = (sch_edge*)malloc(sizeof(sch_edge) * num_edges);
}

void sch_hull_translate (sch_hull* hull, float x, float y, float z) {
  simd4f r = simd4f_create (x, y, z, 0.f);
  hull->center = simd4f_add (hull->center, r);

  for (int vi = 0; vi < hull->num_vertices; vi++) {
    hull->vertices[vi].p = simd4f_add (hull->vertices[vi].p, r);
  }
}

void sch_hull_rotate (sch_hull* hull, const float radians, const simd4f axis) {
  simd4x4f rot_mat;
  simd4x4f_axis_rotation(&rot_mat, radians, axis);
  simd4x4f_matrix_vector_mul(&rot_mat, &hull->center, &hull->center);

  for (int vi = 0; vi < hull->num_vertices; vi++) {
    simd4x4f_matrix_vector_mul(&rot_mat, &hull->vertices[vi].p, &hull->vertices[vi].p);
  }
}

void sch_hull_transform (sch_hull* hull, simd4x4f mat) {
  simd4f v_temp = hull->center;
  simd4x4f_matrix_vector_mul(&mat, &v_temp, &hull->center);

  for (int vi = 0; vi < hull->num_vertices; vi++) {
    v_temp = hull->vertices[vi].p;
    simd4x4f_matrix_vector_mul(&mat, &v_temp, &hull->vertices[vi].p);
  }
}

void sch_hull_free_memory (sch_hull* hull) {
  free (hull->faces);
  hull->faces = NULL;
  hull->num_faces = 0;
  free (hull->vertices);
  hull->vertices = NULL;
  hull->num_vertices = 0;
  free (hull->edges);
  hull->edges = NULL;
  hull->num_edges = 0;
}

SchHullResult sch_builder_create_hull(sch_hull_builder* builder, sch_hull* out_hull) {
  sch_builder_allocate_memory(builder, out_hull);
  out_hull->num_faces = 0;

  int hull_edge_idx = 0;

  for (int face_index = 0; face_index < builder->num_faces; face_index++) {
    sch_face* face = &out_hull->faces[face_index];

    int start_idx = builder->face_vert_idx_start[face_index];
    int end_idx = builder->face_vert_idx_end[face_index];

    sch_edge* prev_edge = NULL;

    for (int face_vert_idx = start_idx; face_vert_idx <= end_idx;
         face_vert_idx++) {
      sch_edge* edge = &out_hull->edges[hull_edge_idx];
      hull_edge_idx++;
      if (face_vert_idx == start_idx) {
        face->edge = edge;
      }

      int vert_idx = builder->face_vert_idx[face_vert_idx];
      sch_vert* vert = &out_hull->vertices[vert_idx];

      vert->p = builder->vertices[vert_idx];
      vert->edge = edge;
      edge->vert = vert;
      edge->twin = NULL;
      edge->face = face;

      if (sch_hull_is_vertex_concave(out_hull, vert->p)) {
        sch_hull_free_memory(out_hull);
        return SchHullResultConcaveVertex;
      }

      if (face_vert_idx == end_idx) {
        edge->next = face->edge;
      }
      if (prev_edge != NULL) {
        prev_edge->next = edge;
      }

      prev_edge = edge;
    }

    SchHullResult edge_add_result = sch_hull_connect_face_edges (out_hull, face_index);
    if (edge_add_result != SchHullResultOK) {
      sch_hull_free_memory(out_hull);
      return edge_add_result;
    }

    out_hull->num_faces = face_index + 1;
  }

  assert (hull_edge_idx == out_hull->num_edges);

  return sch_hull_is_closed (out_hull);
}

void sch_builder_face_vertex(sch_hull_builder* builder, simd4f vertex) {
  int face_index = builder->num_faces;
  int vert_index = -1;
  for (int i = 0; i < builder->num_vertices; i++) {
    if (simd4f_get_x(
            simd4f_length4_squared(simd4f_sub(builder->vertices[i], vertex)))
        < 1.0e-4) {
      vert_index = i;
      break;
    }
  }

  if (vert_index == -1) {
    vert_index = builder->num_vertices;
    builder->vertices[vert_index] = vertex;
    builder->num_vertices++;
  }

  if (builder->face_vert_idx_end[face_index] == -1) {
    builder->face_vert_idx_end[face_index] =
        builder->face_vert_idx_start[face_index];
  } else {
    builder->face_vert_idx_end[face_index]++;
  }

  int face_end_idx = builder->face_vert_idx_end[face_index];
  builder->face_vert_idx[face_end_idx] = vert_index;
}

void sch_face_calc_plane(const sch_face* face, sch_plane* out_plane) {
  assert (face != NULL);
  assert (out_plane != NULL);

  sch_edge* edge0 = face->edge;
  sch_edge* edge1 = edge0->next;
  simd4f dir0;
  simd4f dir1;
  sch_edge_get_dir(edge0, &dir0);
  sch_edge_get_dir(edge1, &dir1);

  out_plane->p = edge0->vert->p;
  out_plane->n = simd4f_cross3(dir0, dir1);
}

void sch_hull_calc_plane(const sch_hull* hull, const int index, sch_plane* out_plane) {
  assert(hull != NULL);
  assert(index >= 0 && index < hull->num_faces);
  assert(out_plane != NULL);

  // TODO move plane calculation to create hull?
  sch_face_calc_plane(&hull->faces[index], out_plane);
}

sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4f v1) {
  for (int fi = 0; fi < hull->num_faces; fi++) {
    sch_face* face = &hull->faces[fi];
    sch_edge* edge0 = face->edge;
    sch_edge* edge = edge0;
    do {
      if (sch_simd4f_equal(edge->vert->p, v0)
          && sch_simd4f_equal(edge->next->vert->p, v1)) {
        return edge;
      }
      edge = edge->next;
    } while (edge != edge0);
  }

  return NULL;
}

void sch_hull_get_support(const sch_hull* hull, simd4x4f* trans, simd4f n, simd4f* out_vert) {
  sch_edge* edge = hull->faces[0].edge;
  sch_edge* last_edge = edge;
  float normal_dot_edge = -1.;

  simd4f n_local;
  simd4x4f_inv_ortho_matrix_vector3_mul(trans, &n, &n_local);

  do {
    simd4f dir = simd4f_normalize3(simd4f_sub (edge->next->vert->p, edge->vert->p));
    normal_dot_edge = simd4f_dot3_scalar(dir, n_local);

    if (normal_dot_edge <= 0.) {
      edge = edge->twin->next;
    } else {
      edge = edge->next;
      last_edge = edge;
    }
  } while (last_edge != edge || normal_dot_edge > 0);

	simd4x4f_matrix_point3_mul(trans, &edge->vert->p, out_vert);
}

float sch_query_face_directions(
    const sch_hull* hull_A,
    simd4x4f* trans_BtoA,
    const sch_hull* hull_B,
    sch_face_query* result) {
  result->dist = -FLT_MAX;
  for (int fi = 0; fi < hull_A->num_faces; fi++) {
    sch_plane plane;
    sch_hull_calc_plane(hull_A, fi, &plane);
    simd4f vert_B;
    sch_hull_get_support(hull_B, trans_BtoA, simd4f_sub(simd4f_zero(), plane.n), &vert_B);

    float distance = sch_plane_distance(&plane, &vert_B);
    if (distance > result->dist) {
      result->dist = distance;
      result->face_idx = fi;
      result->plane = plane;
      result->vert = vert_B;
    }
  }

  return result->dist;
}

float sch_query_edge_directions(
    const sch_hull* hull_A,
    simd4x4f *trans_BtoA,
    const sch_hull* hull_B,
    sch_edge_query* result) {
  result->dist = -FLT_MAX;
  for (int iAe = 0; iAe < hull_A->num_edges; iAe++) {
    for (int iBe = 0; iBe < hull_B->num_edges; iBe++) {
      sch_edge* edge_A = &hull_A->edges[iAe];
      sch_edge* edge_B = &hull_B->edges[iBe];
      simd4f dir_A;
      sch_edge_get_dir(edge_A, &dir_A);

      simd4f dir_B_B;
      sch_edge_get_dir(edge_B, &dir_B_B);
      simd4f dir_B;
      simd4x4f_inv_ortho_matrix_vector3_mul(trans_BtoA, &dir_B_B, &dir_B);

      simd4f axis = simd4f_cross3 (dir_A, dir_B);
      if (simd4f_dot3_scalar(dir_A, simd4f_sub(edge_A->vert->p, hull_A->center) ) < 0.f) {
        axis = simd4f_sub(simd4f_zero(), axis);
      }

      // TODO: properly handle touching parallel edges.
      if (simd4f_get_x(simd4f_length3_squared(axis)) < SCH_EPS) {
        continue;
      }

      axis = simd4f_normalize3(axis);

      sch_plane plane_A;
      plane_A.n = axis;
      // Note: in Gregorius talk he uses the origin of edge_A. This is wrong as
      // there are likely points in hull_A that are further out along the plane
      // normal. We therefore use the support point of hull_A along the axis.
      simd4x4f identity;
      simd4x4f_identity(&identity);
      sch_hull_get_support(hull_A, &identity, plane_A.n, &plane_A.p);

      simd4f vert_B_B;
      sch_hull_get_support(hull_B, trans_BtoA, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B_B);
      simd4f vert_B;
      simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B_B, &vert_B);


      float distance = sch_plane_distance(&plane_A, &vert_B);
      if (distance > result->dist) {
        result->dist = distance;
        result->plane = plane_A;
        result->edge_A_idx = iAe;
        result->edge_B_idx = iBe;
      }
    }
  }

  return result->dist;
}

void sch_clip_faces (const sch_face* ref_face, const sch_face* inc_face, sch_manifold* manifold) {
  simd4f* input_vertices = (simd4f*) malloc(sizeof(simd4f) * manifold->num_vertices);
  assert (input_vertices != NULL);

  sch_edge* inc_start_edge = inc_face->edge;
  sch_edge* inc_cur_edge = inc_face->edge;

  sch_manifold_reset(manifold);
  int num_input_vertices = 0;
  do {
    sch_manifold_add_point(manifold, inc_cur_edge->vert->p);
    inc_cur_edge = inc_cur_edge->next;
  } while (inc_cur_edge != inc_start_edge);

  sch_edge* ref_start_edge = ref_face->edge;
  sch_edge* ref_cur_edge = ref_face->edge;

  sch_plane ref_face_plane;
  sch_face_calc_plane(ref_face, &ref_face_plane);

  do {
    memcpy(input_vertices, manifold->vertices, sizeof(simd4f) * manifold->num_points);
    num_input_vertices = manifold->num_points;
    sch_manifold_reset(manifold);

    // construct plane orthogonal to reference face that contains current edge.
    sch_plane edge_plane;
    edge_plane.n = simd4f_normalize3(simd4f_cross3(ref_face_plane.n, simd4f_sub(ref_cur_edge->next->vert->p, ref_cur_edge->vert->p)));
    edge_plane.p = ref_cur_edge->vert->p;

    for (int i = 0; i < num_input_vertices; i++) {
      simd4f current = input_vertices[i];
      simd4f prev = input_vertices[(i + num_input_vertices - 1) % num_input_vertices];
      simd4f intersection;
      bool is_intersecting = sch_plane_intersection(&edge_plane, &current, &prev, &intersection);

      if (sch_plane_distance(&edge_plane, &current) >= SCH_EPS) {
        if (sch_plane_distance(&edge_plane, &prev) <= SCH_EPS) {
          sch_manifold_add_point(manifold, intersection);
        }
        sch_manifold_add_point(manifold, current);
      } else if (sch_plane_distance(&edge_plane, &prev) >= SCH_EPS) {
        sch_manifold_add_point(manifold, intersection);
      }
    }

    ref_cur_edge = ref_cur_edge->next;
  } while (ref_cur_edge != ref_start_edge);

  free(input_vertices);
}

void sch_create_face_contact (const sch_face_query* query_A_B, const sch_hull* hull_A, const sch_face_query* query_B_A, const sch_hull* hull_B, sch_manifold *result) {
  const sch_plane* ref_plane = &query_A_B->plane;
  int ref_face_idx = query_A_B->face_idx;
  const sch_hull* ref_hull = hull_A;
  const sch_hull* inc_hull = hull_B;
  const sch_face* ref_face = &hull_A->faces[query_A_B->face_idx];

  // normalize input
  if (query_A_B->dist < query_B_A->dist) {
    ref_plane = &query_B_A->plane;
    ref_face_idx = query_B_A->face_idx;
    ref_face = &hull_B->faces[query_B_A->face_idx];
    ref_hull = hull_B;
    inc_hull = hull_A;
  }

  result->normal = ref_plane->n;

  // find most antiparallel face
  float dot_min = 1.0f;
  int inc_face_idx = -1;
  for (int i = 0; i < inc_hull->num_faces; i++) {
    sch_plane face_plane;
    sch_hull_calc_plane(inc_hull, i, &face_plane);
    float normal_dot = simd4f_dot3_scalar(result->normal, face_plane.n);
    if (normal_dot < dot_min) {
      dot_min = normal_dot;
      inc_face_idx = i;
    }
  }
  assert (inc_face_idx >= 0);

  const sch_face* inc_face = &inc_hull->faces[inc_face_idx];
  sch_clip_faces(ref_face, inc_face, result);

  // project points onto plane of reference face
  for (int i = 0; i < result->num_points; i++) {
    sch_plane_point_project(ref_plane, &result->vertices[i]);
  }
}

void sch_create_edge_contact (const sch_edge_query* query_edge, const sch_hull* hull_A, const sch_hull* hull_B, sch_manifold* result) {
  sch_edge edge_A = hull_A->edges[query_edge->edge_A_idx];
  sch_edge edge_B = hull_B->edges[query_edge->edge_B_idx];

  sch_plane plane_A_n;
  plane_A_n.p = edge_A.vert->p;
  plane_A_n.n = simd4f_normalize3(simd4f_cross3(simd4f_sub(edge_A.vert->p, edge_A.next->vert->p), query_edge->plane.n));
  simd4f edge_B_projected;
  bool is_edge_B_intersecting;
  is_edge_B_intersecting = sch_plane_intersection(&plane_A_n, &edge_B.vert->p, &edge_B.next->vert->p, &edge_B_projected);
  assert (is_edge_B_intersecting);

  sch_plane plane_B_n;
  plane_B_n.p = edge_B.vert->p;
  plane_B_n.n = simd4f_normalize3(simd4f_cross3(simd4f_sub(edge_B.vert->p, edge_B.next->vert->p), query_edge->plane.n));
  simd4f edge_A_projected;
  bool is_edge_A_intersecting;
  is_edge_A_intersecting = sch_plane_intersection(&plane_B_n, &edge_A.vert->p, &edge_A.next->vert->p, &edge_A_projected);
  assert (is_edge_A_intersecting);

  sch_manifold_add_point(result,simd4f_add(simd4f_mul(edge_B_projected, simd4f_splat(0.5f)),
      simd4f_mul(edge_B_projected, simd4f_splat(0.5f))));
  result->normal = query_edge->plane.n;

#ifndef NDEBUG
  simd4f closest_points_dir = simd4f_normalize3(simd4f_sub (edge_B_projected, edge_A_projected));
  float normal_error = 1.0f - simd4f_dot3_scalar(closest_points_dir, result->normal);
  assert (fabs(normal_error) < SCH_EPS);
#endif
}

bool sch_hull_sat(
    const sch_hull* hull_A,
    simd4x4f* trans_A,
    const sch_hull* hull_B,
    simd4x4f* trans_B,
    sch_manifold* result) {
  simd4x4f trans_Binv;
  sch_simd4x4f_transform_inv(trans_B, &trans_Binv);
  simd4x4f trans_BtoA;
  simd4x4f_matrix_mul(&trans_Binv, trans_A, &trans_BtoA);

  sch_face_query query_A_B;
  sch_query_face_directions(hull_A, &trans_BtoA, hull_B, &query_A_B);
  if (query_A_B.dist > 0.f) {
    return true;
  }

  sch_face_query query_B_A;
  simd4x4f trans_AtoB;
  sch_simd4x4f_transform_inv(&trans_BtoA, &trans_AtoB);
  sch_query_face_directions(hull_B, &trans_AtoB, hull_A, &query_B_A);
  if (query_B_A.dist > 0.f) {
    return true;
  }

  sch_edge_query query_edge;
  sch_query_edge_directions(hull_A, &trans_BtoA, hull_B, &query_edge);
  if (query_edge.dist > 0.f) {
    return true;
  }

  bool is_face_contact_A = query_A_B.dist > query_edge.dist;
  bool is_face_contact_B = query_B_A.dist > query_edge.dist;
  if (is_face_contact_A && is_face_contact_B) {
    sch_create_face_contact (&query_A_B, hull_A, &query_B_A, hull_B, result);
  } else {
    sch_create_edge_contact (&query_edge, hull_A, hull_B, result);
  }

  return false;
}

int sch_hull_is_vertex_concave(const sch_hull* hull, const simd4f v) {
  sch_plane plane;
  for (int i = 0; i < hull->num_faces; i++) {
    sch_hull_calc_plane(hull, i, &plane);
    float distance = sch_plane_distance(&plane, &v);
    return (distance > 0.);
  }

  return 0;
}

SchHullResult sch_hull_connect_face_edges(const sch_hull* hull, int new_face_index) {
  sch_face* new_face = &hull->faces[new_face_index];

  sch_edge* new_face_edge0 = new_face->edge;
  sch_edge* new_face_edge = new_face_edge0;

  do {
    sch_vert* new_edge_v0 = new_face_edge->vert;
    sch_vert* new_edge_v1 = new_face_edge->next->vert;

    for (int fi = 0; fi < hull->num_faces; fi++) {
      if (fi == new_face_index) {
        continue;
      }

      sch_face* face = &hull->faces[fi];
      sch_edge* face_edge0 = face->edge;
      sch_edge* face_edge = face_edge0;

      do {
        sch_vert* edge_v0 = face_edge->vert;
        sch_vert* edge_v1 = face_edge->next->vert;

        if (sch_simd4f_equal(new_edge_v0->p, edge_v0->p)
            && sch_simd4f_equal(new_edge_v1->p, edge_v1->p)) {
          return SchHullResultWrongWinding;
        } else if (sch_simd4f_equal(new_edge_v0->p, edge_v1->p)
                              && sch_simd4f_equal(new_edge_v1->p, edge_v0->p)) {
          if (new_face_edge->twin == NULL && face_edge->twin == NULL) {
            new_face_edge->twin = face_edge;
            face_edge->twin = new_face_edge;
          } else if (new_face_edge->twin == face_edge->twin) {
            // nothing to do
          } else if ((new_face_edge->twin == NULL && face_edge->twin != NULL)
              || (new_face_edge->twin != NULL && face_edge->twin == NULL)) {
            return SchHullResultInvalidTwinEdges;
          }
        }

        face_edge = face_edge->next;
      } while (face_edge != face_edge0);
    }

    new_face_edge = new_face_edge->next;
  } while (new_face_edge != new_face_edge0);

  return SchHullResultOK;
}

SchHullResult sch_hull_is_closed (const sch_hull* hull) {
  for (int ei = 0; ei < hull->num_edges; ei++) {
    if (hull->edges[ei].twin == NULL) {
      return SchHullResultOpenHull;
    }
  }

  return SchHullResultOK;
}

void sch_create_unitbox(sch_hull* out_hull) {
  sch_hull_builder builder;
  sch_builder_reset(&builder);

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

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

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

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

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

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

  int hull_result = sch_builder_create_hull(&builder, out_hull);
  out_hull->center = simd4f_create(0.f, 0.f, 0.f, 1.f);
  assert (hull_result == SchHullResultOK);
}

#endif /* SCONVCOL_IMPLEMENTATION */

#ifdef __cplusplus
}
#endif

#endif /* SCONVCOL_H */