From effe6484c73b277a5f26ab9c778c0ba20dadd6a9 Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Thu, 8 Oct 2026 15:18:34 -0300 Subject: [PATCH 1/2] wip: joint sim inertias in float --- aephysics/dynamics/module.ae | 4 +- aephysics/joint_solver/module.ae | 86 ++++++++++++++++---------------- 2 files changed, 45 insertions(+), 45 deletions(-) diff --git a/aephysics/dynamics/module.ae b/aephysics/dynamics/module.ae index e9f03c4..c241601 100644 --- a/aephysics/dynamics/module.ae +++ b/aephysics/dynamics/module.ae @@ -984,8 +984,8 @@ struct JointSim { fixed_rotation: bool inv_mass_a: float inv_mass_b: float - inv_inertia_a: Matrix3 - inv_inertia_b: Matrix3 + inv_inertia_a: Matrix3f // the body sims' world inertias, single precision as they are kept + inv_inertia_b: Matrix3f constraint_softness: Softness force_threshold: float // the solve's joint event test torque_threshold: float diff --git a/aephysics/joint_solver/module.ae b/aephysics/joint_solver/module.ae index 6fe9d40..853521e 100644 --- a/aephysics/joint_solver/module.ae +++ b/aephysics/joint_solver/module.ae @@ -126,9 +126,9 @@ prepare_base(base: *JointSim, ctx: *StepContext) -> JointBodies { sim_b = dynamics.get_body_sim(w, body_b) base.inv_mass_a = sim_a.inv_mass base.inv_mass_b = sim_b.inv_mass - base.inv_inertia_a = math.widen_mat3(sim_a.inv_inertia_world) - base.inv_inertia_b = math.widen_mat3(sim_b.inv_inertia_world) - base.fixed_rotation = math.det(math.add_mm(base.inv_inertia_a, base.inv_inertia_b)) < 1000.0 * math.TINY + base.inv_inertia_a = sim_a.inv_inertia_world + base.inv_inertia_b = sim_b.inv_inertia_world + base.fixed_rotation = math.det(math.add_mm(math.widen_mat3(sim_a.inv_inertia_world), math.widen_mat3(sim_b.inv_inertia_world))) < 1000.0 * math.TINY index_a = NULL_INDEX if body_a.set_index == dynamics.AWAKE_SET { index_a = body_a.local_index } index_b = NULL_INDEX @@ -232,7 +232,7 @@ prepare_distance_joint(base: *JointSim, ctx: *StepContext) { axis = math.normalize(math.add(math.sub(r_b, r_a), j.delta_center)) cr_a = math.cross(r_a, axis) cr_b = math.cross(r_b, axis) - k = base.inv_mass_a + base.inv_mass_b + math.dot(cr_a, math.mul_mv(base.inv_inertia_a, cr_a)) + math.dot(cr_b, math.mul_mv(base.inv_inertia_b, cr_b)) + k = base.inv_mass_a + base.inv_mass_b + math.dot(cr_a, math.mul_mv(math.widen_mat3(base.inv_inertia_a), cr_a)) + math.dot(cr_b, math.mul_mv(math.widen_mat3(base.inv_inertia_b), cr_b)) j.axial_mass = 0.0 if k > 0.0 { j.axial_mass = 1.0 / k } j.distance_softness = dynamics.make_soft(j.hertz, j.damping_ratio, ctx.h) @@ -269,11 +269,11 @@ warm_start_distance_joint(base: *JointSim, ctx: *StepContext) { p = math.mul_sv(j.impulse + j.lower_impulse - j.upper_impulse + j.motor_impulse, axis) if (state_a.flags & dynamics.DYNAMIC_FLAG) != 0 { state_a.v = math.narrow3(math.mul_sub(math.widen3(state_a.v), base.inv_mass_a, p)) - state_a.w = math.narrow3(math.sub(math.widen3(state_a.w), math.mul_mv(base.inv_inertia_a, math.cross(r_a, p)))) + state_a.w = math.narrow3(math.sub(math.widen3(state_a.w), math.mul_mv(math.widen_mat3(base.inv_inertia_a), math.cross(r_a, p)))) } if (state_b.flags & dynamics.DYNAMIC_FLAG) != 0 { state_b.v = math.narrow3(math.mul_add(math.widen3(state_b.v), base.inv_mass_b, p)) - state_b.w = math.narrow3(math.add(math.widen3(state_b.w), math.mul_mv(base.inv_inertia_b, math.cross(r_b, p)))) + state_b.w = math.narrow3(math.add(math.widen3(state_b.w), math.mul_mv(math.widen_mat3(base.inv_inertia_b), math.cross(r_b, p)))) } } @@ -282,8 +282,8 @@ warm_start_distance_joint(base: *JointSim, ctx: *StepContext) { solve_distance_joint(base: *JointSim, ctx: *StepContext, use_bias: bool) { m_a = base.inv_mass_a m_b = base.inv_mass_b - i_a = base.inv_inertia_a - i_b = base.inv_inertia_b + i_a = math.widen_mat3(base.inv_inertia_a) + i_b = math.widen_mat3(base.inv_inertia_b) j = dynamics.distance_joint_of(base) state_a = state_of(ctx, j.index_a) state_b = state_of(ctx, j.index_b) @@ -495,7 +495,7 @@ prepare_motor_joint(base: *JointSim, ctx: *StepContext) { j.delta_center = math.sub(bodies.sim_b.center, bodies.sim_a.center) j.linear_spring = dynamics.make_soft(j.linear_hertz, j.linear_damping_ratio, ctx.h) j.angular_spring = dynamics.make_soft(j.angular_hertz, j.angular_damping_ratio, ctx.h) - j.angular_mass = math.invert_matrix(math.add_mm(base.inv_inertia_a, base.inv_inertia_b)) + j.angular_mass = math.invert_matrix(math.add_mm(math.widen_mat3(base.inv_inertia_a), math.widen_mat3(base.inv_inertia_b))) if ctx.enable_warm_starting == false { j.linear_velocity_impulse = math.vec3_zero() j.angular_velocity_impulse = math.vec3_zero() @@ -534,11 +534,11 @@ warm_start_motor_joint(base: *JointSim, ctx: *StepContext) { // static body's dummy state is written and forgotten). if (state_a.flags & dynamics.DYNAMIC_FLAG) != 0 { state_a.v = math.narrow3(math.mul_sub(math.widen3(state_a.v), base.inv_mass_a, linear)) - state_a.w = math.narrow3(math.sub(math.widen3(state_a.w), math.mul_mv(base.inv_inertia_a, math.add(math.cross(r_a, linear), angular)))) + state_a.w = math.narrow3(math.sub(math.widen3(state_a.w), math.mul_mv(math.widen_mat3(base.inv_inertia_a), math.add(math.cross(r_a, linear), angular)))) } if (state_b.flags & dynamics.DYNAMIC_FLAG) != 0 { state_b.v = math.narrow3(math.mul_add(math.widen3(state_b.v), base.inv_mass_b, linear)) - state_b.w = math.narrow3(math.add(math.widen3(state_b.w), math.mul_mv(base.inv_inertia_b, math.add(math.cross(r_b, linear), angular)))) + state_b.w = math.narrow3(math.add(math.widen3(state_b.w), math.mul_mv(math.widen_mat3(base.inv_inertia_b), math.add(math.cross(r_b, linear), angular)))) } } @@ -548,8 +548,8 @@ warm_start_motor_joint(base: *JointSim, ctx: *StepContext) { solve_motor_joint(base: *JointSim, ctx: *StepContext) { m_a = base.inv_mass_a m_b = base.inv_mass_b - i_a = base.inv_inertia_a - i_b = base.inv_inertia_b + i_a = math.widen_mat3(base.inv_inertia_a) + i_b = math.widen_mat3(base.inv_inertia_b) j = dynamics.motor_joint_of(base) state_a = state_of(ctx, j.index_a) state_b = state_of(ctx, j.index_b) @@ -723,15 +723,15 @@ warm_start_parallel_joint(base: *JointSim, ctx: *StepContext) { state_a = state_of(ctx, j.index_a) state_b = state_of(ctx, j.index_b) angular = math.blend2(j.perp_impulse.x, j.perp_axis_x, j.perp_impulse.y, j.perp_axis_y) - if (state_a.flags & dynamics.DYNAMIC_FLAG) != 0 { state_a.w = math.narrow3(math.sub(math.widen3(state_a.w), math.mul_mv(base.inv_inertia_a, angular))) } - if (state_b.flags & dynamics.DYNAMIC_FLAG) != 0 { state_b.w = math.narrow3(math.add(math.widen3(state_b.w), math.mul_mv(base.inv_inertia_b, angular))) } + if (state_a.flags & dynamics.DYNAMIC_FLAG) != 0 { state_a.w = math.narrow3(math.sub(math.widen3(state_a.w), math.mul_mv(math.widen_mat3(base.inv_inertia_a), angular))) } + if (state_b.flags & dynamics.DYNAMIC_FLAG) != 0 { state_b.w = math.narrow3(math.add(math.widen3(state_b.w), math.mul_mv(math.widen_mat3(base.inv_inertia_b), angular))) } } // The z axes kept collinear by a two-row spring on the perpendicular // axes, bounded by the maximum torque over the sub-step. solve_parallel_joint(base: *JointSim, ctx: *StepContext) { - i_a = base.inv_inertia_a - i_b = base.inv_inertia_b + i_a = math.widen_mat3(base.inv_inertia_a) + i_b = math.widen_mat3(base.inv_inertia_b) j = dynamics.parallel_joint_of(base) state_a = state_of(ctx, j.index_a) state_b = state_of(ctx, j.index_b) @@ -822,7 +822,7 @@ prepare_prismatic_joint(base: *JointSim, ctx: *StepContext) { j.frame_a = world_frame(bodies.sim_a, base.local_frame_a) j.frame_b = world_frame(bodies.sim_b, base.local_frame_b) j.delta_center = math.sub(bodies.sim_b.center, bodies.sim_a.center) - j.rotation_mass = math.invert_matrix(math.add_mm(base.inv_inertia_a, base.inv_inertia_b)) + j.rotation_mass = math.invert_matrix(math.add_mm(math.widen_mat3(base.inv_inertia_a), math.widen_mat3(base.inv_inertia_b))) matrix_a = math.make_matrix_from_quat(j.frame_a.q) j.joint_axis = matrix_a.cx j.perp_axis_y = matrix_a.cy @@ -862,9 +862,9 @@ warm_start_prismatic_joint(base: *JointSim, ctx: *StepContext) { l_a = math.add(math.blend3(axial, s_ax, j.perp_impulse.x, s_ay, j.perp_impulse.y, s_az), j.angular_impulse) l_b = math.add(math.blend3(axial, s_bx, j.perp_impulse.x, s_by, j.perp_impulse.y, s_bz), j.angular_impulse) v_a = math.mul_sub(math.widen3(state_a.v), base.inv_mass_a, p) - w_a = math.sub(math.widen3(state_a.w), math.mul_mv(base.inv_inertia_a, l_a)) + w_a = math.sub(math.widen3(state_a.w), math.mul_mv(math.widen_mat3(base.inv_inertia_a), l_a)) v_b = math.mul_add(math.widen3(state_b.v), base.inv_mass_b, p) - w_b = math.add(math.widen3(state_b.w), math.mul_mv(base.inv_inertia_b, l_b)) + w_b = math.add(math.widen3(state_b.w), math.mul_mv(math.widen_mat3(base.inv_inertia_b), l_b)) store_velocities(state_a, v_a, w_a, state_b, v_b, w_b) } @@ -875,8 +875,8 @@ warm_start_prismatic_joint(base: *JointSim, ctx: *StepContext) { solve_prismatic_joint(base: *JointSim, ctx: *StepContext, use_bias: bool) { m_a = base.inv_mass_a m_b = base.inv_mass_b - i_a = base.inv_inertia_a - i_b = base.inv_inertia_b + i_a = math.widen_mat3(base.inv_inertia_a) + i_b = math.widen_mat3(base.inv_inertia_b) j = dynamics.prismatic_joint_of(base) state_a = state_of(ctx, j.index_a) state_b = state_of(ctx, j.index_b) @@ -1156,7 +1156,7 @@ prepare_revolute_joint(base: *JointSim, ctx: *StepContext) { j.frame_a = world_frame(bodies.sim_a, base.local_frame_a) j.frame_b = world_frame(bodies.sim_b, base.local_frame_b) j.delta_center = math.sub(bodies.sim_b.center, bodies.sim_a.center) - inertia_sum = math.add_mm(base.inv_inertia_a, base.inv_inertia_b) + inertia_sum = math.add_mm(math.widen_mat3(base.inv_inertia_a), math.widen_mat3(base.inv_inertia_b)) axis_z = math.rotate_vector(j.frame_a.q, math.vec3_axis_z()) k = math.dot(axis_z, math.mul_mv(inertia_sum, axis_z)) j.axial_mass = 0.0 @@ -1189,9 +1189,9 @@ warm_start_revolute_joint(base: *JointSim, ctx: *StepContext) { angular = math.blend2(j.perp_impulse.x, j.perp_axis_x, j.perp_impulse.y, j.perp_axis_y) angular = math.mul_add(angular, axial, j.rotation_axis_z) v_a = math.mul_sub(math.widen3(state_a.v), base.inv_mass_a, j.linear_impulse) - w_a = math.sub(math.widen3(state_a.w), math.mul_mv(base.inv_inertia_a, math.add(math.cross(r_a, j.linear_impulse), angular))) + w_a = math.sub(math.widen3(state_a.w), math.mul_mv(math.widen_mat3(base.inv_inertia_a), math.add(math.cross(r_a, j.linear_impulse), angular))) v_b = math.mul_add(math.widen3(state_b.v), base.inv_mass_b, j.linear_impulse) - w_b = math.add(math.widen3(state_b.w), math.mul_mv(base.inv_inertia_b, math.add(math.cross(r_b, j.linear_impulse), angular))) + w_b = math.add(math.widen3(state_b.w), math.mul_mv(math.widen_mat3(base.inv_inertia_b), math.add(math.cross(r_b, j.linear_impulse), angular))) store_velocities(state_a, v_a, w_a, state_b, v_b, w_b) } @@ -1201,8 +1201,8 @@ warm_start_revolute_joint(base: *JointSim, ctx: *StepContext) { solve_revolute_joint(base: *JointSim, ctx: *StepContext, use_bias: bool) { m_a = base.inv_mass_a m_b = base.inv_mass_b - i_a = base.inv_inertia_a - i_b = base.inv_inertia_b + i_a = math.widen_mat3(base.inv_inertia_a) + i_b = math.widen_mat3(base.inv_inertia_b) j = dynamics.revolute_joint_of(base) state_a = state_of(ctx, j.index_a) state_b = state_of(ctx, j.index_b) @@ -1421,7 +1421,7 @@ prepare_spherical_joint(base: *JointSim, ctx: *StepContext) { j.frame_a = world_frame(bodies.sim_a, base.local_frame_a) j.frame_b = world_frame(bodies.sim_b, base.local_frame_b) j.delta_center = math.sub(bodies.sim_b.center, bodies.sim_a.center) - inertia_sum = math.add_mm(base.inv_inertia_a, base.inv_inertia_b) + inertia_sum = math.add_mm(math.widen_mat3(base.inv_inertia_a), math.widen_mat3(base.inv_inertia_b)) cone_axis = math.rotate_vector(j.frame_a.q, math.vec3_axis_z()) twist_axis = math.rotate_vector(j.frame_b.q, math.vec3_axis_z()) if j.enable_cone_limit { @@ -1465,9 +1465,9 @@ warm_start_spherical_joint(base: *JointSim, ctx: *StepContext) { r_b = math.rotate_vector(math.widen_quat(state_b.dq), j.frame_b.p) angular = spherical_angular_impulse(j) v_a = math.mul_sub(math.widen3(state_a.v), base.inv_mass_a, j.linear_impulse) - w_a = math.sub(math.widen3(state_a.w), math.mul_mv(base.inv_inertia_a, math.add(math.cross(r_a, j.linear_impulse), angular))) + w_a = math.sub(math.widen3(state_a.w), math.mul_mv(math.widen_mat3(base.inv_inertia_a), math.add(math.cross(r_a, j.linear_impulse), angular))) v_b = math.mul_add(math.widen3(state_b.v), base.inv_mass_b, j.linear_impulse) - w_b = math.add(math.widen3(state_b.w), math.mul_mv(base.inv_inertia_b, math.add(math.cross(r_b, j.linear_impulse), angular))) + w_b = math.add(math.widen3(state_b.w), math.mul_mv(math.widen_mat3(base.inv_inertia_b), math.add(math.cross(r_b, j.linear_impulse), angular))) store_velocities(state_a, v_a, w_a, state_b, v_b, w_b) } @@ -1477,8 +1477,8 @@ warm_start_spherical_joint(base: *JointSim, ctx: *StepContext) { solve_spherical_joint(base: *JointSim, ctx: *StepContext, use_bias: bool) { m_a = base.inv_mass_a m_b = base.inv_mass_b - i_a = base.inv_inertia_a - i_b = base.inv_inertia_b + i_a = math.widen_mat3(base.inv_inertia_a) + i_b = math.widen_mat3(base.inv_inertia_b) j = dynamics.spherical_joint_of(base) state_a = state_of(ctx, j.index_a) state_b = state_of(ctx, j.index_b) @@ -1737,7 +1737,7 @@ prepare_weld_joint(base: *JointSim, ctx: *StepContext) { j.frame_a = world_frame(bodies.sim_a, base.local_frame_a) j.frame_b = world_frame(bodies.sim_b, base.local_frame_b) j.delta_center = math.sub(bodies.sim_b.center, bodies.sim_a.center) - j.angular_mass = math.invert_matrix(math.add_mm(base.inv_inertia_a, base.inv_inertia_b)) + j.angular_mass = math.invert_matrix(math.add_mm(math.widen_mat3(base.inv_inertia_a), math.widen_mat3(base.inv_inertia_b))) if j.linear_hertz == 0.0 { j.linear_spring = base.constraint_softness } else { j.linear_spring = dynamics.make_soft(j.linear_hertz, j.linear_damping_ratio, ctx.h) } if j.angular_hertz == 0.0 { j.angular_spring = base.constraint_softness } else { j.angular_spring = dynamics.make_soft(j.angular_hertz, j.angular_damping_ratio, ctx.h) } if ctx.enable_warm_starting == false { @@ -1753,9 +1753,9 @@ warm_start_weld_joint(base: *JointSim, ctx: *StepContext) { r_a = math.rotate_vector(math.widen_quat(state_a.dq), j.frame_a.p) r_b = math.rotate_vector(math.widen_quat(state_b.dq), j.frame_b.p) v_a = math.mul_sub(math.widen3(state_a.v), base.inv_mass_a, j.linear_impulse) - w_a = math.sub(math.widen3(state_a.w), math.mul_mv(base.inv_inertia_a, math.add(math.cross(r_a, j.linear_impulse), j.angular_impulse))) + w_a = math.sub(math.widen3(state_a.w), math.mul_mv(math.widen_mat3(base.inv_inertia_a), math.add(math.cross(r_a, j.linear_impulse), j.angular_impulse))) v_b = math.mul_add(math.widen3(state_b.v), base.inv_mass_b, j.linear_impulse) - w_b = math.add(math.widen3(state_b.w), math.mul_mv(base.inv_inertia_b, math.add(math.cross(r_b, j.linear_impulse), j.angular_impulse))) + w_b = math.add(math.widen3(state_b.w), math.mul_mv(math.widen_mat3(base.inv_inertia_b), math.add(math.cross(r_b, j.linear_impulse), j.angular_impulse))) store_velocities(state_a, v_a, w_a, state_b, v_b, w_b) } @@ -1764,8 +1764,8 @@ warm_start_weld_joint(base: *JointSim, ctx: *StepContext) { solve_weld_joint(base: *JointSim, ctx: *StepContext, use_bias: bool) { m_a = base.inv_mass_a m_b = base.inv_mass_b - i_a = base.inv_inertia_a - i_b = base.inv_inertia_b + i_a = math.widen_mat3(base.inv_inertia_a) + i_b = math.widen_mat3(base.inv_inertia_b) j = dynamics.weld_joint_of(base) state_a = state_of(ctx, j.index_a) state_b = state_of(ctx, j.index_b) @@ -1863,7 +1863,7 @@ prepare_wheel_joint(base: *JointSim, ctx: *StepContext) { j.frame_a = world_frame(bodies.sim_a, base.local_frame_a) j.frame_b = world_frame(bodies.sim_b, base.local_frame_b) j.delta_center = math.sub(bodies.sim_b.center, bodies.sim_a.center) - inertia_sum = math.add_mm(base.inv_inertia_a, base.inv_inertia_b) + inertia_sum = math.add_mm(math.widen_mat3(base.inv_inertia_a), math.widen_mat3(base.inv_inertia_b)) r_a = j.frame_a.p r_b = j.frame_b.p matrix_a = math.make_matrix_from_quat(j.frame_a.q) @@ -1871,7 +1871,7 @@ prepare_wheel_joint(base: *JointSim, ctx: *StepContext) { suspension_axis = matrix_a.cx r_an = math.cross(r_a, suspension_axis) r_bn = math.cross(r_b, suspension_axis) - k = base.inv_mass_a + base.inv_mass_b + math.dot(r_an, math.mul_mv(base.inv_inertia_a, r_an)) + math.dot(r_bn, math.mul_mv(base.inv_inertia_b, r_bn)) + k = base.inv_mass_a + base.inv_mass_b + math.dot(r_an, math.mul_mv(math.widen_mat3(base.inv_inertia_a), r_an)) + math.dot(r_bn, math.mul_mv(math.widen_mat3(base.inv_inertia_b), r_bn)) j.suspension_mass = 0.0 if k > 0.0 { j.suspension_mass = 1.0 / k } j.suspension_softness = dynamics.make_soft(j.suspension_hertz, j.suspension_damping_ratio, ctx.h) @@ -1946,11 +1946,11 @@ warm_start_wheel_joint(base: *JointSim, ctx: *StepContext) { angular = wheel_angular_impulse(j, quat_a, quat_b, matrix_a, matrix_b) if (state_a.flags & dynamics.DYNAMIC_FLAG) != 0 { state_a.v = math.narrow3(math.mul_sub(math.widen3(state_a.v), base.inv_mass_a, linear)) - state_a.w = math.narrow3(math.sub(math.widen3(state_a.w), math.mul_mv(base.inv_inertia_a, math.add(angular_a, angular)))) + state_a.w = math.narrow3(math.sub(math.widen3(state_a.w), math.mul_mv(math.widen_mat3(base.inv_inertia_a), math.add(angular_a, angular)))) } if (state_b.flags & dynamics.DYNAMIC_FLAG) != 0 { state_b.v = math.narrow3(math.mul_add(math.widen3(state_b.v), base.inv_mass_b, linear)) - state_b.w = math.narrow3(math.add(math.widen3(state_b.w), math.mul_mv(base.inv_inertia_b, math.add(angular_b, angular)))) + state_b.w = math.narrow3(math.add(math.widen3(state_b.w), math.mul_mv(math.widen_mat3(base.inv_inertia_b), math.add(angular_b, angular)))) } } @@ -1962,8 +1962,8 @@ warm_start_wheel_joint(base: *JointSim, ctx: *StepContext) { solve_wheel_joint(base: *JointSim, ctx: *StepContext, use_bias: bool) { m_a = base.inv_mass_a m_b = base.inv_mass_b - i_a = base.inv_inertia_a - i_b = base.inv_inertia_b + i_a = math.widen_mat3(base.inv_inertia_a) + i_b = math.widen_mat3(base.inv_inertia_b) j = dynamics.wheel_joint_of(base) state_a = state_of(ctx, j.index_a) state_b = state_of(ctx, j.index_b) From 3adcc527a5d5719b117d2b0567c51d4ca70a3a2f Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Thu, 8 Oct 2026 15:31:31 -0300 Subject: [PATCH 2/2] Joint grid warm start: float joint inertias, no zero angular sum for a plain point (#121) --- README.md | 2 +- aephysics/joint_solver/module.ae | 5 +++- bench/RESULTS.md | 48 ++++++++++++++++++++++++++++++++ tests/test_joint_solver.ae | 25 +++++++++++++++++ 4 files changed, 78 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index d225abd..4b95068 100644 --- a/README.md +++ b/README.md @@ -49,7 +49,7 @@ so a test written against the reference reads the same here. | `aephysics.contact_solver_wide` | a colour's convex contacts eight at a time: the constraint of eight contacts as one single-precision record in lanes (`f32x8`, std.lanes -- the reference's SIMD path; one AVX2 register with `AETHER_AEPHYSICS_CFLAGS=-mavx2`, two four-lane halves without), the same prepare, warm start, solve, restitution and store, the bodies gathered into lanes and scattered back | `test_contact_solver_wide.ae` (one scene two ways -- the stack, the bounce with hit events, rolling resistance, a conveyor, a slide -- the lanes within a millimetre of the scalar solve, and the same lanes twice bit for bit) | [the falling grid: scalar 241 ms, lanes 142, the reference 155](bench/RESULTS.md#solver) | | `aephysics.joint_solver` | the seven joints solved: each kind's prepare (frames relative to the centres of mass, effective masses, spring softness), warm start and solve (rigid or soft constraints, speculative limits, springs, motors), the kinds' accessors (limits, springs, motors, current angles and translations, forces and torques), joint.c's dispatch with the constraint hertz clamp; beyond the reference, a spherical joint's cone may be centred off frame A's z and may be an ellipse of two half-angles (the circle stays the default, bit for bit) | `test_joint_solver.ae` (181 checks: test_joint.c's accessors on every kind, each kind solved by hand) | [ours alone](bench/RESULTS.md#joint_solver) | | `aephysics.solver` | the Soft Step over the workers, the reference's stages: the work in blocks (the bodies', each colour's joints, wide and scalar constraints) that workers claim by compare-and-swap, the stages published as sync bits by worker 0 -- the constraints prepared, per sub-step the velocities integrated (gravity, damping, the gyroscopic torque), warm start, solve, positions, relax, colour by colour with the overflow first on worker 0; restitution, the impulses stored; the bodies finalised in parallel (sleep velocities, move events, fast bodies swept for the time of impact, bounds and proxies), the joint and hit events, the trees refit, bullets, the sensors' hits, islands put to sleep | `test_solver.ae` (45 checks: free fall against the closed form, resting and sleeping, a stack, a bounce and its hit event, a joint event, a pendulum, a fast sphere stopped by a thin wall, a bullet, a sensor swept, two worlds bit for bit alike) | [1.2-1.3x the reference's step, stage by stage](bench/RESULTS.md#stage-by-stage) | -| `aephysics.physics_world` | the world's face: the step (pairs, narrow phase, solve, sensors, events), the events read back, the settings and counters, queries over every shape (overlap of a box or a proxy, the mover's planes, ray, shape and mover casts, the closest ray hit) and against one body at a transform of the caller's, explosions | `test_physics_world.ae` (118 checks: test_world.c's HelloWorld, contact, hit, move and sensor events, the explosion near and far; test_body_query.c's casts, overlaps, mover planes and time of impact; the world queries; a wave pile stepping alike twice) | [the reference's own benchmark scenes with the same checksums: 1.25x its AVX2 build on the large pyramid, 1.2x on the many pyramids, 1.1x on the joint grid, ours at eight lanes in AVX2 with float records (#121)](bench/RESULTS.md#body-sims-in-float-2026-10-07-121-37) | +| `aephysics.physics_world` | the world's face: the step (pairs, narrow phase, solve, sensors, events), the events read back, the settings and counters, queries over every shape (overlap of a box or a proxy, the mover's planes, ray, shape and mover casts, the closest ray hit) and against one body at a transform of the caller's, explosions | `test_physics_world.ae` (118 checks: test_world.c's HelloWorld, contact, hit, move and sensor events, the explosion near and far; test_body_query.c's casts, overlaps, mover planes and time of impact; the world queries; a wave pile stepping alike twice) | [the reference's own benchmark scenes with the same checksums: 1.25x its AVX2 build on the large pyramid, 1.2x on the many pyramids, 1.05x on the joint grid, ours at eight lanes in AVX2 with float records (#121)](bench/RESULTS.md#body-sims-in-float-2026-10-07-121-37) | | `aephysics.human` | the ragdoll: twelve capsule bones on spherical joints with cone and twist limits and revolute joints with angle limits -- a person's ranges by default (shoulders that reach overhead on a cone centred off the rest, hips on an elliptical cone through 125 degrees ahead, 25 back, 45 out and 30 across, knees and elbows that never pass straight), the reference's tight ones on request -- soles that grip, a spring on every joint toward the reference pose, a motor whose torque limit is joint friction, filter joints for the limbs that clash; the align spring, kinematic anchors through motor or parallel joints and their targets driven over a step (the pose drive of an active ragdoll), velocity, kicks, bullets | `test_human.ae` (96 checks: the figure's shape, a fall to rest in one piece, the setters, a kick, standing under the align spring, the pose held on anchors, the anchors driven two metres and the figure walked behind them, the same drop twice bit for bit, a fitted shape, each arm reaching flexion 170, abduction 160 and extension 45 degrees standing and prone, the hips' cone through a person's four ranges and short of a split, the hinges stopping at straight and folding to their range, the soles' grip) | [the reference's rain benchmark, 300 ragdolls over 400 steps, at 0.9x](bench/RESULTS.md#human) | | `aephysics` | the public API | | diff --git a/aephysics/joint_solver/module.ae b/aephysics/joint_solver/module.ae index 853521e..5094c47 100644 --- a/aephysics/joint_solver/module.ae +++ b/aephysics/joint_solver/module.ae @@ -1463,7 +1463,10 @@ warm_start_spherical_joint(base: *JointSim, ctx: *StepContext) { state_b = state_of(ctx, j.index_b) r_a = math.rotate_vector(math.widen_quat(state_a.dq), j.frame_a.p) r_b = math.rotate_vector(math.widen_quat(state_b.dq), j.frame_b.p) - angular = spherical_angular_impulse(j) + // The angular impulses are each zeroed when their part is off, so a + // plain point (no spring, motor or limit) has none to give back. + angular = math.vec3_zero() + if j.enable_spring || j.enable_motor || j.enable_cone_limit || j.enable_twist_limit { angular = spherical_angular_impulse(j) } v_a = math.mul_sub(math.widen3(state_a.v), base.inv_mass_a, j.linear_impulse) w_a = math.sub(math.widen3(state_a.w), math.mul_mv(math.widen_mat3(base.inv_inertia_a), math.add(math.cross(r_a, j.linear_impulse), angular))) v_b = math.mul_add(math.widen3(state_b.v), base.inv_mass_b, j.linear_impulse) diff --git a/bench/RESULTS.md b/bench/RESULTS.md index a933947..31d01c7 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -799,6 +799,54 @@ reference's algorithm in double). After this morning's #124, #126, #127 and this, the reference's three scenes stand at 1.25x, 1.2x and 1.1x its AVX2 build, from 1.8x, 1.8x and 1.2x. +### The joint grid's warm start (2026-10-08, #121) + +The joint grid's solve and relax were already ahead of the reference's; +its gap was the warm start and the prepare. Two changes, neither of which +moves a bit of any result: + +- `JointSim` keeps the bodies' world inertias in float, as the body sims + hold them, and the joint solvers widen them where they read them. They + were floats widened at the prepare, so nothing rounds differently, and + the record shrinks from 912 bytes to 840. +- The spherical joint's warm start adds its angular impulses (the + spring's, the motor's, the cone's and the twist limits') only when one + of those parts is on. Each is zeroed when its part is off, so the sum + it skips is exactly zero; a plain point joint, the grid's, no longer + reads the 120 bytes of impulses and axes behind it. + +Pinned to one core, 8 runs each interleaved, ms per step: + +| | min | median | +|---|---|---| +| main | 11.23 | 11.91 | +| this change | 10.78 | 10.90 | +| Box3D `e77352c`, AVX2 | 10.21 | 10.34 | + +Stage by stage, three rounds: + +| stage | main | this change | Box3D | +|---|---|---|---| +| prepare joints | 1.10-1.23 | 0.93-0.95 | 0.51-0.58 | +| integrate velocities | 1.21-1.25 | 1.20-1.22 | 1.09 | +| warm start | 1.97-2.23 | 1.37-1.40 | 1.17-1.26 | +| solve | 3.23-3.48 | 3.14-3.19 | 3.32-3.34 | +| integrate positions | 0.30-0.31 | 0.30-0.31 | 0.20 | +| relax | 3.12-3.36 | 3.06-3.11 | 3.22-3.23 | + +The grid stands at 1.05x the reference, from 1.1x. Two more things were +tried and dropped. Keeping the spherical joint's frames, impulses, axes +and masses in float, as the reference does (a 664-byte record), took the +warm start only to 1.5-1.6 ms and cost the solve and the relax 0.3 ms +each: the solve is bound by its chain from the impulse to the +velocities, and that chain gains a narrowing and a widening every +iteration. Computing the limits' axes only for a joint with a limit +gained nothing, since GCC already sinks them. What remains (the prepare, +the two integrations, a fifth of the warm start) is the arithmetic in +double over float records, each read widened and each store narrowed, +where the reference computes in float throughout; a line-level sample +of the warm start puts a fifth of it in those conversions. + ### The joint grid, stage by stage (2026-10-05) The joint grid has no contacts, so the eight-wide lanes don't explain its diff --git a/tests/test_joint_solver.ae b/tests/test_joint_solver.ae index 6af96cd..289a84a 100644 --- a/tests/test_joint_solver.ae +++ b/tests/test_joint_solver.ae @@ -611,6 +611,31 @@ test_spherical() { set_velocity(&f, math.vec3_zero(), math.vec3_zero()) solve_biased(&f, id) ensure("twist: pushed back (${math.widen3(state.w).z})", math.widen3(state.w).z < 0.0 - 0.1) + // Every angular part off again (the motor and the cone above, the twist + // here, a spring after one step): each zeroes its impulse as it goes + // off, so the angular sum the warm start skips for a plain point is + // exactly zero. + j = dynamics.spherical_joint_of(sim(&f, id)) + ensure("twist: an impulse to drop", j.lower_twist_impulse > 0.0 || j.upper_twist_impulse > 0.0) + joint_solver.spherical_joint_enable_twist_limit(id, false) + joint_solver.spherical_joint_enable_spring(id, true) + joint_solver.spherical_joint_set_spring_hertz(id, 2.0) + set_velocity(&f, math.vec3_zero(), math.vec3(1.0, 0.0, 0.0)) + solve_biased(&f, id) + ensure("spring: an impulse to drop", math.length(j.spring_impulse) > 0.0) + joint_solver.spherical_joint_enable_spring(id, false) + ensure("every angular part off: no angular impulse left to warm-start", + math.length(j.spring_impulse) == 0.0 && math.length(j.motor_impulse) == 0.0 && j.swing_impulse == 0.0 && + j.lower_twist_impulse == 0.0 && j.upper_twist_impulse == 0.0) + // The warm start of the plain point gives back the point's impulse + // alone: the cube, held at its centre, moves by it and does not turn. + linear = j.linear_impulse + ensure("plain point: an impulse to give back", math.length(linear) > 0.0) + set_velocity(&f, math.vec3_zero(), math.vec3_zero()) + joint_solver.warm_start_joint(sim(&f, id), &f.ctx) + v = math.narrow3(math.mul_sv(sim(&f, id).inv_mass_b, linear)) + ensure("plain point: the warm start moves the cube by the point's impulse", state.v.x == v.x && state.v.y == v.y && state.v.z == v.z) + ensure("plain point: and does not turn it", state.w.x == 0.0 && state.w.y == 0.0 && state.w.z == 0.0) dynamics.destroy_world(f.world_id) }