// Copyright 2025 International Digital Economy Academy
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
///|
/// A Rapier-style sequential impulse contact solver (2D) built from NarrowPhase contact manifolds.
/// It supports:
/// - multiple contact points per manifold (e.g. cuboid-cuboid clipping)
/// - restitution + Coulomb friction
/// - warmstarting through a per-pipeline cache keyed by collider handles + feature ids
///
/// It follows Rapier's staged constraint-set flow (`init`/`update`/`warmstart`/`solve`/
/// `solve_wo_bias`/`writeback_impulses`) in scalar 2D form.
///|
pub struct ContactImpulseCacheEntry {
collider1 : @collision.ColliderHandle
collider2 : @collision.ColliderHandle
fid1 : @collision.FeatureId
fid2 : @collision.FeatureId
impulse_n : @core.Real
impulse_t : @core.Real
}
///|
fn solver_vec2_scale(v : @core.Vec2, s : @core.Real) -> @core.Vec2 {
Vec2(v.x * s, v.y * s)
}
///|
fn cross_z_scalar(w : @core.Real, v : @core.Vec2) -> @core.Vec2 {
Vec2(-w * v.y, w * v.x)
}
///|
fn cross_vec2(a : @core.Vec2, b : @core.Vec2) -> @core.Real {
a.x * b.y - a.y * b.x
}
///|
fn solver_clamp_value(
value : @core.Real,
min_val : @core.Real,
max_val : @core.Real,
) -> @core.Real {
if value < min_val {
min_val
} else if value > max_val {
max_val
} else {
value
}
}
///|
fn normalize_or_fallback(v : @core.Vec2, fallback : @core.Vec2) -> @core.Vec2 {
if v.dot(v) <= 1.0e-12F {
fallback
} else {
v.normalize()
}
}
///|
fn solver_collider_handle_equals(
a : @collision.ColliderHandle,
b : @collision.ColliderHandle,
) -> Bool {
@collision.ColliderHandle::equals(a, b)
}
///|
fn feature_id_equals(
a : @collision.FeatureId,
b : @collision.FeatureId,
) -> Bool {
match (a, b) {
(Unknown, Unknown) => true
(Vertex(i), Vertex(j)) => i == j
(Edge(i), Edge(j)) => i == j
(Face(i), Face(j)) => i == j
_ => false
}
}
///|
fn cache_key_equals(
entry : ContactImpulseCacheEntry,
collider1 : @collision.ColliderHandle,
collider2 : @collision.ColliderHandle,
fid1 : @collision.FeatureId,
fid2 : @collision.FeatureId,
) -> Bool {
solver_collider_handle_equals(entry.collider1, collider1) &&
solver_collider_handle_equals(entry.collider2, collider2) &&
feature_id_equals(entry.fid1, fid1) &&
feature_id_equals(entry.fid2, fid2)
}
///|
fn cache_find(
cache : Array[ContactImpulseCacheEntry],
collider1 : @collision.ColliderHandle,
collider2 : @collision.ColliderHandle,
fid1 : @collision.FeatureId,
fid2 : @collision.FeatureId,
) -> ContactImpulseCacheEntry? {
for i in 0.. Unit {
if handle is Some(h) {
let idx = h.id
let (inv_m, inv_i) = get_body_inv_mass_inertia(
bodies, solver_bodies, handle,
)
if inv_m == 0.0F && inv_i == 0.0F {
return
}
if idx >= 0 && idx < solver_bodies.vels.length() {
let vel = solver_bodies.vels[idx]
vel.linear = vel.linear.add(solver_vec2_scale(impulse, inv_m))
let com = solver_bodies.poses[idx].translation
let r = point.sub(com)
let torque_impulse = cross_vec2(r, impulse)
vel.angular = vel.angular + torque_impulse * inv_i
solver_bodies.vels[idx] = vel
// Fallback for rare handle/id mismatch.
} else if bodies.get_mut(h) is Some(body) {
if body.is_dynamic() && body.is_enabled() {
body.set_linvel(
body.linvel().add(solver_vec2_scale(impulse, inv_m)),
true,
)
|> ignore
let r = point.sub(body.world_com())
let torque_impulse = cross_vec2(r, impulse)
body.set_angvel(body.angvel() + torque_impulse * inv_i, true) |> ignore
}
}
}
}
///|
priv struct SolverConstraint {
collider1 : @collision.ColliderHandle
collider2 : @collision.ColliderHandle
fid1 : @collision.FeatureId
fid2 : @collision.FeatureId
body1 : @dynamics.RigidBodyHandle?
body2 : @dynamics.RigidBodyHandle?
// Direction of the force/impulse applied to body1 (Rapier: -manifold.normal).
dir1 : @core.Vec2
tangent1 : @core.Vec2
// Target relative velocity along `tangent1` (Rapier's SolverContact.tangent_velocity projected).
tangent_velocity : @core.Real
world_p1 : @core.Vec2
world_p2 : @core.Vec2
mut dist : @core.Real
restitution : @core.Real
mut restitution_vel : @core.Real
friction : @core.Real
mut inv_mass_n : @core.Real
mut inv_mass_n_nocfm : @core.Real
mut inv_mass_t : @core.Real
mut rhs_bias : @core.Real
mut impulse_n : @core.Real
mut impulse_t : @core.Real
}
///|
priv struct SolverManifold {
start : Int
len : Int
}
///|
fn build_constraints(
parameters : IntegrationParameters,
bodies : @dynamics.RigidBodySet,
colliders : @collision.ColliderSet,
hooks : PhysicsHooks,
narrow_phase : @collision.NarrowPhase,
dt : @core.Real,
cache : Array[ContactImpulseCacheEntry],
warmstart_coefficient : @core.Real,
island_stamps : Array[Int]?,
stamp : Int,
) -> (Array[SolverConstraint], Array[SolverManifold]) {
let allowed_error = parameters.allowed_linear_error()
let max_corrective_velocity = parameters.max_corrective_velocity()
let erp_inv_dt = parameters.contact_softness.erp_inv_dt(dt)
let cfm_coeff = parameters.contact_softness.cfm_coeff(dt)
let pairs = narrow_phase.contact_pairs()
let constraints : Array[SolverConstraint] = []
let solver_manifolds : Array[SolverManifold] = []
for i in 0..= 0 && id < stamps.length() && stamps[id] == stamp {
in_island = true
}
}
if body2 is Some(bh2) {
let id = bh2.id
if id >= 0 && id < stamps.length() && stamps[id] == stamp {
in_island = true
}
}
if !in_island {
continue
}
}
// Like Rapier: only solve if at least one body is dynamic.
let mut any_dynamic = false
if body1 is Some(bh1) {
if bodies.get(bh1) is Some(rb1) {
if rb1.is_dynamic() && rb1.is_enabled() {
any_dynamic = true
}
}
}
if body2 is Some(bh2) {
if bodies.get(bh2) is Some(rb2) {
if rb2.is_dynamic() && rb2.is_enabled() {
any_dynamic = true
}
}
}
if !any_dynamic {
continue
}
let friction = @dynamics.CoefficientCombineRule::combine(
c1.friction(),
c2.friction(),
c1.friction_combine_rule(),
c2.friction_combine_rule(),
)
let restitution = @dynamics.CoefficientCombineRule::combine(
c1.restitution(),
c2.restitution(),
c1.restitution_combine_rule(),
c2.restitution_combine_rule(),
)
// For some manifolds (segment-* and capsule-*), we may not have a negative distance for
// penetrations. Use the separation depth as a fallback to preserve depenetration behavior.
let sep = separation_vector(
c1.shape(),
c1.translation(),
c1.rotation(),
c2.shape(),
c2.translation(),
c2.rotation(),
)
let sep_vec = if sep is Some(v) { v } else { @core.Vec2::zero() }
let sep_depth = sep_vec.length()
let sep_dir1 = if sep_depth > 1.0e-12F {
// sep points from collider2 toward collider1, so it matches dir1 convention.
sep_vec.normalize()
} else {
@core.Vec2::zero()
}
let contact_manifolds = pair.2.manifolds()
for mi in 0.. 0.0F { sep_dir1 } else { dir1_from_manifold }
let tangent1 = @core.Vec2(-dir1.y, dir1.x)
let has_modify_hook = c1
.active_hooks()
.contains(@collision.ActiveHooks::modify_solver_contacts()) ||
c2
.active_hooks()
.contains(@collision.ActiveHooks::modify_solver_contacts())
let mut legacy_tangent_velocity = @core.Vec2::zero()
if has_modify_hook {
let (accept, tv) = hooks.modify_solver_contacts_internal(
bodies, colliders, h1, h2, manifold,
)
if !accept {
continue
}
legacy_tangent_velocity = tv
}
fn push_constraint(
fid1 : @collision.FeatureId,
fid2 : @collision.FeatureId,
world_p1 : @core.Vec2,
world_p2 : @core.Vec2,
dist_in : @core.Real,
friction_in : @core.Real,
restitution_in : @core.Real,
tangent_velocity_proj : @core.Real,
) -> Unit {
let mut dist = dist_in
// Some manifold generators (notably segment-*) only provide a non-negative distance.
// When we have an MTV from `separation_vector`, use it to recover a penetration depth.
if has_segment && sep_depth > 0.0F && dist >= 0.0F {
dist = -sep_depth
}
// A small velocity-level bias is needed so overlapping objects separate even when their
// relative velocity is zero (this also drives contact-force events).
let rhs_bias0 = solver_clamp_value(
erp_inv_dt * (dist + allowed_error),
-max_corrective_velocity,
0.0F,
)
let mut inv_mass1 = 0.0F
let mut inv_inertia1 = 0.0F
let mut v1 = @core.Vec2::zero()
let mut w1 = 0.0F
let mut com1 = @core.Vec2::zero()
if body1 is Some(bh1) {
if bodies.get(bh1) is Some(rb1) {
v1 = rb1.linvel()
w1 = rb1.angvel()
com1 = rb1.world_com()
if rb1.is_dynamic() && rb1.is_enabled() {
inv_mass1 = if rb1.locked_translations {
0.0F
} else {
rb1.inv_mass()
}
inv_inertia1 = if rb1.locked_rotations {
0.0F
} else {
rb1.inv_inertia()
}
}
}
}
let mut inv_mass2 = 0.0F
let mut inv_inertia2 = 0.0F
let mut v2 = @core.Vec2::zero()
let mut w2 = 0.0F
let mut com2 = @core.Vec2::zero()
if body2 is Some(bh2) {
if bodies.get(bh2) is Some(rb2) {
v2 = rb2.linvel()
w2 = rb2.angvel()
com2 = rb2.world_com()
if rb2.is_dynamic() && rb2.is_enabled() {
inv_mass2 = if rb2.locked_translations {
0.0F
} else {
rb2.inv_mass()
}
inv_inertia2 = if rb2.locked_rotations {
0.0F
} else {
rb2.inv_inertia()
}
}
}
}
// Effective mass denominators (2D).
let r1 = world_p1.sub(com1)
let r2 = world_p2.sub(com2)
let rt1 = cross_vec2(r1, tangent1)
let rt2 = cross_vec2(r2, tangent1)
let rn1 = cross_vec2(r1, dir1)
let rn2 = cross_vec2(r2, dir1)
let denom_n = inv_mass1 +
inv_mass2 +
inv_inertia1 * rn1 * rn1 +
inv_inertia2 * rn2 * rn2
let denom_t = inv_mass1 +
inv_mass2 +
inv_inertia1 * rt1 * rt1 +
inv_inertia2 * rt2 * rt2
let inv_mass_n_nocfm = if denom_n <= 1.0e-12F {
0.0F
} else {
1.0F / denom_n
}
let inv_mass_n = if denom_n + cfm_coeff <= 1.0e-12F {
0.0F
} else {
1.0F / (denom_n + cfm_coeff)
}
let inv_mass_t = if denom_t <= 1.0e-12F {
0.0F
} else {
1.0F / denom_t
}
let cached = cache_find(cache, h1, h2, fid1, fid2)
let impulse_n = if cached is Some(e) {
e.impulse_n * warmstart_coefficient
} else {
0.0F
}
let impulse_t = if cached is Some(e) {
e.impulse_t * warmstart_coefficient
} else {
0.0F
}
// Restitution is applied as a velocity target based on the pre-solve normal speed,
// like Rapier (use the initial impact speed, not the iterative vn which changes).
let vel1 = v1.add(cross_z_scalar(w1, r1))
let vel2 = v2.add(cross_z_scalar(w2, r2))
let rv = vel1.sub(vel2)
let vn0 = rv.dot(dir1)
let restitution_vel = if dist <= 0.0F && vn0 < -1.0e-3F {
-restitution_in * vn0
} else {
0.0F
}
// We keep the bias smaller than Rapier's default spring model because we also apply
// explicit position corrections; a full split-impulse solver would be ideal, but this
// balance avoids the high-mass-ratio "launch" instability while still resolving overlaps.
let rhs_bias = rhs_bias0 * 0.2F
constraints.push({
collider1: h1,
collider2: h2,
fid1,
fid2,
body1,
body2,
dir1,
tangent1,
tangent_velocity: tangent_velocity_proj,
world_p1,
world_p2,
dist,
restitution: restitution_in,
restitution_vel,
friction: friction_in,
inv_mass_n,
inv_mass_n_nocfm,
inv_mass_t,
rhs_bias,
impulse_n,
impulse_t,
})
// Precomputed vels are not stored, but this keeps the constraint build close to Rapier.
v1 |> ignore
w1 |> ignore
v2 |> ignore
w2 |> ignore
}
let points = manifold.points()
if points.length() == 0 {
continue
}
let world_p1s : Array[@core.Vec2] = []
let world_p2s : Array[@core.Vec2] = []
let fids : Array[(@collision.FeatureId, @collision.FeatureId)] = []
let solver_contacts : Array[@collision.SolverContact] = []
for pi in 0.. 0 {
solver_manifolds.push({ start, len })
}
}
}
}
(constraints, solver_manifolds)
}
///|
fn warmstart(
bodies : @dynamics.RigidBodySet,
solver_bodies : SolverBodies,
constraints : Array[SolverConstraint],
) -> Unit {
for i in 0.. (@core.Vec2, @core.Real, @core.Vec2) {
if handle is Some(h) {
let idx = h.id
if bodies.get(h) is Some(rb) {
let com = if idx >= 0 && idx < solver_bodies.poses.length() {
solver_bodies.poses[idx].translation
} else {
rb.world_com()
}
let (v, w) = if idx >= 0 && idx < solver_bodies.vels.length() {
let vel = solver_bodies.vels[idx]
(vel.linear, vel.angular)
} else {
(rb.linvel(), rb.angvel())
}
(v, w, com)
} else {
(@core.Vec2::zero(), 0.0F, @core.Vec2::zero())
}
} else {
(@core.Vec2::zero(), 0.0F, @core.Vec2::zero())
}
}
///|
fn get_body_inv_mass_inertia(
bodies : @dynamics.RigidBodySet,
solver_bodies : SolverBodies,
handle : @dynamics.RigidBodyHandle?,
) -> (@core.Real, @core.Real) {
if handle is Some(h) {
if bodies.get(h) is Some(rb) {
if rb.is_dynamic() && rb.is_enabled() {
let idx = h.id
let (im, ii) = if idx >= 0 && idx < solver_bodies.poses.length() {
let p = solver_bodies.poses[idx]
// For dim2, effective inv mass is uniform across axes.
(p.im.x, p.ii)
} else {
(rb.inv_mass(), rb.inv_inertia())
}
let inv_m = if rb.locked_translations { 0.0F } else { im }
let inv_i = if rb.locked_rotations { 0.0F } else { ii }
(inv_m, inv_i)
} else {
(0.0F, 0.0F)
}
} else {
(0.0F, 0.0F)
}
} else {
(0.0F, 0.0F)
}
}
///|
fn update_contact_constraints(
parameters : IntegrationParameters,
small_step_id : Int,
bodies : @dynamics.RigidBodySet,
solver_bodies : SolverBodies,
constraints : Array[SolverConstraint],
) -> Unit {
small_step_id |> ignore
let allowed_error = parameters.allowed_linear_error()
let max_corrective_velocity = parameters.max_corrective_velocity()
let erp_inv_dt = parameters.contact_softness.erp_inv_dt(parameters.dt)
let cfm_coeff = parameters.contact_softness.cfm_coeff(parameters.dt)
for i in 0.. Unit {
if dt <= 0.0F {
return
}
let inv_dt = 1.0F / dt
let iters = if iterations <= 0 { 1 } else { iterations }
for _ in 0.. 0.0F { c1.dist * inv_dt } else { 0.0F }) +
c1.rhs_bias
let bias2 = (if c2.dist > 0.0F { c2.dist * inv_dt } else { 0.0F }) +
c2.rhs_bias
let b1 = vn1 + bias1
let b2 = vn2 + bias2
// Approximate Rapier's CFM by recovering the diagonal term from inv_mass_n (which already
// includes the `cfm_coeff` contribution).
fn cfm_diag(c : SolverConstraint) -> @core.Real {
if c.inv_mass_n <= 1.0e-12F || c.inv_mass_n_nocfm <= 1.0e-12F {
0.0F
} else {
let d_cfm = 1.0F / c.inv_mass_n
let d0 = 1.0F / c.inv_mass_n_nocfm
let cfm = d_cfm - d0
if cfm > 0.0F {
cfm
} else {
0.0F
}
}
}
let rn1a = cross_vec2(r1a, n)
let rn2a = cross_vec2(r2a, n)
let rn1b = cross_vec2(r1b, n)
let rn2b = cross_vec2(r2b, n)
let k_base = inv_m1 + inv_m2
let k11 = k_base +
inv_i1 * rn1a * rn1a +
inv_i2 * rn2a * rn2a +
cfm_diag(c1)
let k22 = k_base +
inv_i1 * rn1b * rn1b +
inv_i2 * rn2b * rn2b +
cfm_diag(c2)
let k12 = k_base + inv_i1 * rn1a * rn1b + inv_i2 * rn2a * rn2b
let old1 = constraints[i1].impulse_n
let old2 = constraints[i2].impulse_n
let eps = 1.0e-12F
let tol = -1.0e-6F
let mut new1 = old1
let mut new2 = old2
let mut delta1 = 0.0F
let mut delta2 = 0.0F
let mut solved = false
// Case 1: solve both points.
let det = k11 * k22 - k12 * k12
if det > eps {
let inv_det = 1.0F / det
let inv11 = k22 * inv_det
let inv12 = -k12 * inv_det
let inv22 = k11 * inv_det
delta1 = -(inv11 * b1 + inv12 * b2)
delta2 = -(inv12 * b1 + inv22 * b2)
new1 = old1 + delta1
new2 = old2 + delta2
if new1 >= 0.0F && new2 >= 0.0F {
solved = true
}
}
// Case 2: new1 = 0.
if !solved {
let d1 = -old1
let d2 = if k22 > eps { -(b2 + k12 * d1) / k22 } else { 0.0F }
let n2 = old2 + d2
let vn1p = b1 + k11 * d1 + k12 * d2
if n2 >= 0.0F && vn1p >= tol {
new1 = 0.0F
new2 = n2
delta1 = d1
delta2 = d2
solved = true
}
}
// Case 3: new2 = 0.
if !solved {
let d2 = -old2
let d1 = if k11 > eps { -(b1 + k12 * d2) / k11 } else { 0.0F }
let n1 = old1 + d1
let vn2p = b2 + k12 * d1 + k22 * d2
if n1 >= 0.0F && vn2p >= tol {
new1 = n1
new2 = 0.0F
delta1 = d1
delta2 = d2
solved = true
}
}
// Case 4: new1 = new2 = 0.
if !solved {
let d1 = -old1
let d2 = -old2
let vn1p = b1 + k11 * d1 + k12 * d2
let vn2p = b2 + k12 * d1 + k22 * d2
if vn1p >= tol && vn2p >= tol {
new1 = 0.0F
new2 = 0.0F
delta1 = d1
delta2 = d2
solved = true
}
}
if solved {
constraints[i1].impulse_n = new1
constraints[i2].impulse_n = new2
let impulse1 = solver_vec2_scale(n, delta1)
let impulse2 = solver_vec2_scale(n, delta2)
apply_impulse_delta_dynamic(
bodies,
solver_bodies,
c1.body1,
impulse1,
c1.world_p1,
)
apply_impulse_delta_dynamic(
bodies,
solver_bodies,
c1.body2,
Vec2(-impulse1.x, -impulse1.y),
c1.world_p2,
)
apply_impulse_delta_dynamic(
bodies,
solver_bodies,
c2.body1,
impulse2,
c2.world_p1,
)
apply_impulse_delta_dynamic(
bodies,
solver_bodies,
c2.body2,
Vec2(-impulse2.x, -impulse2.y),
c2.world_p2,
)
} else {
// Degenerate case fallback: solve sequentially.
for k in 0..<2 {
let i = if k == 0 { i1 } else { i2 }
let c = constraints[i]
let (v1, w1, com1) = get_body_kinematic_state(
bodies,
solver_bodies,
c.body1,
)
let (v2, w2, com2) = get_body_kinematic_state(
bodies,
solver_bodies,
c.body2,
)
let r1 = c.world_p1.sub(com1)
let r2 = c.world_p2.sub(com2)
let vel1 = v1.add(cross_z_scalar(w1, r1))
let vel2 = v2.add(cross_z_scalar(w2, r2))
let rv = vel1.sub(vel2)
let vn = rv.dot(c.dir1)
let sep_term = if c.dist > 0.0F { c.dist * inv_dt } else { 0.0F }
let lambda_n = -(vn + sep_term + c.rhs_bias) * c.inv_mass_n
let new_impulse_n = if constraints[i].impulse_n + lambda_n > 0.0F {
constraints[i].impulse_n + lambda_n
} else {
0.0F
}
let delta_n = new_impulse_n - constraints[i].impulse_n
constraints[i].impulse_n = new_impulse_n
let impulse_n_vec = solver_vec2_scale(c.dir1, delta_n)
apply_impulse_delta_dynamic(
bodies,
solver_bodies,
c.body1,
impulse_n_vec,
c.world_p1,
)
apply_impulse_delta_dynamic(
bodies,
solver_bodies,
c.body2,
Vec2(-impulse_n_vec.x, -impulse_n_vec.y),
c.world_p2,
)
}
}
} else {
for j in 0.. 0.0F { c.dist * inv_dt } else { 0.0F }
let lambda_n = -(vn + sep_term + c.rhs_bias) * c.inv_mass_n
let new_impulse_n = if constraints[i].impulse_n + lambda_n > 0.0F {
constraints[i].impulse_n + lambda_n
} else {
0.0F
}
let delta_n = new_impulse_n - constraints[i].impulse_n
constraints[i].impulse_n = new_impulse_n
let impulse_n_vec = solver_vec2_scale(c.dir1, delta_n)
apply_impulse_delta_dynamic(
bodies,
solver_bodies,
c.body1,
impulse_n_vec,
c.world_p1,
)
apply_impulse_delta_dynamic(
bodies,
solver_bodies,
c.body2,
Vec2(-impulse_n_vec.x, -impulse_n_vec.y),
c.world_p2,
)
}
}
}
// Then solve all friction constraints using the updated normal impulses.
for mi in 0.. Unit {
for i in 0.. 0.0F {
constraints[i].impulse_n + lambda_n
} else {
0.0F
}
let delta_n = new_impulse_n - constraints[i].impulse_n
constraints[i].impulse_n = new_impulse_n
let impulse_n_vec = solver_vec2_scale(c.dir1, delta_n)
apply_impulse_delta_dynamic(
bodies,
solver_bodies,
c.body1,
impulse_n_vec,
c.world_p1,
)
apply_impulse_delta_dynamic(
bodies,
solver_bodies,
c.body2,
Vec2(-impulse_n_vec.x, -impulse_n_vec.y),
c.world_p2,
)
}
}
///|
/// A Rapier-like container for per-island (or global, in this port) contact constraints.
///
/// This is a lightweight wrapper over `SolverConstraint` + `SolverManifold` produced by
/// `build_constraints`. Follow-up work will align it more closely with Rapier's
/// `ContactConstraintsSet` (interaction grouping, SIMD, generic Jacobians).
priv struct ContactConstraintsSet {
mut constraints : Array[SolverConstraint]
mut manifolds : Array[SolverManifold]
mut builders : Array[SolverManifold]
cache_entries : Array[ContactImpulseCacheEntry]
}
///|
fn ContactConstraintsSet::ContactConstraintsSet() -> ContactConstraintsSet {
{ constraints: [], manifolds: [], builders: [], cache_entries: [] }
}
///|
fn ContactConstraintsSet::init(
self : ContactConstraintsSet,
parameters : IntegrationParameters,
bodies : @dynamics.RigidBodySet,
colliders : @collision.ColliderSet,
hooks : PhysicsHooks,
narrow_phase : @collision.NarrowPhase,
dt : @core.Real,
cache : Array[ContactImpulseCacheEntry],
warmstart_coefficient : @core.Real,
island_stamps : Array[Int]?,
stamp : Int,
) -> Unit {
let (constraints, manifolds) = build_constraints(
parameters, bodies, colliders, hooks, narrow_phase, dt, cache, warmstart_coefficient,
island_stamps, stamp,
)
self.constraints = constraints
self.manifolds = manifolds
self.builders = manifolds
self.cache_entries.clear()
for i in 0.. Unit {
self.builders.length() |> ignore
update_contact_constraints(
parameters,
small_step_id,
bodies,
solver_bodies,
self.constraints,
)
}
///|
fn ContactConstraintsSet::warmstart(
self : ContactConstraintsSet,
bodies : @dynamics.RigidBodySet,
solver_bodies : SolverBodies,
) -> Unit {
warmstart(bodies, solver_bodies, self.constraints)
}
///|
fn ContactConstraintsSet::solve(
self : ContactConstraintsSet,
bodies : @dynamics.RigidBodySet,
solver_bodies : SolverBodies,
dt : @core.Real,
) -> Unit {
solve_velocity_constraints(
bodies,
solver_bodies,
dt,
self.constraints,
self.manifolds,
1,
)
}
///|
fn ContactConstraintsSet::solve_wo_bias(
self : ContactConstraintsSet,
bodies : @dynamics.RigidBodySet,
solver_bodies : SolverBodies,
dt : @core.Real,
) -> Unit {
let saved_dist : Array[@core.Real] = []
let saved_bias : Array[@core.Real] = []
for i in 0.. Unit {
solve_velocity_constraints(
bodies,
solver_bodies,
dt,
self.constraints,
self.manifolds,
iterations,
)
}
///|
fn ContactConstraintsSet::solve_restitution_constraints(
self : ContactConstraintsSet,
bodies : @dynamics.RigidBodySet,
solver_bodies : SolverBodies,
) -> Unit {
solve_restitution_constraints(bodies, solver_bodies, self.constraints)
}
///|
fn ContactConstraintsSet::position_correction(
self : ContactConstraintsSet,
bodies : @dynamics.RigidBodySet,
colliders : @collision.ColliderSet,
allowed_linear_error : @core.Real,
) -> Unit {
position_correction(
bodies,
colliders,
self.constraints,
self.manifolds,
allowed_linear_error,
)
}
///|
fn ContactConstraintsSet::position_correction_all(
self : ContactConstraintsSet,
bodies : @dynamics.RigidBodySet,
colliders : @collision.ColliderSet,
allowed_linear_error : @core.Real,
) -> Unit {
position_correction_all(
bodies,
colliders,
self.constraints,
self.manifolds,
allowed_linear_error,
)
}
///|
fn ContactConstraintsSet::emit_contact_force_events(
self : ContactConstraintsSet,
bodies : @dynamics.RigidBodySet,
colliders : @collision.ColliderSet,
dt : @core.Real,
events : EventHandler,
) -> Unit {
emit_contact_force_events(bodies, colliders, dt, events, self.constraints)
}
///|
fn ContactConstraintsSet::append_cache_entries(
self : ContactConstraintsSet,
cache : Array[ContactImpulseCacheEntry],
) -> Unit {
append_cache_entries(cache, self.constraints)
}
///|
fn ContactConstraintsSet::writeback_impulses(
self : ContactConstraintsSet,
cache : Array[ContactImpulseCacheEntry],
) -> Unit {
cache.clear()
append_cache_entries(cache, self.constraints)
self.cache_entries.clear()
for i in 0.. Unit {
for mi in 0..= -allowed_error {
continue
}
let depth = -c.dist - allowed_error
let mut correction = solver_vec2_scale(c.dir1, depth + 1.0e-3F)
if colliders.get(c.collider1) is Some(co1) &&
colliders.get(c.collider2) is Some(co2) {
if separation_vector(
co1.shape(),
co1.translation(),
co1.rotation(),
co2.shape(),
co2.translation(),
co2.rotation(),
)
is Some(sep) {
let depth = sep.length()
if depth > 1.0e-6F {
let dir = sep.normalize()
correction = solver_vec2_scale(dir, depth + 1.0e-3F)
}
}
}
let mut inv_m1 = 0.0F
if c.body1 is Some(h1) {
if bodies.get(h1) is Some(rb1) {
if rb1.is_dynamic() && rb1.is_enabled() {
inv_m1 = if rb1.locked_translations {
0.0F
} else {
rb1.inv_mass()
}
}
}
}
let mut inv_m2 = 0.0F
if c.body2 is Some(h2) {
if bodies.get(h2) is Some(rb2) {
if rb2.is_dynamic() && rb2.is_enabled() {
inv_m2 = if rb2.locked_translations {
0.0F
} else {
rb2.inv_mass()
}
}
}
}
let denom = inv_m1 + inv_m2
if denom <= 1.0e-12F {
continue
}
let ratio1 = inv_m1 / denom
let ratio2 = inv_m2 / denom
if c.body1 is Some(h1m) {
if bodies.get_mut(h1m) is Some(b1) {
if b1.is_dynamic() && b1.is_enabled() && !b1.locked_translations {
b1.set_translation(
b1.translation().add(solver_vec2_scale(correction, ratio1)),
true,
)
|> ignore
}
}
}
if c.body2 is Some(h2m) {
if bodies.get_mut(h2m) is Some(b2) {
if b2.is_dynamic() && b2.is_enabled() && !b2.locked_translations {
b2.set_translation(
b2.translation().sub(solver_vec2_scale(correction, ratio2)),
true,
)
|> ignore
}
}
}
}
} else {
// For non-segment manifolds, correct once per manifold to avoid over-correcting
// multi-point contacts (Rapier relies on stabilization to do this robustly).
let mut dist_min = c0.dist
for j in 0..= -allowed_error {
continue
}
let depth = -dist_min - allowed_error
let factor = if has_convex { 1.0F } else { 0.2F }
let correction = solver_vec2_scale(c0.dir1, depth * factor + 1.0e-3F)
let mut inv_m1 = 0.0F
if c0.body1 is Some(h1) {
if bodies.get(h1) is Some(rb1) {
if rb1.is_dynamic() && rb1.is_enabled() {
inv_m1 = if rb1.locked_translations { 0.0F } else { rb1.inv_mass() }
}
}
}
let mut inv_m2 = 0.0F
if c0.body2 is Some(h2) {
if bodies.get(h2) is Some(rb2) {
if rb2.is_dynamic() && rb2.is_enabled() {
inv_m2 = if rb2.locked_translations { 0.0F } else { rb2.inv_mass() }
}
}
}
let denom = inv_m1 + inv_m2
if denom <= 1.0e-12F {
continue
}
let ratio1 = inv_m1 / denom
let ratio2 = inv_m2 / denom
if c0.body1 is Some(h1m) {
if bodies.get_mut(h1m) is Some(b1) {
if b1.is_dynamic() && b1.is_enabled() && !b1.locked_translations {
b1.set_translation(
b1.translation().add(solver_vec2_scale(correction, ratio1)),
true,
)
|> ignore
}
}
}
if c0.body2 is Some(h2m) {
if bodies.get_mut(h2m) is Some(b2) {
if b2.is_dynamic() && b2.is_enabled() && !b2.locked_translations {
b2.set_translation(
b2.translation().sub(solver_vec2_scale(correction, ratio2)),
true,
)
|> ignore
}
}
}
}
}
}
///|
const CONTACT_POSITION_CORRECTION_BAUMGARTE : @core.Real = 0.2F
///|
fn position_correction_all(
bodies : @dynamics.RigidBodySet,
colliders : @collision.ColliderSet,
constraints : Array[SolverConstraint],
manifolds : Array[SolverManifold],
allowed_error : @core.Real,
) -> Unit {
for mi in 0..= -allowed_error {
continue
}
// Apply only a fraction of the penetration depth to reduce energy injection.
let correction_mag = -dist_min * CONTACT_POSITION_CORRECTION_BAUMGARTE
let correction = solver_vec2_scale(c0.dir1, correction_mag)
let mut inv_m1 = 0.0F
if c0.body1 is Some(h1) {
if bodies.get(h1) is Some(rb1) {
if rb1.is_dynamic() && rb1.is_enabled() {
inv_m1 = if rb1.locked_translations { 0.0F } else { rb1.inv_mass() }
}
}
}
let mut inv_m2 = 0.0F
if c0.body2 is Some(h2) {
if bodies.get(h2) is Some(rb2) {
if rb2.is_dynamic() && rb2.is_enabled() {
inv_m2 = if rb2.locked_translations { 0.0F } else { rb2.inv_mass() }
}
}
}
let denom = inv_m1 + inv_m2
if denom <= 1.0e-12F {
continue
}
let ratio1 = inv_m1 / denom
let ratio2 = inv_m2 / denom
if c0.body1 is Some(h1m) {
if bodies.get_mut(h1m) is Some(b1) {
if b1.is_dynamic() && b1.is_enabled() && !b1.locked_translations {
b1.set_translation(
b1.translation().add(solver_vec2_scale(correction, ratio1)),
true,
)
|> ignore
}
}
}
if c0.body2 is Some(h2m) {
if bodies.get_mut(h2m) is Some(b2) {
if b2.is_dynamic() && b2.is_enabled() && !b2.locked_translations {
b2.set_translation(
b2.translation().sub(solver_vec2_scale(correction, ratio2)),
true,
)
|> ignore
}
}
}
}
}
///|
fn emit_contact_force_events(
bodies : @dynamics.RigidBodySet,
colliders : @collision.ColliderSet,
dt : @core.Real,
event_handler : EventHandler,
constraints : Array[SolverConstraint],
) -> Unit {
if dt <= 0.0F {
return
}
// Rapier parity:
// - total_force = sum(manifold_normal * total_manifold_impulse) / dt
// - total_force_magnitude = sum(total_manifold_impulse) / dt (not |total_force|)
// - max_force_{direction,magnitude} comes from the strongest individual contact impulse.
let pairs : Array[
(
@collision.ColliderHandle,
@collision.ColliderHandle,
@core.Vec2,
@core.Real,
@core.Vec2,
@core.Real,
),
] = []
for i in 0.. p.5 { normal } else { p.4 }
let max_impulse = if impulse > p.5 { impulse } else { p.5 }
pairs[j] = (
p.0,
p.1,
p.2.add(impulse_vec),
p.3 + impulse,
max_dir,
max_impulse,
)
found = true
break
}
}
if !found {
pairs.push(
(c.collider1, c.collider2, impulse_vec, impulse, normal, impulse),
)
}
}
for i in 0.. threshold {
let total_force = solver_vec2_scale(p.2, inv_dt)
event_handler.handle_contact_force_event(
dt,
bodies,
colliders,
ContactForceEvent(
h1,
h2,
total_force,
total_force_magnitude,
p.4,
p.5 * inv_dt,
),
)
}
}
}
}
}
///|
fn append_cache_entries(
cache : Array[ContactImpulseCacheEntry],
constraints : Array[SolverConstraint],
) -> Unit {
for i in 0.. Unit {
cache.clear()
append_cache_entries(cache, constraints)
}
///|
pub fn solve_contacts(
parameters : IntegrationParameters,
bodies : @dynamics.RigidBodySet,
colliders : @collision.ColliderSet,
hooks : PhysicsHooks,
narrow_phase : @collision.NarrowPhase,
dt : @core.Real,
event_handler : EventHandler,
cache : Array[ContactImpulseCacheEntry],
) -> Unit {
let warmstart_coefficient = if parameters.warmstart_coefficient < 0.0F {
0.0F
} else {
parameters.warmstart_coefficient
}
let (constraints, manifolds) = build_constraints(
parameters,
bodies,
colliders,
hooks,
narrow_phase,
dt,
cache,
warmstart_coefficient,
None,
0,
)
// Standalone entry-point: build a solver-side snapshot so the solver implementation can use
// cached poses/velocities consistently.
let solver_bodies = SolverBodies::default()
let n = bodies.bodies.length()
solver_bodies.resize(n)
for i in 0.. (Array[SolverConstraint], Array[SolverManifold]) {
build_constraints(
parameters,
bodies.inner,
colliders.inner,
hooks,
narrow_phase,
dt,
cache,
warmstart_coefficient,
island_stamps,
stamp,
)
}
///|
fn position_correction3(
bodies : @dynamics.RigidBodySet3,
colliders : @collision.ColliderSet3,
constraints : Array[SolverConstraint],
manifolds : Array[SolverManifold],
allowed_error : @core.Real,
) -> Unit {
position_correction(
bodies.inner,
colliders.inner,
constraints,
manifolds,
allowed_error,
)
}
///|
fn position_correction_all3(
bodies : @dynamics.RigidBodySet3,
colliders : @collision.ColliderSet3,
constraints : Array[SolverConstraint],
manifolds : Array[SolverManifold],
allowed_error : @core.Real,
) -> Unit {
position_correction_all(
bodies.inner,
colliders.inner,
constraints,
manifolds,
allowed_error,
)
}