You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

677 lines
19 KiB
Plaintext

#import "Basic";
#import "Math";
#import "Window_Creation";
Debug :: #import "Debug";
Input :: #import "Input";
Simp :: #import "Simp";
TIMESTEP: float = 1.0 / 60.0;
DEBUGGING :: true;
window_width : s32 = 1280;
window_height : s32 = 720;
dbgprint :: print; // so can be
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;
}
normalize_or_zero :: inline (using _v: Vector2) -> Vector2 #must {
v := _v;
normalize_or_zero(*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;
to := _to;
if drawing_in_world_space {
from = world_to_screen(from);
to = world_to_screen(to);
}
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)
{
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;
static: bool;
mass: float = 1.0;
};
moment_of_inertia :: (using r: Rect) -> float {
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;
}
apply_impulse_at_point :: (using r: *Rect, impulse: Vector2, point_world_space: Vector2) {
if !static
{
vel += impulse / mass;
offset_from_center_of_mass := point_world_space - pos;
angle_vel += cross(offset_from_center_of_mass, impulse) / moment_of_inertia(r);
#if DEBUGGING { if isnan(angle_vel) Debug.breakpoint(); }
}
}
velocity_of_point :: (using r: *Rect, point: Vector2) -> Vector2 #must {
r: Vector2 = point - pos;
return vel + perp(r) * angle_vel;
}
// everything needed to resolve the collision
Manifold :: struct {
a, b: *Rect;
count: int;
depths: [2] float;
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, dt: float) {
a := m.a;
b := m.b;
for 0..m.count-1
{
total_momentum: float = length(a.vel) * a.mass + length(b.vel) * b.mass;
impulse_strength: float = 0.0;
//impulse_strength += m.depths[it] * 3.0;
//impulse_strength += total_momentum / 2.0;
restitution: float = 0.5;
v_point_a := velocity_of_point(a, m.contact_points[it]);
v_point_b := velocity_of_point(b, m.contact_points[it]);
vr := v_point_b - v_point_a;
r1 := m.contact_points[it] - a.pos;
r2 := m.contact_points[it] - b.pos;
I1 := moment_of_inertia(a);
I2 := moment_of_inertia(b);
impulse_strength += (-(1.0 + restitution) * dot(vr, m.normal)) / ( 1.0/a.mass + 1.0/b.mass + dot(cross(cross(r1, m.normal)/I1, r1) + cross(cross(r2, m.normal), r2)/I2, m.normal));
// the momentum would be unused if put into static
if a.static || b.static impulse_strength *= 2.0;
impulse := m.normal * impulse_strength;
apply_impulse_at_point(b, impulse, m.contact_points[it]);
apply_impulse_at_point(a, -impulse, m.contact_points[it]);
}
/*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;
};
// a halfspace (aka plane, aka line)
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;
// 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
//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;
}
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])};
}
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];
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, .{};
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]);
}
m.a = a;
m.b = b;
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) {
facing_to_right := rotate(xy(halfsize.x,0.0), angle);
facing_to_up := rotate(xy(0.0,halfsize.y), angle);
upper_right := pos + facing_to_right + facing_to_up;
upper_left := pos - facing_to_right + facing_to_up;
lower_left := pos - facing_to_right - facing_to_up;
lower_right := pos + facing_to_right - facing_to_up;
line(upper_left, upper_right);
line(upper_right, lower_right);
line(lower_right, lower_left);
line(lower_left, upper_left);
}
mouse :: () -> Vector2 {
x, y := get_mouse_pointer_position();
pos: Vector2 = xy(cast(float)x, cast(float)y);
// simp is lower left is (0, 0) and y+ is up
pos.y = window_height - pos.y;
return pos;
}
Camera :: struct {
pos: Vector2;
zoom: float = 1.0;
};
camera : Camera;
screen_to_world :: (screen: Vector2) -> Vector2 {
using camera;
return (screen + pos)*zoom;
}
world_to_screen :: (world: Vector2) -> Vector2 {
using camera;
// world = (screen + pos)*zoom;
// world/zoom = screen + pos;
// world/zoom - pos = screen;
return (world/zoom) - pos;
}
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);
camera.pos = -xy(window_width, window_height)/2.0;
camera.zoom = 0.01;
Simp.set_render_target(window);
rects: [..]Rect;
array_add(*rects, .{pos = #run xy(0.0, 0.0), halfsize = #run xy(0.3)});
//array_add(*rects, .{pos = #run xy(2.5, 0.0), halfsize = #run xy(0.3)});
//array_add(*rects, .{pos = #run xy(-2.5, 0.0), halfsize = #run xy(0.3)});
array_add(*rects, .{pos = #run xy(0.0, -3.0), halfsize = #run xy(3.0, 0.2), static = true});
quit := false;
last_time := get_time();
panning: bool = false;
last_mouse_pos := mouse();
unprocessed_time: float = 0.0;
while !quit {
dt := cast(float)(get_time() - last_time);
last_time = get_time();
Input.update_window_events();
mouse_delta := mouse() - last_mouse_pos; // using Input.mouse_delta_x seems to incorrectly accumulate
last_mouse_pos = mouse();
for Input.get_window_resizes() {
Simp.update_window(it.window); // Simp will do nothing if it doesn't care about this window.
if it.window == window {
should_reinit := (it.width != window_width) || (it.height != window_height);
window_width = it.width;
window_height = it.height;
//if should_reinit my_init_fonts(); // Resize the font for the new window size.
}
}
camera.zoom *= 1.0 - 0.1*Input.mouse_delta_z/120.0;
if panning {
camera.pos -= mouse_delta;
// this doesn't fix it
//Input.mouse_delta_x = 0;
//Input.mouse_delta_y = 0;
}
// process physics
unprocessed_time += dt;
{
dt: string = "do not use";
while unprocessed_time > TIMESTEP
{
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));
}
collisions: [..] Manifold;
defer array_free(collisions);
for * from_rect: rects {
for * to_rect: rects {
if to_rect != from_rect
{
manifold_out := rect_to_rect(from_rect, to_rect);
if manifold_out.count > 0 {
unique := true;
for collisions {
if (it.a == manifold_out.a && it.b == manifold_out.b) || (it.a == manifold_out.b && it.b == manifold_out.a) {
unique = false;
break;
}
}
if unique array_add(*collisions, manifold_out);
}
}
}
}
for collisions {
handle_collision(it, TIMESTEP);
}
for * rects {
defer it.force = .{};
defer it.torque = 0.0;
// gravity
it.force.y += -9.81;
//if !it.static dbgprint("%\n", it.angle_vel);
if !it.static
{
it.vel += (it.force/it.mass) * TIMESTEP;
it.pos += it.vel * TIMESTEP;
it.angle_vel += (it.torque / moment_of_inertia(it)) * TIMESTEP;
it.angle += it.angle_vel * TIMESTEP;
}
}
}
}
Simp.immediate_begin();
Simp.clear_render_target(0.0, 0.0, 0.0, 1.0);
// draw grid
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));
}
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;
defaults();
Simp.immediate_flush();
Simp.swap_buffers(window);
for Input.events_this_frame {
if it.type == .QUIT then quit = true;
if it.type == {
case .KEYBOARD;
if it.key_pressed && it.key_code == .ESCAPE {
quit = true;
}
if it.key_code == .MOUSE_BUTTON_LEFT {
panning = cast(bool)it.key_pressed;
}
}
}
}
}