From b72fcb07f01d1f008f915c8ef7ab2544ec1ae622 Mon Sep 17 00:00:00 2001 From: drewcassidy Date: Fri, 29 May 2020 02:17:49 -0700 Subject: [PATCH] Add struct for OrientedBounds Add OrientedBounds struct with functions for testing intersection with other OrientedBounds --- Source/ConformalDecals/OrientedBounds.cs | 179 +++++++++++++++++++++++ 1 file changed, 179 insertions(+) create mode 100644 Source/ConformalDecals/OrientedBounds.cs diff --git a/Source/ConformalDecals/OrientedBounds.cs b/Source/ConformalDecals/OrientedBounds.cs new file mode 100644 index 0000000..d544c4f --- /dev/null +++ b/Source/ConformalDecals/OrientedBounds.cs @@ -0,0 +1,179 @@ +using UnityEngine; + +namespace ConformalDecals { + public struct OrientedBounds { + private Bounds _localBounds; + + public Bounds LocalBounds { + get => _localBounds; + set => _localBounds = value; + } + + public Vector3 Center { + get => _localBounds.center; + set => _localBounds.center = value; + } + + public Vector3 Extents { + get => _localBounds.extents; + set => _localBounds.extents = value; + } + + public Vector3 Min { + get => _localBounds.min; + set => _localBounds.min = value; + } + + public Vector3 Max { + get => _localBounds.max; + set => _localBounds.max = value; + } + + public Vector3 Size { + get => _localBounds.size; + set => _localBounds.size = value; + } + + public Vector3 UnitX { + get => OrientationMatrix.GetColumn(0); + private set => OrientationMatrix.SetColumn(0, value); + } + + public Vector3 UnitY { + get => OrientationMatrix.GetColumn(1); + private set => OrientationMatrix.SetColumn(1, value); + } + + public Vector3 UnitZ { + get => OrientationMatrix.GetColumn(2); + private set => OrientationMatrix.SetColumn(2, value); + } + + public Matrix4x4 OrientationMatrix { get; private set; } + + + public OrientedBounds(Matrix4x4 matrix, Bounds aabb) { + Vector3 unitX = matrix.GetColumn(0); + Vector3 unitY = matrix.GetColumn(1); + Vector3 unitZ = matrix.GetColumn(2); + var scale = new Vector3(unitX.magnitude, unitY.magnitude, unitZ.magnitude); + + _localBounds = new Bounds(); + _localBounds.center = matrix.MultiplyPoint3x4(aabb.center); + _localBounds.extents = new Vector3(aabb.extents.x / scale.x, aabb.extents.y / scale.y, aabb.extents.z / scale.z); + + OrientationMatrix = Matrix4x4.zero; + UnitX = unitX / scale.x; + UnitY = unitY / scale.y; + UnitZ = unitZ / scale.z; + } + + public bool Contains(Vector3 point) { + var delta = point - Center; + var localPoint = Center + OrientationMatrix.transpose.MultiplyVector(delta); + return _localBounds.Contains(localPoint); + } + + public bool Intersects(OrientedBounds other) { + // OrientationMatrix should always be orthogonal, + // so we can cheat and use the transpose for the inverse + var inverseOrientationMatrix = OrientationMatrix.transpose; + + // matrix expressing other in our coordinate frame + var R = inverseOrientationMatrix * other.OrientationMatrix; + + // compute translation vector t in our coordinate frame + var t = inverseOrientationMatrix.MultiplyVector(other.Center - Center); + + return IntersectOBBSeperatingAxis(R, t, Extents, other.Extents); + } + + public bool Intersects(Bounds other) { + // matrix expressing other in our coordinate frame + var R = OrientationMatrix.transpose; + + // compute translation vector t in our coordinate frame + var t = R.MultiplyVector(other.center - Center); + + return IntersectOBBSeperatingAxis(R, t, Extents, other.extents); + } + + private static bool IntersectOBBSeperatingAxis(Matrix4x4 R, Vector3 t, Vector3 a, Vector3 b) { + // Compute common subexpressions. Add in an epsilon term to + // counteract arithmetic errors when two edges are parallel and + // their cross product is (near) null + var absR = R; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + absR[i, j] = Mathf.Abs(R[i, j]) + Mathf.Epsilon; + } + } + + float ra, rb; + + // Test axes L = A0, L = A1, L = A2 + for (int i = 0; i < 3; i++) { + ra = a[i]; + rb = b[0] * absR[i, 0] + b[1] * absR[i, 1] + b[2] * absR[i, 2]; + if (Mathf.Abs(t[i]) > ra + rb) return false; + } + + // Test axes L = B0, L = B1, L = B2 + for (int i = 0; i < 3; i++) { + ra = a[0] * absR[0, i] + a[1] * absR[1, i] + a[2] * absR[2, i]; + rb = b[i]; + if (Mathf.Abs(t[0] * R[0, i] + t[1] * R[1, i] + t[2] * R[2, i]) > ra + rb) return false; + } + + // Test axis L = A0 x B0 + ra = a[1] * absR[2, 0] + a[2] * absR[1, 0]; + rb = b[1] * absR[0, 2] + b[2] * absR[0, 1]; + if (Mathf.Abs(t[2] * R[1, 0] - t[1] * R[2, 0]) > ra + rb) return false; + + // Test axis L = A0 x B1 + ra = a[1] * absR[2, 1] + a[2] * absR[1, 1]; + rb = b[0] * absR[0, 2] + b[2] * absR[0, 0]; + if (Mathf.Abs(t[2] * R[1, 1] - t[1] * R[2, 1]) > ra + rb) return false; + + // Test axis L = A0 x B2 + ra = a[1] * absR[2, 2] + a[2] * absR[1, 2]; + rb = b[0] * absR[0, 1] + b[1] * absR[0, 0]; + if (Mathf.Abs(t[2] * R[1, 2] - t[1] * R[2, 2]) > ra + rb) return false; + + + // Test axis L = A1 x B0 + ra = a[0] * absR[2, 0] + a[2] * absR[0, 0]; + rb = b[1] * absR[1, 2] + b[2] * absR[1, 1]; + if (Mathf.Abs(t[0] * R[2, 0] - t[2] * R[0, 0]) > ra + rb) return false; + + // Test axis L = A1 x B1 + ra = a[0] * absR[2, 1] + a[2] * absR[0, 1]; + rb = b[0] * absR[1, 2] + b[2] * absR[1, 0]; + if (Mathf.Abs(t[0] * R[2, 1] - t[2] * R[0, 1]) > ra + rb) return false; + + // Test axis L = A1 x B2 + ra = a[0] * absR[2, 2] + a[2] * absR[0, 2]; + rb = b[0] * absR[1, 1] + b[1] * absR[1, 0]; + if (Mathf.Abs(t[2] * R[1, 2] - t[1] * R[2, 2]) > ra + rb) return false; + + + // Test axis L = A2 x B0 + ra = a[0] * absR[1, 0] + a[1] * absR[0, 1]; + rb = b[1] * absR[2, 2] + b[2] * absR[2, 1]; + if (Mathf.Abs(t[1] * R[0, 0] - t[0] * R[1, 0]) > ra + rb) return false; + + // Test axis L = A2 x B1 + ra = a[0] * absR[1, 1] + a[1] * absR[0, 1]; + rb = b[0] * absR[2, 2] + b[2] * absR[2, 0]; + if (Mathf.Abs(t[1] * R[0, 1] - t[0] * R[1, 1]) > ra + rb) return false; + + // Test axis L = A2 x B2 + ra = a[0] * absR[1, 2] + a[1] * absR[0, 2]; + rb = b[0] * absR[2, 1] + b[1] * absR[2, 0]; + if (Mathf.Abs(t[1] * R[0, 2] - t[0] * R[1, 2]) > ra + rb) return false; + + // Since no separating axis is found, the OBBs must be intersecting + return true; + } + } +} \ No newline at end of file