Updating firstColliderObjectCollidingWith to work with convex mesh co…

…lliders.

unity/Assets/Scripts/BaseFPSAgentController.cs

@@ -3785,6 +3785,34 @@ protected IEnumerator setObjectPoses(ObjectPose[] objectPoses, bool placeStation
             actionFinished(success, errorMessage);
         }
 
+        public void FirstColliderObjectCollidingWith(string objectId) {
+            if (!physicsSceneManager.ObjectIdToSimObjPhysics.ContainsKey(objectId)) {
+                errorMessage = $"Cannot find object with id {objectId}.";
+                actionFinishedEmit(false);
+                return;
+            }
+            Collider collidingWith = UtilityFunctions.firstColliderObjectCollidingWith(
+                go: physicsSceneManager.ObjectIdToSimObjPhysics[objectId].gameObject,
+                ignoreGameObjects: null,
+                expandBy: 0.0f,
+                useBoundingBoxInChecks: false
+            );
+
+            string collidingWithId = null;
+            if (collidingWith != null) {
+                SimObjPhysics otherSop = ancestorSimObjPhysics(collidingWith.gameObject);
+                if (otherSop != null) {
+                    collidingWithId = otherSop.ObjectID;
+                } else {
+                    collidingWithId = collidingWith.gameObject.name;
+                }
+            }
+            #if UNITY_EDITOR
+            Debug.Log(collidingWithId);
+            #endif
+            actionFinishedEmit(true, actionReturn: collidingWithId);
+        }
+
         // pass in a Vector3, presumably from GetReachablePositions, and try to place a sim object flush on a surface below that point
         // unlike PlaceHeldObject or InitialRandomSpawn, this won't be limited by a Receptacle, but only limited by collision
         public void PlaceObjectAtPoint(

unity/Assets/Scripts/PhysicsExtensions.cs

@@ -443,6 +443,62 @@ public static int OverlapCapsuleNonAlloc(
         );
     }
 
+    public static IEnumerable<Collider> OverlapConvex(
+        MeshCollider meshCollider,
+        LayerMask layerMask,
+        QueryTriggerInteraction queryTriggerInteraction,
+        float expandBy = 0f,
+        HashSet<Collider> ignoreColliders = null
+    ) {
+        if (ignoreColliders == null) {
+            ignoreColliders = new HashSet<Collider>();
+        }
+
+        // Get the bounds of the mesh collider
+        Bounds bounds = meshCollider.bounds;
+        if (expandBy > 0f) {
+            bounds.Expand(expandBy * 2); // Expand in all directions
+        }
+
+        // Use OverlapBox to get candidate colliders
+        Collider[] candidateColliders = Physics.OverlapBox(
+            bounds.center,
+            bounds.extents,
+            meshCollider.transform.rotation,
+            layerMask,
+            queryTriggerInteraction
+        );
+
+
+        // Check each candidate collider for actual collision
+        foreach (Collider candidateCollider in candidateColliders) {
+            if (candidateCollider == meshCollider) {
+                continue; // Skip self-collision
+            }
+
+            if (ignoreColliders.Contains(candidateCollider)) {
+                continue; // Skip ignored colliders
+            }
+
+            Vector3 direction;
+            float distance;
+
+            // Use ComputePenetration to check for actual collision
+            if (Physics.ComputePenetration(
+                meshCollider,
+                meshCollider.transform.position,
+                meshCollider.transform.rotation,
+                candidateCollider,
+                candidateCollider.transform.position,
+                candidateCollider.transform.rotation,
+                out direction,
+                out distance)
+            ) {
+                 yield return candidateCollider;
+            }
+        }
+    }
+
     public static void ToWorldSpaceCapsule(
         this CapsuleCollider capsule,
         out Vector3 point0,

unity/Assets/Scripts/UtilityFunctions.cs

@@ -114,7 +114,7 @@ public static Collider firstColliderObjectCollidingWith(
             "Procedural0"
         );
         foreach (CapsuleCollider cc in go.GetComponentsInChildren<CapsuleCollider>()) {
-            if (cc.isTrigger) {
+            if (cc.isTrigger || !cc.enabled) {
                 continue;
             }
             foreach (
@@ -131,8 +131,11 @@ Collider c in PhysicsExtensions.OverlapCapsule(
             }
         }
         foreach (BoxCollider bc in go.GetComponentsInChildren<BoxCollider>()) {
-            if (bc.isTrigger || ("BoundingBox" == bc.gameObject.name && (!useBoundingBoxInChecks))) {
-                continue;
+            if ("BoundingBox" != bc.gameObject.name || !useBoundingBoxInChecks) {
+                // If we're useBoundingBoxInChecks in we want to ignore the below checks
+                if (bc.isTrigger || !bc.enabled) {
+                    continue;
+                }
             }
             foreach (
                 Collider c in PhysicsExtensions.OverlapBox(
@@ -148,7 +151,7 @@ Collider c in PhysicsExtensions.OverlapBox(
             }
         }
         foreach (SphereCollider sc in go.GetComponentsInChildren<SphereCollider>()) {
-            if (sc.isTrigger) {
+            if (sc.isTrigger || !sc.enabled) {
                 continue;
             }
             foreach (
@@ -164,6 +167,26 @@ Collider c in PhysicsExtensions.OverlapSphere(
                 }
             }
         }
+
+        foreach (MeshCollider mc in go.GetComponentsInChildren<MeshCollider>()) {
+            if (mc.isTrigger || (!mc.convex) || (!mc.enabled)) {
+                continue;
+            }
+            foreach (
+                Collider c in PhysicsExtensions.OverlapConvex(
+                    mc,
+                    layerMask,
+                    QueryTriggerInteraction.Ignore,
+                    expandBy,
+                    ignoreColliders
+                )
+            ) {
+                if (!ignoreColliders.Contains(c)) {
+                    return c;
+                }
+            }
+        }
+
         return null;
     }