math: zig fmt

Signed-off-by: Stephen Gutekanst <stephen@hexops.com>
This commit is contained in:
Stephen Gutekanst 2024-10-08 22:48:45 -07:00
parent 12e69752d3
commit dc5c1f69a6

View file

@ -1,9 +1,9 @@
//! # Collision detection //! # Collision detection
//! //!
//! This module provides functions to check for collision between various 2D shape. //! This module provides functions to check for collision between various 2D shape.
//! It also provides functions to determine the contact points of two objects that have collided. //! It also provides functions to determine the contact points of two objects that have collided.
//! The contact information can be used to resolve the collision. //! The contact information can be used to resolve the collision.
//! //!
const std = @import("std"); const std = @import("std");
const math = @import("main.zig"); const math = @import("main.zig");
const testing = @import("../testing.zig"); const testing = @import("../testing.zig");
@ -11,12 +11,12 @@ const Vec2 = math.Vec2;
const vec2 = math.vec2; const vec2 = math.vec2;
/// An axis aligned rectangle. /// An axis aligned rectangle.
/// ///
/// The boundary of the rectangle is considered inside. /// The boundary of the rectangle is considered inside.
pub const Rectangle = struct { pub const Rectangle = struct {
/// Bottom left of the rectangle. /// Bottom left of the rectangle.
pos: Vec2, pos: Vec2,
/// The size of the rectangle along the x and y axis. /// The size of the rectangle along the x and y axis.
size: Vec2, size: Vec2,
/// Returns true of the two rectangles collide. /// Returns true of the two rectangles collide.
@ -72,27 +72,26 @@ pub const Rectangle = struct {
b.pos = vec2(5.0, 5.0); b.pos = vec2(5.0, 5.0);
const r2 = a.collisionRect(b); const r2 = a.collisionRect(b);
try testing.expect(?Rectangle, r2).eql(null); try testing.expect(?Rectangle, r2).eql(null);
} }
/// Returns vertices for the Rectangle in CCW order. /// Returns vertices for the Rectangle in CCW order.
pub fn vertices(self: Rectangle) [4]Vec2 { pub fn vertices(self: Rectangle) [4]Vec2 {
return [_]Vec2{ return [_]Vec2{
self.pos, self.pos,
self.pos.add(&vec2(self.size.x(), 0.0)), self.pos.add(&vec2(self.size.x(), 0.0)),
self.pos.add(&vec2(self.size.x(), self.size.y())), self.pos.add(&vec2(self.size.x(), self.size.y())),
self.pos.add(&vec2(0.0, self.size.y())), self.pos.add(&vec2(0.0, self.size.y())),
}; };
} }
test vertices { test vertices {
const rect = Rectangle{.pos = vec2(0.0, 0.0), .size = vec2(1.0, 1.0)}; const rect = Rectangle{ .pos = vec2(0.0, 0.0), .size = vec2(1.0, 1.0) };
const v = rect.vertices(); const v = rect.vertices();
try testing.expect(Vec2, vec2(0.0, 0.0)).eql(v[0]); try testing.expect(Vec2, vec2(0.0, 0.0)).eql(v[0]);
try testing.expect(Vec2, vec2(1.0, 0.0)).eql(v[1]); try testing.expect(Vec2, vec2(1.0, 0.0)).eql(v[1]);
try testing.expect(Vec2, vec2(1.0, 1.0)).eql(v[2]); try testing.expect(Vec2, vec2(1.0, 1.0)).eql(v[2]);
try testing.expect(Vec2, vec2(0.0, 1.0)).eql(v[3]); try testing.expect(Vec2, vec2(0.0, 1.0)).eql(v[3]);
} }
}; };
// A circle shape defined by position and radius. // A circle shape defined by position and radius.
@ -169,16 +168,16 @@ pub const Point = struct {
test collidesRect { test collidesRect {
const a: Point = .{ .pos = vec2(6, 4) }; const a: Point = .{ .pos = vec2(6, 4) };
const c = Point{.pos = vec2(6.0, 3.0)}; const c = Point{ .pos = vec2(6.0, 3.0) };
var b: Rectangle = .{ .pos = vec2(6, 3), .size = vec2(3, 2) }; var b: Rectangle = .{ .pos = vec2(6, 3), .size = vec2(3, 2) };
try testing.expect(bool, a.collidesRect(b)).eql(true); try testing.expect(bool, a.collidesRect(b)).eql(true);
try testing.expect(bool, c.collidesRect(b)).eql(true); try testing.expect(bool, c.collidesRect(b)).eql(true);
b.pos = vec2(9.1, 4); b.pos = vec2(9.1, 4);
try testing.expect(bool, a.collidesRect(b)).eql(false); try testing.expect(bool, a.collidesRect(b)).eql(false);
const p = Point{ .pos = vec2(0.0, 0.0) }; const p = Point{ .pos = vec2(0.0, 0.0) };
const r = Rectangle{.pos = vec2(0.0, 0.0), .size = vec2(1.0, 1.0)}; const r = Rectangle{ .pos = vec2(0.0, 0.0), .size = vec2(1.0, 1.0) };
try testing.expect(bool, p.collidesRect(r)).eql(true); try testing.expect(bool, p.collidesRect(r)).eql(true);
} }
@ -199,8 +198,8 @@ pub const Point = struct {
try testing.expect(bool, a.collidesCircle(b)).eql(false); try testing.expect(bool, a.collidesCircle(b)).eql(false);
} }
// Returns true if point is inside polygon. // Returns true if point is inside polygon.
// The boundary of the polygon is outside. // The boundary of the polygon is outside.
// A polygon is specified by a list of the polygon vertices in counter clockwise order. // A polygon is specified by a list of the polygon vertices in counter clockwise order.
pub fn collidesPoly(point: Point, vertices: []const Vec2) bool { pub fn collidesPoly(point: Point, vertices: []const Vec2) bool {
std.debug.assert(vertices.len > 2); std.debug.assert(vertices.len > 2);
@ -225,22 +224,22 @@ pub const Point = struct {
} }
test collidesPoly { test collidesPoly {
const poly = [_]Vec2 { const poly = [_]Vec2{
vec2(-1.0, -1.0), vec2(-1.0, -1.0),
vec2(1.0, -1.0), vec2(1.0, -1.0),
vec2(1.0, 1.0), vec2(1.0, 1.0),
vec2(-1.0, 1.0), vec2(-1.0, 1.0),
}; };
try testing.expect(bool, Point.collidesPoly(Point{.pos = vec2(0.0, 0.0)}, &poly)).eql(true); try testing.expect(bool, Point.collidesPoly(Point{ .pos = vec2(0.0, 0.0) }, &poly)).eql(true);
// TODO: decide if boundary is inside or not // TODO: decide if boundary is inside or not
//try testing.expect(bool, Point.collidesPoly(Point{.pos = vec2(1.0, 1.0)}, &poly)).eql(true); //try testing.expect(bool, Point.collidesPoly(Point{.pos = vec2(1.0, 1.0)}, &poly)).eql(true);
try testing.expect(bool, Point.collidesPoly(Point{.pos = vec2(2.0, 2.0)}, &poly)).eql(false); try testing.expect(bool, Point.collidesPoly(Point{ .pos = vec2(2.0, 2.0) }, &poly)).eql(false);
try testing.expect(bool, Point.collidesPoly(Point{.pos = vec2(-2.0, 2.0)}, &poly)).eql(false); try testing.expect(bool, Point.collidesPoly(Point{ .pos = vec2(-2.0, 2.0) }, &poly)).eql(false);
} }
/// Returns true if point is inside triangle. /// Returns true if point is inside triangle.
/// The boundary of the triangle is outside. /// The boundary of the triangle is outside.
/// A triangle is specified by the triangle vertices in counter clockwise order. /// A triangle is specified by the triangle vertices in counter clockwise order.
pub fn collidesTriangle(point: Point, vertices: []const Vec2) bool { pub fn collidesTriangle(point: Point, vertices: []const Vec2) bool {
std.debug.assert(vertices.len == 3); std.debug.assert(vertices.len == 3);
@ -259,23 +258,23 @@ pub const Point = struct {
return (alpha > 0) and (beta > 0) and (gamma > 0); return (alpha > 0) and (beta > 0) and (gamma > 0);
} }
test collidesTriangle { test collidesTriangle {
const triangle = [_]Vec2 { const triangle = [_]Vec2{
vec2(-1.0, -1.0), vec2(-1.0, -1.0),
vec2(1.0, -1.0), vec2(1.0, -1.0),
vec2(0.0, 1.0), vec2(0.0, 1.0),
}; };
try testing.expect(bool, Point.collidesPoly(Point{.pos = vec2(0.0, 0.0)}, &triangle)).eql(true); try testing.expect(bool, Point.collidesPoly(Point{ .pos = vec2(0.0, 0.0) }, &triangle)).eql(true);
// TODO: decide if boundary is inside or not // TODO: decide if boundary is inside or not
//try testing.expect(bool, Point.collidesPoly(Point{.pos = vec2(0.0, 1.0)}, &triangle)).eql(true); //try testing.expect(bool, Point.collidesPoly(Point{.pos = vec2(0.0, 1.0)}, &triangle)).eql(true);
try testing.expect(bool, Point.collidesPoly(Point{.pos = vec2(2.0, 2.0)}, &triangle)).eql(false); try testing.expect(bool, Point.collidesPoly(Point{ .pos = vec2(2.0, 2.0) }, &triangle)).eql(false);
try testing.expect(bool, Point.collidesPoly(Point{.pos = vec2(-2.0, 2.0)}, &triangle)).eql(false); try testing.expect(bool, Point.collidesPoly(Point{ .pos = vec2(-2.0, 2.0) }, &triangle)).eql(false);
// TODO: decide if boundary is inside or not // TODO: decide if boundary is inside or not
// const t = [_]Vec2 { // const t = [_]Vec2 {
// vec2(0.0, 0.0), // vec2(0.0, 0.0),
// vec2(1.0, 0.0), // vec2(1.0, 0.0),
// vec2(1.0, 1.0), // vec2(1.0, 1.0),
// }; // };
// const p = Point{ .pos = vec2(0.0, 0.0) }; // const p = Point{ .pos = vec2(0.0, 0.0) };
// try testing.expect(bool, p.collidesTriangle(&t)).eql(true); // try testing.expect(bool, p.collidesTriangle(&t)).eql(true);
@ -314,13 +313,12 @@ pub const Point = struct {
.end = vec2(1.0, 1.0), .end = vec2(1.0, 1.0),
.threshold = 0.1, .threshold = 0.1,
}; };
try testing.expect(bool, Point.collidesLine(Point{.pos = vec2(0.0, 0.0)}, l)).eql(true); try testing.expect(bool, Point.collidesLine(Point{ .pos = vec2(0.0, 0.0) }, l)).eql(true);
// TODO: decide if boundary is inside or not // TODO: decide if boundary is inside or not
//try testing.expect(bool, Point.collidesLine(Point{.pos = vec2(0.0, 0.1)}, l)).eql(true); //try testing.expect(bool, Point.collidesLine(Point{.pos = vec2(0.0, 0.1)}, l)).eql(true);
try testing.expect(bool, Point.collidesLine(Point{.pos = vec2(0.0, 0.09)}, l)).eql(true); try testing.expect(bool, Point.collidesLine(Point{ .pos = vec2(0.0, 0.09) }, l)).eql(true);
try testing.expect(bool, Point.collidesLine(Point{.pos = vec2(0.0, 1.0)}, l)).eql(false); try testing.expect(bool, Point.collidesLine(Point{ .pos = vec2(0.0, 1.0) }, l)).eql(false);
} }
}; };
/// A line specified by a start and endpoint and a threshold for the line thickness. /// A line specified by a start and endpoint and a threshold for the line thickness.
@ -329,7 +327,7 @@ pub const Line = struct {
end: Vec2, end: Vec2,
threshold: f32, threshold: f32,
/// Return true if line and b intersect. /// Return true if line and b intersect.
/// This function does not take into account the line treshold. /// This function does not take into account the line treshold.
pub fn collidesLine(a: Line, b: Line) bool { pub fn collidesLine(a: Line, b: Line) bool {
const start_dist = a.start.sub(&b.start); const start_dist = a.start.sub(&b.start);
@ -344,39 +342,32 @@ pub const Line = struct {
} }
test collidesLine { test collidesLine {
const l0 = Line{.start = vec2(-1.0, -1.0), .end = vec2(1.0, 1.0), .threshold = 0.0}; const l0 = Line{ .start = vec2(-1.0, -1.0), .end = vec2(1.0, 1.0), .threshold = 0.0 };
try testing.expect(bool, true).eql( try testing.expect(bool, true).eql(l0.collidesLine(Line{ .start = vec2(-1.0, 1.0), .end = vec2(1.0, -1.0), .threshold = 0.0 }));
l0.collidesLine(Line{.start = vec2(-1.0, 1.0), .end = vec2(1.0, -1.0), .threshold=0.0})); try testing.expect(bool, true).eql(l0.collidesLine(Line{ .start = vec2(-10.0, 0.0), .end = vec2(10.0, 0.0), .threshold = 0.0 }));
try testing.expect(bool, true).eql( try testing.expect(bool, true).eql(l0.collidesLine(Line{ .start = vec2(-10.0, 1.0), .end = vec2(10.0, 1.0), .threshold = 0.0 }));
l0.collidesLine(Line{.start = vec2(-10.0, 0.0), .end = vec2(10.0, 0.0), .threshold=0.0})); try testing.expect(bool, true).eql(l0.collidesLine(Line{ .start = vec2(-10.0, -1.0), .end = vec2(10.0, -1.0), .threshold = 0.0 }));
try testing.expect(bool, true).eql(
l0.collidesLine(Line{.start = vec2(-10.0, 1.0), .end = vec2(10.0, 1.0), .threshold=0.0}));
try testing.expect(bool, true).eql(
l0.collidesLine(Line{.start = vec2(-10.0, -1.0), .end = vec2(10.0, -1.0), .threshold=0.0}));
// TODO: fails if same line // TODO: fails if same line
//try testing.expect(bool, true).eql( //try testing.expect(bool, true).eql(
// l0.collidesLine(l0)); // l0.collidesLine(l0));
try testing.expect(bool, false).eql( try testing.expect(bool, false).eql(l0.collidesLine(Line{ .start = vec2(-1.1, -1.1), .end = vec2(1.1, 1.1), .threshold = 0.0 }));
l0.collidesLine(Line{.start = vec2(-1.1, -1.1), .end = vec2(1.1, 1.1), .threshold=0.0})); try testing.expect(bool, false).eql(l0.collidesLine(Line{ .start = vec2(-10.0, 2.0), .end = vec2(10.0, 2.0), .threshold = 0.0 }));
try testing.expect(bool, false).eql(
l0.collidesLine(Line{.start = vec2(-10.0, 2.0), .end = vec2(10.0, 2.0), .threshold=0.0}));
} }
}; };
/// Contains the contact information between two convex 2D shapes. /// Contains the contact information between two convex 2D shapes.
/// There can be up to two contacts point in case the objects collide /// There can be up to two contacts point in case the objects collide
/// on a paralell line. /// on a paralell line.
/// ///
/// The normal points from A to B, so the objects can be separated by moving /// The normal points from A to B, so the objects can be separated by moving
/// B by the vector depth x normal /// B by the vector depth x normal
/// ///
pub const Contact = struct { pub const Contact = struct {
/// The contact normal from A to B /// The contact normal from A to B
normal: Vec2, normal: Vec2,
/// Depth of the peneration. /// Depth of the peneration.
depth: f32, depth: f32,
/// Contact point 1 on obj A /// Contact point 1 on obj A
cp1: ?Vec2 = null, cp1: ?Vec2 = null,
@ -384,7 +375,7 @@ pub const Contact = struct {
cp2: ?Vec2 = null, cp2: ?Vec2 = null,
}; };
/// Compute the minimum and maximum projection of vertices in v on the line through v0 with normal n /// Compute the minimum and maximum projection of vertices in v on the line through v0 with normal n
pub fn minmaxProjectionDistance(n: Vec2, v0: Vec2, v: []const Vec2) [2]f32 { pub fn minmaxProjectionDistance(n: Vec2, v0: Vec2, v: []const Vec2) [2]f32 {
var max_d = n.dot(&v[0].sub(&v0)); var max_d = n.dot(&v[0].sub(&v0));
var min_d = n.dot(&v[0].sub(&v0)); var min_d = n.dot(&v[0].sub(&v0));
@ -396,20 +387,14 @@ pub fn minmaxProjectionDistance(n: Vec2, v0: Vec2, v: []const Vec2) [2]f32 {
max_d = d; max_d = d;
} }
} }
return [2]f32{min_d, max_d}; return [2]f32{ min_d, max_d };
} }
test minmaxProjectionDistance { test minmaxProjectionDistance {
const n_up = vec2(0.0, 1.0); const n_up = vec2(0.0, 1.0);
const n_right = vec2(1.0, 0.0); const n_right = vec2(1.0, 0.0);
const v0 = vec2(0.0, 0.0); const v0 = vec2(0.0, 0.0);
const v = [_]Vec2{ const v = [_]Vec2{ vec2(2.0, 0.0), vec2(1.0, 1.0), vec2(0.0, -2.0), vec2(-1.0, 2.0), vec2(-2.0, 1.0) };
vec2(2.0, 0.0),
vec2(1.0, 1.0),
vec2(0.0, -2.0),
vec2(-1.0, 2.0),
vec2(-2.0, 1.0)
};
{ {
const minmax = minmaxProjectionDistance(n_up, v0, &v); const minmax = minmaxProjectionDistance(n_up, v0, &v);
@ -422,22 +407,18 @@ test minmaxProjectionDistance {
try testing.expect(f32, -2.0).eql(minmax[0]); try testing.expect(f32, -2.0).eql(minmax[0]);
try testing.expect(f32, 2.0).eql(minmax[1]); try testing.expect(f32, 2.0).eql(minmax[1]);
} }
} }
const VertexDepthResult = struct { const VertexDepthResult = struct {
v0: Vec2 = undefined, v0: Vec2 = undefined,
v1: ?Vec2 = null, v1: ?Vec2 = null,
/// Depth of vertex. Positive in the opposite direction of the normal. /// Depth of vertex. Positive in the opposite direction of the normal.
d: f32 d: f32,
}; };
/// Find the vertex in v that is deepest behind the line defined by the point v0 and normal n. /// Find the vertex in v that is deepest behind the line defined by the point v0 and normal n.
pub fn findDeepestVertex(n: Vec2, v0: Vec2, v: []const Vec2) VertexDepthResult { pub fn findDeepestVertex(n: Vec2, v0: Vec2, v: []const Vec2) VertexDepthResult {
var min_depth = VertexDepthResult{ var min_depth = VertexDepthResult{ .v0 = v[0], .d = n.dot(&v[0].sub(&v0)) };
.v0 = v[0],
.d = n.dot(&v[0].sub(&v0))
};
for (v[1..]) |vb| { for (v[1..]) |vb| {
const d = n.dot(&vb.sub(&v0)); const d = n.dot(&vb.sub(&v0));
if (d < min_depth.d) { if (d < min_depth.d) {
@ -459,33 +440,33 @@ test findDeepestVertex {
// Test finding a single deepest vertex // Test finding a single deepest vertex
{ {
const v = [_]Vec2{ const v = [_]Vec2{
vec2(2.0, 0.0), vec2(2.0, 0.0),
vec2(1.0, 1.0), vec2(1.0, 1.0),
vec2(0.0, -2.0), // Deepest vec2(0.0, -2.0), // Deepest
vec2(-1.0, 2.0), vec2(-1.0, 2.0),
vec2(-2.0, 1.0) vec2(-2.0, 1.0),
}; };
const depth = findDeepestVertex(n_up, v0, &v); const depth = findDeepestVertex(n_up, v0, &v);
try testing.expect(Vec2, vec2(0.0, -2.0)).eql(depth.v0); try testing.expect(Vec2, vec2(0.0, -2.0)).eql(depth.v0);
try testing.expect(?Vec2, null).eql(depth.v1); try testing.expect(?Vec2, null).eql(depth.v1);
try testing.expect(f32, 2.0).eql(depth.d); try testing.expect(f32, 2.0).eql(depth.d);
} }
// Test finding two deepest vertices // Test finding two deepest vertices
{ {
const v = [_]Vec2{ const v = [_]Vec2{
vec2(2.0, 0.0), vec2(2.0, 0.0),
vec2(1.0, 1.0), vec2(1.0, 1.0),
vec2(0.0, -3.0), // Deepest vec2(0.0, -3.0), // Deepest
vec2(-1.0, -3.0), // Deepest vec2(-1.0, -3.0), // Deepest
vec2(-2.0, 1.0) vec2(-2.0, 1.0),
}; };
const depth = findDeepestVertex(n_up, v0, &v); const depth = findDeepestVertex(n_up, v0, &v);
try testing.expect(Vec2, vec2(0.0, -3.0)).eql(depth.v0); try testing.expect(Vec2, vec2(0.0, -3.0)).eql(depth.v0);
try testing.expect(Vec2, vec2(-1.0, -3.0)).eql(depth.v1.?); try testing.expect(Vec2, vec2(-1.0, -3.0)).eql(depth.v1.?);
try testing.expect(f32, 3.0).eql(depth.d); try testing.expect(f32, 3.0).eql(depth.d);
} }
// No vertex behind edge - will return closest vertex instead // No vertex behind edge - will return closest vertex instead
{ {
@ -493,38 +474,37 @@ test findDeepestVertex {
vec2(2.0, 0.5), // Closest vec2(2.0, 0.5), // Closest
vec2(1.0, 1.0), vec2(1.0, 1.0),
vec2(0.0, 3.0), vec2(0.0, 3.0),
vec2(-2.0, 1.0) vec2(-2.0, 1.0),
}; };
const depth = findDeepestVertex(n_up, v0, &v); const depth = findDeepestVertex(n_up, v0, &v);
try testing.expect(Vec2, vec2(2.0, 0.5)).eql(depth.v0); try testing.expect(Vec2, vec2(2.0, 0.5)).eql(depth.v0);
try testing.expect(?Vec2, null).eql(depth.v1); try testing.expect(?Vec2, null).eql(depth.v1);
try testing.expect(f32, -0.5).eql(depth.d); try testing.expect(f32, -0.5).eql(depth.d);
} }
} }
/// Contains information to separate two colliding shapes. /// Contains information to separate two colliding shapes.
const SeparationResult = struct { const SeparationResult = struct {
// i0: usize = undefined, // Vertex idx // i0: usize = undefined, // Vertex idx
// i1: ?usize = null, // i1: ?usize = null,
v0: Vec2 = undefined, v0: Vec2 = undefined,
v1: ?Vec2 = null, v1: ?Vec2 = null,
e: usize = undefined, // Edge idx e: usize = undefined, // Edge idx
n: Vec2 = undefined, // Edge normal n: Vec2 = undefined, // Edge normal
d: f32 = std.math.floatMax(f32), // Depth d: f32 = std.math.floatMax(f32), // Depth
}; };
/// Find the edge and vertices for the minimum separation required to /// Find the edge and vertices for the minimum separation required to
/// separate vertices in polygon_a from edge in polygon_b. /// separate vertices in polygon_a from edge in polygon_b.
/// Returns null if a separting axis is found. /// Returns null if a separting axis is found.
pub fn findMinSeparation(polygon_a: []const Vec2, polygon_b: []const Vec2) ?SeparationResult { pub fn findMinSeparation(polygon_a: []const Vec2, polygon_b: []const Vec2) ?SeparationResult {
var min_result = SeparationResult{}; var min_result = SeparationResult{};
var v0 = polygon_b[polygon_b.len-1]; var v0 = polygon_b[polygon_b.len - 1];
for (polygon_b[0..], 0..) |v1, i| { for (polygon_b[0..], 0..) |v1, i| {
const edge = v1.sub(&v0); const edge = v1.sub(&v0);
const n = vec2(edge.y(), -edge.x()).normalize(0.0); const n = vec2(edge.y(), -edge.x()).normalize(0.0);
const min_depth = findDeepestVertex(n, v0, polygon_a); const min_depth = findDeepestVertex(n, v0, polygon_a);
v0 = v1; v0 = v1;
if (min_depth.d < 0.0) { if (min_depth.d < 0.0) {
@ -547,19 +527,19 @@ pub fn findMinSeparation(polygon_a: []const Vec2, polygon_b: []const Vec2) ?Sepa
test findMinSeparation { test findMinSeparation {
const triangle_0 = [_]Vec2{ const triangle_0 = [_]Vec2{
vec2(-1.0, -1.0), vec2(-1.0, -1.0),
vec2(1.0, -1.0), vec2(1.0, -1.0),
vec2(0.0, 1.0), vec2(0.0, 1.0),
}; };
const triangle_1 = [_]Vec2{ const triangle_1 = [_]Vec2{
vec2(-1.0, -2.75), vec2(-1.0, -2.75),
vec2(1.0, -2.75), vec2(1.0, -2.75),
vec2(0.0, -0.75), vec2(0.0, -0.75),
}; };
const triangle_2 = [_]Vec2{ const triangle_2 = [_]Vec2{
vec2(-1.0, -2.75), vec2(-1.0, -2.75),
vec2(1.0, -2.75), vec2(1.0, -2.75),
vec2(0.0, -1.75), vec2(0.0, -1.75),
}; };
// Top point of triangle_1 intersects with first edge of triangle_0 at depth 0.25 // Top point of triangle_1 intersects with first edge of triangle_0 at depth 0.25
@ -571,10 +551,7 @@ test findMinSeparation {
try testing.expect(f32, 0.25).eql(result.?.d); try testing.expect(f32, 0.25).eql(result.?.d);
// Not colliding - bottom edge of triangle_0 separates the two // Not colliding - bottom edge of triangle_0 separates the two
try testing.expect(?SeparationResult, null).eql( try testing.expect(?SeparationResult, null).eql(findMinSeparation(&triangle_2, &triangle_0));
findMinSeparation(&triangle_2, &triangle_0)
);
} }
/// Compute a Contact report between polygon_a and polygon_b if they are colliding. /// Compute a Contact report between polygon_a and polygon_b if they are colliding.
@ -590,21 +567,21 @@ pub fn polygonPolygonContact(polygon_a: []const Vec2, polygon_b: []const Vec2) ?
if (min_separation_a.d < min_separation_b.d) { if (min_separation_a.d < min_separation_b.d) {
// Vertex in a passes an edge in b // Vertex in a passes an edge in b
depth = min_separation_a.d; depth = min_separation_a.d;
normal = min_separation_a.n.mulScalar(-1.0); normal = min_separation_a.n.mulScalar(-1.0);
cp1_a = min_separation_a.v0; cp1_a = min_separation_a.v0;
cp2_a = min_separation_a.v1; cp2_a = min_separation_a.v1;
} else if (min_separation_a.d > min_separation_b.d) { } else if (min_separation_a.d > min_separation_b.d) {
// Vertex in b passes an edge in a // Vertex in b passes an edge in a
depth = min_separation_b.d; depth = min_separation_b.d;
normal = min_separation_b.n; normal = min_separation_b.n;
cp1_a = min_separation_b.v0.add(&normal.mulScalar(depth)); cp1_a = min_separation_b.v0.add(&normal.mulScalar(depth));
if (min_separation_b.v1) |v1| { if (min_separation_b.v1) |v1| {
cp2_a = v1.add(&normal.mulScalar(depth)); cp2_a = v1.add(&normal.mulScalar(depth));
} }
} else { } else {
// Two edges // Two edges
depth = min_separation_a.d; depth = min_separation_a.d;
normal = min_separation_a.n.mulScalar(-1.0); normal = min_separation_a.n.mulScalar(-1.0);
if (@abs(normal.dot(&min_separation_b.n)) != 1.0 or min_separation_a.v1 == null or min_separation_b.v1 == null) { if (@abs(normal.dot(&min_separation_b.n)) != 1.0 or min_separation_a.v1 == null or min_separation_b.v1 == null) {
// Edges are not paralell // Edges are not paralell
cp1_a = min_separation_b.v0; cp1_a = min_separation_b.v0;
@ -612,20 +589,16 @@ pub fn polygonPolygonContact(polygon_a: []const Vec2, polygon_b: []const Vec2) ?
} else { } else {
// Paralell edges - find two contact points // Paralell edges - find two contact points
const edge = vec2(min_separation_a.n.y(), -min_separation_a.n.x()); const edge = vec2(min_separation_a.n.y(), -min_separation_a.n.x());
const vertices = [_]Vec2{ const vertices = [_]Vec2{ min_separation_a.v0, min_separation_a.v1.?, min_separation_b.v0, min_separation_b.v1.? };
min_separation_a.v0, const from_a = [4]bool{ true, true, false, false };
min_separation_a.v1.?,
min_separation_b.v0,
min_separation_b.v1.?};
const from_a = [4]bool {true, true, false, false};
var distances: [4]f32 = undefined; var distances: [4]f32 = undefined;
for (vertices, &distances) |v, *d| { for (vertices, &distances) |v, *d| {
d.* = edge.dot(&v); d.* = edge.dot(&v);
} }
// Sort vertices along the edge // Sort vertices along the edge
var idx = [_]u8{0,1,2,3}; var idx = [_]u8{ 0, 1, 2, 3 };
for (0..3) |i| { for (0..3) |i| {
for (i+1..4) |j| { for (i + 1..4) |j| {
if (distances[idx[i]] > distances[idx[j]]) { if (distances[idx[i]] > distances[idx[j]]) {
const t = idx[i]; const t = idx[i];
idx[i] = idx[j]; idx[i] = idx[j];
@ -635,11 +608,9 @@ pub fn polygonPolygonContact(polygon_a: []const Vec2, polygon_b: []const Vec2) ?
} }
depth = min_separation_a.d; depth = min_separation_a.d;
normal = min_separation_a.n.mulScalar(-1.0); normal = min_separation_a.n.mulScalar(-1.0);
cp1_a = if (from_a[idx[1]]) vertices[idx[1]] cp1_a = if (from_a[idx[1]]) vertices[idx[1]] else vertices[idx[1]].add(&normal.mulScalar(depth));
else vertices[idx[1]].add(&normal.mulScalar(depth)); cp2_a = if (from_a[idx[2]]) vertices[idx[2]] else vertices[idx[2]].add(&normal.mulScalar(depth));
cp2_a = if (from_a[idx[2]]) vertices[idx[2]]
else vertices[idx[2]].add(&normal.mulScalar(depth));
} }
} }
@ -653,19 +624,19 @@ pub fn polygonPolygonContact(polygon_a: []const Vec2, polygon_b: []const Vec2) ?
test polygonPolygonContact { test polygonPolygonContact {
const triangle_0 = [_]Vec2{ const triangle_0 = [_]Vec2{
vec2(-1.0, -1.0), vec2(-1.0, -1.0),
vec2(1.0, -1.0), vec2(1.0, -1.0),
vec2(0.0, 1.0), vec2(0.0, 1.0),
}; };
const triangle_1 = [_]Vec2{ const triangle_1 = [_]Vec2{
vec2(-1.0, -2.75), vec2(-1.0, -2.75),
vec2(1.0, -2.75), vec2(1.0, -2.75),
vec2(0.0, -0.75), vec2(0.0, -0.75),
}; };
const triangle_2 = [_]Vec2{ const triangle_2 = [_]Vec2{
vec2(-1.0, -2.75), vec2(-1.0, -2.75),
vec2(1.0, -2.75), vec2(1.0, -2.75),
vec2(0.0, -1.75), vec2(0.0, -1.75),
}; };
if (polygonPolygonContact(&triangle_0, &triangle_1)) |contact_0_1| { if (polygonPolygonContact(&triangle_0, &triangle_1)) |contact_0_1| {
@ -689,10 +660,10 @@ test polygonPolygonContact {
try testing.expect(?Contact, null).eql(polygonPolygonContact(&triangle_0, &triangle_2)); try testing.expect(?Contact, null).eql(polygonPolygonContact(&triangle_0, &triangle_2));
try testing.expect(?Contact, null).eql(polygonPolygonContact(&triangle_2, &triangle_0)); try testing.expect(?Contact, null).eql(polygonPolygonContact(&triangle_2, &triangle_0));
const rect1 = Rectangle{.pos = vec2(-1.0, -1.0), .size = vec2(2.0, 2.0)}; const rect1 = Rectangle{ .pos = vec2(-1.0, -1.0), .size = vec2(2.0, 2.0) };
const rect2 = Rectangle{.pos = vec2(-1.5, -2.25), .size = vec2(1.0, 1.5)}; const rect2 = Rectangle{ .pos = vec2(-1.5, -2.25), .size = vec2(1.0, 1.5) };
const rect3 = Rectangle{.pos = vec2(-0.5, -2.25), .size = vec2(1.0, 1.5)}; const rect3 = Rectangle{ .pos = vec2(-0.5, -2.25), .size = vec2(1.0, 1.5) };
const rect4 = Rectangle{.pos = vec2( 0.5, -2.25), .size = vec2(1.0, 1.5)}; const rect4 = Rectangle{ .pos = vec2(0.5, -2.25), .size = vec2(1.0, 1.5) };
const r1 = rect1.vertices(); const r1 = rect1.vertices();
const r2 = rect2.vertices(); const r2 = rect2.vertices();
const r3 = rect3.vertices(); const r3 = rect3.vertices();
@ -729,16 +700,8 @@ pub fn circlePolygonContact(circle_a: Circle, polygon_b: []const Vec2) ?Contact
var depth: f32 = 0.0; var depth: f32 = 0.0;
var cp1_a: ?Vec2 = null; var cp1_a: ?Vec2 = null;
var v0 = polygon_b[polygon_b.len-1]; var v0 = polygon_b[polygon_b.len - 1];
var min_result = struct { var min_result = struct { n: Vec2, d: f32, i: usize }{ .n = vec2(0.0, 0.0), .d = std.math.floatMax(f32), .i = undefined };
n: Vec2,
d: f32,
i: usize
}{
.n = vec2(0.0, 0.0),
.d = std.math.floatMax(f32),
.i = undefined
};
var closest_vertex = struct { var closest_vertex = struct {
v: Vec2 = undefined, v: Vec2 = undefined,
d: f32 = std.math.floatMax(f32), d: f32 = std.math.floatMax(f32),
@ -747,7 +710,7 @@ pub fn circlePolygonContact(circle_a: Circle, polygon_b: []const Vec2) ?Contact
for (polygon_b[0..], 0..) |v1, i| { for (polygon_b[0..], 0..) |v1, i| {
const edge = v1.sub(&v0); const edge = v1.sub(&v0);
const n = vec2(edge.y(), -edge.x()).normalize(0.0); const n = vec2(edge.y(), -edge.x()).normalize(0.0);
const vc = circle_a.pos.sub(&v0); const vc = circle_a.pos.sub(&v0);
const d = vc.dot(&n); const d = vc.dot(&n);
if (vc.len() < closest_vertex.d) { if (vc.len() < closest_vertex.d) {
@ -773,13 +736,13 @@ pub fn circlePolygonContact(circle_a: Circle, polygon_b: []const Vec2) ?Contact
// Check circle to closest point axis // Check circle to closest point axis
{ {
v0 = closest_vertex.v; v0 = closest_vertex.v;
const n = circle_a.pos.sub(&v0).normalize(0.0); const n = circle_a.pos.sub(&v0).normalize(0.0);
const minmax_b = minmaxProjectionDistance(n, v0, polygon_b); const minmax_b = minmaxProjectionDistance(n, v0, polygon_b);
// Circle projects to +- radius // Circle projects to +- radius
const vc = circle_a.pos.sub(&v0); const vc = circle_a.pos.sub(&v0);
const d = vc.dot(&n); const d = vc.dot(&n);
const minmax_a = [2]f32{d - circle_a.radius, d + circle_a.radius}; const minmax_a = [2]f32{ d - circle_a.radius, d + circle_a.radius };
if ((minmax_a[0] > minmax_b[1]) or (minmax_a[1] < minmax_b[0])) { if ((minmax_a[0] > minmax_b[1]) or (minmax_a[1] < minmax_b[0])) {
// Circle does not intersect // Circle does not intersect
@ -794,8 +757,8 @@ pub fn circlePolygonContact(circle_a: Circle, polygon_b: []const Vec2) ?Contact
} }
depth = -min_result.d; depth = -min_result.d;
normal = min_result.n.mulScalar(-1.0); normal = min_result.n.mulScalar(-1.0);
cp1_a = circle_a.pos.add(&normal.mulScalar(circle_a.radius)); cp1_a = circle_a.pos.add(&normal.mulScalar(circle_a.radius));
return Contact{ return Contact{
.normal = normal, .normal = normal,
@ -805,9 +768,9 @@ pub fn circlePolygonContact(circle_a: Circle, polygon_b: []const Vec2) ?Contact
} }
test circlePolygonContact { test circlePolygonContact {
const rect1 = Rectangle{.pos = vec2(0.75, -1.0), .size = vec2(2.0, 2.0)}; const rect1 = Rectangle{ .pos = vec2(0.75, -1.0), .size = vec2(2.0, 2.0) };
const r1 = rect1.vertices(); const r1 = rect1.vertices();
if (circlePolygonContact(Circle{.pos=vec2(0.0, 0.0), .radius = 1.0}, &r1)) |contact| { if (circlePolygonContact(Circle{ .pos = vec2(0.0, 0.0), .radius = 1.0 }, &r1)) |contact| {
try testing.expect(Vec2, vec2(1.0, 0.0)).eql(contact.normal); try testing.expect(Vec2, vec2(1.0, 0.0)).eql(contact.normal);
try testing.expect(f32, 0.25).eql(contact.depth); try testing.expect(f32, 0.25).eql(contact.depth);
try testing.expect(Vec2, vec2(1.0, 0.0)).eql(contact.cp1.?); try testing.expect(Vec2, vec2(1.0, 0.0)).eql(contact.cp1.?);
@ -816,20 +779,18 @@ test circlePolygonContact {
try testing.expect(bool, true).eql(false); try testing.expect(bool, true).eql(false);
} }
try testing.expect(?Contact, null).eql( try testing.expect(?Contact, null).eql(circlePolygonContact(Circle{ .pos = vec2(-1.0, 0.0), .radius = 1.0 }, &r1));
circlePolygonContact(Circle{.pos=vec2(-1.0, 0.0), .radius = 1.0}, &r1)
);
} }
/// Compute a Contact report between two circles. /// Compute a Contact report between two circles.
pub fn circleCircleContact(circle_a: Circle, circle_b: Circle) ?Contact { pub fn circleCircleContact(circle_a: Circle, circle_b: Circle) ?Contact {
const delta = circle_b.pos.sub(&circle_a.pos); const delta = circle_b.pos.sub(&circle_a.pos);
const distance = delta.len(); const distance = delta.len();
const depth = circle_a.radius + circle_b.radius - distance; const depth = circle_a.radius + circle_b.radius - distance;
if (depth < 0.0) { if (depth < 0.0) {
return null; return null;
} }
// if distance is zero then all separation directions are equivalent. Pick arbitrary one. // if distance is zero then all separation directions are equivalent. Pick arbitrary one.
const normal = if (distance > 0.0) delta.mulScalar(1.0 / distance) else vec2(1.0, 0.0); const normal = if (distance > 0.0) delta.mulScalar(1.0 / distance) else vec2(1.0, 0.0);
const cp1_a = circle_a.pos.add(&normal.mulScalar(circle_a.radius)); const cp1_a = circle_a.pos.add(&normal.mulScalar(circle_a.radius));
@ -841,9 +802,7 @@ pub fn circleCircleContact(circle_a: Circle, circle_b: Circle) ?Contact {
} }
test circleCircleContact { test circleCircleContact {
if (circleCircleContact(Circle{.pos=vec2(0.0, 0.0), .radius = 1.0}, if (circleCircleContact(Circle{ .pos = vec2(0.0, 0.0), .radius = 1.0 }, Circle{ .pos = vec2(1.75, 0.0), .radius = 1.0 })) |contact| {
Circle{.pos = vec2(1.75, 0.0), .radius = 1.0})) |contact| {
try testing.expect(Vec2, vec2(1.0, 0.0)).eql(contact.normal); try testing.expect(Vec2, vec2(1.0, 0.0)).eql(contact.normal);
try testing.expect(f32, 0.25).eql(contact.depth); try testing.expect(f32, 0.25).eql(contact.depth);
try testing.expect(Vec2, vec2(1.0, 0.0)).eql(contact.cp1.?); try testing.expect(Vec2, vec2(1.0, 0.0)).eql(contact.cp1.?);
@ -852,9 +811,7 @@ test circleCircleContact {
try testing.expect(bool, true).eql(false); try testing.expect(bool, true).eql(false);
} }
if (circleCircleContact(Circle{.pos=vec2(0.0, 0.0), .radius = 1.0}, if (circleCircleContact(Circle{ .pos = vec2(0.0, 0.0), .radius = 1.0 }, Circle{ .pos = vec2(2.0, 0.0), .radius = 1.0 })) |contact| {
Circle{.pos = vec2(2.0, 0.0), .radius = 1.0})) |contact| {
try testing.expect(Vec2, vec2(1.0, 0.0)).eql(contact.normal); try testing.expect(Vec2, vec2(1.0, 0.0)).eql(contact.normal);
try testing.expect(f32, 0.0).eql(contact.depth); try testing.expect(f32, 0.0).eql(contact.depth);
try testing.expect(Vec2, vec2(1.0, 0.0)).eql(contact.cp1.?); try testing.expect(Vec2, vec2(1.0, 0.0)).eql(contact.cp1.?);
@ -863,8 +820,5 @@ test circleCircleContact {
try testing.expect(bool, true).eql(false); try testing.expect(bool, true).eql(false);
} }
try testing.expect(?Contact, null).eql( try testing.expect(?Contact, null).eql(circleCircleContact(Circle{ .pos = vec2(0.0, 0.0), .radius = 1.0 }, Circle{ .pos = vec2(2.01, 0.0), .radius = 1.0 }));
circleCircleContact(Circle{.pos=vec2(0.0, 0.0), .radius = 1.0}, }
Circle{.pos = vec2(2.01, 0.0), .radius = 1.0})
);
}