creikey
/
flight-jai
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Manifold generation, partially copied impulse

Browse Source
main
Cameron Murphy Reikes 1 year ago
parent 17225e5b33
commit a42b9a19bd
6 changed files with 2711 additions and 33 deletions
Show Stats Download Patch File Download Diff File

3
.gitignore vendored
Unescape Escape View File

@ -1,3 +1,6 @@
physics.exe
checker.exe
*.pdb
*.obj
.build/
.vs/

1
build_checker.bat
Unescape Escape View File

@ -0,0 +1 @@
cl /Zi /FS /Fd"checker.pdb" checker.c

68
checker.c
Unescape Escape View File

@ -0,0 +1,68 @@
#include <stdio.h>
#define CUTE_C2_IMPLEMENTATION
#include "cute_c2.h"
int main(int argc, char ** argv)
{
int face_index = -1;
#define V(xin, yin) (c2v){.x = xin, .y = yin}
#define VN(x, y) (c2Norm(V(x, y)))
#if 0
c2v vert = V(1.0, 2.0);
c2v vert_no_macro = (c2v){.x = 1.0, .y = 2.0};
#endif
#if 0
c2Poly A = (c2Poly){
.count = 1,
.verts = {V(-1.0, 26.0)},
.norms = {V(-1.0, 0.0)},
};
printf("%f\n", A.verts[0].y);
#endif
#if 1
c2Poly A = (c2Poly){
.count = 4,
.verts = {V(-1, 1), V(1, 1), V(1, -1), V(-1, -1)},
};
c2MakePoly(&A);
c2Poly B = (c2Poly){
.count = 4,
.verts = {V(-0.5, 1), V(1.5, 1), V(1.5, -1), V(-0.5, -1)},
};
c2MakePoly(&B);
c2v b_offset = V(0.0, 0.0);
for(int i = 0; i < B.count; i++) {
B.verts[i].x += b_offset.x;
B.verts[i].y += b_offset.y;
}
printf("A\n");
for(int i = 0; i < A.count; i++) {
printf("(%f,%f), ", A.verts[i].x, A.verts[i].y);
}
printf("\n");
printf("Normals: ");
for(int i = 0; i < A.count; i++) {
printf("(%f,%f), ", A.norms[i].x, A.norms[i].y);
}
printf("\n");
printf("\n");
printf("B\n");
for(int i = 0; i < B.count; i++) {
printf("(%f,%f), ", B.verts[i].x, B.verts[i].y);
}
printf("\n");
float output = c2CheckFaces(&A, c2xIdentity(), &B, c2xIdentity(), &face_index);
printf("Separation %f face_index %d\n", output, face_index);
c2Manifold m = {0};
c2PolytoPolyManifold(&A, NULL, &B, NULL, &m);
printf("Manifold normal (%f, %f) | depth 0 %f | depth 1 %f |\n", m.n.x, m.n.y, m.depths[0], m.depths[1]);
#endif
getchar(); // so terminal doesn't close immediately in remedybg
}

2265
cute_c2.h
View File

File diff suppressed because it is too large Load Diff

BIN
debug.rdbg
View File

Binary file not shown.

407
physics.jai
Unescape Escape View File

@ -1,6 +1,7 @@
#import "Basic";
#import "Math";
#import "Window_Creation";
Debug :: #import "Debug";
Input :: #import "Input";
Simp :: #import "Simp";
@ -11,11 +12,32 @@ window_height : s32 = 720;
xy :: (x: int, y: int) -> Vector2 {
return xy(cast(float)x, cast(float)y);
}
normalize :: (using v: *Vector2) -> float {
sq := sqrt(x*x + y*y);
factor := 1.0 / sq;
x *= factor;
y *= factor;
return sq;
}
normalize :: (v: Vector2) -> Vector2 #must {
normalize(*v);
return v;
}
negative :: (v: Vector2) -> Vector2 #must {
return xy(-v.x, -v.y);
}
drawing_in_world_space: bool = false;
WHITE :: Vector4.{1.0, 1.0, 1.0, 1.0};
RED :: Vector4.{1.0, 0.0, 0.0, 1.0};
GREEN :: Vector4.{0.0, 1.0, 0.0, 1.0};
drawing_color: Vector4 = WHITE;
line_thickness: float = 1.0;
defaults :: () {
drawing_color = WHITE;
line_thickness = 1.0;
}
line :: (_from: Vector2, _to: Vector2) {
width := line_thickness;
from := _from;
@ -24,33 +46,100 @@ line :: (_from: Vector2, _to: Vector2) {
from = world_to_screen(from);
to = world_to_screen(to);
}
Simp.set_shader_for_color();
Simp.set_shader_for_color(true);
normal := rotate(unit_vector(to - from), PI/2.0);
Simp.immediate_quad(from + normal*width, from - normal*width, to + normal*width, to - normal*width, color = drawing_color);
}
LastingPip :: struct {
alive_for: float;
pos: Vector2;
}
pips : [10] LastingPip;
push_pip :: (at: Vector2)
{
Debug.breakpoint();
for * pips
{
if it.alive_for <= 0.0
{
it.alive_for = 1.0;
it.pos = at;
break;
}
}
}
draw_pips :: (dt: float)
{
drawing_in_world_space = true;
drawing_color = RED;
for * pips if it.alive_for > 0.0
{
drawing_color.w = it.alive_for;
pip(it.pos);
it.alive_for -= dt;
}
defaults();
}
pip :: (_at: Vector2, size: float = 0.05) {
at := _at;
Simp.set_shader_for_color(true);
if drawing_in_world_space {
at = world_to_screen(at);
size /= camera.zoom;
}
Simp.immediate_quad(at + xy(-size, size), at + xy(size, size), at + xy(size, -size), at + xy(-size, -size), color = drawing_color);
}
Rect :: struct {
halfsize: Vector2;
pos , vel , force : Vector2; // pos is in world space
angle, angle_vel, torque: float;
mass: float = 1.0;
};
MASS :: 1.0;
moment_of_inertia :: (using r: Rect) -> float {
return MASS*(halfsize.y*halfsize.y + halfsize.x*halfsize.x)/12.0;
return mass*(halfsize.y*halfsize.y + halfsize.x*halfsize.x)/12.0;
}
apply_force_at_point :: (r: *Rect, force: Vector2, point_world_space: Vector2) {
r.force += force;
offset_from_center_of_mass := point_world_space - r.pos;
r.torque += offset_from_center_of_mass.x * force.y - offset_from_center_of_mass.y * force.x;
}
// everything needed to resolve the collision
Manifold :: struct {
count: int;
depths: [2] float;
contact_points: [2] Vector2;
normal: Vector2;
contact_points: [2] Vector2; // in absolute coordinates
normal: Vector2; // always points from shape A to shape B
};
cross :: (v1: Vector2, v2: Vector2) -> float {
return v1.x*v2.y - v1.y*v2.x;
}
/// Returns a perpendicular vector. (90 degree rotation)
perp :: (v: Vector2) -> Vector2
{
return xy(-v.y, v.x);
}
handle_collision :: (m: Manifold, a: Rect, b: Rect) {
if m.count == 0 return;
k_scalar :: (a: Rectangle, b: Rectangle, r1: Vector2, r2: Vector2, n: Vector2) {
k_scalar_rect :: (a: Rectangle, r: Vector2, n: Vector2)
{
rcn := cross(r, n);
return 1.0/a.mass + rcn*rcn / moment_of_inertia(a);
}
cpFloat value = k_scalar_rect(a, r1, n) + k_scalar_rect(b, r2, n);
assert(value != 0.0, "Unsolvable collision or constraint.");
return value;
}
r1 := m.contact_points[0] - a.pos;
r2 := m.contact_points[0] - b.pos;
nMass := 1.0 / k_scalar(a, b, r1, r2,
}
MAX_POLYGON_VERTS :: 8;
Polygon :: struct {
// verts.count needs to equal norms.count
count: int;
verts: [MAX_POLYGON_VERTS] Vector2;
norms: [MAX_POLYGON_VERTS] Vector2;
@ -60,49 +149,281 @@ Halfspace :: struct {
n: Vector2;
d: float;
}
CCW90 :: (a: Vector2) -> Vector2 {
b: Vector2;
b.x = a.y;
b.y = -a.x;
return b;
}
distance :: (h: Halfspace, p: Vector2) -> float {
return dot(h.n, p) - h.d;
}
intersect :: (a: Vector2, b: Vector2, da: float, db: float) -> Vector2 {
return a + (b - a) * (da / (da - db));
}
poly_from :: (using r: Rect) -> Polygon {
to_return: Polygon;
compute_normals :: (p: *Polygon) {
for 0..p.count-1 {
a: int = it;
b: int = ifx it + 1 < p.count then it + 1 else 0;
e: Vector2 = p.verts[b] - p.verts[a];
p.norms[it] = normalize(CCW90(e));
}
}
facing_to_right := rotate(xy(halfsize.x,0.0), angle);
facing_to_up := rotate(xy(0.0,halfsize.y), angle);
to_return.count = 4;
to_return.verts[0] = -facing_to_right + facing_up; // upper left
to_return.norms[0] = #run normalize(xy(-1.0, 1.0));
to_return.verts[1] = facing_to_right + facing_up; // upper right
to_return.norms[1] = #run normalize(xy(1.0, 1.0));
// the ordering here is important for collision algorithms, not exactly sure why.
// Perhaps it's just important that the winding is counter clockwise
to_return.verts[0] = facing_to_right + -facing_to_up; // lower right
to_return.verts[1] = facing_to_right + facing_to_up; // upper right
to_return.verts[2] = -facing_to_right + facing_to_up; // upper left
to_return.verts[3] = -facing_to_right + -facing_to_up; // lower left
to_return.verts[2] = facing_to_right + -facing_up; // lower right
to_return.norms[2] = #run normalize(xy(1.0, -1.0));
to_return.verts[3] = -facing_to_right + -facing_up; // lower left
to_return.norms[3] = #run normalize(xy(-1.0, -1.0));
//for 0..to_return.count-1 to_return.norms[it] = rotate(to_return.norms[it], angle);
for 0..to_return.count-1 to_return.verts[it] += pos;
compute_normals(*to_return);
return to_return;
}
rect_to_rect :: (a: Rect, b: Rect) -> Manifold {
to_return : Manifold;
check_faces :: (a: Polygon, b: Polygon) -> float {
check_faces :: (a: Polygon, b: Polygon) -> (separation: float, face_index: int) {
plane_at :: (p : Polygon, i: int) -> Halfspace {
return .{n = p.norms[i], d = dot(p.norms[i], p.verts[i]};
}
plane_at :: (p : Polygon, i: int) -> Halfspace {
return .{n = p.norms[i], d = dot(p.norms[i], p.verts[i])};
}
support :: (verts: [] Vector2, d: Vector2 {
support :: (verts: [] Vector2, d: Vector2) -> int {
imax: int = 0;
dmax: float = dot(verts[0], d);
for 1..verts.count-1 {
dot_output: float = dot(verts[it], d);
if(dot_output > dmax)
{
imax = it;
dmax = dot_output;
}
}
return imax;
}
sep : float = -FLOAT32_MAX;
index: int = ~0;
for 0..a.count-1 {
h := plane_at(a, it);
verts_to_use: [] Vector2 = b.verts;
verts_to_use.count = b.count;
idx := support(verts_to_use, negative(h.n));
p := b.verts[idx];
d := distance(h, p);
if (d > sep)
{
sep = d;
index = it;
}
}
return sep, index;
}
rect_to_rect :: (a: Rect, b: Rect) -> Manifold {
to_return : Manifold;
poly_a := poly_from(a);
poly_b := poly_from(b);
sa, ea := check_faces(poly_a, poly_b);
if sa >= 0 return .{};
sb, eb := check_faces(poly_b, poly_a);
if sb >= 0 return .{};
rp, ip: *Polygon;
re: int;
flip: int;
kRelTol := 0.95;
kAbsTol := 0.01;
if(sa * kRelTol > sb + kAbsTol)
{
rp = *poly_a;
ip = *poly_b;
re = ea;
flip = 0;
}
else
{
rp = *poly_b;
ip = *poly_a;
re = eb;
flip = 1;
}
incident: [2] Vector2;
// calculate incident
{
rn_in_incident_space := rp.norms[re];
sep : float = FLT_MAX;
index: int = ~0;
min_dot: float = FLOAT32_MAX;
for 0..ip.count-1 {
dot_output := dot(rn_in_incident_space, ip.norms[it]);
if(dot_output < min_dot)
{
min_dot = dot_output;
index = it;
}
}
incident[0] = ip.verts[index];
incident[1] = ip.verts[ifx index + 1 == ip.count then 0 else index + 1];
}
// clip a segment to the "side planes" of another segment.
// side planes are planes orthogonal to a segment and attached to the
// endpoints of the segment
// side planes from poly
rh: Halfspace;
side_planes_poly_return: int;
{
seg: [2] Vector2 = incident;
p: *Polygon = rp;
e: int = re;
ra := p.verts[e];
rb := p.verts[ifx e + 1 == p.count then 0 else e + 1];
// side planes
side_planes :: (seg: [] Vector2, ra: Vector2, rb: Vector2) -> int, Halfspace {
in: Vector2 = normalize(rb - ra);
left: Halfspace = .{ n = negative(in), d = dot(negative(in), ra) };
right: Halfspace = .{ n = in, d = dot(in, rb) };
// clip a segment to a plane
clip :: (seg: [] Vector2, h: Halfspace) -> int {
out: [2] Vector2;
sp: int = 0;
d0: float = distance(h, seg[0]);
d1: float = distance(h, seg[1]);
if d0 < 0 { out[sp] = seg[0]; sp+=1; }
if d1 < 0 { out[sp] = seg[1]; sp+=1; }
if d0 == 0 && d1 == 0 {
out[sp] = seg[0];
sp+=1;
out[sp] = seg[1];
sp+=1;
} else if d0 * d1 <= 0 {
out[sp] = intersect(seg[0], seg[1], d0, d1);
sp+=1;
}
seg[0] = out[0];
seg[1] = out[1];
return sp;
}
if clip(seg, left) < 2 return 0, .{};
if clip(seg, right) < 2 return 0, .{};
for 0..a.count-1 {
h := plane_at(a, i);
idx :=
h: Halfspace;
h.n = CCW90(in);
h.d = dot(CCW90(in), ra);
return 1, h;
}
side_planes_poly_return, rh = side_planes(seg, ra, rb);
}
if !side_planes_poly_return return .{};
m: Manifold;
// keep deep
{
seg: [2] Vector2 = incident;
h: Halfspace = rh;
cp: int = 0;
for 0..1 {
p := seg[it];
d: float = distance(h, p);
if (d <= 0)
{
m.contact_points[cp] = p;
m.depths[cp] = -d;
cp+=1;
}
}
m.count = cp;
m.normal = h.n;
}
if flip m.normal = negative(m.normal);
for 0..m.count-1 {
push_pip(m.contact_points[it]);
}
return m;
}
dump_polygon :: (poly: Polygon) {
for poly.verts print("V(%, %), ", it.x, it.y);
print("\n");
for poly.norms print("VN(%, %), ", it.x, it.y);
print("\n");
}
do_test :: () {
print("Doing test\n");
size := xy(1.0, 1.0);
A_rect: Rect = .{halfsize = size};
B_rect: Rect = .{halfsize = size, pos = xy(0.5, 0.0)};
#if true {
print("A %\n", A_rect);
print("B %\n", B_rect);
print("Collision %\n", rect_to_rect(A_rect, B_rect));
}
#if false {
A: Polygon;
B: Polygon;
A = poly_from(A_rect);
B = poly_from(B_rect);
print("A\n");
dump_polygon(A);
print("B\n");
dump_polygon(B);
/*for A.verts print("V(%,%), ", it.x, it.y);
print("\n");
for A.norms print("VN(%,%), ", it.x, it.y);
print("\n");*/
/*A.count = 3;
A.verts[0] = xy(-1.0, 0.0);
A.verts[1] = xy(-1.0, 1.0);
A.verts[2] = xy(0.0, 0.0);
A.norms[0] = #run normalize(xy(-1.0, 0.0));
A.norms[1] = #run normalize(xy(0.0, 1.0));
A.norms[2] = #run normalize(xy(1.0, 0.0));
*/
/*B.count = 3;
B.verts[0] = xy(2.0, 0.0);
B.verts[1] = xy(2.0, 1.0);
B.verts[2] = xy(1.0, 0.0);
B.norms[0] = #run normalize(xy(1.0, 0.0));
B.norms[1] = #run normalize(xy(0.0, 1.0));
B.norms[2] = #run normalize(xy(-1.0, 0.0));
*/
//for 0..B.count-1 B.verts[it] += xy(-0.5, 0.0);
seperation, face_index := check_faces(A, B);
print("Separation % face_index %\n", seperation, face_index);
}
Debug.breakpoint();
}
draw_rect :: (using r: Rect) {
@ -149,6 +470,12 @@ world_to_screen :: (world: Vector2) -> Vector2 {
}
main :: () {
#if false
{
do_test();
return;
}
window := create_window(window_width, window_height, "A Window");
// Actual render size in pixels can be different from the window dimensions we specified above (for example on high-resolution displays on macOS/iOS).
window_width, window_height = Simp.get_render_dimensions(window);
@ -205,7 +532,6 @@ main :: () {
{
defer unprocessed_time -= TIMESTEP;
if get_time() < 0.5
{
apply_force_at_point(*rects[0], xy(3, 0), rects[0].pos + xy(-0.1, 0.03));
@ -216,6 +542,18 @@ main :: () {
it.vel += (it.force/MASS) * TIMESTEP;
it.pos += it.vel * TIMESTEP;
from_rect := it;
for * to_rect: rects {
if to_rect != from_rect
{
manifold_out := rect_to_rect(from_rect, to_rect);
if manifold_out.count > 0 {
handle_collision(manifold_out, from_rect, to_rect);
//from_rect.vel *= -1.0;
}
}
}
it.angle_vel += (it.torque / moment_of_inertia(it)) * TIMESTEP;
it.angle += it.angle_vel * TIMESTEP;
}
@ -223,28 +561,31 @@ main :: () {
}
Simp.immediate_begin();
Simp.clear_render_target(0.0, 0.0, 0.0, 1.0);
drawing_in_world_space = true;
// draw grid
drawing_color = .{0.2, 0.2, 0.2, 0.2};
drawing_in_world_space = true;
drawing_color = .{0.2, 0.2, 0.2, 0.5};
for x: -30..30 {
line(xy(x, 30), xy(x, -30));
}
for y: -30..30 {
line(xy(30, y), xy(-30, y));
}
drawing_color = WHITE;
defaults();
draw_pips(dt);
drawing_in_world_space = true;
line_thickness = 2.0;
drawing_color = GREEN;
for rects draw_rect(it);
line_thickness = 1.0;
drawing_color = WHITE;
//line(xy(0.0, 0.0), screen_to_world(mouse()));
drawing_in_world_space = false;
defaults();
Simp.immediate_flush();
Simp.swap_buffers(window);

Write Preview
Loading…
Cancel
Save