1use dynamis_math::{add, dot, length, mul, negate, normalize, sub};
2use dynamis_model::{BodyDesc, BodyHandle, ColliderDesc, QueryFilter, Shape};
3use dynamis_world::{QueryHit, World};
4
5const SKIN: f32 = 0.05;
6
7pub struct CharacterDesc {
8 pub radius: f32,
9 pub half_height: f32,
10 pub step_height: f32,
11 pub slope_limit: f32,
12 pub max_speed: f32,
13 pub jump_speed: f32,
14}
15
16impl Default for CharacterDesc {
17 fn default() -> Self {
18 Self {
19 radius: 0.4,
20 half_height: 0.5,
21 step_height: 0.3,
22 slope_limit: 50.0_f32.to_radians(),
23 max_speed: 4.0,
24 jump_speed: 5.0,
25 }
26 }
27}
28
29pub struct Character {
30 body: BodyHandle,
31 desc: CharacterDesc,
32 up: [f32; 3],
33 vertical: f32,
34 grounded: bool,
35 position: [f32; 3],
36}
37
38impl Character {
39 pub fn spawn(world: &mut World, position: [f32; 3], desc: CharacterDesc) -> Self {
40 assert!(desc.radius > 0.0, "character radius must be positive");
41 assert!(
42 desc.half_height >= 0.0,
43 "character half height must be non-negative"
44 );
45 assert!(
46 desc.step_height >= 0.0,
47 "character step height must be non-negative"
48 );
49 assert!(desc.max_speed > 0.0, "character max speed must be positive");
50 assert!(
51 desc.jump_speed >= 0.0,
52 "character jump speed must be non-negative"
53 );
54 assert!(
55 (0.0..=std::f32::consts::FRAC_PI_2).contains(&desc.slope_limit),
56 "character slope limit must be within [0, pi/2]"
57 );
58 let body = world.spawn(
59 BodyDesc::new(ColliderDesc::new(Shape::capsule(
60 desc.radius,
61 desc.half_height,
62 )))
63 .position(position)
64 .kinematic(true),
65 );
66 Self {
67 body,
68 desc,
69 up: up_of(world.config().gravity),
70 vertical: 0.0,
71 grounded: true,
72 position,
73 }
74 }
75
76 pub fn step(&mut self, world: &mut World, dt: f32, move_dir: [f32; 3], jump: bool) {
77 assert!(dt > 0.0, "character dt must be positive");
78 let up = up_of(world.config().gravity);
79 self.up = up;
80 let gravity_magnitude = length(world.config().gravity);
81 let horizontal = if length(move_dir) > 0.0 {
82 mul(normalize(move_dir), self.desc.max_speed)
83 } else {
84 [0.0; 3]
85 };
86 let mut jumped = false;
87 if self.grounded {
88 self.vertical = 0.0;
89 if jump && gravity_magnitude > 0.0 {
90 self.vertical = self.desc.jump_speed;
91 self.grounded = false;
92 jumped = true;
93 }
94 } else {
95 self.vertical -= gravity_magnitude * dt;
96 }
97 let vertical = self.vertical;
98 let position = self.position;
99 let probe = Shape::sphere(self.desc.radius);
100 let identity = [0.0, 0.0, 0.0, 1.0];
101 let bottom = add(position, mul(up, -self.desc.half_height));
102 let top = add(position, mul(up, self.desc.half_height));
103 let filter = QueryFilter {
104 exclude: Some(self.body),
105 max_hits: 1,
106 ..QueryFilter::default()
107 };
108 let horizontal_speed = length(horizontal);
109 let forward_length = horizontal_speed * dt + SKIN;
110 let down_length = SKIN + (vertical.abs() + horizontal_speed) * dt;
111 let up_length = SKIN + vertical.max(0.0) * dt;
112 let forward_dir = if horizontal_speed > 0.0 {
113 Some(normalize(horizontal))
114 } else {
115 None
116 };
117 let forward = forward_dir.map(|direction| {
118 world.sweep_query(
119 &probe,
120 identity,
121 position,
122 direction,
123 forward_length,
124 &filter,
125 )
126 });
127 let forward_low = forward_dir.map(|direction| {
128 world.sweep_query(&probe, identity, bottom, direction, forward_length, &filter)
129 });
130 let lifted = add(position, mul(up, self.desc.step_height));
131 let lifted_forward = forward_dir.map(|direction| {
132 world.sweep_query(&probe, identity, lifted, direction, forward_length, &filter)
133 });
134 let down = if !jumped {
135 Some(world.sweep_query(&probe, identity, bottom, negate(up), down_length, &filter))
136 } else {
137 None
138 };
139 let up_hit = if vertical > 0.0 {
140 Some(world.sweep_query(&probe, identity, top, up, up_length, &filter))
141 } else {
142 None
143 };
144 world.flush_queries();
145
146 let mut target = position;
147 let mut grounded = false;
148 let mut vertical_after = vertical;
149 let mut cpu_horizontal = horizontal;
150 let mut press_dir = [0.0; 3];
151 let slope_cos = self.desc.slope_limit.cos();
152 let forward_hit = pick_forward(
153 forward.and_then(|handle| world.query_hit(handle)),
154 forward_low.and_then(|handle| world.query_hit(handle)),
155 up,
156 );
157 let lifted_hit = lifted_forward.and_then(|handle| world.query_hit(handle));
158 let down_hit = down.and_then(|handle| world.query_hit(handle));
159 let ceiling_hit = up_hit.and_then(|handle| world.query_hit(handle));
160
161 if !jumped && let Some(landing) = down_hit {
162 target = add(
163 target,
164 mul(up, -(landing.distance - SKIN).clamp(0.0, down_length)),
165 );
166 let surface = negate(landing.normal);
167 grounded = dot(surface, up) > slope_cos;
168 vertical_after = if grounded { 0.0 } else { vertical };
169 }
170 if let Some(_hit) = forward_hit {
171 match lifted_hit {
172 None => {
173 target = lifted;
174 grounded = false;
175 vertical_after = vertical;
176 }
177 Some(_) => {
178 cpu_horizontal = [0.0; 3];
179 press_dir = normalize(horizontal);
180 }
181 }
182 }
183
184 if let Some(_ceiling) = ceiling_hit
185 && vertical_after > 0.0
186 {
187 vertical_after = 0.0;
188 }
189
190 let velocity = add(horizontal, mul(up, vertical_after));
191 let moved = add(mul(cpu_horizontal, dt), mul(up, vertical_after * dt));
192 self.position = add(target, moved);
193 self.grounded = grounded;
194 self.vertical = vertical_after;
195 let press = mul(press_dir, SKIN * 0.5);
196 let pre_move = sub(add(self.position, press), mul(velocity, dt));
197 world.set_position(self.body, pre_move);
198 world.set_velocity(self.body, velocity);
199 }
200
201 pub fn body(&self) -> BodyHandle {
202 self.body
203 }
204
205 pub fn position(&self) -> [f32; 3] {
206 self.position
207 }
208
209 pub fn grounded(&self) -> bool {
210 self.grounded
211 }
212
213 pub fn vertical_speed(&self) -> f32 {
214 self.vertical
215 }
216}
217
218fn up_of(gravity: [f32; 3]) -> [f32; 3] {
219 let magnitude = length(gravity);
220 if magnitude > 0.0 {
221 mul(gravity, -1.0 / magnitude)
222 } else {
223 [0.0, 1.0, 0.0]
224 }
225}
226
227fn pick_forward(
228 first: Option<QueryHit>,
229 second: Option<QueryHit>,
230 up: [f32; 3],
231) -> Option<QueryHit> {
232 let floor_like = |hit: &QueryHit| dot(hit.normal, up) < -0.5;
233 match (first, second) {
234 (Some(a), Some(b)) => {
235 if floor_like(&a) && floor_like(&b) {
236 None
237 } else if floor_like(&a) {
238 Some(b)
239 } else if floor_like(&b) {
240 Some(a)
241 } else {
242 Some(if a.distance <= b.distance { a } else { b })
243 }
244 }
245 (Some(a), None) => {
246 if floor_like(&a) {
247 None
248 } else {
249 Some(a)
250 }
251 }
252 (None, Some(b)) => {
253 if floor_like(&b) {
254 None
255 } else {
256 Some(b)
257 }
258 }
259 (None, None) => None,
260 }
261}