constraint_solver.gd

Go to the documentation of this file.

extends RefCounted
 
 
 
 
 
func resolve_collisions(bodies:Dictionary, contacts:Array, positional_percent:float = 0.8, slop:float = 0.01) -> Dictionary:
    var next = bodies.duplicate(true)
    for contact in contacts:
        var a_id = str(contact.get("a", {}).get("id", ""))
        var b_id = str(contact.get("b", {}).get("id", ""))
        if not next.has(a_id) or not next.has(b_id):
            continue
        var a = next[a_id].duplicate(true)
        var b = next[b_id].duplicate(true)
        var normal = contact.get("normal", Vector2.RIGHT)
        var penetration = float(contact.get("penetration", 0.0))
        var inv_a = float(a.get("inv_mass", 0.0))
        var inv_b = float(b.get("inv_mass", 0.0))
        var inv_total = inv_a + inv_b
        if inv_total <= 0.0:
            continue
        var relative_velocity = b.get("velocity", Vector2.ZERO) - a.get("velocity", Vector2.ZERO)
        var velocity_along_normal = relative_velocity.dot(normal)
        if velocity_along_normal < 0.0:
            var restitution = min(float(a.get("restitution", 0.1)), float(b.get("restitution", 0.1)))
            var impulse_magnitude = -(1.0 + restitution) * velocity_along_normal / inv_total
            var impulse = normal * impulse_magnitude
            a["velocity"] = a.get("velocity", Vector2.ZERO) - impulse * inv_a
            b["velocity"] = b.get("velocity", Vector2.ZERO) + impulse * inv_b
        var correction = normal * max(penetration - slop, 0.0) / inv_total * positional_percent
        a["position"] = a.get("position", Vector2.ZERO) - correction * inv_a
        b["position"] = b.get("position", Vector2.ZERO) + correction * inv_b
        next[a_id] = a
        next[b_id] = b
    return next
 
 
 
func solve_distance_constraint(bodies:Dictionary, constraint:Dictionary) -> Dictionary:
    var next = bodies.duplicate(true)
    var a_id = str(constraint.get("a", ""))
    var b_id = str(constraint.get("b", ""))
    if not next.has(a_id) or not next.has(b_id):
        return next
    var a = next[a_id].duplicate(true)
    var b = next[b_id].duplicate(true)
    var delta = b.get("position", Vector2.ZERO) - a.get("position", Vector2.ZERO)
    var distance = delta.length()
    if distance <= 0.0001:
        return next
    var target = float(constraint.get("distance", distance))
    var stiffness = clamp(float(constraint.get("stiffness", 1.0)), 0.0, 1.0)
    var normal = delta / distance
    var inv_a = float(a.get("inv_mass", 0.0))
    var inv_b = float(b.get("inv_mass", 0.0))
    var inv_total = inv_a + inv_b
    if inv_total <= 0.0:
        return next
    var correction = normal * (distance - target) * stiffness / inv_total
    a["position"] = a.get("position", Vector2.ZERO) + correction * inv_a
    b["position"] = b.get("position", Vector2.ZERO) - correction * inv_b
    next[a_id] = a
    next[b_id] = b
    return next
 
 
 
func solve_constraints(bodies:Dictionary, constraints:Array, iterations:int = 1) -> Dictionary:
    var next = bodies.duplicate(true)
    for _i in range(max(1, iterations)):
        for constraint in constraints:
            if str(constraint.get("type", "distance")) == "distance":
                next = solve_distance_constraint(next, constraint)
    return next