diff --git a/src/shape.c b/src/shape.c new file mode 100644 index 0000000..c6cafbf --- /dev/null +++ b/src/shape.c @@ -0,0 +1,238 @@ +#include "shape.h" +#include "debug.h" +#include + +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; +} diff --git a/src/shape.h b/src/shape.h new file mode 100644 index 0000000..8844f4e --- /dev/null +++ b/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