public bool AABB_VS_AABB(AABB_Collider A, AABB_Collider B) { Vector3 N = B.Center_Of_Mass - A.Center_Of_Mass; float axExtents = (A.MaxExtent.x - A.MinExtent.x) / 2; float bxExtents = (B.MaxExtent.x - B.MinExtent.x) / 2; float xOverlap = axExtents + bxExtents - Mathf.Abs(N.x); if (xOverlap > 0) { float ayExtents = (A.MaxExtent.y - A.MinExtent.y) / 2; float byExtents = (B.MaxExtent.y - B.MinExtent.y) / 2; float yOverlap = ayExtents + byExtents - Mathf.Abs(N.y); if (yOverlap > 0) { float azExtents = (A.MaxExtent.z - A.MinExtent.z) / 2; float bzExtents = (B.MaxExtent.z - B.MinExtent.z) / 2; float zOverlap = azExtents + bzExtents - Mathf.Abs(N.z); if (zOverlap > 0) { if (xOverlap < yOverlap && xOverlap < zOverlap) { if (N.x < 0) { normal_point_of_contact = new Vector3(-1, 0, 0); } else { normal_point_of_contact = new Vector3(1, 0, 0); } penetration = xOverlap; return(true); } if (yOverlap < zOverlap && yOverlap < xOverlap) { if (N.y < 0) { normal_point_of_contact = new Vector3(0, -1, 0); } else { normal_point_of_contact = new Vector3(0, 1, 0); } penetration = yOverlap; return(true); } if (zOverlap < yOverlap && zOverlap < xOverlap) { if (N.z < 0) { normal_point_of_contact = new Vector3(0, 0, -1); } else { normal_point_of_contact = new Vector3(0, 0, 1); } penetration = zOverlap; return(true); } } } } return(false); }
public bool AABB_VS_Sphere(AABB_Collider AABB, Sphere_Collider Sphere) { Vector3 AABB_Position = AABB.transform.position; Vector3 Sphere_Position = Sphere.transform.position; Vector3 N = Sphere_Position - AABB_Position; Vector3 closest = N; float x_extent = (AABB.MaxExtent.x - AABB.MinExtent.x) / 2; float y_extent = (AABB.MaxExtent.y - AABB.MinExtent.y) / 2; float z_extent = (AABB.MaxExtent.z - AABB.MinExtent.z) / 2; closest.x = Mathf.Clamp(closest.x, -x_extent, x_extent); closest.y = Mathf.Clamp(closest.y, -y_extent, y_extent); closest.z = Mathf.Clamp(closest.z, -z_extent, z_extent); bool inside = false; if (N == closest) { inside = true; if (Mathf.Abs(N.x) > Mathf.Abs(N.y) && Mathf.Abs(N.x) > Mathf.Abs(N.z)) { if (closest.x > 0) { closest.x = x_extent; } else { closest.x = -x_extent; } } else if (Mathf.Abs(N.y) > Mathf.Abs(N.x) && Mathf.Abs(N.y) > Mathf.Abs(N.z)) { if (closest.y > 0) { closest.y = y_extent; } else { closest.y = -y_extent; } } else { if (closest.z > 0) { closest.z = z_extent; } else { closest.z = -z_extent; } } } Vector3 localnormal = N - closest; float D = localnormal.sqrMagnitude; float R = Sphere.Radius; if (D > R * R && !inside) { return(false); } D = Mathf.Sqrt(D); if (inside) { normal_point_of_contact = -N; penetration = R - D; } else { normal_point_of_contact = N; penetration = R - D; } return(true); }
public void Solve_Collisions() { AABB_Collider[] boxColliders = (AABB_Collider[])GameObject.FindObjectsOfType(typeof(AABB_Collider)); Sphere_Collider[] sphereColliders = (Sphere_Collider[])GameObject.FindObjectsOfType(typeof(Sphere_Collider)); // Loop for checking all AABB and OBB against other AABB and OBB for (int i = 0; i < boxColliders.Length; i++) { AABB_Collider First_Box = boxColliders[i]; for (int j = 1; j < boxColliders.Length; j++) { if (j > i) { AABB_Collider Second_Box = boxColliders[j]; if (!(First_Box.GetComponent <My_Rigid_Body>().asleep_ == true && Second_Box.GetComponent <My_Rigid_Body>().asleep_ == true)) { if (First_Box.GetComponent <My_Rigid_Body>().mass != 0 || Second_Box.GetComponent <My_Rigid_Body>().mass != 0 || Second_Box.GetComponent <My_Rigid_Body>().mass != 0 || First_Box.GetComponent <My_Rigid_Body>().mass != 0) { if (AABB_VS_AABB(First_Box, Second_Box)) { if (OBB_to_OBB(First_Box.GetComponent <OBB_Collider>(), Second_Box.GetComponent <OBB_Collider>())) { Impulse_for_Two(First_Box.GetComponent <My_Rigid_Body>(), Second_Box.GetComponent <My_Rigid_Body>()); } } } } } } // Loop for all AABB and OBB against spheres for (int k = 0; k < sphereColliders.Length; k++) { Sphere_Collider Sphere = sphereColliders[k]; if (!(First_Box.GetComponent <My_Rigid_Body>().asleep_ == true && Sphere.GetComponent <My_Rigid_Body>().asleep_ == true)) { if (AABB_VS_Sphere(First_Box, Sphere)) { //Debug.Log("Hit"); Impulse_for_Two(First_Box.GetComponent <My_Rigid_Body>(), Sphere.GetComponent <My_Rigid_Body>()); } } } } // Loop for Sphere gainst Sphere for (int i = 0; i < sphereColliders.Length; i++) { Sphere_Collider sphere = sphereColliders[i]; for (int j = 1; j < sphereColliders.Length; j++) { if (j > i) { Sphere_Collider otherSphere = sphereColliders[j]; if (!(sphere.GetComponent <My_Rigid_Body>().asleep_ == true && otherSphere.GetComponent <My_Rigid_Body>().asleep_ == true)) { if (Sphere_Vs_Sphere(sphere, otherSphere)) { Impulse_for_Two(sphere.GetComponent <My_Rigid_Body>(), otherSphere.GetComponent <My_Rigid_Body>()); } } } } } }