Position based dynamics.. I tried.. I failed

pbd.cpp

static void collision_broadphase(ta_rigid_body_pair **pairs, ta_rigid_body *rigid_bodies, double dt)
{
    // Box2D supports 16 collision categories. For each fixture you can_body
    // specify which category it belongs to. You also specify what other
    // categories this fixture can_body collide with.
    //
    //   if ((categoryA & maskB) != 0 && (categoryB & maskA) != 0)
    //
    // Collision groups let you specify an integral group index. You can_body
    // have all fixtures with the same group index always collide
    // (positive index) or never collide (negative index). Group indices
    // are usually used for things that are somehow related, like the
    // parts of a bicycle.
    //
    // Collisions between fixtures of different group indices are
    // filtered according the category and mask bits. In other words,
    // group filtering has higher precedence than category filtering.
    //
    // - A fixture on a static body can_body only collide with a dynamic
    //   body.
    // - A fixture on a kinematic body can_body only collide with a dynamic
    //   body.
    // - Fixtures on the same body never collide with each other.
    // - You can_body optionally enable/disable collision between fixtures on
    //   bodies connected by a joint.
    //
    // Sensor: Fixture which only detects collision, no response. a.k.a. trigger
    // -----------------------------------------------------------------
    // Depth-first traversal of AABB tree to find islands. Put islands
    // to sleep when all objects in island are resting. Wake up when
    // anything interacts or applies a force to any body in the island.

    UNUSED(dt);

    dlb_vec_each(ta_rigid_body *, a, rigid_bodies) {
        dlb_vec_range(ta_rigid_body *, b, a + 1, dlb_vec_end(rigid_bodies)) {
            // Skip self collision checks
            // NOTE: This isn't currently possible due to the loop starting at a + 1 and the fact that entites can only
            // have a single rigid body, but that might change in the future.
            if (a->entity == b->entity) {
                DLB_ASSERT(!"Entity has multiple rigid bodies?");
                continue;
            }
            // Ignore collisions between two immovable objects
            if (a->inv_mass == 0.0f && b->inv_mass == 0.0f) {
                continue;
            }
            // Ignore collisions between two sensors
            if (a->sensor && b->sensor) {
                continue;
            }
            // Ignore collisions between infinite planes
            if (a->collider.type == TA_COLLIDER_PLANE && b->collider.type == TA_COLLIDER_PLANE) {
                continue;
            }

            // NOTE: Skip broadphase for infinite planes (always collect them as potential pairs)
            if (a->collider.type == TA_COLLIDER_PLANE ||
                b->collider.type == TA_COLLIDER_PLANE ||
                ta_aabb_v_aabb(&a->aabb, &b->aabb))
            {
                ta_rigid_body_pair *pair = (ta_rigid_body_pair *)dlb_vec_alloc(*pairs);
                pair->a = a;
                pair->b = b;
            }
        }
    }
}
static void collision_narrowphase(ta_manifold **manifolds, ta_rigid_body_pair *pairs, double dt)
{
    UNUSED(dt);

    dlb_vec_each(ta_rigid_body_pair *, pair, pairs) {
        ta_manifold manifold = { 0 };
        if (ta_rigid_body_intersect(&manifold, pair->a, pair->b)) {
            dlb_vec_push(*manifolds, manifold);
        }
    }
}
static void game_simulate(float dt)
{
    static const float GRAVITY = -9.81f;

    ta_log_write(&tg_debug_log, SRC_GAME, " Sim step...\n");

#if 0
    // TODO: Use mesh instancing for primitives (need scale :/)
    // TODO: Use circular buffer for perma lines instead of dumping everything at arbitrary threshold
    if (dlb_vec_len(primitive_lines_perma.buffers[0]) > 100000) {
        ta_mesh_clear_buffers(&primitive_lines_perma);
    }
#else
    ta_mesh_clear_buffers(&primitive_lines_perma);
#endif

    dlb_vec_zero(tg_game.pairs);
    dlb_vec_zero(tg_game.manifolds);

    ta_rigid_body *rigid_bodies = (ta_rigid_body *)ta_game_resource_pool(RES_COMP_RIGID_BODY);

    // for n particles do
    //     x_prev = x;
    //     v = v + h * f_ext / m;
    //     x = x + h * v;
    // end
    dlb_vec_each(ta_rigid_body *, body, rigid_bodies) {
        {
            // TODO: We can't simulate rigid bodies on things that have parents, that would be super complex. We should consider
            // all of the childrens' colliders though, if they have any. Let's ignore this issue for now.
            ta_transform *transform = (ta_transform *)ta_game_component(body->entity, RES_COMP_TRANSFORM);
            DLB_ASSERT(!transform->parent);

            body->xform.position = transform->xform.position;
            body->xform.orientation = quat_normalize(transform->xform.orientation);
        }

        // Clear per-body collision list
        // HACK: Cast const away to prevent MSVC warnings about nonsensical const incompatibility
        dlb_vec_zero((char **)body->colliding_with);

        // Apply external forces
        if (!body->no_gravity) {
            ta_vec3 gravity = { 0.0f, GRAVITY, 0.0f };
            //gravity = vec3_scalef(gravity, body->mass);
            ta_rigid_body_apply_force(body, gravity);
        }

        // Store starting transform
        body->xform_prev = body->xform;

        // DEBUG: Cleanup
        const char *e_selected = 0;
        ta_editor_selected_entity(&e_selected);
        if (body->name == e_selected) {
            DLB_ASSERT(1);
        }

        // Update position (unless infinite mass)
        if (body->inv_mass) {
            ta_vec3 acceleration = vec3_scalef(body->force_accum, body->inv_mass);
            body->velocity = vec3_add(body->velocity, vec3_scalef(acceleration, dt));
            body->xform.position = vec3_add(body->xform.position, vec3_scalef(body->velocity, dt));

            float dtheta_mag = vec3_len(body->ang_velocity);
            if (dtheta_mag) {
                ta_vec4 delta_orient = quat_from_axis_angle(vec3_normalize(body->ang_velocity), dtheta_mag * dt);
                body->xform.orientation = quat_normalize(quat_mul(delta_orient, body->xform.orientation));
            }
        }

        // TODO: This will also need to update the AABB tree
        // Recalculate AABB
        body->aabb = ta_collider_world_bounds(&body->collider, &body->xform);

        // Reset accumulators
        body->force_accum = VEC3_ZERO;
        body->torque_accum = VEC3_ZERO;
        body->dbg_broadphase = false;
        body->dbg_narrowphase = false;
    }

    // Broad phase
    //
    // for iterations = 1 do
    //     SolvePositions(x1, ... xn, q1, ... qn);
    // end
    //
    // TODO: AABB tree and expand search distance based on body's velocity:
    // > To save computational cost we collect potential collision pairs once per time step instead of once per
    // > sub-step using a tree of axis aligned bounding boxes. We expand the boxes by a distance (k * dt * velocity),
    // > where k >= 1 is a safety multiplier accounting for potential accelerations during the time step. We use k = 2
    // > in our examples.  - PBDBodies.pdf, section 3.5
    collision_broadphase(&tg_game.pairs, rigid_bodies, dt);
    if (tg_game.pairs) {
        // Narrow phase
        collision_narrowphase(&tg_game.manifolds, tg_game.pairs, dt);
        dlb_vec_each(ta_manifold *, manifold, tg_game.manifolds) {
            DLB_ASSERT(manifold->a != manifold->b);

            // Update colliding_with lists
            dlb_vec_push(manifold->a->colliding_with, manifold->b->entity);
            dlb_vec_push(manifold->b->colliding_with, manifold->a->entity);

            ta_rigid_body *a = manifold->a;
            ta_rigid_body *b = manifold->b;

            // Sensors don't need any resolution
            if (a->sensor || b->sensor) {
                continue;
            }

            // All of this code currently assumes body->xform == collider position/rotation, we would need to be
            // more clever to get this to work with arbitrary offsets. "More clever" probably means making the rigid
            // body be the parent of the mesh and letting transform_update handle the offset
            //DLB_ASSERT(vec3_zero(a->centroid_local));
            //DLB_ASSERT(vec3_zero(b->centroid_local));

            // https://github.com/RandyGaul/ImpulseEngine/blob/master/Manifold.cpp#L57
            if (a->inv_mass == 0.0f && b->inv_mass == 0.0f) {
                DLB_ASSERT(0);
                ta_log_write(&tg_debug_log, SRC_RIGID_BODY, "WARNING: Cannot resolve contact between two infinite mass bodies\n");
                a->velocity = VEC3_ZERO;
                b->velocity = VEC3_ZERO;
                a->ang_velocity = VEC3_ZERO;
                b->ang_velocity = VEC3_ZERO;
                continue;
            }

            const char *e_selected = 0;
            ta_editor_selected_entity(&e_selected);
            if (a->name == e_selected || b->name == e_selected) {
                DLB_ASSERT(1);
            }

            for (u32 i = 0; i < manifold->contact_count; i++) {
                // world space contact points
                const ta_vec3 normal_world = manifold->normal_world;
                const ta_vec3 ca_world = rigid_body_local_to_world(a, manifold->contacts[i].ra_local);
                const ta_vec3 cb_world = rigid_body_local_to_world(b, manifold->contacts[i].rb_local);

                //---------------------------------------
                // Penetration correction
                //---------------------------------------
                float d = vec3_dot(vec3_sub(ca_world, cb_world), normal_world);
                if (d <= 0.0f) {
                    continue;
                }

                // Apply Dx = dn with a = 0 and lambda_normal
                ta_vec3 penetration_correction_world = vec3_scalef(normal_world, -d);
                ta_physics_apply_position_correction(
                    a,
                    b,
                    manifold->contacts[i].ra_local,
                    manifold->contacts[i].rb_local,
                    penetration_correction_world,
                    0.0f,
                    &manifold->contacts[i].lambda_n,
                    dt,
                    tg_game.debug_physics_render_penetration_vectors,
                    tg_game.debug_physics_color_penetration_vectors
                );

                //---------------------------------------
                // Static friction correction
                //---------------------------------------

                // relative motion of contacts
                const ta_vec3 ca_world_prev = rigid_body_local_to_world_prev(a, manifold->contacts[i].ra_local);
                const ta_vec3 cb_world_prev = rigid_body_local_to_world_prev(b, manifold->contacts[i].rb_local);

                // "dp" = delta position, i.e. relative motion of contacts
                ta_vec3 dp = vec3_sub(vec3_sub(cb_world, cb_world_prev), vec3_sub(ca_world, ca_world_prev));
                ta_vec3 dp_normal = vec3_scalef(normal_world, vec3_dot(dp, normal_world));
                ta_vec3 dp_tangent = vec3_sub(dp, dp_normal);

                // NOTE: There shouldn't be a fabsf() here for lambda_n according to PBDBodies.pdf, but I clearly have
                // a sign error somewhere. Need to figure this out (I believe the worst case for the current code is
                // a small over-correction when lambda is negative).
                if (vec3_len2(dp_tangent) &&
                    manifold->contacts[i].lambda_t < manifold->coef_static * -manifold->contacts[i].lambda_n)
                {
                    // Apply Dx = Dp_t with a = 0
                    ta_vec3 static_friction_world = dp_tangent;
#if 1
                    ta_physics_apply_position_correction(
                        a,
                        b,
                        manifold->contacts[i].ra_local,
                        manifold->contacts[i].rb_local,
                        static_friction_world,
                        0.0f,
                        &manifold->contacts[i].lambda_t,
                        dt,
                        tg_game.debug_physics_render_static_friction_vectors,
                        tg_game.debug_physics_color_static_friction_vectors
                    );
#endif
                }
            }
        }
    }

    // for n particles do
    //     v = (x − x_prev)/h;
    // end
    const float dt_inv = 1.0f / dt;
    dlb_vec_each(ta_rigid_body *, body, rigid_bodies) {
        // Update world space AABB
        body->aabb = ta_collider_world_bounds(&body->collider, &body->xform);

        body->velocity_prev = body->velocity;
        body->ang_velocity_prev = body->ang_velocity;

        // DEBUG: Cleanup
        const char *e_selected = 0;
        ta_editor_selected_entity(&e_selected);
        if (body->name == e_selected) {
            DLB_ASSERT(1);
        }

        ta_vec3 dp = vec3_sub(body->xform.position, body->xform_prev.position);
        body->velocity = vec3_scalef(dp, dt_inv);

        ta_vec4 dq = quat_mul(body->xform.orientation, quat_inverse(body->xform_prev.orientation));
        dq = quat_normalize(dq);
        body->ang_velocity = axis_angle_from_quat(dq);
        body->ang_velocity = vec3_scalef(body->ang_velocity, dt_inv);

        // check for NaN and infinity
        DLB_ASSERT(vec3_good(body->velocity));
        DLB_ASSERT(vec3_good(body->ang_velocity));

        // TODO: Allow transform to be offset from rigid body position/orientation? Or just make rigidbody the parent..
        ta_transform *transform = (ta_transform *)ta_game_component(body->entity, RES_COMP_TRANSFORM);
        transform->xform.position = body->xform.position;
        transform->xform.orientation = body->xform.orientation;
    }

    // Velocity corrections
    dlb_vec_each(ta_manifold *, manifold, tg_game.manifolds) {
        DLB_ASSERT(manifold->a != manifold->b);

        ta_rigid_body *a = manifold->a;
        ta_rigid_body *b = manifold->b;

        // Sensors don't need any resolution
        if (a->sensor || b->sensor) {
            continue;
        }

        const char *e_selected = 0;
        ta_editor_selected_entity(&e_selected);
        if (a->name == e_selected || b->name == e_selected) {
            DLB_ASSERT(1);
        }

        for (u32 i = 0; i < manifold->contact_count; i++) {
            const ta_vec3 n = manifold->normal_world;
            const ta_vec3 ra = rigid_body_oriented_vector(a, manifold->contacts[i].ra_local);
            const ta_vec3 rb = rigid_body_oriented_vector(b, manifold->contacts[i].rb_local);

            // relative velocity
            ta_vec3 va = vec3_add(a->velocity, vec3_cross(a->ang_velocity, ra));
            ta_vec3 vb = vec3_add(b->velocity, vec3_cross(b->ang_velocity, rb));
            ta_vec3 v = vec3_sub(vb, va);

            // normal/tangential velocity
            float vn = vec3_dot(n, v);
            ta_vec3 n_vn = vec3_scalef(n, vn);
            ta_vec3 vt = vec3_sub(v, n_vn);

            // dynamic friction
            if (!vec3_zero(vt)) {
                float vt_mag = vec3_len(vt);
                DLB_ASSERT(vt_mag);
                ta_vec3 vt_dir = vec3_scalef(vt, 1.0f/vt_mag);

                float kd = manifold->coef_dynamic;

                // NOTE: Can simplify later by removing redundant dt's
                // PBDBodies eq. 31
                //float fn = -manifold->contacts[i].lambda_n / (dt * dt);
                float fn = -manifold->contacts[i].lambda_n / (dt * dt);
                float dv_mag = -MIN(dt * kd * fn, vt_mag);
                ta_vec3 dv_dynamic_friction = vec3_scalef(vt_dir, dv_mag);
#if 1
                ta_physics_apply_velocity_correction(
                    a,
                    b,
                    manifold->contacts[i].ra_local,
                    manifold->contacts[i].rb_local,
                    dv_dynamic_friction,
                    tg_game.debug_physics_render_dynamic_friction_vectors,
                    tg_game.debug_physics_color_dynamic_friction_vectors
                );
#endif
            }

            // damping
            {
                float coef_linear_damping = 0.99f;
                float coef_angular_damping = 0.99f;

                float linear_damping = MIN(coef_linear_damping, 1.0f);
                float angular_damping = MIN(coef_angular_damping, 1.0f);

                // This doesn't seem to do anything useful to prevent jittering..
                //vec3_scalef(a->velocity, linear_damping * dt);
                //vec3_scalef(b->velocity, linear_damping * dt);
                //vec3_scalef(a->ang_velocity, angular_damping * dt);
                //vec3_scalef(b->ang_velocity, angular_damping * dt);

#if 0
                // TODO: Is "joint damping" relevant in the context of contact constraints? (eq. 32 & 33 in PBDBodies)
                // If so, are the contact radii the correct location to apply the impulse?
                ta_vec3 dv_damping = vec3_scalef(vec3_sub(b->velocity, a->velocity), linear_damping);
                ta_physics_apply_velocity_correction(
                    a,
                    b,
                    manifold->contacts[i].ra_local,
                    manifold->contacts[i].rb_local,
                    dv_damping,
                    tg_game.debug_physics_render_damping_vectors,
                    tg_game.debug_physics_color_damping_vectors
                );
#endif

                // TODO: Angular damping
            }

            // restitution
            {
                // previous relative velocity (before velocity integration)
                ta_vec3 va_prev = vec3_add(a->velocity_prev, vec3_cross(a->ang_velocity_prev, ra));
                ta_vec3 vb_prev = vec3_add(b->velocity_prev, vec3_cross(b->ang_velocity_prev, rb));
                ta_vec3 v_prev = vec3_sub(vb_prev, va_prev);
                // previous normal velocity
                float vn_mag_prev = vec3_dot(n, v_prev);
                // TODO: for small vn, |vn| <= 2|g|h, set restitution to 0 to avoid jittering
                //float e = manifold->e; // * (float)(fabs(vn) > (2.0f * fabs(GRAVITY) * dt));
                float e = manifold->e * (fabsf(vn_mag_prev) > (2.0f * fabsf(GRAVITY) * dt));
                //e = 0.9f;
                // NOTE: Using MIN instead of MAX here because my signs are reversed (opposite normal I think) vs.
                // PBDBodies Eq. 35.
                //float impulse_mag = -vn + MIN(-e * vn_mag_prev, 0);
                //float impulse_mag = -vn + MAX(-e * vn_mag_prev, 0);
                float impulse_mag = vn + e * vn_mag_prev;
                ta_vec3 dv_restitution = vec3_scalef(n, impulse_mag);
                ta_physics_apply_velocity_correction(
                    a,
                    b,
                    manifold->contacts[i].ra_local,
                    manifold->contacts[i].rb_local,
                    dv_restitution,
                    tg_game.debug_physics_render_restitution_vectors,
                    tg_game.debug_physics_color_restitution_vectors
                );
            }

            //ta_vec3 dv_total = vec3_add3(dv_dynamic_friction, dv_damping, dv_restitution);
            //ta_vec3 dv_total_a = vec3_scalef(dv_total, 1.0f/(wa + wb));
            //ta_vec3 dv_total_b = vec3_neg(dv_total_a);
            //ta_rigid_body_apply_velocity_correction(a, dv_total_a, ra);
            //ta_rigid_body_apply_velocity_correction(b, dv_total_b, rb);
        }
    }

#if 0
    // TODO: Implement damping in a way that doesn't vary with different timesteps
    body->velocity = vec3_scalef(body->velocity, 0.99f);
    body->ang_velocity = vec3_scalef(body->ang_velocity, 0.99f);
#endif
}