box3d_rust/shape/
wind.rs

1//! Wind force application on shapes from shape.c (`b3Shape_ApplyWind`).
2//!
3//! SPDX-FileCopyrightText: 2025 Erin Catto
4//! SPDX-License-Identifier: MIT
5
6use super::lifecycle::get_shape;
7use super::ShapeGeometry;
8use crate::body::wake_body_with_lock;
9use crate::core::get_length_units_per_meter;
10use crate::geometry::ShapeType;
11use crate::hull::{get_hull_edges, get_hull_faces, get_hull_planes, get_hull_points};
12use crate::id::ShapeId;
13use crate::math_functions::{
14    add, cross, dot, get_length_and_normalize, length, make_matrix_from_quat, min_float, mul_add,
15    mul_mv, mul_sub, mul_sv, normalize, rotate_vector, sub, to_relative_transform, Vec3, PI,
16    POS_ZERO, VEC3_ZERO,
17};
18use crate::solver_set::{AWAKE_SET, DISABLED_SET, FIRST_SLEEPING_SET};
19use crate::types::BodyType;
20use crate::world::World;
21
22// https://en.wikipedia.org/wiki/Density_of_air
23// https://www.engineeringtoolbox.com/wind-load-d_1775.html
24// force = 0.5 * air_density * velocity^2 * area
25// https://en.wikipedia.org/wiki/Lift_(force)
26/// Apply a wind force to the body for this shape using the density of air.
27/// (b3Shape_ApplyWind)
28pub fn shape_apply_wind(
29    world: &mut World,
30    shape_id: ShapeId,
31    wind: Vec3,
32    drag: f32,
33    lift: f32,
34    max_speed: f32,
35    wake: bool,
36) {
37    crate::recording::with_recording(world, |rec| {
38        rec.write_shape_apply_wind(shape_id, wind, drag, lift, max_speed, wake);
39    });
40    let shape_index = get_shape(world, shape_id);
41
42    let shape_type = world.shapes[shape_index as usize].shape_type();
43    if shape_type != ShapeType::Sphere
44        && shape_type != ShapeType::Capsule
45        && shape_type != ShapeType::Hull
46    {
47        return;
48    }
49
50    let body_id = world.shapes[shape_index as usize].body_id;
51
52    if world.bodies[body_id as usize].type_ != BodyType::Dynamic {
53        return;
54    }
55
56    if world.bodies[body_id as usize].set_index == DISABLED_SET {
57        return;
58    }
59
60    if world.bodies[body_id as usize].set_index >= FIRST_SLEEPING_SET && !wake {
61        return;
62    }
63
64    if world.bodies[body_id as usize].set_index != AWAKE_SET {
65        // Must wake for state to exist
66        wake_body_with_lock(world, body_id);
67    }
68
69    debug_assert!(world.bodies[body_id as usize].set_index == AWAKE_SET);
70
71    let local_index = world.bodies[body_id as usize].local_index;
72    let (transform, local_center_of_mass) = {
73        let sim = &world.solver_sets[AWAKE_SET as usize].body_sims[local_index as usize];
74        // Only the rotation is used below, so the demoted world transform is exact
75        (
76            to_relative_transform(sim.transform, POS_ZERO),
77            sim.local_center,
78        )
79    };
80    let (linear_velocity, angular_velocity) = {
81        let state = &world.solver_sets[AWAKE_SET as usize].body_states[local_index as usize];
82        (state.linear_velocity, state.angular_velocity)
83    };
84
85    let length_units = get_length_units_per_meter();
86    let volume_units = length_units * length_units * length_units;
87    let air_density = 1.2250 / volume_units;
88
89    let mut force = VEC3_ZERO;
90    let mut torque = VEC3_ZERO;
91
92    let shape = &world.shapes[shape_index as usize];
93    match &shape.geometry {
94        ShapeGeometry::Sphere(sphere) => {
95            let radius = sphere.radius;
96            let centroid = shape.local_centroid;
97            let lever = rotate_vector(transform.q, sub(centroid, local_center_of_mass));
98            let shape_velocity = add(linear_velocity, cross(angular_velocity, lever));
99            let relative_velocity = mul_sub(wind, drag, shape_velocity);
100            let mut speed = 0.0;
101            let direction = get_length_and_normalize(&mut speed, relative_velocity);
102            let speed = min_float(speed, max_speed);
103            let projected_area = PI * radius * radius;
104            force = mul_sv(
105                0.5 * air_density * projected_area * speed * speed,
106                direction,
107            );
108            torque = cross(lever, force);
109        }
110
111        ShapeGeometry::Capsule(capsule) => {
112            let centroid = shape.local_centroid;
113            let lever = rotate_vector(transform.q, sub(centroid, local_center_of_mass));
114            let shape_velocity = add(linear_velocity, cross(angular_velocity, lever));
115            let relative_velocity = mul_sub(wind, drag, shape_velocity);
116            let mut speed = 0.0;
117            let direction = get_length_and_normalize(&mut speed, relative_velocity);
118            let speed = min_float(speed, max_speed);
119
120            let mut d = sub(capsule.center2, capsule.center1);
121            d = rotate_vector(transform.q, d);
122
123            let radius = capsule.radius;
124            let projected_area = PI * radius * radius + 2.0 * radius * length(cross(d, direction));
125
126            // Normal that opposes the wind
127            let e = normalize(d);
128            let normal = sub(mul_sv(dot(direction, e), e), direction);
129
130            // portion of wind that is perpendicular to surface
131            let lift_direction = cross(cross(normal, direction), direction);
132
133            let force_magnitude = 0.5 * air_density * projected_area * speed * speed;
134            force = mul_sv(force_magnitude, mul_add(direction, lift, lift_direction));
135
136            let edge_lever = mul_add(lever, radius, normal);
137            torque = cross(edge_lever, force);
138        }
139
140        ShapeGeometry::Hull(hull) => {
141            let matrix = make_matrix_from_quat(transform.q);
142
143            let face_count = hull.face_count;
144            let points = get_hull_points(hull);
145            let faces = get_hull_faces(hull);
146            let edges = get_hull_edges(hull);
147            let planes = get_hull_planes(hull);
148
149            for i in 0..face_count {
150                let face = &faces[i as usize];
151                let edge1_index = face.edge;
152                let edge2_index = edges[edge1_index as usize].next;
153                let mut edge3_index = edges[edge2_index as usize].next;
154
155                debug_assert!(edge1_index != edge3_index);
156                debug_assert!((edges[edge1_index as usize].origin as i32) < hull.vertex_count);
157                debug_assert!((edges[edge2_index as usize].origin as i32) < hull.vertex_count);
158
159                let local_point1 = points[edges[edge1_index as usize].origin as usize];
160                let mut local_point2 = points[edges[edge2_index as usize].origin as usize];
161                let v1 = mul_mv(matrix, local_point1);
162                let mut v2 = mul_mv(matrix, local_point2);
163                let normal = mul_mv(matrix, planes[i as usize].normal);
164
165                loop {
166                    debug_assert!((edges[edge3_index as usize].origin as i32) < hull.vertex_count);
167                    let local_point3 = points[edges[edge3_index as usize].origin as usize];
168                    let v3 = mul_mv(matrix, local_point3);
169
170                    // Triangle center
171                    let triangle_local_center =
172                        mul_sv(0.333333, add(local_point1, add(local_point2, local_point3)));
173
174                    // Lever arm from center of mass to triangle center in world space
175                    let lever = mul_mv(matrix, sub(triangle_local_center, local_center_of_mass));
176
177                    // Velocity of the triangle center in world space
178                    let center_velocity = add(linear_velocity, cross(angular_velocity, lever));
179
180                    let relative_velocity = mul_sub(wind, drag, center_velocity);
181                    let mut speed = 0.0;
182                    let direction = get_length_and_normalize(&mut speed, relative_velocity);
183
184                    // Check for back-side
185                    if dot(normal, direction) < -f32::EPSILON {
186                        let projected_area = -0.5 * dot(cross(sub(v2, v1), sub(v3, v1)), direction);
187                        debug_assert!(projected_area >= -f32::EPSILON);
188
189                        let lift_direction = cross(cross(normal, direction), direction);
190
191                        let speed = min_float(speed, max_speed);
192
193                        let force_magnitude = 0.5 * air_density * projected_area * speed * speed;
194                        let delta_force =
195                            mul_sv(force_magnitude, mul_add(direction, lift, lift_direction));
196                        let delta_torque = cross(lever, delta_force);
197
198                        force = add(force, delta_force);
199                        torque = add(torque, delta_torque);
200                    }
201
202                    // Advance the fan: C sets edge2 = edge3 then edge3 = next(edge3).
203                    edge3_index = edges[edge3_index as usize].next;
204                    v2 = v3;
205                    local_point2 = local_point3;
206
207                    if edge1_index == edge3_index {
208                        break;
209                    }
210                }
211            }
212        }
213
214        _ => {}
215    }
216
217    let sim = &mut world.solver_sets[AWAKE_SET as usize].body_sims[local_index as usize];
218    sim.force = add(sim.force, force);
219    sim.torque = add(sim.torque, torque);
220}
box3d_rust/shape/wind.rs

box3d_rust/shape/
wind.rs
