public bool Sphere_Vs_Sphere(Sphere_Collider A, Sphere_Collider B)
{
Vector3 A_pos = A.transform.position;
Vector3 B_pos = B.transform.position;
Vector3 N = B_pos - A_pos;
float R = A.Radius + B.Radius;
if (N.sqrMagnitude > R * R)
{
return(false);
}
float D = N.magnitude;
if (D != 0)
{
penetration = R - D;
normal_point_of_contact = N / D;
return(true);
}
else
{
penetration = A.Radius;
normal_point_of_contact = new Vector3(1, 0, 0);
return(true);
}
}
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>());
}
}
}
}
}
}