init

shaders/rigid_solve.comp

@@ -0,0 +1,310 @@
+#version 450
+#extension GL_GOOGLE_include_directive : require
+layout(local_size_x = 64) in;
+#include "rigid_common.glsl"
+layout(std430, set = 0, binding = 8) readonly buffer Tables { uint tbl[]; };   // after the noise words: per material friction and restitution of a rigid contact (6 words per material precede)
+
+// One sub-step of the rigid-body engine, one workgroup per body (a thread per outline sample). Port of PhysicsWorld::step.
+shared vec4 sP, sV;                       // p, v of the body being solved
+shared int accN, accPX, accPY, accNX, accNY, accD, accWet, woke;
+shared float sImpact;   // fastest approach speed into a contact during this sub-step (fragile bodies take damage from it)
+
+void contact_static(inout vec3 u, vec2 r, vec2 n, float e, float mu, float invM, float invI) {
+    vec2 vc = u.xy + u.z * vec2(-r.y, r.x);
+    float vn = dot(vc, n);
+    if (vn >= 0.0) return;
+    float rn = r.x * n.y - r.y * n.x;
+    float k = invM + rn * rn * invI;
+    float rest = -vn < 0.1667 ? 0.0 : e;   // 009c1ec0: restitution only for vn < -1 unit/s (10 px/s = 0.1667 cells per frame, pixel scale 10)
+    float j = -(1.0 + rest) * vn / k;
+    u.xy += j * n * invM;
+    u.z += invI * (r.x * (j * n.y) - r.y * (j * n.x));
+    vec2 wc = u.xy + u.z * vec2(-r.y, r.x);
+    vec2 t = vec2(-n.y, n.x);
+    float vt = dot(wc, t);
+    float rt = r.x * t.y - r.y * t.x;
+    float kt = invM + rt * rt * invI;
+    float jt = clamp(-vt / kt, -mu * j, mu * j);
+    u.xy += jt * t * invM;
+    u.z += invI * (r.x * (jt * t.y) - r.y * (jt * t.x));
+}
+
+vec2 snap_axis(vec2 g) {
+    if (abs(g.x) > 2.0 * abs(g.y)) g.y = 0.0; else if (abs(g.y) > 2.0 * abs(g.x)) g.x = 0.0;
+    return g;
+}
+
+vec2 terrain_normal(ivec2 c) {
+    vec2 g = vec2(0.0);
+    for (int dy = -2; dy <= 2; ++dy)
+        for (int dx = -2; dx <= 2; ++dx)
+            if ((dx != 0 || dy != 0) && solid_at(c + ivec2(dx, dy))) {
+                float d = length(vec2(dx, dy));
+                g -= vec2(dx, dy) / (d * d);
+            }
+    g = snap_axis(g);
+    float l = length(g);
+    return l < 1e-6 ? vec2(0.0, -1.0) : g / l;
+}
+
+// A world point inside the mask of body C? Gives the outward normal of C's surface (world, pointing into the point's owner) and the depth.
+bool mask_contact(vec2 wpnt, vec4 cp, int sc, out vec2 nw, out float depth) {
+    float qc = cos(cp.z), qs = sin(cp.z);
+    vec2 d = wpnt - cp.xy;
+    vec2 l = vec2(qc * d.x + qs * d.y, -qs * d.x + qc * d.y);
+    vec2 im = l + vec2(shf(sc, 5), shf(sc, 6));
+    ivec2 ic = ivec2(floor(im));
+    nw = vec2(0.0, -1.0); depth = 0.5;
+    if (!shape_solid(sc, ic.x, ic.y)) return false;
+    vec2 g = vec2(0.0);
+    for (int dy = -2; dy <= 2; ++dy)
+        for (int dx = -2; dx <= 2; ++dx)
+            if ((dx != 0 || dy != 0) && shape_solid(sc, ic.x + dx, ic.y + dy)) {
+                float dd = length(vec2(dx, dy));
+                g -= vec2(dx, dy) / (dd * dd);
+            }
+    g = snap_axis(g);
+    float gl = length(g);
+    if (gl < 1e-6) { g = l; gl = length(g); if (gl < 1e-6) { g = vec2(0.0, -1.0); gl = 1.0; } }
+    g /= gl;
+    for (float t = 0.5; t <= 6.0; t += 0.5) {
+        depth = t;
+        if (!shape_solid(sc, int(floor(im.x + g.x * t)), int(floor(im.y + g.y * t)))) break;
+    }
+    nw = rot2(qc, qs, g);
+    return true;
+}
+
+void add_contact(vec2 rel, vec2 n, float depth) {   // rel: world point relative to the centre of mass of the body being solved
+    atomicAdd(accN, 1);
+    atomicAdd(accPX, int(round(rel.x * 256.0)));
+    atomicAdd(accPY, int(round(rel.y * 256.0)));
+    atomicAdd(accNX, int(round(n.x * 1024.0)));
+    atomicAdd(accNY, int(round(n.y * 1024.0)));
+    atomicMax(accD, int(depth * 256.0));
+}
+
+void main() {
+    int bi = int(gl_WorkGroupID.x);
+    int t = int(gl_LocalInvocationID.x);
+    if (bi >= bhdr.x) return;
+    Body B = bodies[bi];
+    bool asleep = (B.i.z & B_ASLEEP) != 0;
+    vec2 wp0 = B.p.xy - vec2(pc.view_x, pc.view_y);
+    if ((B.i.z & B_REMOVED) != 0 || wp0.x < 4.0 || wp0.y < 4.0 || wp0.x > float(pc.gw - 4) || wp0.y > float(pc.gh - 4)) {   // frozen outside the window
+        if (t == 0) bnext[bi] = B;
+        return;
+    }
+    if ((B.i.z & B_NAILED) != 0 && (B.i.z & B_REMOVED) == 0) {   // hangs on the wall while there is terrain around the nail
+        vec2 org = B.p.xy + rot2(cos(B.p.z), sin(B.p.z), B.q.xy - vec2(shf(B.i.x, 5), shf(B.i.x, 6)));
+        ivec2 nc = ivec2(floor(org + B.n.yz));
+        bool held = false;
+        int nr = int(B.n.w);
+        for (int dy = -nr; dy <= nr; ++dy)
+            for (int dx = -nr; dx <= nr; ++dx) held = held || (dx * dx + dy * dy <= nr * nr && solid_at(nc + ivec2(dx, dy)));   // a surface near the nail point
+        if (held && B.l.w > 0.0) {   // the joint breaks when its reaction force is above (mass * 160) * break_force (docs/IMPLEMENTATION_BLOCKERS.md): the nail holds the body still, so the
+            // reaction is the whole acceleration it would have had: gravity plus the velocity change the impulses of this frame gave it (explosions, kicks), in cells per frame^2 (x 360 = m/s^2)
+            const vec2 a = B.v.xy + vec2(0.0, 0.1667);
+            const float lim = 160.0 * B.l.w / 360.0;
+            if (dot(a, a) > lim * lim) held = false;
+        }
+        if (held) { B.v.xy = vec2(0.0); B.p.w = 0.0; if (t == 0) bnext[bi] = B; return; }   // (the impulses of the frame went into the wall)
+        B.i.z &= ~(B_NAILED | B_ASLEEP);   // the wall is gone or the joint broke: it falls
+        B.i.w = 0;
+        asleep = false;
+    }
+    int s = B.i.x;
+    int nS = shi(s, 4);
+    float mass = shf(s, 7), inertia = shf(s, 8), radius = shf(s, 9);
+    float invM = 1.0 / (mass * B.q.z), invI = 1.0 / (inertia * B.q.z);
+    const float dt = 0.5;
+    if (t == 0) { sP = B.p; sV = B.v; accWet = 0; woke = 0; accN = 0; accPX = 0; accPY = 0; accNX = 0; accNY = 0; accD = 0; sImpact = 0.0; }
+    barrier();
+
+    if (!asleep) {
+        // buoyancy and drag in liquids
+        // 00bda390 / 00bde0f0 (docs/LIQUID_PHYSICS_COMPLETE_GUIDE.md section 8): ONE probe at (int(x), int(y) + 8) decides; in a liquid (not a powder) the force is -b m (v + g gravity_scale) at the centre,
+        // b = the component's buoyancy (PhysicsBody2Component constructor 00672980: 0.7). dv/dt = g (1 - b) - b v, no submerged fraction, no density, no extra rotation damping.
+        float c = cos(sP.z), sn = sin(sP.z);
+        if (t == 0) {   // the probe sits at the ENTITY ORIGIN (the image point B.q, not the centre of mass) plus 8 in y, float coordinates truncating toward zero
+            const vec2 org = sP.xy + rot2(c, sn, B.q.xy - vec2(shf(s, 5), shf(s, 6)));
+            if (liquid_at(ivec2(int(org.x), int(org.y) + 8))) accWet = 1;
+        }
+        barrier();
+        if (t == 0) {
+            const float buoy = B.d.x * float(accWet);   // the component's buoyancy
+            vec4 P = sP, V = sV;
+            const float h = dt / 60.0;   // 009be020: v = (v + h (g + F / m)) / (1 + h linear_damping), the damping acts on the gravity just added
+            V.y += 0.1667 * dt * (1.0 - buoy);
+            V.xy *= (1.0 - buoy * dt / 60.0) / (1.0 + h * B.d.y);
+            P.w /= 1.0 + h * B.d.z;
+            float sp = length(V.xy);
+            if (sp > 2500.0 / 60.0) V.xy *= (2500.0 / 60.0) / sp;   // the initial cap: 250 physics units per second (x 10 pixels)
+            P.w = clamp(P.w, -1.5707963, 1.5707963);   // 009be020: |h omega| <= pi / 2
+            vec2 stepv = V.xy * dt;
+            if (length(stepv) > 20.0) stepv *= 20.0 / length(stepv);   // after the constraints a translation of at most 2 physics units (20 pixels) per step
+            P.xy += stepv;
+            P.z += P.w * dt;
+            sP = P; sV = V;
+        }
+        barrier();
+        // terrain contacts: outline samples inside solid cells form one aggregated manifold
+        c = cos(sP.z); sn = sin(sP.z);
+        if (t < nS) {
+            vec2 r = rot2(c, sn, sample_of(s, t));
+            vec2 pnt = sP.xy + r;
+            ivec2 cell = ivec2(floor(pnt));
+            if (solid_at(cell)) {
+                vec2 nm = terrain_normal(cell);
+                float depth = 0.5;
+                for (float tt = 0.5; tt <= 6.0; tt += 0.5) {
+                    depth = tt;
+                    if (!solid_at(ivec2(floor(pnt + nm * tt)))) break;
+                }
+                add_contact(r, nm, depth);
+            }
+        }
+        barrier();
+        if (t == 0 && accN > 0) {
+            vec2 cp = vec2(float(accPX), float(accPY)) / (256.0 * float(accN));
+            vec2 an = vec2(float(accNX), float(accNY)) / 1024.0;
+            float al = length(an);
+            if (al > 1e-6) {
+                vec3 u = vec3(sV.xy, sP.w);
+                sImpact = max(sImpact, -dot(u.xy + u.z * vec2(-cp.y, cp.x), an / al));
+                // the contacted terrain material: its own solid_friction / solid_restitution (009c18c0: friction sqrt(fA fB), restitution max)
+                float tf = 0.3, tr = 0.2;
+                {
+                    const ivec2 cc = ivec2(floor(sP.xy + cp));
+                    for (int k = 0; k < 9; ++k) {
+                        const ivec2 q = cc + ivec2(k % 3 - 1, k / 3 - 1) - ivec2(pc.view_x, pc.view_y);
+                        if (q.x < 0 || q.y < 0 || q.x >= pc.gw || q.y >= pc.gh) continue;
+                        const uint tm = cell_mat(cells[q.y * pc.gw + q.x]);
+                        if (tm != 0u && (mflags(tm) & F_STATIC) != 0u) {
+                            const uint tb = 1024u + 6u * mats[mats.length() - 1u].w + 2u * tm;
+                            tf = uintBitsToFloat(tbl[tb]); tr = uintBitsToFloat(tbl[tb + 1u]);
+                            break;
+                        }
+                    }
+                }
+                for (int it = 0; it < 2; ++it) contact_static(u, cp, an / al, max(B.v.z, tr), sqrt(B.v.w * tf), invM, invI);
+                sV.xy = u.xy; sP.w = u.z;
+                float push = min(float(accD) / 256.0, 3.0) * 0.7;
+                sP.xy += an / al * push;
+            }
+        }
+        barrier();
+    }
+
+    // contacts with other bodies: my outline in their mask and theirs in mine
+    for (int j = 0; j < bhdr.x; ++j) {
+        if (j == bi) continue;
+        Body C = bodies[j];
+        if ((C.i.z & B_REMOVED) != 0) continue;
+        if (B.m.x > 0.5 && B.m.x == C.m.x) continue;   // parts of one corpse do not collide with each other
+        bool casleep = (C.i.z & B_ASLEEP) != 0;
+        if (asleep && casleep) continue;
+        int sc = C.i.x;
+        float rc = shf(sc, 9);
+        if (distance(sP.xy, C.p.xy) > radius + rc) continue;
+        if (t == 0) { accN = 0; accPX = 0; accPY = 0; accNX = 0; accNY = 0; accD = 0; }
+        barrier();
+        vec4 myp = vec4(sP.xy, sP.z, 0.0);
+        float c = cos(sP.z), sn = sin(sP.z);
+        if (t < nS) {
+            vec2 w = sP.xy + rot2(c, sn, sample_of(s, t));
+            vec2 nw; float depth;
+            if (mask_contact(w, C.p, sc, nw, depth)) add_contact(w - sP.xy, nw, depth);
+        }
+        int ncs = shi(sc, 4);
+        if (t < ncs) {
+            float cc = cos(C.p.z), cs = sin(C.p.z);
+            vec2 w = C.p.xy + rot2(cc, cs, sample_of(sc, t));
+            vec2 nw; float depth;
+            if (mask_contact(w, myp, s, nw, depth)) add_contact(w - sP.xy, -nw, depth);
+        }
+        barrier();
+        if (t == 0 && accN > 0) {
+            if (asleep && !casleep && length(C.v.xy) > 0.15) woke = 1;   // a moving neighbour wakes a sleeping body; resting contacts do not
+            if (!asleep) {
+                float invMc = 1.0 / (shf(sc, 7) * C.q.z), invIc = 1.0 / (shf(sc, 8) * C.q.z);
+                float e = max(B.v.z, C.v.z), mu = sqrt(B.v.w * C.v.w);
+                vec2 ra = vec2(float(accPX), float(accPY)) / (256.0 * float(accN));
+                vec2 rcv = sP.xy + ra - C.p.xy;
+                vec2 sn_ = vec2(float(accNX), float(accNY)) / 1024.0;
+                float al = length(sn_);
+                if (al > 1e-6) {
+                    vec2 n = sn_ / al;
+                    vec3 u = vec3(sV.xy, sP.w), uc = vec3(C.v.xy, C.p.w);   // uc follows the partner's response virtually, only my side is applied
+                    for (int it = 0; it < 2; ++it) {
+                        vec2 va = u.xy + u.z * vec2(-ra.y, ra.x), vc = uc.xy + uc.z * vec2(-rcv.y, rcv.x);
+                        float vn = dot(va - vc, n);
+                        if (vn >= 0.0) break;
+                        if (it == 0) sImpact = max(sImpact, -vn);
+                        float ran = ra.x * n.y - ra.y * n.x, rcn = rcv.x * n.y - rcv.y * n.x;
+                        float kk = invM + invMc + ran * ran * invI + rcn * rcn * invIc;
+                        float jn = -(1.0 + (-vn < 0.1667 ? 0.0 : e)) * vn / kk;
+                        u.xy += jn * n * invM; u.z += invI * (ra.x * (jn * n.y) - ra.y * (jn * n.x));
+                        uc.xy -= jn * n * invMc; uc.z -= invIc * (rcv.x * (jn * n.y) - rcv.y * (jn * n.x));
+                        vec2 wa = u.xy + u.z * vec2(-ra.y, ra.x), wc = uc.xy + uc.z * vec2(-rcv.y, rcv.x);
+                        vec2 tg = vec2(-n.y, n.x);
+                        float vt = dot(wa - wc, tg);
+                        float rat = ra.x * tg.y - ra.y * tg.x, rct = rcv.x * tg.y - rcv.y * tg.x;
+                        float kt = invM + invMc + rat * rat * invI + rct * rct * invIc;
+                        float jt = clamp(-vt / kt, -mu * jn, mu * jn);
+                        u.xy += jt * tg * invM; u.z += invI * (ra.x * (jt * tg.y) - ra.y * (jt * tg.x));
+                        uc.xy -= jt * tg * invMc; uc.z -= invIc * (rcv.x * (jt * tg.y) - rcv.y * (jt * tg.x));
+                    }
+                    sV.xy = u.xy; sP.w = u.z;
+                    float push = min(float(accD) / 256.0, 3.0) * 0.6;
+                    sP.xy += n * push * invM / (invM + invMc);
+                }
+            }
+        }
+        barrier();
+    }
+
+    // pin joints (ragdolls): the two anchors are pulled together; every body applies its own side of the impulse
+    int brokenJoints = 0;
+    if (t == 0) {
+        for (int k = 0; k < 4; ++k) {
+            int j = B.jn[k];
+            if (j < 0) continue;
+            Body C = bodies[j];
+            if ((C.i.z & B_REMOVED) != 0) { brokenJoints |= 1 << k; continue; }   // the partner is gone (cut away, burnt): the joint is dropped and the body wakes
+            if (asleep) { if ((C.i.z & B_ASLEEP) == 0 && length(C.v.xy) > 0.15) woke = 1; continue; }
+            vec4 ja = k == 0 ? B.ja0 : (k == 1 ? B.ja1 : (k == 2 ? B.ja2 : B.ja3));
+            int sc = C.i.x;
+            float invMc = 1.0 / (shf(sc, 7) * C.q.z), invIc = 1.0 / (shf(sc, 8) * C.q.z);
+            vec2 ra = rot2(cos(sP.z), sin(sP.z), ja.xy), rb = rot2(cos(C.p.z), sin(C.p.z), ja.zw);
+            vec2 err = (C.p.xy + rb) - (sP.xy + ra);
+            vec2 va = sV.xy + sP.w * vec2(-ra.y, ra.x), vb = C.v.xy + C.p.w * vec2(-rb.y, rb.x);
+            vec2 r = (vb - va) + err * 0.35;   // residual relative velocity of the anchors, with a pull toward each other
+            float m = invM + invMc;
+            mat2 K = mat2(m + invI * ra.y * ra.y + invIc * rb.y * rb.y, -invI * ra.x * ra.y - invIc * rb.x * rb.y,
+                          -invI * ra.x * ra.y - invIc * rb.x * rb.y, m + invI * ra.x * ra.x + invIc * rb.x * rb.x);
+            vec2 J = inverse(K) * r;
+            const float jbk = k == 0 ? B.jb.x : (k == 1 ? B.jb.y : (k == 2 ? B.jb.z : B.jb.w));
+            if (jbk != 0.0) {   // force of the joint: impulse / dt, in newtons (mass 0.01 per pixel, 360 = cells/frame^2 -> m/s^2); limit = (m_a + m_b) * 160 * break_force,
+                // for the ragdoll kinds 0x2b67 / 0x2b68 (stored negative) max((m_a + m_b) * 400, 200) * break_force
+                const float M = (shf(s, 7) * B.q.z + shf(sc, 7) * C.q.z) * 0.01;
+                const float F = length(J) * 0.01 * 360.0 / dt, lim = jbk > 0.0 ? M * 160.0 * jbk : max(M * 400.0, 200.0) * -jbk;
+                if (F * F > lim * lim) brokenJoints |= 1 << k;
+            }
+            sV.xy += J * invM;
+            sP.w += invI * (ra.x * J.y - ra.y * J.x);
+            sP.xy += err * 0.15 * invM / m;   // and a little of the error is corrected directly
+            woke = max(woke, 0);
+        }
+    }
+    barrier();
+    if (t == 0) {
+        if (B.n.x > 0.0 && B.i.y >= 0 && sImpact > B.n.x) atomicAdd(ents[B.i.y].x.x, int((sImpact - B.n.x) * 750.0));   // fall / impact damage of fragile bodies
+        Body o = B;
+        o.p = sP;
+        o.v.xy = sV.xy;
+        for (int k = 0; k < 4; ++k) if ((brokenJoints & (1 << k)) != 0) { o.jn[k] = -1; woke = 1; }
+        if (woke != 0) { o.i.z &= ~B_ASLEEP; o.i.w = 0; }
+        bnext[bi] = o;
+    }
+}