box2d_rust/body/
forces.rs

1// Body forces and impulses from body.c (b2Body_ApplyForce family). Split
2// from api.rs to stay under the file-length limit.
3//
4// SPDX-FileCopyrightText: 2023 Erin Catto
5// SPDX-License-Identifier: MIT
6
7use super::api::body_is_valid;
8use super::plumbing::{get_body_full_id, limit_velocity, wake_body};
9use crate::id::BodyId;
10use crate::math_functions::{add, cross, mul_add, sub_pos, Pos, Vec2, VEC2_ZERO};
11use crate::solver_set::{AWAKE_SET, DISABLED_SET, FIRST_SLEEPING_SET};
12use crate::types::BodyType;
13use crate::world::World;
14
15/// (b2Body_ApplyForce)
16pub fn body_apply_force(world: &mut World, body_id: BodyId, force: Vec2, point: Pos, wake: bool) {
17    crate::recording::record_op(world, |rec, _| {
18        crate::recording::write_body_vec2_point_bool(
19            rec,
20            crate::recording::OP_BODY_APPLY_FORCE,
21            body_id,
22            force,
23            point,
24            wake,
25        )
26    });
27    let body_index = get_body_full_id(world, body_id);
28
29    {
30        let body = &world.bodies[body_index as usize];
31        if body.type_ != BodyType::Dynamic || body.set_index == DISABLED_SET {
32            return;
33        }
34    }
35
36    if wake && world.bodies[body_index as usize].set_index >= FIRST_SLEEPING_SET {
37        wake_body(world, body_index);
38    }
39
40    let body = &world.bodies[body_index as usize];
41    if body.set_index == AWAKE_SET {
42        let local_index = body.local_index;
43        let body_sim = &mut world.solver_sets[AWAKE_SET as usize].body_sims[local_index as usize];
44        body_sim.force = add(body_sim.force, force);
45        body_sim.torque += cross(sub_pos(point, body_sim.center), force);
46    }
47}
48
49/// (b2Body_ApplyForceToCenter)
50pub fn body_apply_force_to_center(world: &mut World, body_id: BodyId, force: Vec2, wake: bool) {
51    crate::recording::record_op(world, |rec, _| {
52        crate::recording::write_body_vec2_bool(
53            rec,
54            crate::recording::OP_BODY_APPLY_FORCE_TO_CENTER,
55            body_id,
56            force,
57            wake,
58        )
59    });
60    let body_index = get_body_full_id(world, body_id);
61
62    {
63        let body = &world.bodies[body_index as usize];
64        if body.type_ != BodyType::Dynamic || body.set_index == DISABLED_SET {
65            return;
66        }
67    }
68
69    if wake && world.bodies[body_index as usize].set_index >= FIRST_SLEEPING_SET {
70        wake_body(world, body_index);
71    }
72
73    let body = &world.bodies[body_index as usize];
74    if body.set_index == AWAKE_SET {
75        let local_index = body.local_index;
76        let body_sim = &mut world.solver_sets[AWAKE_SET as usize].body_sims[local_index as usize];
77        body_sim.force = add(body_sim.force, force);
78    }
79}
80
81/// (b2Body_ApplyTorque)
82pub fn body_apply_torque(world: &mut World, body_id: BodyId, torque: f32, wake: bool) {
83    crate::recording::record_op(world, |rec, _| {
84        crate::recording::write_body_f32_bool(
85            rec,
86            crate::recording::OP_BODY_APPLY_TORQUE,
87            body_id,
88            torque,
89            wake,
90        )
91    });
92    let body_index = get_body_full_id(world, body_id);
93
94    {
95        let body = &world.bodies[body_index as usize];
96        if body.type_ != BodyType::Dynamic || body.set_index == DISABLED_SET {
97            return;
98        }
99    }
100
101    if wake && world.bodies[body_index as usize].set_index >= FIRST_SLEEPING_SET {
102        wake_body(world, body_index);
103    }
104
105    let body = &world.bodies[body_index as usize];
106    if body.set_index == AWAKE_SET {
107        let local_index = body.local_index;
108        let body_sim = &mut world.solver_sets[AWAKE_SET as usize].body_sims[local_index as usize];
109        body_sim.torque += torque;
110    }
111}
112
113/// (b2Body_ClearForces)
114pub fn body_clear_forces(world: &mut World, body_id: BodyId) {
115    crate::recording::record_op(world, |rec, _| {
116        crate::recording::write_body_marker(rec, crate::recording::OP_BODY_CLEAR_FORCES, body_id)
117    });
118    let body_index = get_body_full_id(world, body_id);
119    let (set_index, local_index) = {
120        let body = &world.bodies[body_index as usize];
121        (body.set_index, body.local_index)
122    };
123    let body_sim = &mut world.solver_sets[set_index as usize].body_sims[local_index as usize];
124    body_sim.force = VEC2_ZERO;
125    body_sim.torque = 0.0;
126}
127
128/// (b2Body_ApplyLinearImpulse)
129pub fn body_apply_linear_impulse(
130    world: &mut World,
131    body_id: BodyId,
132    impulse: Vec2,
133    point: Pos,
134    wake: bool,
135) {
136    crate::recording::record_op(world, |rec, _| {
137        crate::recording::write_body_vec2_point_bool(
138            rec,
139            crate::recording::OP_BODY_APPLY_LINEAR_IMPULSE,
140            body_id,
141            impulse,
142            point,
143            wake,
144        )
145    });
146    let body_index = get_body_full_id(world, body_id);
147
148    {
149        let body = &world.bodies[body_index as usize];
150        if body.type_ != BodyType::Dynamic || body.set_index == DISABLED_SET {
151            return;
152        }
153    }
154
155    if wake && world.bodies[body_index as usize].set_index >= FIRST_SLEEPING_SET {
156        wake_body(world, body_index);
157    }
158
159    let max_linear_speed = world.max_linear_speed;
160    let body = &world.bodies[body_index as usize];
161    if body.set_index == AWAKE_SET {
162        let local_index = body.local_index as usize;
163        let set = &mut world.solver_sets[AWAKE_SET as usize];
164        let body_sim = set.body_sims[local_index];
165        let state = &mut set.body_states[local_index];
166        state.linear_velocity = mul_add(state.linear_velocity, body_sim.inv_mass, impulse);
167        state.angular_velocity +=
168            body_sim.inv_inertia * cross(sub_pos(point, body_sim.center), impulse);
169
170        limit_velocity(state, max_linear_speed);
171    }
172}
173
174/// (b2Body_ApplyLinearImpulseToCenter)
175pub fn body_apply_linear_impulse_to_center(
176    world: &mut World,
177    body_id: BodyId,
178    impulse: Vec2,
179    wake: bool,
180) {
181    crate::recording::record_op(world, |rec, _| {
182        crate::recording::write_body_vec2_bool(
183            rec,
184            crate::recording::OP_BODY_APPLY_LINEAR_IMPULSE_TO_CENTER,
185            body_id,
186            impulse,
187            wake,
188        )
189    });
190    let body_index = get_body_full_id(world, body_id);
191
192    {
193        let body = &world.bodies[body_index as usize];
194        if body.type_ != BodyType::Dynamic || body.set_index == DISABLED_SET {
195            return;
196        }
197    }
198
199    if wake && world.bodies[body_index as usize].set_index >= FIRST_SLEEPING_SET {
200        wake_body(world, body_index);
201    }
202
203    let max_linear_speed = world.max_linear_speed;
204    let body = &world.bodies[body_index as usize];
205    if body.set_index == AWAKE_SET {
206        let local_index = body.local_index as usize;
207        let set = &mut world.solver_sets[AWAKE_SET as usize];
208        let inv_mass = set.body_sims[local_index].inv_mass;
209        let state = &mut set.body_states[local_index];
210        state.linear_velocity = mul_add(state.linear_velocity, inv_mass, impulse);
211
212        limit_velocity(state, max_linear_speed);
213    }
214}
215
216/// (b2Body_ApplyAngularImpulse)
217pub fn body_apply_angular_impulse(world: &mut World, body_id: BodyId, impulse: f32, wake: bool) {
218    crate::recording::record_op(world, |rec, _| {
219        crate::recording::write_body_f32_bool(
220            rec,
221            crate::recording::OP_BODY_APPLY_ANGULAR_IMPULSE,
222            body_id,
223            impulse,
224            wake,
225        )
226    });
227    debug_assert!(body_is_valid(world, body_id));
228    let body_index = get_body_full_id(world, body_id);
229
230    {
231        let body = &world.bodies[body_index as usize];
232        if body.type_ != BodyType::Dynamic || body.set_index == DISABLED_SET {
233            return;
234        }
235    }
236
237    if wake && world.bodies[body_index as usize].set_index >= FIRST_SLEEPING_SET {
238        // this will not invalidate body index
239        wake_body(world, body_index);
240    }
241
242    let body = &world.bodies[body_index as usize];
243    if body.set_index == AWAKE_SET {
244        let local_index = body.local_index as usize;
245        let set = &mut world.solver_sets[AWAKE_SET as usize];
246        let inv_inertia = set.body_sims[local_index].inv_inertia;
247        set.body_states[local_index].angular_velocity += inv_inertia * impulse;
248    }
249}
box2d_rust/body/forces.rs

box2d_rust/body/
forces.rs
