Add support for Dropshot

rocketsim/Cargo.toml

@@ -19,7 +19,7 @@ byteorder = "1.5.0"
 derive_hash_fast = "0.2.3"
 env_logger = "0.11.8"
 fastrand = "2.3.0"
-glam = "0.32.1"
+glam = { version = "0.33.0", features = ["debug-glam-assert"] }
 log = "0.4.29"
 rocketsim_derive = { path = "./rocketsim_derive" }
 zerocopy = { version = "0.8.34", features = ["derive"] }
@@ -42,7 +42,7 @@ test-case = "3.3.1"
 [target.'cfg(not(any(target_arch = "x86", target_arch = "x86_64")))'.dependencies]
 # use glam with experimental support for portable SIMD when not running on x86
 # architectures
-glam = { version = "0.32.1", features = ["core-simd"], optional = true }
+glam = { version = "0.33.0", features = ["core-simd", "debug-glam-assert"], optional = true }
 
 [features]
 # default = ["vis"]

rocketsim/src/bullet/collision/broadphase/broadphase_proxy.rs

@@ -1,3 +1,5 @@
+use std::ops::{BitAnd, BitAndAssign, BitOr, BitOrAssign, BitXor};
+
 use glam::USizeVec3;
 
 use crate::shared::Aabb;
@@ -8,7 +10,66 @@ pub enum CollisionFilterGroups {
     HoopsNet = (1 << 2),
     DropshotTile = (1 << 3),
     DropshotFloor = (1 << 4),
-    All = -1,
+}
+
+impl CollisionFilterGroups {
+    pub const ALL: u8 = Self::Default as u8
+        | Self::Static as u8
+        | Self::HoopsNet as u8
+        | Self::DropshotTile as u8
+        | Self::DropshotFloor as u8;
+}
+
+impl BitOrAssign<CollisionFilterGroups> for u8 {
+    fn bitor_assign(&mut self, rhs: CollisionFilterGroups) {
+        *self |= rhs as u8;
+    }
+}
+
+impl BitAndAssign<CollisionFilterGroups> for u8 {
+    fn bitand_assign(&mut self, rhs: CollisionFilterGroups) {
+        *self &= rhs as u8;
+    }
+}
+
+impl BitAnd<CollisionFilterGroups> for u8 {
+    type Output = u8;
+
+    fn bitand(self, rhs: CollisionFilterGroups) -> Self::Output {
+        self & (rhs as u8)
+    }
+}
+
+impl BitOr<CollisionFilterGroups> for u8 {
+    type Output = u8;
+
+    fn bitor(self, rhs: CollisionFilterGroups) -> Self::Output {
+        self | (rhs as u8)
+    }
+}
+
+impl BitXor<CollisionFilterGroups> for u8 {
+    type Output = u8;
+
+    fn bitxor(self, rhs: CollisionFilterGroups) -> Self::Output {
+        self ^ (rhs as u8)
+    }
+}
+
+impl BitXor for CollisionFilterGroups {
+    type Output = u8;
+
+    fn bitxor(self, rhs: Self) -> Self::Output {
+        self as u8 ^ rhs as u8
+    }
+}
+
+impl BitOr for CollisionFilterGroups {
+    type Output = u8;
+
+    fn bitor(self, rhs: Self) -> Self::Output {
+        self as u8 | rhs as u8
+    }
 }
 
 pub struct BroadphaseProxy {

rocketsim/src/bullet/collision/broadphase/grid_broadphase.rs

@@ -204,7 +204,7 @@ impl GridBroadphase {
         let sbp = &mut self.handles[proxy_idx];
         sbp.aabb = aabb;
 
-        if sbp.collision_filter_group & CollisionFilterGroups::Static as u8 != 0 {
+        if (sbp.collision_filter_group & CollisionFilterGroups::Static) != 0 {
             self.cell_grid.update_cells_static(sbp, col_obj, proxy_idx);
         } else {
             let old_idx = sbp.cell_idx;
@@ -231,7 +231,7 @@ impl GridBroadphase {
     ) -> usize {
         debug_assert!(aabb.min.cmple(aabb.max).all());
 
-        let is_static = collision_filter_group & CollisionFilterGroups::Static as u8 != 0;
+        let is_static = (collision_filter_group & CollisionFilterGroups::Static) != 0;
         let new_handle_idx = self.handles.len();
         let indices = self.cell_grid.get_cell_indices(aabb.min);
         let cell_idx = self.cell_grid.cell_indices_to_idx(indices);
@@ -274,7 +274,7 @@ impl GridBroadphase {
             .enumerate()
             .skip(self.min_dyn_handle_idx)
             .filter(|(_, proxy)| {
-                proxy.collision_filter_group & CollisionFilterGroups::Static as u8 == 0
+                (proxy.collision_filter_group & CollisionFilterGroups::Static) == 0
             })
         {
             let cell = &self.cell_grid.cells[proxy.cell_idx];

rocketsim/src/bullet/collision/dispatch/collision_dispatcher.rs

@@ -6,6 +6,7 @@ use super::{
 use crate::bullet::{
     collision::{
         broadphase::{BroadphaseProxy, GridBroadphase},
+        dispatch::convex_convex_collision_alg,
         narrowphase::persistent_manifold::{ContactAddedCallback, PersistentManifold},
         shapes::collision_shape::CollisionShapes,
     },
@@ -79,6 +80,14 @@ impl CollisionDispatcher {
                     compound,
                     contact_added_callback,
                 ),
+                CollisionShapes::ConvexHull(_) => convex_convex_collision_alg::process_collision(
+                    &RigidBodyWrapper {
+                        obj: col_obj_a,
+                        world_trans: *col_obj_a.get_world_trans(),
+                    },
+                    col_obj_b,
+                    contact_added_callback,
+                ),
                 _ => unreachable!(),
             },
             CollisionShapes::TriangleMesh(mesh) => match col_obj_b.get_collision_shape() {
@@ -160,6 +169,14 @@ impl CollisionDispatcher {
                         contact_added_callback,
                     )
                 }
+                CollisionShapes::Sphere(_) => convex_convex_collision_alg::process_collision(
+                    &RigidBodyWrapper {
+                        obj: col_obj_a,
+                        world_trans: *col_obj_a.get_world_trans(),
+                    },
+                    col_obj_b,
+                    contact_added_callback,
+                ),
                 _ => unreachable!(),
             },
             CollisionShapes::Triangle(_) => unreachable!(),

rocketsim/src/bullet/collision/dispatch/collision_world.rs

@@ -39,18 +39,16 @@ impl CollisionWorld {
         filter_group: u8,
         filter_mask: u8,
     ) -> usize {
-        obj.world_array_idx = self.collision_objs.len();
+        let idx = self.collision_objs.len();
+        obj.world_array_idx = idx;
 
         let trans = obj.get_world_trans();
         let aabb = obj.get_collision_shape().get_aabb(trans);
-
         let proxy = self
             .broadphase_pair_cache
             .create_proxy(aabb, &obj, filter_group, filter_mask);
 
         obj.set_broadphase_handle(proxy);
-
-        let idx = self.collision_objs.len();
         self.collision_objs.push(obj);
 
         idx
@@ -91,7 +89,12 @@ impl CollisionWorld {
                 aabb += aabb2;
             }
 
-            debug_assert!(col_obj.is_static_obj() || (aabb.max - aabb.min).length_squared() < 1e12);
+            debug_assert!(
+                col_obj.is_static_obj() || (aabb.max - aabb.min).length_squared() < 1e12,
+                "object #{i} {:?} has invalid aabb: {:?}",
+                col_obj.user_idx,
+                aabb
+            );
             self.broadphase_pair_cache
                 .set_aabb(col_obj, col_obj.get_broadphase_handle(), aabb);
         }

rocketsim/src/bullet/collision/dispatch/internal_edge_utility.rs

@@ -115,12 +115,12 @@ impl ProcessTriangle for ConnectivityProcessor<'_> {
                     let mut computed_normal_b = orn * self.shape.normal;
                     if computed_normal_b.dot(tri.normal) < 0.0 {
                         computed_normal_b *= -1.0;
-                        info.flags |= TriInfoFlag::V0V1SwapNormalB as u8;
+                        info.flags |= TriInfoFlag::V0V1SwapNormalB;
                     }
 
                     info.edge_v0_v1_angle = -corrected_angle;
                     if is_convex {
-                        info.flags |= TriInfoFlag::V0V1Convex as u8;
+                        info.flags |= TriInfoFlag::V0V1Convex;
                     }
                 }
                 2 => {
@@ -129,12 +129,12 @@ impl ProcessTriangle for ConnectivityProcessor<'_> {
                     let mut computed_normal_b = orn * self.shape.normal;
                     if computed_normal_b.dot(tri.normal) < 0.0 {
                         computed_normal_b *= -1.0;
-                        info.flags |= TriInfoFlag::V2V0SwapNormalB as u8;
+                        info.flags |= TriInfoFlag::V2V0SwapNormalB;
                     }
 
                     info.edge_v2_v0_angle = -corrected_angle;
                     if is_convex {
-                        info.flags |= TriInfoFlag::V2V0Convex as u8;
+                        info.flags |= TriInfoFlag::V2V0Convex;
                     }
                 }
                 3 => {
@@ -143,12 +143,12 @@ impl ProcessTriangle for ConnectivityProcessor<'_> {
                     let mut computed_normal_b = orn * self.shape.normal;
                     if computed_normal_b.dot(tri.normal) < 0.0 {
                         computed_normal_b *= -1.0;
-                        info.flags |= TriInfoFlag::V1V2SwapNormalB as u8;
+                        info.flags |= TriInfoFlag::V1V2SwapNormalB;
                     }
 
                     info.edge_v1_v2_angle = -corrected_angle;
                     if is_convex {
-                        info.flags |= TriInfoFlag::V1V2Convex as u8;
+                        info.flags |= TriInfoFlag::V1V2Convex;
                     }
                 }
                 _ => unreachable!(),
@@ -273,14 +273,14 @@ pub fn adjust_internal_edge_contacts(
             }
 
             if info.edge_v0_v1_angle != 0.0 {
-                let is_edge_convex = (info.flags & TriInfoFlag::V0V1Convex as u8) != 0;
+                let is_edge_convex = (info.flags & TriInfoFlag::V0V1Convex) != 0;
                 let swap_factor = f32::from(is_edge_convex) * 2.0 - 1.0;
 
                 let edge = -tri.edges[0];
                 let n_a = swap_factor * tri.normal;
                 let orn = Quat::from_axis_angle_simd(edge.normalize(), info.edge_v0_v1_angle);
                 let mut computed_normal_b = orn * tri.normal;
-                if (info.flags & TriInfoFlag::V0V1SwapNormalB as u8) != 0 {
+                if (info.flags & TriInfoFlag::V0V1SwapNormalB) != 0 {
                     computed_normal_b *= -1.0;
                 }
                 let n_b = swap_factor * computed_normal_b;
@@ -319,14 +319,14 @@ pub fn adjust_internal_edge_contacts(
             }
 
             if info.edge_v1_v2_angle != 0.0 {
-                let is_edge_convex = (info.flags & TriInfoFlag::V1V2Convex as u8) != 0;
+                let is_edge_convex = (info.flags & TriInfoFlag::V1V2Convex) != 0;
                 let swap_factor = f32::from(is_edge_convex) * 2.0 - 1.0;
 
                 let edge = -tri.edges[1];
                 let n_a = swap_factor * tri.normal;
                 let orn = Quat::from_axis_angle_simd(edge.normalize(), info.edge_v1_v2_angle);
                 let mut computed_normal_b = orn * tri.normal;
-                if (info.flags & TriInfoFlag::V1V2SwapNormalB as u8) != 0 {
+                if (info.flags & TriInfoFlag::V1V2SwapNormalB) != 0 {
                     computed_normal_b *= -1.0;
                 }
                 let n_b = swap_factor * computed_normal_b;
@@ -365,14 +365,14 @@ pub fn adjust_internal_edge_contacts(
             }
 
             if info.edge_v2_v0_angle != 0.0 {
-                let is_edge_convex = (info.flags & TriInfoFlag::V2V0Convex as u8) != 0;
+                let is_edge_convex = (info.flags & TriInfoFlag::V2V0Convex) != 0;
                 let swap_factor = f32::from(is_edge_convex) * 2.0 - 1.0;
 
                 let edge = -tri.edges[2];
                 let n_a = swap_factor * tri.normal;
                 let orn = Quat::from_axis_angle_simd(edge.normalize(), info.edge_v2_v0_angle);
                 let mut computed_normal_b = orn * tri.normal;
-                if (info.flags & TriInfoFlag::V2V0SwapNormalB as u8) != 0 {
+                if (info.flags & TriInfoFlag::V2V0SwapNormalB) != 0 {
                     computed_normal_b *= -1.0;
                 }
                 let n_b = swap_factor * computed_normal_b;

rocketsim/src/bullet/collision/dispatch/ray_packet_callbacks.rs

@@ -31,7 +31,7 @@ impl Default for RayResultCallbackBase {
             closest_hit_fraction: Vec4::ONE,
             collision_obj_idx: [None; 4],
             collision_filter_group: CollisionFilterGroups::Default as u8,
-            collision_filter_mask: CollisionFilterGroups::All as u8,
+            collision_filter_mask: CollisionFilterGroups::ALL,
             ignore_obj_world_idx: None,
         }
     }
@@ -48,8 +48,8 @@ pub trait RayResultCallback {
             return false;
         }
 
-        proxy0.collision_filter_group & base.collision_filter_mask != 0
-            && base.collision_filter_group & proxy0.collision_filter_mask != 0
+        (proxy0.collision_filter_group & base.collision_filter_mask) != 0
+            && (base.collision_filter_group & proxy0.collision_filter_mask) != 0
     }
     fn add_single_result(&mut self, ray_result: LocalRayResult, ray_idx: usize);
 }

rocketsim/src/bullet/collision/narrowphase/gjk/gjk_pair_detector.rs

@@ -1,4 +1,4 @@
-use glam::Vec3A;
+use glam::{Affine3A, Vec3A};
 
 use super::{
     GjkResult, closest_point_input::ClosestPointInput, penetration::calc_pen_depth,
@@ -59,7 +59,13 @@ impl GjkPairDetector {
             check_simplex,
             mut degenerate_simplex,
             squared_distance,
-        } = self.update_separating_axis_and_simplex(input, shape_a, shape_b);
+        } = self.update_separating_axis_and_simplex(
+            &local_trans_a,
+            &local_trans_b,
+            shape_a,
+            shape_b,
+            input.maximum_distance_squared,
+        );
 
         let mut is_valid = false;
         let mut distance = 0.0;
@@ -111,18 +117,6 @@ impl GjkPairDetector {
         }
 
         if is_valid && distance * distance < input.maximum_distance_squared {
-            {
-                // in some degenerate cases (usually when the use uses very small margins)
-                // the contact normal is pointing the wrong direction
-                // todo: see if this can be removed :/
-                let pos_a = shape_a.get_aabb(&local_trans_a).center();
-                let pos_b = shape_b.get_aabb(&local_trans_b).center();
-                let diff = pos_a - pos_b;
-                if diff.dot(diff).is_sign_negative() {
-                    normal_in_b *= -1.0;
-                }
-            }
-
             self.separating_axis = normal_in_b;
             self.separating_distance = distance;
 
@@ -167,35 +161,35 @@ impl GjkPairDetector {
 
     fn update_separating_axis_and_simplex(
         &mut self,
-        input: &ClosestPointInput<'_>,
+        transform_a: &Affine3A,
+        transform_b: &Affine3A,
         shape_a: &CollisionShapes,
         shape_b: &CollisionShapes,
+        maximum_distance_squared: f32,
     ) -> GjkIterationResult {
         let mut check_simplex = false;
         let mut degenerate_simplex = 0;
         let mut squared_distance = f32::MAX;
 
         for _ in 0..MAX_ITERATIONS {
-            let separating_axis_in_a = input
-                .transform_a
+            let separating_axis_in_a = transform_a
                 .matrix3
                 .mul_transpose_vec3a(-self.separating_axis);
-            let separating_axis_in_b = input
-                .transform_b
+            let separating_axis_in_b = transform_b
                 .matrix3
                 .mul_transpose_vec3a(self.separating_axis);
 
             let p_in_a = shape_a.local_get_support_vertex_without_margin(separating_axis_in_a);
-            let p_world = input.transform_a.transform_point3a(p_in_a);
+            let p_world = transform_a.transform_point3a(p_in_a);
 
             let q_in_b = shape_b.local_get_support_vertex_without_margin(separating_axis_in_b);
-            let q_world = input.transform_b.transform_point3a(q_in_b);
+            let q_world = transform_b.transform_point3a(q_in_b);
 
             let w = p_world - q_world;
             let delta = self.separating_axis.dot(w);
 
             // Potential exit: the shapes are separated far enough.
-            if delta > 0.0 && delta * delta > squared_distance * input.maximum_distance_squared {
+            if delta > 0.0 && delta * delta > squared_distance * maximum_distance_squared {
                 degenerate_simplex = 10;
                 check_simplex = true;
                 break;