refactors SAT to be less wet, adds spline and spline collision using SAT
This commit is contained in:
212
collision/sat.ts
212
collision/sat.ts
@@ -1,113 +1,125 @@
|
||||
import { Polygon } from "../geometry/polygon.ts";
|
||||
import { Point, Vector } from "../geometry/vector.ts";
|
||||
import { SplineSegment } from "../geometry/spline.ts";
|
||||
import { Vector } from "../geometry/vector.ts";
|
||||
import { CircleLike } from "./circular.ts";
|
||||
|
||||
export const satCollision = (s1: Polygon, s2: Polygon) => {
|
||||
const shape1 = s1.points.map((p) => new Vector(p).add(s1.center));
|
||||
const shape2 = s2.points.map((p) => new Vector(p).add(s2.center));
|
||||
export function satCollisionSpline(p: Polygon, spline: SplineSegment): boolean {
|
||||
const numSegments = 100; // You can adjust the number of segments based on your needs
|
||||
|
||||
if (shape1.length < 2 || shape2.length < 2) {
|
||||
throw "Insufficient shape data in satCollision";
|
||||
}
|
||||
for (let i = 0; i < shape1.length; i++) {
|
||||
const axis = shape1[i].normal(shape1.at(i - 1)!);
|
||||
let [_, p1minDot] = Vector.vectorProjectionAndDot(shape1[0], axis);
|
||||
let p1maxDot = p1minDot;
|
||||
for (const point of shape1) {
|
||||
const [_, dot] = Vector.vectorProjectionAndDot(point, axis);
|
||||
p1minDot = Math.min(dot, p1minDot);
|
||||
p1maxDot = Math.max(dot, p1maxDot);
|
||||
}
|
||||
let [__, p2minDot] = Vector.vectorProjectionAndDot(shape2[0], axis);
|
||||
let p2maxDot = p2minDot;
|
||||
for (const point of shape2) {
|
||||
const [_, dot] = Vector.vectorProjectionAndDot(point, axis);
|
||||
p2minDot = Math.min(dot, p2minDot);
|
||||
p2maxDot = Math.max(dot, p2maxDot);
|
||||
}
|
||||
for (let i = 0; i < numSegments; i++) {
|
||||
const t1 = i / numSegments;
|
||||
const t2 = (i + 1) / numSegments;
|
||||
|
||||
if (p1minDot - p2maxDot > 0 || p2minDot - p1maxDot > 0) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
for (let i = 0; i < shape2.length; i++) {
|
||||
const axis = shape2[i].normal(shape2.at(i - 1)!);
|
||||
let [_, p1minDot] = Vector.vectorProjectionAndDot(shape2[0], axis);
|
||||
let p1maxDot = p1minDot;
|
||||
for (const point of shape2) {
|
||||
const [_, dot] = Vector.vectorProjectionAndDot(point, axis);
|
||||
// p1min = dot < p1minDot ? projected : p1min;
|
||||
p1minDot = Math.min(dot, p1minDot);
|
||||
// p1max = dot > p1maxDot ? projected : p1max;
|
||||
p1maxDot = Math.max(dot, p1maxDot);
|
||||
}
|
||||
let [__, p2minDot] = Vector.vectorProjectionAndDot(shape1[0], axis);
|
||||
let p2maxDot = p2minDot;
|
||||
for (const point of shape1) {
|
||||
const [_, dot] = Vector.vectorProjectionAndDot(point, axis);
|
||||
// p2min = dot < p2minDot ? projected : p2min;
|
||||
p2minDot = Math.min(dot, p2minDot);
|
||||
// p2max = dot > p2maxDot ? projected : p2max;
|
||||
p2maxDot = Math.max(dot, p2maxDot);
|
||||
}
|
||||
const segmentStart = spline.getPointAtT(t1);
|
||||
const segmentEnd = spline.getPointAtT(t2);
|
||||
|
||||
if (p1minDot - p2maxDot > 0 || p2minDot - p1maxDot > 0) {
|
||||
return false;
|
||||
if (segmentIntersectsPolygon(p, segmentStart, segmentEnd)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
export function satCollisionPolygon(poly: Polygon, poly2: Polygon): boolean {
|
||||
for (const edge of poly.getEdges()) {
|
||||
const axis = edge.copy().normal().normalize();
|
||||
const proj1 = projectPolygonOntoAxis(poly, axis);
|
||||
const proj2 = projectPolygonOntoAxis(poly2, axis);
|
||||
|
||||
if (!overlap(proj1, proj2)) return false;
|
||||
}
|
||||
for (const edge of poly2.getEdges()) {
|
||||
const axis = edge.copy().normal().normalize();
|
||||
const proj1 = projectPolygonOntoAxis(poly, axis);
|
||||
const proj2 = projectPolygonOntoAxis(poly2, axis);
|
||||
|
||||
if (!overlap(proj1, proj2)) return false;
|
||||
}
|
||||
return true;
|
||||
};
|
||||
}
|
||||
export function satCollisionCircle(p: Polygon, circle: CircleLike): boolean {
|
||||
for (const edge of p.getEdges()) {
|
||||
const axis = edge.copy().normal().normalize();
|
||||
const proj1 = projectPolygonOntoAxis(p, axis);
|
||||
const proj2 = projectCircleOntoAxis(circle, axis);
|
||||
|
||||
export const satCollisionCircle = (s: Polygon, c: CircleLike) => {
|
||||
const shape = s.points.map((p) => new Vector(p).add(s.center));
|
||||
|
||||
if (shape.length < 2) {
|
||||
throw "Insufficient shape data in satCollisionCircle";
|
||||
}
|
||||
for (let i = 0; i < shape.length; i++) {
|
||||
const axis = shape[i].normal(shape.at(i - 1)!);
|
||||
let [_, p1minDot] = Vector.vectorProjectionAndDot(shape[0], axis);
|
||||
let p1maxDot = p1minDot;
|
||||
for (const point of shape) {
|
||||
const [_, dot] = Vector.vectorProjectionAndDot(point, axis);
|
||||
p1minDot = Math.min(dot, p1minDot);
|
||||
p1maxDot = Math.max(dot, p1maxDot);
|
||||
}
|
||||
const [__, circleDot] = Vector.vectorProjectionAndDot(
|
||||
new Vector(c.center),
|
||||
axis,
|
||||
);
|
||||
const p2minDot = circleDot - c.radius;
|
||||
const p2maxDot = circleDot + c.radius;
|
||||
|
||||
if (p1minDot - p2maxDot > 0 || p2minDot - p1maxDot > 0) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
const center = new Vector(c.center);
|
||||
let nearest = shape[0];
|
||||
for (const p of shape) {
|
||||
if (center.dist(p) < center.dist(nearest)) nearest = p;
|
||||
}
|
||||
const axis = center.sub(nearest);
|
||||
let [_, p1minDot] = Vector.vectorProjectionAndDot(shape[0], axis);
|
||||
let p1maxDot = p1minDot;
|
||||
for (const point of shape) {
|
||||
const [_, dot] = Vector.vectorProjectionAndDot(point, axis);
|
||||
p1minDot = Math.min(dot, p1minDot);
|
||||
p1maxDot = Math.max(dot, p1maxDot);
|
||||
}
|
||||
const [__, circleDot] = Vector.vectorProjectionAndDot(
|
||||
new Vector(c.center),
|
||||
axis,
|
||||
);
|
||||
const p2minDot = circleDot - c.radius;
|
||||
const p2maxDot = circleDot + c.radius;
|
||||
|
||||
if (p1minDot - p2maxDot > 0 || p2minDot - p1maxDot > 0) {
|
||||
return false;
|
||||
}
|
||||
if (!overlap(proj1, proj2)) return false;
|
||||
}
|
||||
const center = new Vector(circle.center);
|
||||
const nearest = p.getNearestPoint(center);
|
||||
const axis = nearest.copy().normal(center).normalize();
|
||||
const proj1 = projectPolygonOntoAxis(p, axis);
|
||||
const proj2 = projectCircleOntoAxis(circle, axis);
|
||||
|
||||
if (!overlap(proj1, proj2)) return false;
|
||||
return true;
|
||||
};
|
||||
}
|
||||
|
||||
function segmentIntersectsPolygon(
|
||||
p: Polygon,
|
||||
start: Vector,
|
||||
end: Vector,
|
||||
): boolean {
|
||||
const edges = p.getEdges();
|
||||
|
||||
for (const edge of edges) {
|
||||
// const axis = new Vector(-edge.y, edge.x).normalize();
|
||||
const axis = edge.copy().normal().normalize();
|
||||
|
||||
const proj1 = projectPolygonOntoAxis(p, axis);
|
||||
const proj2 = projectSegmentOntoAxis(start, end, axis);
|
||||
|
||||
if (!overlap(proj1, proj2)) {
|
||||
return false; // No overlap, no intersection
|
||||
}
|
||||
}
|
||||
|
||||
return true; // Overlapping on all axes, intersection detected
|
||||
}
|
||||
|
||||
function projectPolygonOntoAxis(
|
||||
p: Polygon,
|
||||
axis: Vector,
|
||||
): { min: number; max: number } {
|
||||
let min = Infinity;
|
||||
let max = -Infinity;
|
||||
|
||||
for (const point of p.points) {
|
||||
const dotProduct = point.copy().add(p.center).dot(axis);
|
||||
min = Math.min(min, dotProduct);
|
||||
max = Math.max(max, dotProduct);
|
||||
}
|
||||
|
||||
return { min, max };
|
||||
}
|
||||
|
||||
function projectSegmentOntoAxis(
|
||||
start: Vector,
|
||||
end: Vector,
|
||||
axis: Vector,
|
||||
): { min: number; max: number } {
|
||||
const dotProductStart = start.dot(axis);
|
||||
const dotProductEnd = end.dot(axis);
|
||||
return {
|
||||
min: Math.min(dotProductStart, dotProductEnd),
|
||||
max: Math.max(dotProductStart, dotProductEnd),
|
||||
};
|
||||
}
|
||||
|
||||
function projectCircleOntoAxis(
|
||||
c: CircleLike,
|
||||
axis: Vector,
|
||||
): { min: number; max: number } {
|
||||
const dot = new Vector(c.center).dot(axis);
|
||||
const min = dot - c.radius;
|
||||
const max = dot + c.radius;
|
||||
return { min, max };
|
||||
}
|
||||
|
||||
function overlap(
|
||||
proj1: { min: number; max: number },
|
||||
proj2: { min: number; max: number },
|
||||
): boolean {
|
||||
return proj1.min <= proj2.max && proj1.max >= proj2.min;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user