1use crate::stability::{PlacedBox, StabilityAnalyzer, StabilityConstraint, StabilityReport};
29use nalgebra::{Point3, Vector3};
30use std::time::Instant;
31
32#[cfg(feature = "serde")]
33use serde::{Deserialize, Serialize};
34
35#[derive(Debug, Clone)]
37#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
38pub struct PhysicsConfig {
39 pub gravity: f64,
41 pub time_step: f64,
43 pub max_simulation_time: f64,
45 pub rest_velocity_threshold: f64,
47 pub convergence_threshold: f64,
49 pub friction: f64,
51 pub restitution: f64,
53 pub solver_iterations: usize,
55 pub enable_shaking: bool,
57 pub shake_amplitude: f64,
59 pub shake_frequency: f64,
61}
62
63impl Default for PhysicsConfig {
64 fn default() -> Self {
65 Self {
66 gravity: -9.81,
67 time_step: 0.016, max_simulation_time: 5.0,
69 rest_velocity_threshold: 0.001,
70 convergence_threshold: 0.0001,
71 friction: 0.5,
72 restitution: 0.1,
73 solver_iterations: 10,
74 enable_shaking: false,
75 shake_amplitude: 0.5,
76 shake_frequency: 5.0,
77 }
78 }
79}
80
81impl PhysicsConfig {
82 pub fn new() -> Self {
84 Self::default()
85 }
86
87 pub fn with_gravity(mut self, gravity: f64) -> Self {
89 self.gravity = gravity;
90 self
91 }
92
93 pub fn with_time_step(mut self, dt: f64) -> Self {
95 self.time_step = dt.max(0.001);
96 self
97 }
98
99 pub fn with_max_time(mut self, max_time: f64) -> Self {
101 self.max_simulation_time = max_time;
102 self
103 }
104
105 pub fn with_friction(mut self, friction: f64) -> Self {
107 self.friction = friction.clamp(0.0, 1.0);
108 self
109 }
110
111 pub fn with_restitution(mut self, restitution: f64) -> Self {
113 self.restitution = restitution.clamp(0.0, 1.0);
114 self
115 }
116
117 pub fn with_shaking(mut self, amplitude: f64, frequency: f64) -> Self {
119 self.enable_shaking = true;
120 self.shake_amplitude = amplitude.max(0.0);
121 self.shake_frequency = frequency.max(0.1);
122 self
123 }
124}
125
126#[derive(Debug, Clone)]
128struct PhysicsBody {
129 index: usize,
131 position: Point3<f64>,
133 velocity: Vector3<f64>,
135 dimensions: Vector3<f64>,
137 mass: f64,
139 at_rest: bool,
141 on_floor: bool,
143}
144
145impl PhysicsBody {
146 fn from_placed_box(index: usize, placed: &PlacedBox) -> Self {
147 Self {
148 index,
149 position: placed.position,
150 velocity: Vector3::zeros(),
151 dimensions: placed.dimensions,
152 mass: placed.mass.unwrap_or(1.0),
153 at_rest: false,
154 on_floor: false,
155 }
156 }
157
158 fn aabb_min(&self) -> Point3<f64> {
159 self.position
160 }
161
162 fn aabb_max(&self) -> Point3<f64> {
163 Point3::new(
164 self.position.x + self.dimensions.x,
165 self.position.y + self.dimensions.y,
166 self.position.z + self.dimensions.z,
167 )
168 }
169
170 fn center(&self) -> Point3<f64> {
171 Point3::new(
172 self.position.x + self.dimensions.x / 2.0,
173 self.position.y + self.dimensions.y / 2.0,
174 self.position.z + self.dimensions.z / 2.0,
175 )
176 }
177}
178
179#[derive(Debug, Clone)]
181#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
182pub struct PhysicsResult {
183 pub final_positions: Vec<(String, usize, Point3<f64>)>,
185 pub position_changes: Vec<(String, usize, Vector3<f64>)>,
187 pub steps: usize,
189 pub simulation_time: f64,
191 pub boxes_moved: usize,
193 pub boxes_at_rest: usize,
195 pub avg_change: f64,
197 pub max_change: f64,
199 pub converged: bool,
201 pub computation_time_ms: u64,
203 pub stability_report: Option<StabilityReport>,
205}
206
207impl PhysicsResult {
208 pub fn new() -> Self {
210 Self {
211 final_positions: Vec::new(),
212 position_changes: Vec::new(),
213 steps: 0,
214 simulation_time: 0.0,
215 boxes_moved: 0,
216 boxes_at_rest: 0,
217 avg_change: 0.0,
218 max_change: 0.0,
219 converged: false,
220 computation_time_ms: 0,
221 stability_report: None,
222 }
223 }
224}
225
226impl Default for PhysicsResult {
227 fn default() -> Self {
228 Self::new()
229 }
230}
231
232pub struct PhysicsSimulator {
234 config: PhysicsConfig,
235}
236
237impl PhysicsSimulator {
238 pub fn new(config: PhysicsConfig) -> Self {
240 Self { config }
241 }
242
243 pub fn simulate_settlement(
245 &self,
246 boxes: &[PlacedBox],
247 container_dims: Vector3<f64>,
248 floor_z: f64,
249 ) -> PhysicsResult {
250 let start = Instant::now();
251 let mut result = PhysicsResult::new();
252
253 if boxes.is_empty() {
254 result.computation_time_ms = start.elapsed().as_millis() as u64;
255 return result;
256 }
257
258 let mut bodies: Vec<PhysicsBody> = boxes
260 .iter()
261 .enumerate()
262 .map(|(i, b)| PhysicsBody::from_placed_box(i, b))
263 .collect();
264
265 let initial_positions: Vec<Point3<f64>> = bodies.iter().map(|b| b.position).collect();
266
267 let mut sim_time = 0.0;
269 let mut steps = 0;
270 let max_steps = (self.config.max_simulation_time / self.config.time_step) as usize;
271
272 while sim_time < self.config.max_simulation_time && steps < max_steps {
273 for body in &mut bodies {
275 if !body.at_rest {
276 body.velocity.z += self.config.gravity * self.config.time_step;
277 }
278 }
279
280 if self.config.enable_shaking {
282 let shake_offset = self.config.shake_amplitude
283 * (2.0 * std::f64::consts::PI * self.config.shake_frequency * sim_time).sin();
284
285 for body in &mut bodies {
286 if !body.at_rest {
287 body.velocity.x += shake_offset * 0.1;
288 body.velocity.y += shake_offset * 0.1;
289 }
290 }
291 }
292
293 for body in &mut bodies {
295 if !body.at_rest {
296 body.position.x += body.velocity.x * self.config.time_step;
297 body.position.y += body.velocity.y * self.config.time_step;
298 body.position.z += body.velocity.z * self.config.time_step;
299 }
300 }
301
302 self.resolve_floor_collisions(&mut bodies, floor_z);
304 self.resolve_container_collisions(&mut bodies, container_dims);
305 self.resolve_box_collisions(&mut bodies);
306
307 let mut all_at_rest = true;
309 for body in &mut bodies {
310 if body.velocity.norm() < self.config.rest_velocity_threshold && body.on_floor {
311 body.at_rest = true;
312 }
313 if !body.at_rest {
314 all_at_rest = false;
315 }
316 }
317
318 if all_at_rest {
319 result.converged = true;
320 break;
321 }
322
323 sim_time += self.config.time_step;
324 steps += 1;
325 }
326
327 result.steps = steps;
329 result.simulation_time = sim_time;
330 result.boxes_at_rest = bodies.iter().filter(|b| b.at_rest).count();
331
332 let mut total_change = 0.0;
333 for (i, body) in bodies.iter().enumerate() {
334 let initial = initial_positions[i];
335 let change = Vector3::new(
336 body.position.x - initial.x,
337 body.position.y - initial.y,
338 body.position.z - initial.z,
339 );
340
341 let change_mag = change.norm();
342 total_change += change_mag;
343
344 if change_mag > self.config.convergence_threshold {
345 result.boxes_moved += 1;
346 result.max_change = result.max_change.max(change_mag);
347 }
348
349 result.final_positions.push((
350 boxes[body.index].id.clone(),
351 boxes[body.index].instance,
352 body.position,
353 ));
354
355 result.position_changes.push((
356 boxes[body.index].id.clone(),
357 boxes[body.index].instance,
358 change,
359 ));
360 }
361
362 result.avg_change = total_change / bodies.len() as f64;
363
364 let final_boxes: Vec<PlacedBox> = bodies
366 .iter()
367 .enumerate()
368 .map(|(i, body)| {
369 PlacedBox::new(
370 boxes[i].id.clone(),
371 boxes[i].instance,
372 body.position,
373 body.dimensions,
374 )
375 .with_mass(body.mass)
376 })
377 .collect();
378
379 let analyzer = StabilityAnalyzer::new(StabilityConstraint::partial_base(0.5));
380 result.stability_report = Some(analyzer.analyze(&final_boxes, floor_z));
381
382 result.computation_time_ms = start.elapsed().as_millis() as u64;
383 result
384 }
385
386 pub fn validate_stability(
388 &self,
389 boxes: &[PlacedBox],
390 container_dims: Vector3<f64>,
391 floor_z: f64,
392 ) -> StabilityReport {
393 let result = self.simulate_settlement(boxes, container_dims, floor_z);
395
396 result.stability_report.unwrap_or_default()
397 }
398
399 fn resolve_floor_collisions(&self, bodies: &mut [PhysicsBody], floor_z: f64) {
401 for body in bodies.iter_mut() {
402 if body.position.z < floor_z {
403 body.position.z = floor_z;
404 body.velocity.z = -body.velocity.z * self.config.restitution;
405
406 body.velocity.x *= 1.0 - self.config.friction;
408 body.velocity.y *= 1.0 - self.config.friction;
409
410 body.on_floor = true;
411 }
412 }
413 }
414
415 fn resolve_container_collisions(
417 &self,
418 bodies: &mut [PhysicsBody],
419 container_dims: Vector3<f64>,
420 ) {
421 for body in bodies.iter_mut() {
422 if body.position.x < 0.0 {
424 body.position.x = 0.0;
425 body.velocity.x = -body.velocity.x * self.config.restitution;
426 } else if body.position.x + body.dimensions.x > container_dims.x {
427 body.position.x = container_dims.x - body.dimensions.x;
428 body.velocity.x = -body.velocity.x * self.config.restitution;
429 }
430
431 if body.position.y < 0.0 {
433 body.position.y = 0.0;
434 body.velocity.y = -body.velocity.y * self.config.restitution;
435 } else if body.position.y + body.dimensions.y > container_dims.y {
436 body.position.y = container_dims.y - body.dimensions.y;
437 body.velocity.y = -body.velocity.y * self.config.restitution;
438 }
439
440 if body.position.z + body.dimensions.z > container_dims.z {
442 body.position.z = container_dims.z - body.dimensions.z;
443 body.velocity.z = -body.velocity.z * self.config.restitution;
444 }
445 }
446 }
447
448 fn resolve_box_collisions(&self, bodies: &mut [PhysicsBody]) {
450 let n = bodies.len();
451
452 for _ in 0..self.config.solver_iterations {
453 for i in 0..n {
454 for j in (i + 1)..n {
455 let a_min = bodies[i].aabb_min();
457 let a_max = bodies[i].aabb_max();
458 let b_min = bodies[j].aabb_min();
459 let b_max = bodies[j].aabb_max();
460
461 if a_max.x <= b_min.x
462 || b_max.x <= a_min.x
463 || a_max.y <= b_min.y
464 || b_max.y <= a_min.y
465 || a_max.z <= b_min.z
466 || b_max.z <= a_min.z
467 {
468 continue; }
470
471 let overlap_x = (a_max.x.min(b_max.x) - a_min.x.max(b_min.x)).max(0.0);
473 let overlap_y = (a_max.y.min(b_max.y) - a_min.y.max(b_min.y)).max(0.0);
474 let overlap_z = (a_max.z.min(b_max.z) - a_min.z.max(b_min.z)).max(0.0);
475
476 let min_overlap = overlap_x.min(overlap_y).min(overlap_z);
478
479 if min_overlap <= 0.0 {
480 continue;
481 }
482
483 let center_a = bodies[i].center();
485 let center_b = bodies[j].center();
486
487 let total_mass = bodies[i].mass + bodies[j].mass;
489 let ratio_a = bodies[j].mass / total_mass;
490 let ratio_b = bodies[i].mass / total_mass;
491
492 if overlap_z == min_overlap {
493 if center_a.z < center_b.z {
495 bodies[i].position.z -= min_overlap * ratio_a;
496 bodies[j].position.z += min_overlap * ratio_b;
497 } else {
498 bodies[i].position.z += min_overlap * ratio_a;
499 bodies[j].position.z -= min_overlap * ratio_b;
500 }
501 let v_a = bodies[i].velocity.z;
503 let v_b = bodies[j].velocity.z;
504 bodies[i].velocity.z =
505 v_b * self.config.restitution * ratio_b + v_a * (1.0 - ratio_b);
506 bodies[j].velocity.z =
507 v_a * self.config.restitution * ratio_a + v_b * (1.0 - ratio_a);
508
509 if center_a.z > center_b.z {
511 bodies[i].on_floor = bodies[j].on_floor;
513 }
514 } else if overlap_x == min_overlap {
515 if center_a.x < center_b.x {
517 bodies[i].position.x -= min_overlap * ratio_a;
518 bodies[j].position.x += min_overlap * ratio_b;
519 } else {
520 bodies[i].position.x += min_overlap * ratio_a;
521 bodies[j].position.x -= min_overlap * ratio_b;
522 }
523 } else {
524 if center_a.y < center_b.y {
526 bodies[i].position.y -= min_overlap * ratio_a;
527 bodies[j].position.y += min_overlap * ratio_b;
528 } else {
529 bodies[i].position.y += min_overlap * ratio_a;
530 bodies[j].position.y -= min_overlap * ratio_b;
531 }
532 }
533 }
534 }
535 }
536 }
537
538 pub fn shake_compact(
540 &self,
541 boxes: &[PlacedBox],
542 container_dims: Vector3<f64>,
543 floor_z: f64,
544 shake_time: f64,
545 ) -> PhysicsResult {
546 let mut shake_config = self.config.clone();
547 shake_config.enable_shaking = true;
548 shake_config.max_simulation_time = shake_time;
549
550 let shaking_sim = PhysicsSimulator::new(shake_config);
551 let shake_result = shaking_sim.simulate_settlement(boxes, container_dims, floor_z);
552
553 let final_boxes: Vec<PlacedBox> = shake_result
555 .final_positions
556 .iter()
557 .zip(boxes.iter())
558 .map(|((_, _, pos), original)| {
559 PlacedBox::new(
560 original.id.clone(),
561 original.instance,
562 *pos,
563 original.dimensions,
564 )
565 .with_mass(original.mass.unwrap_or(1.0))
566 })
567 .collect();
568
569 self.simulate_settlement(&final_boxes, container_dims, floor_z)
571 }
572}
573
574impl Default for PhysicsSimulator {
575 fn default() -> Self {
576 Self::new(PhysicsConfig::default())
577 }
578}
579
580pub fn compute_utilization(boxes: &[PlacedBox], container_dims: Vector3<f64>) -> f64 {
582 let total_volume: f64 = boxes.iter().map(|b| b.volume()).sum();
583 let container_volume = container_dims.x * container_dims.y * container_dims.z;
584 total_volume / container_volume
585}
586
587pub fn compute_effective_height(boxes: &[PlacedBox]) -> f64 {
589 boxes
590 .iter()
591 .map(|b| b.position.z + b.dimensions.z)
592 .fold(0.0, f64::max)
593}
594
595#[cfg(test)]
596mod tests {
597 use super::*;
598
599 #[test]
600 fn test_physics_config_default() {
601 let config = PhysicsConfig::default();
602 assert!(config.gravity < 0.0);
603 assert!(config.time_step > 0.0);
604 assert!(!config.enable_shaking);
605 }
606
607 #[test]
608 fn test_physics_config_builder() {
609 let config = PhysicsConfig::new()
610 .with_gravity(-10.0)
611 .with_time_step(0.01)
612 .with_friction(0.8)
613 .with_restitution(0.2);
614
615 assert!((config.gravity - (-10.0)).abs() < 0.001);
616 assert!((config.time_step - 0.01).abs() < 0.001);
617 assert!((config.friction - 0.8).abs() < 0.001);
618 assert!((config.restitution - 0.2).abs() < 0.001);
619 }
620
621 #[test]
622 fn test_single_box_on_floor() {
623 let simulator = PhysicsSimulator::default();
624
625 let boxes = vec![PlacedBox::new(
626 "B1",
627 0,
628 Point3::new(10.0, 10.0, 0.0), Vector3::new(20.0, 20.0, 20.0),
630 )];
631
632 let container = Vector3::new(100.0, 100.0, 100.0);
633 let result = simulator.simulate_settlement(&boxes, container, 0.0);
634
635 assert!(result.steps > 0 || result.converged);
637 assert!(result.max_change < 5.0);
639 }
640
641 #[test]
642 fn test_falling_box() {
643 let simulator = PhysicsSimulator::new(
644 PhysicsConfig::default()
645 .with_max_time(5.0) .with_time_step(0.01),
647 );
648
649 let boxes = vec![PlacedBox::new(
650 "B1",
651 0,
652 Point3::new(10.0, 10.0, 50.0), Vector3::new(20.0, 20.0, 20.0),
654 )];
655
656 let container = Vector3::new(100.0, 100.0, 100.0);
657 let result = simulator.simulate_settlement(&boxes, container, 0.0);
658
659 assert!(result.steps > 0);
661
662 let (_, _, final_pos) = &result.final_positions[0];
664 assert!(final_pos.z < 50.0); }
666
667 #[test]
668 fn test_stacked_boxes_settlement() {
669 let simulator = PhysicsSimulator::new(PhysicsConfig::default().with_max_time(3.0));
670
671 let boxes = vec![
672 PlacedBox::new(
673 "B1",
674 0,
675 Point3::new(0.0, 0.0, 0.0),
676 Vector3::new(50.0, 50.0, 20.0),
677 )
678 .with_mass(10.0),
679 PlacedBox::new(
680 "B2",
681 0,
682 Point3::new(0.0, 0.0, 22.0), Vector3::new(50.0, 50.0, 20.0),
684 )
685 .with_mass(5.0),
686 ];
687
688 let container = Vector3::new(100.0, 100.0, 100.0);
689 let result = simulator.simulate_settlement(&boxes, container, 0.0);
690
691 assert!(result.converged || result.simulation_time > 0.0);
693
694 assert!(result.stability_report.is_some());
696 }
697
698 #[test]
699 fn test_container_collision() {
700 let simulator = PhysicsSimulator::new(PhysicsConfig::default().with_max_time(1.0));
701
702 let boxes = vec![PlacedBox::new(
704 "B1",
705 0,
706 Point3::new(-10.0, 50.0, 0.0), Vector3::new(20.0, 20.0, 20.0),
708 )];
709
710 let container = Vector3::new(100.0, 100.0, 100.0);
711 let result = simulator.simulate_settlement(&boxes, container, 0.0);
712
713 let (_, _, final_pos) = &result.final_positions[0];
715 assert!(final_pos.x >= 0.0);
716 }
717
718 #[test]
719 fn test_shaking_compaction() {
720 let simulator = PhysicsSimulator::new(
721 PhysicsConfig::default().with_shaking(1.0, 10.0), );
723
724 let boxes = vec![
725 PlacedBox::new(
726 "B1",
727 0,
728 Point3::new(0.0, 0.0, 0.0),
729 Vector3::new(30.0, 30.0, 20.0),
730 ),
731 PlacedBox::new(
732 "B2",
733 0,
734 Point3::new(35.0, 0.0, 0.0), Vector3::new(30.0, 30.0, 20.0),
736 ),
737 ];
738
739 let container = Vector3::new(100.0, 100.0, 100.0);
740 let result = simulator.shake_compact(&boxes, container, 0.0, 2.0);
741
742 assert_eq!(result.final_positions.len(), 2);
744 }
745
746 #[test]
747 fn test_compute_utilization() {
748 let boxes = vec![
749 PlacedBox::new(
750 "B1",
751 0,
752 Point3::new(0.0, 0.0, 0.0),
753 Vector3::new(10.0, 10.0, 10.0),
754 ), PlacedBox::new(
756 "B2",
757 0,
758 Point3::new(10.0, 0.0, 0.0),
759 Vector3::new(10.0, 10.0, 10.0),
760 ), ];
762
763 let container = Vector3::new(100.0, 100.0, 100.0); let util = compute_utilization(&boxes, container);
765
766 assert!((util - 0.002).abs() < 0.001); }
768
769 #[test]
770 fn test_compute_effective_height() {
771 let boxes = vec![
772 PlacedBox::new(
773 "B1",
774 0,
775 Point3::new(0.0, 0.0, 0.0),
776 Vector3::new(10.0, 10.0, 30.0),
777 ),
778 PlacedBox::new(
779 "B2",
780 0,
781 Point3::new(10.0, 0.0, 0.0),
782 Vector3::new(10.0, 10.0, 50.0),
783 ),
784 ];
785
786 let height = compute_effective_height(&boxes);
787 assert!((height - 50.0).abs() < 0.001);
788 }
789
790 #[test]
791 fn test_validate_stability() {
792 let simulator = PhysicsSimulator::new(PhysicsConfig::default().with_max_time(1.0));
793
794 let boxes = vec![
795 PlacedBox::new(
796 "B1",
797 0,
798 Point3::new(0.0, 0.0, 0.0),
799 Vector3::new(50.0, 50.0, 20.0),
800 ),
801 PlacedBox::new(
802 "B2",
803 0,
804 Point3::new(0.0, 0.0, 20.0), Vector3::new(50.0, 50.0, 20.0),
806 ),
807 ];
808
809 let container = Vector3::new(100.0, 100.0, 100.0);
810 let report = simulator.validate_stability(&boxes, container, 0.0);
811
812 assert!(report.stable_count >= 1);
814 }
815
816 #[test]
817 fn test_empty_simulation() {
818 let simulator = PhysicsSimulator::default();
819 let container = Vector3::new(100.0, 100.0, 100.0);
820 let result = simulator.simulate_settlement(&[], container, 0.0);
821
822 assert!(result.final_positions.is_empty());
823 assert!(result.converged || result.steps == 0);
824 }
825}