flight-jai/physics.jai

#import "Basic";
#import "Math";
#import "Window_Creation";
Debug :: #import "Debug";
Input :: #import "Input";
Simp :: #import "Simp";

TIMESTEP: float = 1.0 / 60.0;
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 false
    {
    apply_force_at_point(r, impulse / TIMESTEP, point_world_space);
} else {
    if !static
    {
        vel += impulse / r.mass;
        angle_vel += length(impulse) * (cross(point_world_space - pos, normalize_or_zero(impulse))/moment_of_inertia(r));
        if isnan(angle_vel) Debug.breakpoint();
    }
}
}
// 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;

        // 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;
                }
            }
        }

    }
}