fully implemented slide collision for any combination of aabb and circle colliders

src/corelib/world.c

@@ -1,4 +1,5 @@
 #include "world.h"
+#include "math/vec.h"
 
 object_t g_objects[WORLD_NUM_OBJECTS];
 
@@ -106,6 +107,93 @@ float fclampf(float x, float min_, float max_) {
 	return fminf(max_, fmaxf(min_, x));
 }
 
+static inline
+float _circle_aabb_overlap(const object_t* circle, const object_t* aabb, float* out_px, float* out_py) {
+	// generate a point on the edge of the rectangle that is closest to the circle
+	const float bbminx = aabb->collider.aabb.x + aabb->sprite.x, bbmaxx = bbminx + aabb->collider.aabb.w,
+	            bbminy = aabb->collider.aabb.y + aabb->sprite.y, bbmaxy = bbminy + aabb->collider.aabb.h;
+	// the centre of the circle in world space
+	const float cx = circle->sprite.x + circle->collider.circle.x,
+	            cy = circle->sprite.y + circle->collider.circle.y;
+	// the point on the rectangle closest to the centre of the circle
+	const float x = fclampf(cx, bbminx, bbmaxx),
+	            y = fclampf(cy, bbminy, bbmaxy);
+	// the relative position of the point on the rectangle 
+	const float dif_x = cx - x,
+	            dif_y = cy - y;
+	// absolute difference for use in calculating euclidean distance 
+	const float dist_x = fabsf(dif_x),
+	            dist_y = fabsf(dif_y);
+	// euclidean distance
+	const float dist = sqrt(dist_x*dist_x + dist_y*dist_y);
+	const float solve_distance = circle->collider.circle.radius - dist;
+	// distance to solve collision
+	float solve_x, solve_y;
+	normalize(dif_x, dif_y, &solve_x, &solve_y);
+	*out_px = solve_x * solve_distance;
+	*out_py = solve_y * solve_distance;
+	return solve_distance;
+}
+
+static inline
+float _circle_circle_overlap(const object_t* a, const object_t* b, float* out_px, float* out_py) {
+	const float x1 = a->collider.circle.x + a->sprite.x, y1 = a->collider.circle.y + a->sprite.y;
+	const float x2 = b->collider.circle.x + b->sprite.x, y2 = b->collider.circle.y + b->sprite.y;
+	const float dif_x = x1 - x2, dif_y = y1 - y2;
+	const float difference = sqrtf(fabsf(dif_x*dif_x) + fabsf(dif_y*dif_y));
+	const float target_difference = a->collider.circle.radius + b->collider.circle.radius;
+	float dir_x, dir_y;
+	normalize(dif_x, dif_y, &dir_x, &dir_y);
+	*out_px = dir_x * target_difference;
+	*out_py = dir_y * target_difference;
+	return target_difference;
+}
+
+static inline
+float _aabb_aabb_overlap(const object_t* a, const object_t* b, float* out_px, float* out_py) {
+	float right = (a->collider.aabb.x + a->collider.aabb.w + a->sprite.x) - (b->collider.aabb.x + b->sprite.x);
+	float left = (a->collider.aabb.x + a->sprite.x) - (b->collider.aabb.x + b->collider.aabb.w + b->sprite.x);
+	float top = (a->collider.aabb.y + a->sprite.y) - (b->collider.aabb.y + b->collider.aabb.w + b->sprite.y);
+	float bottom = (a->collider.aabb.y + a->collider.aabb.h) - (b->collider.aabb.y + b->sprite.y);
+
+	float ret = right;
+	*out_px = right;
+	*out_py = 0.f;
+	if(fabsf(left) < fabsf(ret)) {
+		*out_px = left;
+		*out_py = 0.f;
+		ret = left;
+	}
+	if(fabsf(top) < fabsf(ret)) {
+		*out_px = 0.f;
+		*out_py = top;
+		ret = top;
+	}
+
+	if(fabsf(bottom) < fabsf(ret)) {
+		*out_px = 0.f;
+		*out_py = bottom;
+		return bottom;
+	}
+
+	return ret;
+}
+
+static inline
+float _get_overlap(const object_t* a, const object_t* b, float* out_px, float* out_py) {
+	if(a->collider.type == COLLIDERTYPE_AABB && b->collider.type == COLLIDERTYPE_AABB) {
+		return _aabb_aabb_overlap(a, b, out_px, out_py);
+	} else if(a->collider.type == COLLIDERTYPE_AABB && b->collider.type == COLLIDERTYPE_CIRCLE) {
+		float penetration_distance = _circle_aabb_overlap(b, a, out_px, out_py);
+		*out_px = -(*out_px);
+		*out_py = -(*out_py);
+	} else if(a->collider.type == COLLIDERTYPE_CIRCLE && b->collider.type == COLLIDERTYPE_AABB) {
+		return _circle_aabb_overlap(a, b, out_px, out_py);
+	} else if(a->collider.type == COLLIDERTYPE_CIRCLE && b->collider.type == COLLIDERTYPE_CIRCLE) {
+		return _circle_circle_overlap(a, b, out_px, out_py);
+	}
+}
+
 static inline
 short _collision_circle_aabb(const object_t* circle, const object_t* aabb) {
 	// generate a point on the edge of the rectangle that is closest to the circle
@@ -166,9 +254,40 @@ void update_collision() {
 	}
 }
 
-void interpolate_move(object_t* object, float target_x, float target_y, float max_step_size, int slide) {
+static inline
+void _slide_collision(object_t* this, object_t* other, float old_x, float old_y, float new_x, float new_y) {
+	float dx, dy;
+	const float d = _get_overlap(this, other, &dx, &dy);
+	this->sprite.x += dx;
+	this->sprite.y += dy;
+	printf("collision solving step: %f %f, distance: %f\n", dx, dy, d);
+	printf("collision solved, resulting position: %f, %f\n", this->sprite.x, this->sprite.y);
+	
+	return;
+	this->sprite.x = old_x;
+	this->sprite.y = new_y;
+	if(!_collision_check(other, this)) {
+		return;
+	}
+
+	this->sprite.x = new_x;
+	this->sprite.y = old_y;
+	if(!_collision_check(other, this)) {
+		return;
+	}
+
+	this->sprite.x = old_x;
+	this->sprite.y = old_y;
+	if(!_collision_check(other, this)) {
+		return;
+	}
+}
+
+void interpolate_move(object_t* object, const float target_x, const float target_y, const float max_step_size, const int slide) {
 	// calculate step delta
 	float dx = target_x - object->sprite.x, dy = target_y - object->sprite.y;
+	if(dx == 0 && dy == 0)
+		return;
 	// calculate direction x,y
 	float m = sqrtf(dx*dx + dy*dy);
 	if(dx != 0)
@@ -194,10 +313,14 @@ void interpolate_move(object_t* object, float target_x, float target_y, float ma
 		// move towards target, snap to target if distance is too low
 		const float old_x = object->sprite.x, old_y = object->sprite.y;
 		float new_x, new_y;
+
 		const float distx = fabsf(object->sprite.x - target_x), disty = fabsf(object->sprite.y - target_y);
-		if(distx < fabsf(dx) && disty < fabsf(dy)) {
+		const float sqdist = distx*distx + disty*disty;
-			new_x = object->sprite.x += dx;
+		if(sqdist > max_step_size) {
-			new_y = object->sprite.y += dy;
+			object->sprite.x += dx;
+			object->sprite.y += dy;
+			new_x = object->sprite.x;
+			new_y = object->sprite.y;
 		} else {
 			new_x = object->sprite.x = target_x;
 			new_y = object->sprite.y = target_y;
@@ -212,14 +335,7 @@ void interpolate_move(object_t* object, float target_x, float target_y, float ma
 				object_broadcast_collision(other, object);
 				object_broadcast_collision(object, other);
 				if(slide) {
-					object->sprite.x = old_x;
+					_slide_collision(object, other, old_x, old_y, new_x, new_y);
-					if(!_collision_check(other, object)) {
-						object->sprite.x = new_x;
-					}
-					object->sprite.y = old_y;
-					if(!_collision_check(other, object)) {
-						object->sprite.y = new_y;
-					}
 				} else {
 					object->sprite.x = old_x;
 					object->sprite.y = old_y;
@@ -228,7 +344,4 @@ void interpolate_move(object_t* object, float target_x, float target_y, float ma
 			}
 		}
 	}
-
-	// no collision, return nothing
-	return;
 }