weld joint linear part working

src/phys.c

@@ -593,7 +593,7 @@ void phys_prepare_motor_joints(struct phys_step_ctx *ctx)
             struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og);
             struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og);
 
-            struct xform linear_mass_xf;
+            struct xform linear_mass_xf = ZI;
             linear_mass_xf.bx.x = inv_m0 + inv_m1 + vcp0.y * vcp0.y * inv_i0 + vcp1.y * vcp1.y * inv_i1;
             linear_mass_xf.bx.y = -vcp0.y * vcp0.x * inv_i0 - vcp1.y * vcp1.x * inv_i1;
             linear_mass_xf.by.x = linear_mass_xf.bx.y;
@@ -780,7 +780,7 @@ void phys_prepare_mouse_joints(struct phys_step_ctx *ctx)
 
             struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_start), xf.og);
 
-            struct xform linear_mass_xf;
+            struct xform linear_mass_xf = ZI;
             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;
@@ -862,25 +862,23 @@ void phys_solve_mouse_joints(struct phys_step_ctx *ctx, f32 dt)
                 struct v2 separation = v2_sub(point_start, point_end);
 
                 struct math_spring_result softness = math_spring(joint->linear_spring_hz, joint->linear_spring_damp, dt);
-                struct v2 bias = v2_mul(separation, softness.bias_rate);
+                f32 bias_rate = softness.bias_rate;
                 f32 mass_scale = softness.mass_scale;
                 f32 impulse_scale = softness.impulse_scale;
 
+                struct v2 bias = v2_mul(separation, bias_rate);
                 struct v2 vel = v2_add(v, v2_perp_mul(vcp, w));
                 struct v2 b = xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vel, bias));
 
-                struct v2 impulse;
+                struct v2 impulse = v2_mul(b, -mass_scale);
-                impulse.x = -mass_scale * b.x - impulse_scale * joint->linear_impulse.x;
+                impulse = v2_sub(impulse, v2_mul(joint->linear_impulse, impulse_scale));
-                impulse.y = -mass_scale * b.y - impulse_scale * joint->linear_impulse.y;
 
                 struct v2 old_impulse = joint->linear_impulse;
-                joint->linear_impulse.x += impulse.x;
+                joint->linear_impulse = v2_add(joint->linear_impulse, impulse);
-                joint->linear_impulse.y += impulse.y;
 
                 joint->linear_impulse = v2_clamp_len(joint->linear_impulse, max_impulse);
 
-                impulse.x = joint->linear_impulse.x - old_impulse.x;
+                impulse = v2_sub(joint->linear_impulse, old_impulse);
-                impulse.y = joint->linear_impulse.y - old_impulse.y;
 
                 v = v2_add(v, v2_mul(impulse, inv_m));
                 w += v2_wedge(vcp, impulse) * inv_i;
@@ -892,6 +890,157 @@ void phys_solve_mouse_joints(struct phys_step_ctx *ctx, f32 dt)
     }
 }
 
+/* ========================== *
+ * Weld joint
+ * ========================== */
+
+struct phys_weld_joint_def phys_weld_joint_def_init(void)
+{
+    struct phys_weld_joint_def def = ZI;
+    return def;
+}
+
+struct phys_weld_joint phys_weld_joint_from_def(struct phys_weld_joint_def def)
+{
+    struct phys_weld_joint res = ZI;
+    res.e0 = def.e0;
+    res.e1 = def.e1;
+    res.xf0_to_xf1 = def.xf;
+    return res;
+}
+
+void phys_prepare_weld_joints(struct phys_step_ctx *ctx)
+{
+    __prof;
+    struct sim_snapshot *ss = ctx->sim_step_ctx->world;
+    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
+        struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
+        if (!sim_ent_should_simulate(joint_ent)) continue;
+        if (!sim_ent_has_prop(joint_ent, SEPROP_WELD_JOINT)) continue;
+
+        /* TODO: Lookup and disable collision for any contacts between e0 & e1? */
+
+        struct phys_weld_joint *joint = &joint_ent->weld_joint_data;
+        struct sim_ent *e0 = sim_ent_from_id(ss, joint->e0);
+        struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1);
+        if (sim_ent_should_simulate(e0) && sim_ent_should_simulate(e1)) {
+            struct xform e0_xf = sim_ent_get_xform(e0);
+            struct xform e1_xf = sim_ent_get_xform(e1);
+
+            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);
+            }
+            joint->inv_m0 = inv_m0;
+            joint->inv_m1 = inv_m1;
+            joint->inv_i0 = inv_i0;
+            joint->inv_i1 = inv_i1;
+
+#if !SIM_PHYSICS_ENABLE_WARM_STARTING
+            joint->linear_impulse0 = V2(0, 0);
+            joint->linear_impulse1 = V2(0, 0);
+#endif
+        } else {
+            /* Mark joint for removal */
+            sim_ent_disable_prop(joint_ent, SEPROP_ACTIVE);
+            sim_ent_enable_prop(joint_ent, SEPROP_RELEASE);
+        }
+    }
+}
+
+void phys_warm_start_weld_joints(struct phys_step_ctx *ctx)
+{
+    __prof;
+    struct sim_snapshot *ss = ctx->sim_step_ctx->world;
+    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
+        struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
+        if (!sim_ent_should_simulate(joint_ent)) continue;
+        if (!sim_ent_has_prop(joint_ent, SEPROP_WELD_JOINT)) continue;
+
+        struct phys_weld_joint *joint = &joint_ent->weld_joint_data;
+
+#if 0
+        struct sim_ent *e0 = sim_ent_from_id(ss, joint->e0);
+        if (sim_ent_should_simulate(e0)) {
+            f32 inv_m = joint->inv_m0;
+            f32 inv_i = joint->inv_i0;
+            struct xform xf = sim_ent_get_xform(e1);
+            struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_start), xf.og);
+            sim_ent_set_linear_velocity(ent, v2_add(ent->linear_velocity, v2_mul(joint->linear_impulse, inv_m)));
+            ent->angular_velocity += (v2_wedge(vcp, joint->linear_impulse) + joint->angular_impulse) * inv_i;
+        }
+#endif
+
+#if 1
+        struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1);
+        if (sim_ent_should_simulate(e1)) {
+            f32 inv_m = joint->inv_m1;
+            sim_ent_set_linear_velocity(e1, v2_add(e1->linear_velocity, v2_mul(joint->linear_impulse1, inv_m)));
+        }
+#else
+        (UNUSED)joint;
+#endif
+    }
+}
+
+void phys_solve_weld_joints(struct phys_step_ctx *ctx, f32 dt)
+{
+    __prof;
+    struct sim_snapshot *ss = ctx->sim_step_ctx->world;
+    for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) {
+        struct sim_ent *joint_ent = &ss->ents[sim_ent_index];
+        if (!sim_ent_should_simulate(joint_ent)) continue;
+        if (!sim_ent_has_prop(joint_ent, SEPROP_WELD_JOINT)) continue;
+
+        struct phys_weld_joint *joint = &joint_ent->weld_joint_data;
+        struct sim_ent *e0 = sim_ent_from_id(ss, joint->e0);
+        struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1);
+        if (sim_ent_should_simulate(e0) && sim_ent_should_simulate(e1)) {
+            struct math_spring_result softness = math_spring(50, 0.0f, dt);
+
+            struct v2 v1 = e1->linear_velocity;
+
+            struct xform xf0 = sim_ent_get_xform(e0);
+            struct xform xf1 = sim_ent_get_xform(e1);
+
+            f32 inv_m1 = joint->inv_m1;
+            struct v2 target_p1 = xform_mul(xf0, joint->xf0_to_xf1).og;
+
+            struct v2 separation = v2_sub(xf1.og, target_p1);
+
+            f32 k = 1 / inv_m1;
+
+            f32 bias_rate = softness.bias_rate;
+            f32 mass_scale = softness.mass_scale;
+            f32 impulse_scale = softness.impulse_scale;
+
+            struct v2 bias = v2_mul(separation, bias_rate);
+            struct v2 vel = v1;
+            struct v2 b = v2_mul(v2_add(vel, bias), k);
+
+            struct v2 impulse = v2_mul(b, -mass_scale);
+            impulse = v2_sub(impulse, v2_mul(joint->linear_impulse1, impulse_scale));
+
+            struct v2 old_impulse = joint->linear_impulse1;
+            joint->linear_impulse1 = v2_add(joint->linear_impulse1, impulse);
+
+            impulse = v2_sub(joint->linear_impulse1, old_impulse);
+
+            v1 = v2_add(v1, v2_mul(impulse, inv_m1));
+
+            sim_ent_set_linear_velocity(e1, v1);
+        }
+    }
+}
+
 /* ========================== *
  * Integration
  * ========================== */
@@ -1080,6 +1229,7 @@ void phys_step(struct phys_step_ctx *ctx, f32 timestep)
         phys_prepare_contacts(ctx, phys_iteration);
         phys_prepare_motor_joints(ctx);
         phys_prepare_mouse_joints(ctx);
+        phys_prepare_weld_joints(ctx);
 
         f32 substep_dt = step_dt / SIM_PHYSICS_SUBSTEPS;
         for (u32 i = 0; i < SIM_PHYSICS_SUBSTEPS; ++i) {
@@ -1090,6 +1240,7 @@ void phys_step(struct phys_step_ctx *ctx, f32 timestep)
             phys_warm_start_contacts(ctx);
             phys_warm_start_motor_joints(ctx);
             phys_warm_start_mouse_joints(ctx);
+            phys_warm_start_weld_joints(ctx);
 #endif
 
             /* Solve */
@@ -1098,6 +1249,7 @@ void phys_step(struct phys_step_ctx *ctx, f32 timestep)
 #endif
             phys_solve_motor_joints(ctx, substep_dt);
             phys_solve_mouse_joints(ctx, substep_dt);
+            phys_solve_weld_joints(ctx, substep_dt);
 
             /* Integrate */
             phys_integrate_velocities(ctx, substep_dt);

src/phys.h

@@ -172,6 +172,36 @@ void phys_prepare_mouse_joints(struct phys_step_ctx *ctx);
 void phys_warm_start_mouse_joints(struct phys_step_ctx *ctx);
 void phys_solve_mouse_joints(struct phys_step_ctx *ctx, f32 dt);
 
+/* ========================== *
+ * Weld joint
+ * ========================== */
+
+struct phys_weld_joint_def {
+    struct sim_ent_id e0;
+    struct sim_ent_id e1;
+    struct xform xf;
+};
+
+struct phys_weld_joint {
+    struct sim_ent_id e0;
+    struct sim_ent_id e1;
+    struct xform xf0_to_xf1;
+
+    f32 inv_m0;
+    f32 inv_m1;
+    f32 inv_i0;
+    f32 inv_i1;
+
+    struct v2 linear_impulse0;
+    struct v2 linear_impulse1;
+};
+
+struct phys_weld_joint_def phys_weld_joint_def_init(void);
+struct phys_weld_joint phys_weld_joint_from_def(struct phys_weld_joint_def def);
+void phys_prepare_weld_joints(struct phys_step_ctx *ctx);
+void phys_warm_start_weld_joints(struct phys_step_ctx *ctx);
+void phys_solve_weld_joints(struct phys_step_ctx *ctx, f32 dt);
+
 /* ========================== *
  * Integration
  * ========================== */

src/sim_ent.h

@@ -41,6 +41,7 @@ enum sim_ent_prop {
     SEPROP_CONTACT_CONSTRAINT,
     SEPROP_MOTOR_JOINT,
     SEPROP_MOUSE_JOINT,
+    SEPROP_WELD_JOINT,
 
     SEPROP_CAMERA,
     SEPROP_CAMERA_ACTIVE,
@@ -193,23 +194,20 @@ struct sim_ent {
     struct space_entry_handle space_handle;
 
     /* ====================================================================== */
-    /* Contact constraint */
+    /* Constraints / joints */
 
     /* SEPROP_CONSTRAINT_CONTACT */
     struct phys_contact_constraint contact_constraint_data;
 
-    /* ====================================================================== */
-    /* Motor joint */
-
     /* SEPROP_MOTOR_JOINT */
     struct phys_motor_joint motor_joint_data;
 
-    /* ====================================================================== */
-    /* Mouse joint */
-
     /* SEPROP_MOUSE_JOINT */
     struct phys_mouse_joint mouse_joint_data;
 
+    /* SEPROP_WELD_JOINT */
+    struct phys_weld_joint weld_joint_data;
+
     /* ====================================================================== */
     /* Control */
 
@@ -289,6 +287,7 @@ struct sim_ent {
     /* SEPROP_WEAPON_CHUCKER */
 
     struct sim_ent_id chucker_zone;
+    struct sim_ent_id chucker_joint;
 
     /* ====================================================================== */
     /* Chucker zone */

src/sim_step.c

@@ -1035,7 +1035,26 @@ void sim_step(struct sim_step_ctx *ctx)
                 struct sim_ent *zone = sim_ent_from_id(world, ent->chucker_zone);
                 struct sim_ent *target = sim_ent_from_id(world, zone->chucker_zone_ent);
                 if (sim_ent_is_valid_and_active(target) && zone->chucker_zone_ent_tick == world->tick - 1) {
-                    sim_ent_enable_prop(target, SEPROP_RELEASE);
+                    struct sim_ent *old_joint_ent = sim_ent_from_id(world, ent->chucker_joint);
+                    if (!sim_ent_id_eq(old_joint_ent->weld_joint_data.e1, target->id)) {
+                        if (old_joint_ent->valid) {
+                            sim_ent_enable_prop(old_joint_ent, SEPROP_RELEASE);
+                        }
+                        struct sim_ent *joint_ent = sim_ent_alloc_sync_src(root);
+                        sim_ent_enable_prop(joint_ent, SEPROP_ACTIVE);
+
+                        struct xform xf0 = sim_ent_get_xform(ent);
+                        struct xform xf1 = sim_ent_get_xform(target);
+                        struct xform xf0_to_xf1 = xform_mul(xform_invert(xf0), xf1);
+
+                        sim_ent_enable_prop(joint_ent, SEPROP_WELD_JOINT);
+                        struct phys_weld_joint_def def = phys_weld_joint_def_init();
+                        def.e0 = ent->id;
+                        def.e1 = target->id;
+                        def.xf = xf0_to_xf1;
+                        joint_ent->weld_joint_data = phys_weld_joint_from_def(def);
+                        ent->chucker_joint = joint_ent->id;
+                    }
                 }
             }
         }
@@ -1056,10 +1075,10 @@ void sim_step(struct sim_step_ctx *ctx)
                 joint_ent->predictor = ent->predictor;
                 joint_ent->mass_unscaled = F32_INFINITY;
                 joint_ent->inertia_unscaled = F32_INFINITY;
-                sim_ent_enable_prop(joint_ent, SEPROP_MOTOR_JOINT);
                 sim_ent_enable_prop(joint_ent, SEPROP_ACTIVE);
                 ent->move_joint = joint_ent->id;
 
+                sim_ent_enable_prop(joint_ent, SEPROP_MOTOR_JOINT);
                 struct phys_motor_joint_def def = phys_motor_joint_def_init();
                 def.e0 = joint_ent->id;  /* Re-using joint entity as e0 */
                 def.e1 = ent->id;

src/user.c

@@ -1172,6 +1172,36 @@ INTERNAL void user_update(void)
                 }
 #endif
 
+                /* Draw weld joint */
+#if 0
+                if (sim_ent_has_prop(ent, SEPROP_WELD_JOINT)) {
+                    struct sim_ent *e1 = sim_ent_from_id(G.ss_blended, ent->weld_joint_data.e1);
+                    struct xform e1_xf = sim_ent_get_xform(e1);
+
+                    u32 color = COLOR_YELLOW;
+                    f32 radius = 3;
+                    struct v2 point = xform_mul_v2(e1_xf, ent->weld_joint_data.point_local_e1);
+                    point = xform_mul_v2(G.world_to_ui_xf, point);
+                    draw_circle(G.ui_cmd_buffer, point, radius, color, 10);
+
+                    DEBUGBREAKABLE;
+                }
+#endif
+
+                /* Draw mouse joint */
+                if (sim_ent_has_prop(ent, SEPROP_MOUSE_JOINT)) {
+                    struct sim_ent *target = sim_ent_from_id(G.ss_blended, ent->mouse_joint_data.target);
+                    struct xform target_xf = sim_ent_get_xform(target);
+
+                    u32 color = COLOR_YELLOW;
+                    f32 radius = 3;
+                    struct v2 point = xform_mul_v2(target_xf, ent->mouse_joint_data.point_local_end);
+                    point = xform_mul_v2(G.world_to_ui_xf, point);
+                    draw_circle(G.ui_cmd_buffer, point, radius, color, 10);
+
+                    DEBUGBREAKABLE;
+                }
+
                 /* Draw collider */
                 if (ent->local_collider.count > 0) {
                     struct collider_shape collider = ent->local_collider;