start mouse joints. separate contact creation & preparation

src/entity.h

@@ -14,6 +14,7 @@ enum entity_prop {
     ENTITY_PROP_PHYSICAL,
     ENTITY_PROP_CONTACT_CONSTRAINT,
     ENTITY_PROP_MOTOR_JOINT,
+    ENTITY_PROP_MOUSE_JOINT,
 
     ENTITY_PROP_PLAYER_CONTROLLED,
     ENTITY_PROP_CAMERA,
@@ -58,6 +59,7 @@ struct entity_store {
 
 /* TODO: Remove this */
 #include "collider.h"
+#include "math.h"
 
 struct contact_point {
     /* Contact point in local space of each entity */
@@ -89,6 +91,9 @@ struct contact_constraint {
     struct contact_point points[2];
     u32 num_points;
 
+    struct math_spring_result softness;
+    f32 pushout_velocity;
+
     /* TODO: Remove this (debugging) */
     struct collider_collision_points_result res;
     struct xform dbg_xf0;
@@ -146,6 +151,31 @@ INLINE struct motor_joint motor_joint_from_def(struct motor_joint_def def)
 
 
 
+struct mouse_joint {
+    struct entity_handle target;
+
+    struct v2 point_local_target;
+    struct v2 point_local_mouse;
+
+    f32 max_force;
+
+    f32 inv_m;
+    f32 inv_i;
+
+    struct v2 linear_impulse;
+    f32 angular_impulse;
+
+    struct xform linear_mass_xf;
+
+    struct math_spring_result linear_softness;
+    struct math_spring_result angular_softness;
+};
+
+
+
+
+
+
 
 
 struct entity {
@@ -199,6 +229,12 @@ struct entity {
     /* ENTITY_PROP_MOTOR_JOINT */
     struct motor_joint motor_joint_data;
 
+    /* ====================================================================== */
+    /* Mouse joint */
+
+    /* ENTITY_PROP_MOUSE_JOINT */
+    struct mouse_joint mouse_joint_data;
+
 
 
 

src/game.c

@@ -138,44 +138,39 @@ INTERNAL void spawn_test_entities(f32 offset)
 
         struct entity *e = entity_alloc(root);
 
-        //struct v2 pos = V2(0.25, -10);
-        //struct v2 pos = V2(0.25, -7);
-        //struct v2 pos = V2(0.25, -5.27);
-        //struct v2 pos = V2(0.5, -1.5);
         struct v2 pos = V2(1, -1);
-        //struct v2 pos = V2(0.300121694803, -1.322724342346);
-        //struct v2 pos = V2(1.0295, -1);
 
         pos = v2_add(pos, V2(0, offset));
         pos = v2_add(pos, V2(0, offset_all));
 
-        //struct v2 size = V2(1, 1);
-        //struct v2 size = V2(0.25, 0.25);
         //struct v2 size = V2(0.5, 0.5);
         //struct v2 size = V2(0.5, 0.25);
         struct v2 size = V2(1.0, 1.0);
-        //struct v2 size = V2(1.5, 1.5);
+
-        //f32 r = PI;
+        //f32 r = PI / 4;
-        f32 r = PI / 4;
+        f32 r = 0;
-        //f32 r = PI / 3;
+
-        //f32 r = 0.05;
+        if (!G.extra_spawn) {
-        //f32 r = PI / 2;
+            e->sprite = sprite_tag_from_path(STR("res/graphics/tim.ase"));
-        //f32 r = PI;
+        } else {
-        //f32 r = 0;
+            e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase"));
+            size = V2(0.5, 0.5);
+        }
+
+        //e->sprite = sprite_tag_from_path(STR("res/graphics/box_rounded.ase"));
+        //e->sprite_span_name = STR("idle.unarmed");
+        //e->sprite_span_name = STR("idle.one_handed");
+        e->sprite_span_name = STR("idle.two_handed");
+
 
         struct xform xf = XFORM_TRS(.t = pos, .r = r, .s = size);
         //xf.bx.y = -1.f;
 
         entity_set_xform(e, xf);
 
-        e->sprite = sprite_tag_from_path(STR("res/graphics/tim.ase"));
+        if (!G.extra_spawn) {
-        //e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase"));
-        //e->sprite = sprite_tag_from_path(STR("res/graphics/box_rounded.ase"));
-        //e->sprite_span_name = STR("idle.unarmed");
-        //e->sprite_span_name = STR("idle.one_handed");
-        e->sprite_span_name = STR("idle.two_handed");
-
             entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED);
+        }
 
         e->linear_ground_friction = 150;
         e->angular_ground_friction = 100;
@@ -309,8 +304,8 @@ INTERNAL void spawn_test_entities(f32 offset)
         struct v2 size = V2(1, 1);
 #else
         //struct v2 size = V2(5000, 1);
-        //struct v2 size = V2(50, 1);
+        struct v2 size = V2(50, 1);
-        struct v2 size = V2(1, 1);
+        //struct v2 size = V2(1, 1);
 #endif
         //f32 rot = PI / 4;
         f32 rot = 0;
@@ -369,7 +364,7 @@ INTERNAL void spawn_test_entities(f32 offset)
  * TESTING CONTACT CONSTRAINT
  * ========================== */
 
-INTERNAL void prepare_contacts(void)
+INTERNAL void create_contacts(void)
 {
     /* TODO: Remove this */
     /* FIXME: Dict has leaks: Entries never removed, even when constraint is no longer valid, or either entities are no longer valid. */
@@ -492,28 +487,6 @@ INTERNAL void prepare_contacts(void)
             if (res.num_points > 0) {
                 struct contact_constraint *constraint = &constraint_ent->contact_constraint_data;
 
-                struct v2 normal = res.normal;
-                struct v2 tangent = v2_perp(normal);
-
-                /* TODO: Cache this */
-                /* Calculate masses */
-                f32 inv_m0;
-                f32 inv_m1;
-                f32 inv_i0;
-                f32 inv_i1;
-                {
-                    f32 scale0 = math_fabs(xform_get_determinant(e0_xf));
-                    f32 scale1 = math_fabs(xform_get_determinant(e1_xf));
-                    inv_m0 = 1.f / (e0->mass_unscaled * scale0);
-                    inv_m1 = 1.f / (e1->mass_unscaled * scale1);
-                    inv_i0 = 1.f / (e0->inertia_unscaled * scale0);
-                    inv_i1 = 1.f / (e1->inertia_unscaled * scale1);
-                }
-                constraint->inv_m0 = inv_m0;
-                constraint->inv_m1 = inv_m1;
-                constraint->inv_i0 = inv_i0;
-                constraint->inv_i1 = inv_i1;
-
                 /* Delete old contacts that are no longer present */
                 for (u32 i = 0; i < constraint->num_points; ++i) {
                     struct contact_point *old = &constraint->points[i];
@@ -547,32 +520,81 @@ INTERNAL void prepare_contacts(void)
                             break;
                         }
                     }
-                    if (contact) {
+                    if (!contact) {
-                        /* Update existing */
+                        /* Insert */
-#if !GAME_PHYSICS_ENABLE_WARM_STARTING
-                        contact->normal_impulse = 0;
-                        contact->tangent_impulse = 0;
-#endif
-                    } else {
-                        /* Insert new */
                         contact = &constraint->points[constraint->num_points++];
                         MEMZERO_STRUCT(contact);
                         contact->id = id;
+                        f32 substep_dt = (1.f / ((f32)GAME_FPS * (f32)GAME_PHYSICS_SUBSTEPS));
+                        f32 damping_ratio = 10.0f;
+                        f32 frequency = (1.f / substep_dt) / 8.f;
+                        constraint->softness = math_spring(frequency, damping_ratio, substep_dt);
+                        constraint->pushout_velocity = 3.0f;
                     }
 
+                    /* Update points & separation */
                     contact->point_local_e0 = xform_invert_mul_v2(e0_xf, point);
                     contact->point_local_e1 = xform_invert_mul_v2(e1_xf, point);
                     contact->starting_separation = sep;
 
-                    /* TODO: Remove this (debugging) */
 #if COLLIDER_DEBUG
-                    {
                     contact->dbg_pt = point;
-                    }
 #endif
+                }
+
+
+            } else if (constraint_ent) {
+                constraint_ent->contact_constraint_data.num_points = 0;
+            }
+        }
+    }
+}
+
+INTERNAL void prepare_contacts(void)
+{
+    struct entity_store *store = G.tick.entity_store;
+    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
+        struct entity *constraint_ent = &store->entities[entity_index];
+        if (!entity_is_valid_and_active(constraint_ent)) continue;
+        if (!entity_has_prop(constraint_ent, ENTITY_PROP_CONTACT_CONSTRAINT)) continue;
+
+        struct contact_constraint *constraint = &constraint_ent->contact_constraint_data;
+
+        u32 num_points = constraint->num_points;
+        struct entity *e0 = entity_from_handle(store, constraint->e0);
+        struct entity *e1 = entity_from_handle(store, constraint->e1);
+        if (num_points > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) {
+            struct v2 normal = constraint->normal;
+            struct v2 tangent = v2_perp(normal);
+
+            struct xform e0_xf = entity_get_xform(e0);
+            struct xform e1_xf = entity_get_xform(e1);
+
+            /* TODO: Cache this */
+            /* Calculate masses */
+            f32 inv_m0;
+            f32 inv_m1;
+            f32 inv_i0;
+            f32 inv_i1;
             {
-                        struct v2 vcp0 = v2_sub(point, e0_xf.og);
+                f32 scale0 = math_fabs(xform_get_determinant(e0_xf));
-                        struct v2 vcp1 = v2_sub(point, e1_xf.og);
+                f32 scale1 = math_fabs(xform_get_determinant(e1_xf));
+                inv_m0 = 1.f / (e0->mass_unscaled * scale0);
+                inv_m1 = 1.f / (e1->mass_unscaled * scale1);
+                inv_i0 = 1.f / (e0->inertia_unscaled * scale0);
+                inv_i1 = 1.f / (e1->inertia_unscaled * scale1);
+            }
+            constraint->inv_m0 = inv_m0;
+            constraint->inv_m1 = inv_m1;
+            constraint->inv_i0 = inv_i0;
+            constraint->inv_i1 = inv_i1;
+
+            /* Update / insert returned contacts */
+            for (u32 i = 0; i < num_points; ++i) {
+                struct contact_point *contact = &constraint->points[i];
+
+                struct v2 vcp0 = v2_sub(xform_basis_mul_v2(e0_xf, contact->point_local_e0), e0_xf.og);
+                struct v2 vcp1 = v2_sub(xform_basis_mul_v2(e1_xf, contact->point_local_e1), e1_xf.og);
 
                 /* Normal mass */
                 {
@@ -589,16 +611,10 @@ INTERNAL void prepare_contacts(void)
                     f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge);
                     contact->inv_tangent_mass = k > 0.0f ? 1.0f / k : 0.0f;
                 }
-                    }
-                }
 
-
+#if !GAME_PHYSICS_ENABLE_WARM_STARTING
-            } else if (constraint_ent) {
+                contact->normal_impulse = 0;
-                constraint_ent->contact_constraint_data.num_points = 0;
+                contact->tangent_impulse = 0;
-#if !COLLIDER_DEBUG
-                /* No longer colliding, delete constraint */
-                fixed_dict_set(&dict_arena, &dict, constraint_key, NULL);
-                entity_enable_prop(constraint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
 #endif
             }
         }
@@ -659,24 +675,6 @@ INTERNAL void warm_start_contacts(void)
     }
 }
 
-struct soft_result { f32 bias_rate; f32 mass_scale; f32 impulse_scale; };
-INTERNAL struct soft_result make_soft(f32 hertz, f32 zeta, f32 h)
-{
-    if (hertz == 0.0f) {
-        return (struct soft_result) { .mass_scale = 1.0f };
-    } else {
-        f32 omega = 2.0f * PI * hertz;
-        f32 a1 = 2.0f * zeta + h * omega;
-        f32 a2 = h * omega * a1;
-        f32 a3 = 1.0f / (1.0f + a2);
-        return (struct soft_result) {
-            .bias_rate = omega / a1,
-            .mass_scale = a2 * a3,
-            .impulse_scale = 1.0f / (1.0f + a2)
-        };
-    }
-}
-
 INTERNAL void solve_contacts(f32 dt, b32 apply_bias)
 {
     struct entity_store *store = G.tick.entity_store;
@@ -725,17 +723,11 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias)
                     velocity_bias = separation / dt;
                 } else if (apply_bias) {
                     /* Soft constraint */
-
+                    struct math_spring_result softness = constraint->softness;
-                    f32 contact_damping_ratio = 10.0f;
+                    f32 pushout_velocity = constraint->pushout_velocity;
-                    f32 contact_hertz = (1.f / dt) / 8;
-
-                    struct soft_result softness = make_soft(contact_hertz, contact_damping_ratio, dt);
-
-                    f32 contact_pushout_velocity = 3.0f;
-                    velocity_bias = max_f32(softness.bias_rate * separation, -contact_pushout_velocity);
-
                     mass_scale = softness.mass_scale;
                     impulse_scale = softness.impulse_scale;
+                    velocity_bias = max_f32(softness.bias_rate * separation, -pushout_velocity);
                 }
 
                 struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
@@ -811,8 +803,6 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias)
  * TESTING MOTOR JOINT
  * ========================== */
 
-#if 1
-
 INTERNAL void prepare_motor_joints(void)
 {
     struct entity_store *store = G.tick.entity_store;
@@ -868,9 +858,6 @@ INTERNAL void prepare_motor_joints(void)
             joint->linear_impulse = V2(0, 0);
             joint->angular_impulse = 0;
 #endif
-        } else {
-            entity_disable_prop(joint_ent, ENTITY_PROP_ACTIVE);
-            entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME);
         }
     }
 }
@@ -979,20 +966,156 @@ INTERNAL void solve_motor_joints(f32 dt)
         e1->linear_velocity = v1;
         e1->angular_velocity = w1;
     }
+}
 
+
+
+
+
+#if 1
+
+/* ========================== *
+ * TESTING MOUSE JOINT
+ * ========================== */
+
+INTERNAL void create_mouse_joints(void)
+{
+}
+
+INTERNAL void prepare_mouse_joints(void)
+{
+    struct entity_store *store = G.tick.entity_store;
+    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
+        struct entity *joint_ent = &store->entities[entity_index];
+        if (!entity_is_valid_and_active(joint_ent)) continue;
+        if (!entity_has_prop(joint_ent, ENTITY_PROP_MOUSE_JOINT)) continue;
+
+        struct mouse_joint *joint = &joint_ent->mouse_joint_data;
+        struct entity *ent = entity_from_handle(store, joint->target);
+        if (entity_is_valid_and_active(ent)) {
+            struct xform xf = entity_get_xform(ent);
+
+            /* TODO: Cache this */
+            /* Calculate masses */
+            f32 inv_m;
+            f32 inv_i;
+            {
+                f32 scale = math_fabs(xform_get_determinant(xf));
+                inv_m = 1.f / (ent->mass_unscaled * scale);
+                inv_i = 1.f / (ent->inertia_unscaled * scale);
+            }
+            joint->inv_m = inv_m;
+            joint->inv_i = inv_i;
+
+            struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_target), xf.og);
+
+            struct xform linear_mass_xf;
+            linear_mass_xf.bx.x = inv_m + inv_i * vcp.y * vcp.y;
+            linear_mass_xf.bx.y = -inv_i * vcp.x * vcp.y;
+            linear_mass_xf.by.x = linear_mass_xf.bx.y;
+            linear_mass_xf.by.y = inv_m + inv_i * vcp.x * vcp.x;
+            joint->linear_mass_xf = xform_invert(linear_mass_xf);
+
+#if !GAME_PHYSICS_ENABLE_WARM_STARTING
+            joint->linear_impulse = V2(0, 0);
+            joint->angular_impulse = 0;
+#endif
+        }
+
+    }
+}
+
+INTERNAL void warm_start_mouse_joints(void)
+{
+    struct entity_store *store = G.tick.entity_store;
+    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
+        struct entity *joint_ent = &store->entities[entity_index];
+        if (!entity_is_valid_and_active(joint_ent)) continue;
+        if (!entity_has_prop(joint_ent, ENTITY_PROP_MOUSE_JOINT)) continue;
+
+        struct mouse_joint *joint = &joint_ent->mouse_joint_data;
+        struct entity *ent = entity_from_handle(store, joint->target);
+        if (entity_is_valid_and_active(ent)) {
+            f32 inv_m = joint->inv_m;
+            f32 inv_i = joint->inv_i;
+            struct xform xf = entity_get_xform(ent);
+            struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_target), xf.og);
+            ent->linear_velocity = v2_add(ent->linear_velocity, v2_mul(joint->linear_impulse, inv_m));
+            ent->angular_velocity += (v2_wedge(joint->linear_impulse, vcp) + joint->angular_impulse) * inv_i;
+        }
+    }
+}
+
+INTERNAL void solve_mouse_joints(f32 dt)
+{
+    struct entity_store *store = G.tick.entity_store;
+    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
+        struct entity *joint_ent = &store->entities[entity_index];
+        if (!entity_is_valid_and_active(joint_ent)) continue;
+        if (!entity_has_prop(joint_ent, ENTITY_PROP_MOUSE_JOINT)) continue;
+
+        struct mouse_joint *joint = &joint_ent->mouse_joint_data;
+        struct entity *ent = entity_from_handle(store, joint->target);
+        if (entity_is_valid_and_active(ent)) {
+            struct v2 v = ent->linear_velocity;
+            f32 w = ent->angular_velocity;
+
+            f32 inv_m = joint->inv_m;
+            f32 inv_i = joint->inv_i;
+
+            /* Angular impulse */
+            {
+                struct math_spring_result softness = joint->angular_softness;
+                f32 mass_scale = softness.mass_scale;
+                f32 impulse_scale = softness.impulse_scale;
+                f32 impulse = mass_scale * (-w / inv_i) - impulse_scale * joint->angular_impulse;
+                joint->angular_impulse += impulse;
+                w += impulse * inv_i;
+            }
+
+            /* Linear impulse */
+            {
+                f32 max_impulse = joint->max_force / dt;
+
+                struct xform xf = entity_get_xform(ent);
+
+                struct v2 point_target = xform_mul_v2(xf, joint->point_local_target);
+                struct v2 point_mouse = xform_mul_v2(xf, joint->point_local_mouse);
+
+                struct v2 vcp = v2_sub(point_target, xf.og);
+                struct v2 separation = v2_sub(point_mouse, point_target);
+
+                struct math_spring_result softness = joint->linear_softness;
+                struct v2 bias = v2_mul(separation, softness.bias_rate);
+                f32 mass_scale = softness.mass_scale;
+                f32 impulse_scale = softness.impulse_scale;
+
+                struct v2 vel = v2_add(v, v2_perp_mul(vcp, w));
+                struct v2 b = v2_mul(xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vel, bias)), mass_scale);
+
+                struct v2 old_impulse = joint->linear_impulse;
+                struct v2 impulse = v2_add(v2_mul(joint->linear_impulse, impulse_scale), b);
+                joint->linear_impulse = v2_clamp_len(v2_add(joint->linear_impulse, impulse), max_impulse);
+                impulse = v2_sub(joint->linear_impulse, old_impulse);
+
+                v = v2_add(v, v2_mul(impulse, inv_m));
+                w += v2_wedge(impulse, vcp) * inv_i;
+            }
+        }
+    }
 }
 
 #else
 
-INTERNAL void prepare_motor_joints(void)
+INTERNAL void prepare_mouse_joints(void)
 {
 }
 
-INTERNAL void warm_start_motor_joints(void)
+INTERNAL void warm_start_mouse_joints(void)
 {
 }
 
-INTERNAL void solve_motor_joints(f32 dt)
+INTERNAL void solve_mouse_joints(f32 dt)
 {
     (UNUSED)dt;
 }
@@ -1003,8 +1126,6 @@ INTERNAL void solve_motor_joints(f32 dt)
 
 
 
-
-
 /* ========================== *
  * TESTING PHYSICS INTEGRATION
  * ========================== */
@@ -1544,7 +1665,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
             }
 
             entity_set_xform(joint_ent, XFORM_IDENT);  /* Reset joint ent position */
-            joint_ent->linear_velocity = v2_with_len(ent->control.move, ent->control_force_max_speed);
+            joint_ent->linear_velocity = v2_mul(v2_clamp_len(ent->control.move, 1), ent->control_force_max_speed);
         }
     }
 #endif
@@ -1718,7 +1839,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
      * Physics
      * ========================== */
 
-
+    (UNUSED)create_contacts;
     (UNUSED)prepare_contacts;
     (UNUSED)warm_start_contacts;
     (UNUSED)solve_contacts;
@@ -1727,26 +1848,36 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
     (UNUSED)warm_start_motor_joints;
     (UNUSED)solve_motor_joints;
 
+    (UNUSED)create_mouse_joints;
+    (UNUSED)prepare_mouse_joints;
+    (UNUSED)warm_start_mouse_joints;
+    (UNUSED)solve_mouse_joints;
+
     (UNUSED)integrate_velocities_from_forces;
     (UNUSED)integrate_positions_from_velocities;
 #if 1
     {
         integrate_velocities_from_forces(dt);
+        create_contacts();
+        create_mouse_joints();
+
         prepare_contacts();
         prepare_motor_joints();
+        prepare_mouse_joints();
 
         f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS;
         for (u32 i = 0; i < GAME_PHYSICS_SUBSTEPS; ++i) {
 #if GAME_PHYSICS_ENABLE_WARM_STARTING
             warm_start_contacts();
             warm_start_motor_joints();
+            warm_start_mouse_joints();
 #endif
 
 #if GAME_PHYSICS_ENABLE_COLLISION
             solve_contacts(substep_dt, true);
 #endif
             solve_motor_joints(substep_dt);
-
+            solve_mouse_joints(substep_dt);
 
             integrate_positions_from_velocities(substep_dt);
 

src/math.h

@@ -1193,4 +1193,29 @@ INLINE struct v2 math_poly_center(struct v2_array a)
     return v2_div(sum, a.count);
 }
 
+/* ========================== *
+ * Other
+ * ========================== */
+
+/* https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf */
+struct math_spring_result { f32 bias_rate; f32 mass_scale; f32 impulse_scale; };
+INLINE struct math_spring_result math_spring(f32 hertz, f32 damping_ratio, f32 dt)
+{
+    struct math_spring_result res;
+    if (hertz == 0.0f) {
+        res.bias_rate = 0;
+        res.mass_scale = 1;
+        res.impulse_scale = 0;
+    } else {
+        f32 angular_frequency = TAU * hertz;
+        f32 a = 2 * damping_ratio + angular_frequency * dt;
+        f32 b = angular_frequency * a * dt;
+        f32 c = 1 / (b + 1);
+        res.bias_rate = angular_frequency / a;
+        res.mass_scale = b * c;
+        res.impulse_scale = c;
+    }
+    return res;
+}
+
 #endif