implemented shape class with SAT collision detection

src/shape.c

@@ -0,0 +1,238 @@
+#include "shape.h"
+#include "debug.h"
+#include <stdlib.h>
+
+struct Shape {
+    Vector* points;
+    size_t points_len;
+
+    Vector mean;
+    int is_convex;
+};
+
+static
+Vector* _shape_get_furthest_in_direction(Shape* self, Vector direction) {
+    // ensure direction is normalized
+    direction = vnormalizedf(direction);
+    Vector* end = self->points + self->points_len;
+    float furthest_dot = vdotf(direction, vsubf(self->points[0], self->mean));
+    Vector* furthest = self->points;
+
+    float dot;
+    for(Vector* point = self->points; point < end; ++point) {
+        dot = vdotf(direction, vsubf(*point, self->mean));
+        if(dot > furthest_dot) {
+            furthest = point;
+        }
+    }
+
+    return furthest;
+}
+
+// go through each point,
+// in order to be convex, none of the points in the shape can be "inset".
+// This means that if one of the points is not the furthest in it's own direction
+// measured from the median.
+static
+int _shape_calculate_is_convex(Shape* self) {
+    Vector* end = self->points + self->points_len;
+    // point relative to mean
+    Vector relative;
+    for(Vector* point = self->points; point < end; ++point) {
+        relative = vsubf(*point, self->mean);
+        if(point != _shape_get_furthest_in_direction(self, relative)) {
+            return 0;
+        }
+    }
+
+    return 1;
+}
+
+static
+Vector _shape_calculate_mean(Shape* self) {
+    Vector* const end = self->points + self->points_len;
+    Vector avg = ZeroVector;
+    size_t count = 0;
+    for(Vector* point = self->points; point < end; ++point) {
+        ++count;
+        avg = vaddf(avg, vmulff(*point, 1.0/count));
+    }
+
+    return avg;
+}
+
+Shape* shape_new(const Vector* points, size_t points_len) {
+    // allocate required space for shape and points array
+    Shape* self = malloc(sizeof(Shape));
+    ASSERT_RETURN(self != NULL, NULL, "Failed to allocate enough space for a shape object.");
+
+    self->points = malloc(points_len * sizeof(Vector));
+    if(self->points == NULL) {
+        free(self);
+        RETURN_ERROR(NULL, "Failed to allocate enough space for %zu points", points_len);
+    }
+
+    // initialize data based on function arguments
+    self->points_len = points_len;
+    memcpy(self->points, points, points_len * sizeof(Vector));
+
+    // derive metadata
+    self->mean = _shape_calculate_mean(self);
+    self->is_convex = _shape_calculate_is_convex(self);
+
+    return self;
+}
+
+Shape* shape_clone(const Shape* source) {
+    Shape* self = malloc(sizeof(Shape));
+    ASSERT_RETURN(self != NULL, NULL, "Failed to allocate space for shape object.");
+
+    self->points = malloc(source->points_len * sizeof(Vector));
+    ASSERT_RETURN(self->points != NULL, NULL, "Failed to allocate space for shape object.");
+
+    // copy data from source
+    self->points_len = source->points_len;
+    memcpy(self->points, source->points, source->points_len * sizeof(Vector));
+    self->mean = source->mean;
+    self->is_convex = source->is_convex;
+
+    return self;
+}
+
+void shape_destroy(Shape* self) {
+    free(self->points);
+    free(self);
+}
+
+Vector* shape_get_start(Shape* self) {
+    return self->points;
+}
+
+Vector* shape_get_end(Shape* self) {
+    return self->points + self->points_len;
+}
+
+size_t shape_get_points_count(const Shape* self) {
+    return self->points_len;
+}
+
+Vector shape_get_point(const Shape* self, size_t at) {
+    ASSERT_RETURN(at < self->points_len, self->mean, "Point index %zu out of bounds for shape", at);
+    return self->points[at];
+}
+
+Vector shape_get_point_transformed(const Shape* self, size_t at, Transform transform) {
+    return transform_point(&transform, shape_get_point(self, at));
+}
+
+void shape_set_point(Shape* self, size_t at, Vector point) {
+    ASSERT_RETURN(at < self->points_len,, "Point index %zu out of bounds for shape", at);
+    self->points[at] = point;
+}
+
+void shape_add_point(Shape* self, Vector point) {
+    Vector* new = realloc(self->points, (self->points_len + 1) * sizeof(Vector));
+    ASSERT_RETURN(new != NULL,, "Failed to allocate space for new point in shape");
+    
+    new[self->points_len] = point;
+    ++self->points_len;
+    self->points = new;
+}
+
+void shape_insert_point(Shape* self, size_t at, Vector point) {
+    ASSERT_RETURN(at < self->points_len + 1,, "Point index %zu out of bounds for shape", at);
+    Vector* new = realloc(self->points, (self->points_len + 1) * sizeof(Vector));
+    ASSERT_RETURN(new != NULL,, "Failed to allocate space to insert new point in shape");
+
+    size_t after_at = at + 1;
+    // both should add one
+    // (len increased and we want to measure the difference starting offset by one from at)
+    // which cancels out
+    size_t dif = self->points_len - after_at;
+    memmove(new + at, new + at + 1, dif);
+    new[at] = point;
+    self->points = new;
+    ++self->points_len;
+}
+
+Vector shape_remove_point(Shape* self, size_t at) {
+    ASSERT_RETURN(at < self->points_len, ZeroVector, "Point index %zu out of bounds for shape", at);
+    Vector point = self->points[at];
+    memmove(self->points + at, self->points + at + 1, (self->points_len - (at + 1)) * sizeof(Vector));
+    Vector* new = realloc(self->points, (self->points_len - 1) * sizeof(Vector));
+    ASSERT_RETURN(new != NULL, point, "Failed to shrink points array to %zu", self->points_len - 1);
+    self->points = new;
+    --self->points_len;
+    return point;
+}
+
+Vector shape_get_median_point(Shape* self) {
+    return self->mean;
+}
+
+int shape_is_convex(Shape* self) {
+    return self->is_convex;
+}
+
+// =====================================================
+// Shape overlap test using the separating axis theorem
+// =====================================================
+
+typedef struct Range {float min; float max; } Range;
+
+static
+Range _shape_get_range_on_axis(Shape* self, Transform self_transform, Vector axis) {
+    Vector point = shape_get_point_transformed(self, 0, self_transform);
+    float dot = vdotf(axis, point);
+    Range range = {dot, dot};
+
+    for(size_t point_index = 1; point_index < self->points_len; ++point_index) {
+        point = shape_get_point_transformed(self, point_index, self_transform);
+        dot = vdotf(axis, point);
+        if(dot < range.min)
+            range.min = fminf(dot, range.min);
+        if(dot > range.max)
+            range.max = fmaxf(dot, range.max);
+    }
+
+    return range;
+}
+
+static
+Vector _shape_overlap_on_axis(Shape* self, Transform self_transform, Shape* other, Transform other_transform, Vector axis) {
+    Range a_range = _shape_get_range_on_axis(self, self_transform, axis);
+    Range b_range = _shape_get_range_on_axis(other, other_transform, axis);
+
+    if(a_range.min <= b_range.max && b_range.min <= b_range.max)
+        return vmulff(axis, fminf(b_range.max - b_range.min, b_range.min - a_range.max));
+    else
+        return ZeroVector;
+}
+
+static
+Vector _shape_get_overlap_internal(Shape* self, Transform self_transform, Shape* other, Transform other_transform) {
+    Vector shortest_escape = InfinityVector;
+    float shortest_sqrmag = INFINITY;
+
+    for(size_t point_index = 0; point_index < self->points_len; ++point_index) {
+        size_t next_index = (point_index + 1) % self->points_len;
+        Vector a = shape_get_point_transformed(self, point_index, self_transform);
+        Vector b = shape_get_point_transformed(self, next_index, other_transform);
+        Vector diff = vsubf(b, a);
+
+        Vector escape = _shape_overlap_on_axis(self, self_transform, other, other_transform, vnormalizedf(diff));
+        float sqr_mag = vsqrmagnitudef(escape);
+        if(sqr_mag < shortest_sqrmag) {
+            shortest_sqrmag = sqr_mag;
+            shortest_escape = escape;
+        }
+    }
+
+    return shortest_escape;
+}
+
+Vector shape_get_overlap(Shape* self, Transform self_transform, Shape* other, Transform other_transform) {
+    Vector self_first = _shape_get_overlap_internal(self, self_transform, other, other_transform);
+    Vector other_first = _shape_get_overlap_internal(other, other_transform, self, self_transform);
+    return vsqrmagnitudef(self_first) < vsqrmagnitudef(other_first) ? self_first : other_first;
+}

src/shape.h

@@ -0,0 +1,30 @@
+#ifndef _fencer_shape_h
+#define _fencer_shape_h
+
+#include "vmath.h"
+#include "transform.h"
+
+typedef struct Shape Shape;
+
+extern Shape* shape_new(const Vector* points, size_t points_len);
+extern Shape* shape_clone(const Shape* source);
+extern void shape_destroy(Shape* self);
+
+extern Vector* shape_get_start(Shape* self);
+extern Vector* shape_get_end(Shape* self);
+
+extern size_t shape_get_points_count(const Shape* self);
+extern Vector shape_get_point(const Shape* self, size_t at);
+extern Vector shape_get_point_transformed(const Shape* self, size_t at, Transform transform);
+
+extern void shape_set_point(Shape* self, size_t at, Vector point);
+extern void shape_add_point(Shape* self, Vector point);
+extern void shape_insert_point(Shape* self, size_t at, Vector point);
+extern Vector shape_remove_point(Shape* self, size_t at);
+
+extern Vector shape_get_median_point(Shape* self);
+extern int shape_is_convex(Shape* self);
+
+extern Vector shape_get_overlap(Shape* self, Transform self_transform, Shape* other, Transform other_transform);
+
+#endif // !_fencer_shape_h