anput_physics/density_fields/
cube.rs

1use std::cmp::Ordering;
2
3use crate::{Scalar, components::BodyAccessInfo, density_fields::DensityField};
4use vek::{Aabb, Vec3};
5
6pub struct CubeDensityField<const LOCKING: bool> {
7    pub density: Scalar,
8    pub extents: Vec3<Scalar>,
9    pub edge_thickness: Vec3<Scalar>,
10}
11
12impl<const LOCKING: bool> CubeDensityField<LOCKING> {
13    pub fn new_hard(density: Scalar, extents: Vec3<Scalar>) -> Self {
14        Self {
15            density,
16            extents,
17            edge_thickness: Default::default(),
18        }
19    }
20
21    pub fn new_soft(density: Scalar, extents: Vec3<Scalar>) -> Self {
22        Self {
23            density,
24            extents: Default::default(),
25            edge_thickness: extents,
26        }
27    }
28
29    pub fn new_soft_edge(
30        density: Scalar,
31        extents: Vec3<Scalar>,
32        edge_thickness: Vec3<Scalar>,
33    ) -> Self {
34        Self {
35            density,
36            extents,
37            edge_thickness,
38        }
39    }
40
41    #[inline]
42    pub fn total_extents(&self) -> Vec3<Scalar> {
43        self.extents + self.edge_thickness
44    }
45}
46
47impl<const LOCKING: bool> DensityField for CubeDensityField<LOCKING> {
48    fn aabb(&self, info: &BodyAccessInfo) -> Aabb<Scalar> {
49        info.world_space_particles::<LOCKING, ()>()
50            .map(|(matrix, _)| {
51                let extents = self.total_extents();
52                let mut aabb = Aabb::new_empty(matrix.mul_point(Default::default()));
53                for corner in [
54                    Vec3::new(-extents.x, -extents.y, -extents.z),
55                    Vec3::new(extents.x, -extents.y, -extents.z),
56                    Vec3::new(extents.x, extents.y, -extents.z),
57                    Vec3::new(-extents.x, extents.y, -extents.z),
58                    Vec3::new(-extents.x, -extents.y, extents.z),
59                    Vec3::new(extents.x, -extents.y, extents.z),
60                    Vec3::new(extents.x, extents.y, extents.z),
61                    Vec3::new(-extents.x, extents.y, extents.z),
62                ] {
63                    aabb.expand_to_contain_point(matrix.mul_point(corner));
64                }
65                aabb
66            })
67            .reduce(|accum, aabb| accum.union(aabb))
68            .unwrap_or_default()
69    }
70
71    fn density_at_point(&self, point: Vec3<Scalar>, info: &BodyAccessInfo) -> Scalar {
72        info.world_space_particles::<LOCKING, ()>()
73            .map(|(matrix, _)| {
74                let point = matrix.inverted().mul_point(point).into_array();
75                let extents = self.extents.into_array();
76                let edge_thickness = self.edge_thickness.into_array();
77                (0..2)
78                    .map(|index| {
79                        let point = point[index].abs();
80                        let extent = extents[index];
81                        let edge = edge_thickness[index];
82                        let distance = point - extent;
83                        (edge, distance)
84                    })
85                    .max_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap_or(Ordering::Equal))
86                    .map(|(edge, distance)| {
87                        let factor = if distance < 0.0 {
88                            1.0
89                        } else if edge > Scalar::EPSILON {
90                            1.0 - (distance / edge).clamp(0.0, 1.0)
91                        } else {
92                            0.0
93                        };
94                        factor * self.density
95                    })
96                    .unwrap_or_default()
97            })
98            .reduce(|accum, density| accum.max(density))
99            .unwrap_or_default()
100    }
101
102    fn normal_at_point(
103        &self,
104        point: Vec3<Scalar>,
105        _: Vec3<Scalar>,
106        info: &BodyAccessInfo,
107    ) -> Vec3<Scalar> {
108        let extents = self.total_extents().into_array();
109        info.world_space_particles::<LOCKING, ()>()
110            .map(|(matrix, _)| {
111                let inv_matrix = matrix.inverted();
112                let point = inv_matrix.mul_point(point);
113                if point.is_approx_zero() {
114                    return Vec3::zero();
115                }
116                let direction = [
117                    (
118                        extents[0],
119                        (point.x + extents[0]).abs(),
120                        Vec3::new(-1.0, 0.0, 0.0),
121                    ),
122                    (
123                        extents[1],
124                        (point.y + extents[1]).abs(),
125                        Vec3::new(0.0, -1.0, 0.0),
126                    ),
127                    (
128                        extents[2],
129                        (point.z + extents[2]).abs(),
130                        Vec3::new(0.0, 0.0, -1.0),
131                    ),
132                    (
133                        extents[0],
134                        (extents[0] - point.x).abs(),
135                        Vec3::new(1.0, 0.0, 0.0),
136                    ),
137                    (
138                        extents[1],
139                        (extents[1] - point.y).abs(),
140                        Vec3::new(0.0, 1.0, 0.0),
141                    ),
142                    (
143                        extents[2],
144                        (extents[2] - point.z).abs(),
145                        Vec3::new(0.0, 0.0, 1.0),
146                    ),
147                ]
148                .into_iter()
149                .filter(|(size, _, _)| *size > Scalar::EPSILON)
150                .min_by(|(_, a, _), (_, b, _)| a.partial_cmp(b).unwrap_or(Ordering::Equal))
151                .map(|(_, _, normal)| normal)
152                .unwrap_or_default();
153                matrix.mul_direction(direction)
154            })
155            .reduce(|accum, direction| accum + direction)
156            // TODO: maybe weight directions so ones closer to the surface have more influence?
157            .and_then(|normal| normal.try_normalized())
158            .unwrap_or_default()
159    }
160}
161
162#[cfg(test)]
163mod tests {
164    use super::*;
165    use crate::{
166        components::{
167            BodyDensityFieldRelation, BodyParentRelation, BodyParticleRelation, PhysicsBody,
168            PhysicsParticle, Position,
169        },
170        density_fields::{DensityFieldBox, DensityRange},
171    };
172    use anput::world::World;
173
174    #[test]
175    fn test_cube_density_field() {
176        let mut world = World::default();
177        let object = world
178            .spawn((
179                PhysicsBody,
180                PhysicsParticle,
181                Position::new(Vec3::new(1.0, 2.0, 3.0)),
182                DensityFieldBox::new(CubeDensityField::<true>::new_hard(1.0, 10.0.into())),
183            ))
184            .unwrap();
185        world
186            .relate::<true, _>(BodyParticleRelation, object, object)
187            .unwrap();
188        world
189            .relate::<true, _>(BodyDensityFieldRelation, object, object)
190            .unwrap();
191        world
192            .relate::<true, _>(BodyParentRelation, object, object)
193            .unwrap();
194
195        let cube = world
196            .entity::<true, &DensityFieldBox>(object)
197            .unwrap()
198            .as_any()
199            .downcast_ref::<CubeDensityField<true>>()
200            .unwrap();
201        let info = BodyAccessInfo::of_world(object, &world);
202
203        assert_eq!(
204            cube.aabb(&info),
205            Aabb {
206                min: Vec3::new(-9.0, -8.0, -7.0),
207                max: Vec3::new(11.0, 12.0, 13.0),
208            }
209        );
210
211        assert_eq!(cube.density_at_point(Vec3::new(1.0, 2.0, 3.0), &info), 1.0);
212        assert_eq!(
213            cube.density_at_point(Vec3::new(-9.0, -8.0, -7.0), &info),
214            0.0
215        );
216        assert_eq!(
217            cube.density_at_point(Vec3::new(11.0, 12.0, 13.0), &info),
218            0.0
219        );
220        assert_eq!(
221            cube.density_at_point(Vec3::new(-8.0, -7.0, -6.0), &info),
222            1.0
223        );
224        assert_eq!(
225            cube.density_at_point(Vec3::new(10.0, 11.0, 12.0), &info),
226            1.0
227        );
228
229        assert_eq!(
230            cube.density_at_region(
231                Aabb {
232                    min: Vec3::new(-8.0, -7.0, -6.0),
233                    max: Vec3::new(9.0, 10.0, 11.0)
234                },
235                &info
236            ),
237            DensityRange { min: 1.0, max: 1.0 }
238        );
239        assert_eq!(
240            cube.density_at_region(
241                Aabb {
242                    min: Vec3::new(-10.0, -9.0, -8.0),
243                    max: Vec3::new(12.0, 13.0, 14.0)
244                },
245                &info
246            ),
247            DensityRange { min: 0.0, max: 1.0 }
248        );
249        assert_eq!(
250            cube.density_at_region(
251                Aabb {
252                    min: Vec3::new(100.0, 100.0, 100.0),
253                    max: Vec3::new(200.0, 200.0, 200.0)
254                },
255                &info
256            ),
257            DensityRange { min: 0.0, max: 0.0 }
258        );
259
260        assert_eq!(
261            cube.normal_at_point(Vec3::new(1.0, 2.0, 3.0), Default::default(), &info),
262            Vec3::new(0.0, 0.0, 0.0)
263        );
264        assert_eq!(
265            cube.normal_at_point(Vec3::new(2.0, 2.0, 3.0), Default::default(), &info),
266            Vec3::new(1.0, 0.0, 0.0)
267        );
268        assert_eq!(
269            cube.normal_at_point(Vec3::new(0.0, 2.0, 3.0), Default::default(), &info),
270            Vec3::new(-1.0, 0.0, 0.0)
271        );
272        assert_eq!(
273            cube.normal_at_point(Vec3::new(1.0, 3.0, 3.0), Default::default(), &info),
274            Vec3::new(0.0, 1.0, 0.0)
275        );
276        assert_eq!(
277            cube.normal_at_point(Vec3::new(1.0, 1.0, 3.0), Default::default(), &info),
278            Vec3::new(0.0, -1.0, 0.0)
279        );
280    }
281}
anput_physics/density_fields/cube.rs

anput_physics/density_fields/
cube.rs
