Current section
Files
Jump to
Current section
Files
src/expresso@internal@solver.erl
-module(expresso@internal@solver).
-compile([no_auto_import, nowarn_unused_vars, nowarn_unused_function, nowarn_nomatch, inline]).
-define(FILEPATH, "src/expresso/internal/solver.gleam").
-export([solve_position_constraints/3, solve_velocity_constraints/3]).
-if(?OTP_RELEASE >= 27).
-define(MODULEDOC(Str), -moduledoc(Str)).
-define(DOC(Str), -doc(Str)).
-else.
-define(MODULEDOC(Str), -compile([])).
-define(DOC(Str), -compile([])).
-endif.
?MODULEDOC(false).
-file("src/expresso/internal/solver.gleam", 92).
?DOC(false).
-spec apply_contact_correction(
gleam@dict:dict(MUH, expresso@body:body(MUH)),
expresso@collision:contact(MUH),
float()
) -> gleam@dict:dict(MUH, expresso@body:body(MUH)).
apply_contact_correction(Bodies, Contact, Relaxation) ->
case {gleam_stdlib:map_get(Bodies, erlang:element(2, Contact)),
gleam_stdlib:map_get(Bodies, erlang:element(3, Contact))} of
{{ok, Body_a}, {ok, Body_b}} ->
case erlang:element(16, Body_a) orelse erlang:element(16, Body_b) of
true ->
Bodies;
false ->
Inv_mass_a = expresso@body:inverse_mass(Body_a),
Inv_mass_b = expresso@body:inverse_mass(Body_b),
Total_inv_mass = Inv_mass_a + Inv_mass_b,
case Total_inv_mass < 0.0001 of
true ->
Bodies;
false ->
Correction_magnitude = case Total_inv_mass of
+0.0 -> +0.0;
-0.0 -> -0.0;
Gleam@denominator -> erlang:element(5, Contact)
* Relaxation
/ Gleam@denominator
end,
Correction_a = vec@vec3f:scale(
erlang:element(4, Contact),
(-1.0 * Correction_magnitude) * Inv_mass_a
),
Correction_b = vec@vec3f:scale(
erlang:element(4, Contact),
Correction_magnitude * Inv_mass_b
),
New_body_a = {body,
erlang:element(2, Body_a),
vec@vec3f:add(
erlang:element(3, Body_a),
Correction_a
),
erlang:element(4, Body_a),
erlang:element(5, Body_a),
erlang:element(6, Body_a),
erlang:element(7, Body_a),
erlang:element(8, Body_a),
erlang:element(9, Body_a),
erlang:element(10, Body_a),
erlang:element(11, Body_a),
erlang:element(12, Body_a),
erlang:element(13, Body_a),
erlang:element(14, Body_a),
erlang:element(15, Body_a),
erlang:element(16, Body_a)},
New_body_b = {body,
erlang:element(2, Body_b),
vec@vec3f:add(
erlang:element(3, Body_b),
Correction_b
),
erlang:element(4, Body_b),
erlang:element(5, Body_b),
erlang:element(6, Body_b),
erlang:element(7, Body_b),
erlang:element(8, Body_b),
erlang:element(9, Body_b),
erlang:element(10, Body_b),
erlang:element(11, Body_b),
erlang:element(12, Body_b),
erlang:element(13, Body_b),
erlang:element(14, Body_b),
erlang:element(15, Body_b),
erlang:element(16, Body_b)},
_pipe = Bodies,
_pipe@1 = gleam@dict:insert(
_pipe,
erlang:element(2, Contact),
New_body_a
),
gleam@dict:insert(
_pipe@1,
erlang:element(3, Contact),
New_body_b
)
end
end;
{_, _} ->
Bodies
end.
-file("src/expresso/internal/solver.gleam", 72).
?DOC(false).
-spec solve_position_constraints(
gleam@dict:dict(MTY, expresso@body:body(MTY)),
list(expresso@collision:contact(MTY)),
integer()
) -> gleam@dict:dict(MTY, expresso@body:body(MTY)).
solve_position_constraints(Bodies, Contacts, Iterations) ->
Relaxation = 0.6,
_pipe = gleam@list:range(0, Iterations - 1),
gleam@list:fold(
_pipe,
Bodies,
fun(Current_bodies, _) ->
gleam@list:fold(
Contacts,
Current_bodies,
fun(Bodies_acc, Contact) ->
apply_contact_correction(Bodies_acc, Contact, Relaxation)
end
)
end
).
-file("src/expresso/internal/solver.gleam", 271).
?DOC(false).
-spec apply_torque(expresso@body:body(any()), vec@vec3:vec3(float()), float()) -> vec@vec3:vec3(float()).
apply_torque(Body_val, Torque, Inverse_mass) ->
case Inverse_mass < 0.0001 of
true ->
{vec3, +0.0, +0.0, +0.0};
false ->
Mass = case Inverse_mass of
+0.0 -> +0.0;
-0.0 -> -0.0;
Gleam@denominator -> 1.0 / Gleam@denominator
end,
Radius = expresso@body:bounding_radius(Body_val),
Moment_of_inertia = ((0.4 * Mass) * Radius) * Radius,
case Moment_of_inertia < 0.0001 of
true ->
{vec3, +0.0, +0.0, +0.0};
false ->
vec@vec3f:scale(Torque, case Moment_of_inertia of
+0.0 -> +0.0;
-0.0 -> -0.0;
Gleam@denominator@1 -> 1.0 / Gleam@denominator@1
end)
end
end.
-file("src/expresso/internal/solver.gleam", 300).
?DOC(false).
-spec apply_friction_impulse(
expresso@body:body(MVK),
expresso@body:body(MVK),
expresso@collision:contact(MVK),
float()
) -> {expresso@body:body(MVK), expresso@body:body(MVK)}.
apply_friction_impulse(Body_a, Body_b, Contact, Normal_impulse_magnitude) ->
Relative_velocity = vec@vec3f:subtract(
erlang:element(4, Body_b),
erlang:element(4, Body_a)
),
Velocity_along_normal = vec@vec3f:dot(
Relative_velocity,
erlang:element(4, Contact)
),
Normal_velocity = vec@vec3f:scale(
erlang:element(4, Contact),
Velocity_along_normal
),
Tangential_velocity = vec@vec3f:subtract(Relative_velocity, Normal_velocity),
Tangential_speed = vec@vec3f:length(Tangential_velocity),
case Tangential_speed < 0.001 of
true ->
{Body_a, Body_b};
false ->
Tangent = vec@vec3f:normalize(Tangential_velocity),
Static_friction = (erlang:element(11, Body_a) + erlang:element(
11,
Body_b
))
/ 2.0,
Dynamic_friction = (erlang:element(12, Body_a) + erlang:element(
12,
Body_b
))
/ 2.0,
Inv_mass_a = expresso@body:inverse_mass(Body_a),
Inv_mass_b = expresso@body:inverse_mass(Body_b),
Total_inv_mass = Inv_mass_a + Inv_mass_b,
case Total_inv_mass < 0.0001 of
true ->
{Body_a, Body_b};
false ->
Static_impulse_magnitude = case Total_inv_mass of
+0.0 -> +0.0;
-0.0 -> -0.0;
Gleam@denominator -> Tangential_speed / Gleam@denominator
end,
Dynamic_impulse_magnitude = Dynamic_friction * Normal_impulse_magnitude,
Friction_impulse_magnitude = case Static_impulse_magnitude < (Static_friction
* Normal_impulse_magnitude) of
true ->
Static_impulse_magnitude;
false ->
Dynamic_impulse_magnitude
end,
Friction_impulse = vec@vec3f:scale(
Tangent,
-1.0 * Friction_impulse_magnitude
),
New_body_a = {body,
erlang:element(2, Body_a),
erlang:element(3, Body_a),
vec@vec3f:add(
erlang:element(4, Body_a),
vec@vec3f:scale(Friction_impulse, -1.0 * Inv_mass_a)
),
erlang:element(5, Body_a),
erlang:element(6, Body_a),
erlang:element(7, Body_a),
erlang:element(8, Body_a),
erlang:element(9, Body_a),
erlang:element(10, Body_a),
erlang:element(11, Body_a),
erlang:element(12, Body_a),
erlang:element(13, Body_a),
erlang:element(14, Body_a),
erlang:element(15, Body_a),
erlang:element(16, Body_a)},
New_body_b = {body,
erlang:element(2, Body_b),
erlang:element(3, Body_b),
vec@vec3f:add(
erlang:element(4, Body_b),
vec@vec3f:scale(Friction_impulse, Inv_mass_b)
),
erlang:element(5, Body_b),
erlang:element(6, Body_b),
erlang:element(7, Body_b),
erlang:element(8, Body_b),
erlang:element(9, Body_b),
erlang:element(10, Body_b),
erlang:element(11, Body_b),
erlang:element(12, Body_b),
erlang:element(13, Body_b),
erlang:element(14, Body_b),
erlang:element(15, Body_b),
erlang:element(16, Body_b)},
{New_body_a, New_body_b}
end
end.
-file("src/expresso/internal/solver.gleam", 167).
?DOC(false).
-spec apply_velocity_impulse(
gleam@dict:dict(MUY, expresso@body:body(MUY)),
expresso@collision:contact(MUY),
float()
) -> gleam@dict:dict(MUY, expresso@body:body(MUY)).
apply_velocity_impulse(Bodies, Contact, Restitution) ->
case {gleam_stdlib:map_get(Bodies, erlang:element(2, Contact)),
gleam_stdlib:map_get(Bodies, erlang:element(3, Contact))} of
{{ok, Body_a}, {ok, Body_b}} ->
case erlang:element(16, Body_a) orelse erlang:element(16, Body_b) of
true ->
Bodies;
false ->
Relative_velocity = vec@vec3f:subtract(
erlang:element(4, Body_b),
erlang:element(4, Body_a)
),
Velocity_along_normal = vec@vec3f:dot(
Relative_velocity,
erlang:element(4, Contact)
),
case Velocity_along_normal >= +0.0 of
true ->
Bodies;
false ->
Inv_mass_a = expresso@body:inverse_mass(Body_a),
Inv_mass_b = expresso@body:inverse_mass(Body_b),
Total_inv_mass = Inv_mass_a + Inv_mass_b,
case Total_inv_mass < 0.0001 of
true ->
Bodies;
false ->
Impulse_magnitude = case Total_inv_mass of
+0.0 -> +0.0;
-0.0 -> -0.0;
Gleam@denominator -> (-1.0 * (1.0 + Restitution))
* Velocity_along_normal
/ Gleam@denominator
end,
Impulse = vec@vec3f:scale(
erlang:element(4, Contact),
Impulse_magnitude
),
R_a = vec@vec3f:subtract(
erlang:element(6, Contact),
erlang:element(3, Body_a)
),
Torque_a = vec@vec3f:cross(
R_a,
vec@vec3f:scale(Impulse, -1.0)
),
Angular_impulse_a = apply_torque(
Body_a,
Torque_a,
Inv_mass_a
),
R_b = vec@vec3f:subtract(
erlang:element(6, Contact),
erlang:element(3, Body_b)
),
Torque_b = vec@vec3f:cross(R_b, Impulse),
Angular_impulse_b = apply_torque(
Body_b,
Torque_b,
Inv_mass_b
),
Body_a_after_normal = {body,
erlang:element(2, Body_a),
erlang:element(3, Body_a),
vec@vec3f:add(
erlang:element(4, Body_a),
vec@vec3f:scale(
Impulse,
-1.0 * Inv_mass_a
)
),
erlang:element(5, Body_a),
vec@vec3f:add(
erlang:element(6, Body_a),
Angular_impulse_a
),
erlang:element(7, Body_a),
erlang:element(8, Body_a),
erlang:element(9, Body_a),
erlang:element(10, Body_a),
erlang:element(11, Body_a),
erlang:element(12, Body_a),
erlang:element(13, Body_a),
erlang:element(14, Body_a),
erlang:element(15, Body_a),
erlang:element(16, Body_a)},
Body_b_after_normal = {body,
erlang:element(2, Body_b),
erlang:element(3, Body_b),
vec@vec3f:add(
erlang:element(4, Body_b),
vec@vec3f:scale(Impulse, Inv_mass_b)
),
erlang:element(5, Body_b),
vec@vec3f:add(
erlang:element(6, Body_b),
Angular_impulse_b
),
erlang:element(7, Body_b),
erlang:element(8, Body_b),
erlang:element(9, Body_b),
erlang:element(10, Body_b),
erlang:element(11, Body_b),
erlang:element(12, Body_b),
erlang:element(13, Body_b),
erlang:element(14, Body_b),
erlang:element(15, Body_b),
erlang:element(16, Body_b)},
{New_body_a, New_body_b} = apply_friction_impulse(
Body_a_after_normal,
Body_b_after_normal,
Contact,
Impulse_magnitude
),
_pipe = Bodies,
_pipe@1 = gleam@dict:insert(
_pipe,
erlang:element(2, Contact),
New_body_a
),
gleam@dict:insert(
_pipe@1,
erlang:element(3, Contact),
New_body_b
)
end
end
end;
{_, _} ->
Bodies
end.
-file("src/expresso/internal/solver.gleam", 156).
?DOC(false).
-spec solve_velocity_constraints(
gleam@dict:dict(MUP, expresso@body:body(MUP)),
list(expresso@collision:contact(MUP)),
float()
) -> gleam@dict:dict(MUP, expresso@body:body(MUP)).
solve_velocity_constraints(Bodies, Contacts, Restitution) ->
gleam@list:fold(
Contacts,
Bodies,
fun(Bodies_acc, Contact) ->
apply_velocity_impulse(Bodies_acc, Contact, Restitution)
end
).