public void TestConvexColliderCalculateAabbLocal()
{
var points = new NativeArray <float3>(6, Allocator.Temp)
{
[0] = new float3(1.45f, 8.67f, 3.45f),
[1] = new float3(8.75f, 1.23f, 6.44f),
[2] = new float3(100.34f, 5.33f, -2.55f),
[3] = new float3(8.76f, 4.56f, -4.54f),
[4] = new float3(9.75f, -0.45f, -8.99f),
[5] = new float3(7.66f, 3.44f, 0.0f)
};
float convexRadius = 1.25f;
Aabb expectedAabb = Aabb.CreateFromPoints(new float3x4(points[0], points[1], points[2], points[3]));
expectedAabb.Include(points[4]);
expectedAabb.Include(points[5]);
// Currently the convex hull is not shrunk, so we have to expand by the convex radius
expectedAabb.Expand(convexRadius);
var collider = ConvexCollider.Create(points, convexRadius);
points.Dispose();
Aabb actualAabb = collider.Value.CalculateAabb();
TestUtils.AreEqual(expectedAabb.Min, actualAabb.Min, 1e-3f);
TestUtils.AreEqual(expectedAabb.Max, actualAabb.Max, 1e-3f);
}
public unsafe void TestConvexColliderCalculateAabbLocal([Values(0, 0.01f, 1.25f)] float maxShrinkMovement)
{
var points = new NativeArray <float3>(6, Allocator.TempJob)
{
[0] = new float3(1.45f, 8.67f, 3.45f),
[1] = new float3(8.75f, 1.23f, 6.44f),
[2] = new float3(100.34f, 5.33f, -2.55f),
[3] = new float3(8.76f, 4.56f, -4.54f),
[4] = new float3(9.75f, -0.45f, -8.99f),
[5] = new float3(7.66f, 3.44f, 0.0f)
};
Aabb expectedAabb = Aabb.CreateFromPoints(new float3x4(points[0], points[1], points[2], points[3]));
expectedAabb.Include(points[4]);
expectedAabb.Include(points[5]);
var collider = ConvexCollider.Create(
points, new ConvexHullGenerationParameters {
BevelRadius = maxShrinkMovement
}, CollisionFilter.Default
);
points.Dispose();
Aabb actualAabb = collider.Value.CalculateAabb();
float convexRadius = ((ConvexCollider *)collider.GetUnsafePtr())->ConvexHull.ConvexRadius;
float maxError = 1e-3f + maxShrinkMovement - convexRadius;
TestUtils.AreEqual(expectedAabb.Min, actualAabb.Min, maxError);
TestUtils.AreEqual(expectedAabb.Max, actualAabb.Max, maxError);
}
public unsafe void BuildConvexHull2D()
{
// Build circle.
var expectedCom = new float3(4, 5, 3);
const int n = 1024;
float3 * points = stackalloc float3[n];
Aabb domain = Aabb.Empty;
for (int i = 0; i < n; ++i)
{
float angle = (float)i / n * 2 * (float)math.PI;
points[i] = expectedCom + new float3(math.cos(angle), math.sin(angle), 0);
domain.Include(points[i]);
}
ConvexHullBuilderStorage builder = new ConvexHullBuilderStorage(8192, Allocator.Temp, domain, 0.0f, ConvexHullBuilder.IntResolution.High);
for (int i = 0; i < n; ++i)
{
builder.Builder.AddPoint(points[i]);
}
builder.Builder.UpdateHullMassProperties();
var massProperties = builder.Builder.HullMassProperties;
Assert.IsTrue(math.all(math.abs(massProperties.CenterOfMass - expectedCom) < 1e-4f));
Assert.AreEqual(math.PI, massProperties.SurfaceArea, 1e-4f);
}
public static unsafe Hash128 GetHash128(
uint uniqueIdentifier,
ConvexHullGenerationParameters hullGenerationParameters,
Material material,
CollisionFilter filter,
float4x4 bakeFromShape,
NativeArray <HashableShapeInputs> inputs,
NativeArray <int> allIncludedIndices,
NativeArray <float> allBlendShapeWeights,
float linearPrecision = k_DefaultLinearPrecision
)
{
var numInputs = inputs.IsCreated ? inputs.Length : 0;
// quantize shape-level transforms
var bounds = new Aabb {
Min = float.MaxValue, Max = float.MinValue
};
for (int i = 0; i < numInputs; i++)
{
var d = inputs[i];
bounds.Include(math.mul(d.BodyFromShape, new float4(d.Bounds.Min, 1f)).xyz);
bounds.Include(math.mul(d.BodyFromShape, new float4(d.Bounds.Max, 1f)).xyz);
}
GetQuantizedTransformations(bakeFromShape, bounds, out var bakeMatrix, linearPrecision);
// bakeFromShape only contains scale/shear information, so only the inner 3x3 needs to contribute to the hash
var scaleShear = new float3x3(bakeMatrix.c0.xyz, bakeMatrix.c1.xyz, bakeMatrix.c2.xyz);
var bytes = new NativeList <byte>(Allocator.Temp);
bytes.Append(ref uniqueIdentifier);
bytes.Append(ref hullGenerationParameters);
bytes.Append(ref material);
bytes.Append(ref filter);
bytes.Append(ref scaleShear);
bytes.Append(inputs);
bytes.Append(allIncludedIndices);
bytes.Append(allBlendShapeWeights);
return(HashUtility.Hash128(bytes.GetUnsafeReadOnlyPtr(), bytes.Length));
}
public unsafe void TestConvexColliderCalculateAabbTransformed([Values(0, 0.01f, 1.25f)] float maxShrinkMovement)
{
var points = new NativeArray <float3>(6, Allocator.TempJob)
{
[0] = new float3(1.45f, 8.67f, 3.45f),
[1] = new float3(8.75f, 1.23f, 6.44f),
[2] = new float3(100.34f, 5.33f, -2.55f),
[3] = new float3(8.76f, 4.56f, -4.54f),
[4] = new float3(9.75f, -0.45f, -8.99f),
[5] = new float3(7.66f, 3.44f, 0.0f)
};
float3 translation = new float3(43.56f, -87.32f, -0.02f);
quaternion rotation = quaternion.AxisAngle(math.normalize(new float3(8.45f, -2.34f, 0.82f)), 43.21f);
float3[] transformedPoints = new float3[points.Length];
for (int i = 0; i < points.Length; ++i)
{
transformedPoints[i] = translation + math.mul(rotation, points[i]);
}
Aabb expectedAabb = Aabb.CreateFromPoints(new float3x4(transformedPoints[0], transformedPoints[1], transformedPoints[2], transformedPoints[3]));
expectedAabb.Include(transformedPoints[4]);
expectedAabb.Include(transformedPoints[5]);
var collider = ConvexCollider.Create(
points, new ConvexHullGenerationParameters {
BevelRadius = maxShrinkMovement
}, CollisionFilter.Default
);
points.Dispose();
Aabb actualAabb = collider.Value.CalculateAabb(new RigidTransform(rotation, translation));
float convexRadius = ((ConvexCollider *)collider.GetUnsafePtr())->ConvexHull.ConvexRadius;
float maxError = 1e-3f + maxShrinkMovement - convexRadius;
TestUtils.AreEqual(expectedAabb.Min, actualAabb.Min, maxError);
TestUtils.AreEqual(expectedAabb.Max, actualAabb.Max, maxError);
}
public void Execute()
{
var aabb = new Aabb {
Min = float.MaxValue, Max = float.MinValue
};
for (var i = 0; i < Points.Length; ++i)
{
aabb.Include(Points[i]);
}
Aabb[0] = aabb;
}
public void TestConvexColliderCalculateAabbTransformed()
{
var points = new NativeArray <float3>(6, Allocator.Temp)
{
[0] = new float3(1.45f, 8.67f, 3.45f),
[1] = new float3(8.75f, 1.23f, 6.44f),
[2] = new float3(100.34f, 5.33f, -2.55f),
[3] = new float3(8.76f, 4.56f, -4.54f),
[4] = new float3(9.75f, -0.45f, -8.99f),
[5] = new float3(7.66f, 3.44f, 0.0f)
};
float convexRadius = 1.25f;
float3 translation = new float3(43.56f, -87.32f, -0.02f);
quaternion rotation = quaternion.AxisAngle(math.normalize(new float3(8.45f, -2.34f, 0.82f)), 43.21f);
float3[] transformedPoints = new float3[points.Length];
for (int i = 0; i < points.Length; ++i)
{
transformedPoints[i] = translation + math.mul(rotation, points[i]);
}
Aabb expectedAabb = Aabb.CreateFromPoints(new float3x4(transformedPoints[0], transformedPoints[1], transformedPoints[2], transformedPoints[3]));
expectedAabb.Include(transformedPoints[4]);
expectedAabb.Include(transformedPoints[5]);
// Currently the convex hull is not shrunk, so we have to expand by the convex radius
expectedAabb.Expand(convexRadius);
var collider = ConvexCollider.Create(points, convexRadius);
points.Dispose();
Aabb actualAabb = collider.Value.CalculateAabb(new RigidTransform(rotation, translation));
TestUtils.AreEqual(expectedAabb.Min, actualAabb.Min, 1e-3f);
TestUtils.AreEqual(expectedAabb.Max, actualAabb.Max, 1e-3f);
}
public void TestAabbTransform()
{
Random rnd = new Random(0x12345678);
for (int i = 0; i < 100; i++)
{
quaternion r = rnd.NextQuaternionRotation();
float3 t = rnd.NextFloat3();
Aabb orig = new Aabb();
orig.Include(rnd.NextFloat3());
orig.Include(rnd.NextFloat3());
Aabb outAabb1 = Unity.Physics.Math.TransformAabb(new RigidTransform(r, t), orig);
Physics.Math.MTransform bFromA = new Physics.Math.MTransform(r, t);
Aabb outAabb2 = Unity.Physics.Math.TransformAabb(bFromA, orig);
TestUtils.AreEqual(outAabb1.Min, outAabb2.Min, 1e-3f);
TestUtils.AreEqual(outAabb1.Max, outAabb2.Max, 1e-3f);
}
}
static Aabb RotatedBoxAabb(float3 center, float3 size, quaternion orientation)
{
var extents = 0.5f * size;
var aabb = new Aabb {
Min = float.MaxValue, Max = float.MinValue
};
aabb.Include(center + math.mul(orientation, math.mul(extents, new float3(-1f, -1f, -1f)))); // 000
aabb.Include(center + math.mul(orientation, math.mul(extents, new float3(-1f, -1f, 1f)))); // 001
aabb.Include(center + math.mul(orientation, math.mul(extents, new float3(-1f, 1f, -1f)))); // 010
aabb.Include(center + math.mul(orientation, math.mul(extents, new float3(-1f, 1f, 1f)))); // 011
aabb.Include(center + math.mul(orientation, math.mul(extents, new float3(1f, -1f, -1f)))); // 100
aabb.Include(center + math.mul(orientation, math.mul(extents, new float3(1f, -1f, 1f)))); // 101
aabb.Include(center + math.mul(orientation, math.mul(extents, new float3(1f, 1f, -1f)))); // 110
aabb.Include(center + math.mul(orientation, math.mul(extents, new float3(1f, 1f, 1f)))); // 111
return(aabb);
}
//
// Reference implementations of queries using simple brute-force methods
//
static unsafe float RefConvexConvexDistance(ref ConvexHull a, ref ConvexHull b, MTransform aFromB)
{
bool success = false;
if (a.NumVertices + b.NumVertices < 64) // too slow without burst
{
// Build the minkowski difference in a-space
int maxNumVertices = a.NumVertices * b.NumVertices;
Aabb aabb = Aabb.Empty;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
aabb.Include(vertexA - vertexB);
}
}
ConvexHullBuilderStorage diffStorage = new ConvexHullBuilderStorage(maxNumVertices, Allocator.Temp, aabb, 0.0f, ConvexHullBuilder.IntResolution.Low);
ref ConvexHullBuilder diff = ref diffStorage.Builder;
success = true;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
diff.AddPoint(vertexA - vertexB, (uint)(iA | iB << 16));
}
}
float distance = 0.0f;
if (success && diff.Dimension == 3)
{
// Find the closest triangle to the origin
distance = float.MaxValue;
bool penetrating = true;
foreach (int t in diff.Triangles.Indices)
{
ConvexHullBuilder.Triangle triangle = diff.Triangles[t];
float3 v0 = diff.Vertices[triangle.GetVertex(0)].Position;
float3 v1 = diff.Vertices[triangle.GetVertex(1)].Position;
float3 v2 = diff.Vertices[triangle.GetVertex(2)].Position;
float3 n = diff.ComputePlane(t).Normal;
DistanceQueries.Result result = DistanceQueries.TriangleSphere(v0, v1, v2, n, float3.zero, 0.0f, MTransform.Identity);
if (result.Distance < distance)
{
distance = result.Distance;
}
penetrating = penetrating & (math.dot(n, -result.NormalInA) < 0.0f); // only penetrating if inside of all planes
}
if (penetrating)
{
distance = -distance;
}
distance -= a.ConvexRadius + b.ConvexRadius;
}
else
{
success = false;
}
diffStorage.Dispose();
if (success)
{
return(distance);
}
}
public bool RayCast(Vector3 from, Vector3 to, RayCastCallback callback = null)
{
Vector3 r = to - from;
r.Normalize();
float maxFraction = 1.0f;
// v is perpendicular to the segment.
Vector3 v = VectorUtil.FindOrthogonal(r).normalized;
Vector3 absV = VectorUtil.Abs(v);
// build a bounding box for the segment.
Aabb rayBounds = Aabb.Empty;
rayBounds.Include(from);
rayBounds.Include(to);
m_stack.Clear();
m_stack.Push(m_root);
bool hitAnyBounds = false;
while (m_stack.Count > 0)
{
int index = m_stack.Pop();
if (index == Null)
{
continue;
}
if (!Aabb.Intersects(m_nodes[index].Bounds, rayBounds))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, a - c)| > dot(|v|, h)
Vector3 c = m_nodes[index].Bounds.Center;
Vector3 h = m_nodes[index].Bounds.HalfExtents;
float separation = Mathf.Abs(Vector3.Dot(v, from - c)) - Vector3.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (m_nodes[index].IsLeaf)
{
Aabb tightBounds = m_nodes[index].Bounds;
tightBounds.Expand(-FatBoundsRadius);
float t = tightBounds.RayCast(from, to, maxFraction);
if (t < 0.0f)
{
continue;
}
hitAnyBounds = true;
float newMaxFraction =
callback != null
? callback(from, to, m_nodes[index].UserData)
: maxFraction;
if (newMaxFraction >= 0.0f)
{
// Update segment bounding box.
maxFraction = newMaxFraction;
Vector3 newTo = from + maxFraction * (to - from);
rayBounds.Min = VectorUtil.Min(from, newTo);
rayBounds.Max = VectorUtil.Max(from, newTo);
}
}
else
{
m_stack.Push(m_nodes[index].ChildA);
m_stack.Push(m_nodes[index].ChildB);
}
}
return(hitAnyBounds);
}
public override void OnInspectorGUI()
{
DrawDefaultInspector();
ConvexConvexDistanceTest cvx = (ConvexConvexDistanceTest)target;
GUILayout.BeginHorizontal();
if (GUILayout.Button("Reset"))
{
cvx.Reset();
}
if (GUILayout.Button("Rebuild faces"))
{
cvx.UpdateMesh = true;
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
if (GUILayout.Button("Simplify vertices"))
{
cvx.Hull.SimplifyVertices(cvx.VertexSimplificationError, cvx.MinVertices, cvx.VolumeConservation);
cvx.UpdateMesh = true;
}
if (GUILayout.Button("Simplify faces"))
{
cvx.Hull.SimplifyFaces(cvx.FaceSimplificationError, cvx.MaxFaces, int.MaxValue, cvx.FaceMinAngle);
cvx.UpdateMesh = true;
}
if (GUILayout.Button("Offset vertices"))
{
cvx.Hull.OffsetVertices(cvx.Offset);
cvx.UpdateMesh = true;
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
if (GUILayout.Button("X*2"))
{
cvx.Scale(new float3(2, 1, 1));
}
if (GUILayout.Button("Y*2"))
{
cvx.Scale(new float3(1, 2, 1));
}
if (GUILayout.Button("Z*2"))
{
cvx.Scale(new float3(1, 1, 2));
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
if (GUILayout.Button("X/2"))
{
cvx.Scale(new float3(0.5f, 1, 1));
}
if (GUILayout.Button("Y/2"))
{
cvx.Scale(new float3(1, 0.5f, 1));
}
if (GUILayout.Button("Z/2"))
{
cvx.Scale(new float3(1, 1, 0.5f));
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
if (GUILayout.Button("Cut X-"))
{
cvx.SplitByPlane(new Plane(new float3(1, 0, 0), 0));
}
if (GUILayout.Button("Cut Y-"))
{
cvx.SplitByPlane(new Plane(new float3(0, 1, 0), 0));
}
if (GUILayout.Button("Cut Z-"))
{
cvx.SplitByPlane(new Plane(new float3(0, 0, 1), 0));
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
if (GUILayout.Button("Cut X+"))
{
cvx.SplitByPlane(new Plane(new float3(-1, 0, 0), 0));
}
if (GUILayout.Button("Cut Y+"))
{
cvx.SplitByPlane(new Plane(new float3(0, -1, 0), 0));
}
if (GUILayout.Button("Cut Z+"))
{
cvx.SplitByPlane(new Plane(new float3(0, 0, -1), 0));
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
if (GUILayout.Button("+1 point"))
{
cvx.AddRandomPoints(1);
}
if (GUILayout.Button("+10 point"))
{
cvx.AddRandomPoints(10);
}
if (GUILayout.Button("+100 point"))
{
cvx.AddRandomPoints(100);
}
if (GUILayout.Button("+1000 point"))
{
cvx.AddRandomPoints(1000);
}
if (GUILayout.Button("Debug insert"))
{
cvx.Hull.AddPoint(new float3(0, 1, 0), 16384);
cvx.UpdateMesh = true;
}
GUILayout.EndHorizontal();
GUILayout.BeginHorizontal();
if (cvx.SourceMesh != null && GUILayout.Button("Mesh"))
{
cvx.Reset();
var vertices = cvx.SourceMesh.vertices;
var sw = new Stopwatch();
sw.Start();
Aabb aabb = Aabb.Empty;
for (int i = 0; i < vertices.Length; ++i)
{
aabb.Include(vertices[i]);
}
cvx.Hull.IntegerSpaceAabb = aabb;
for (int i = 0; i < vertices.Length; ++i)
{
cvx.Hull.AddPoint(vertices[i]);
}
Debug.Log($"Build time {sw.ElapsedMilliseconds} ms");
cvx.UpdateMesh = true;
}
if (GUILayout.Button("Box"))
{
cvx.Reset();
cvx.Hull.AddPoint(new float3(-1, -1, -1));
cvx.Hull.AddPoint(new float3(+1, -1, -1));
cvx.Hull.AddPoint(new float3(+1, +1, -1));
cvx.Hull.AddPoint(new float3(-1, +1, -1));
cvx.Hull.AddPoint(new float3(-1, -1, +1));
cvx.Hull.AddPoint(new float3(+1, -1, +1));
cvx.Hull.AddPoint(new float3(+1, +1, +1));
cvx.Hull.AddPoint(new float3(-1, +1, +1));
cvx.UpdateMesh = true;
}
if (GUILayout.Button("Cylinder"))
{
cvx.Reset();
var pi2 = math.acos(0) * 4;
for (int i = 0, n = 32; i < n; ++i)
{
var angle = pi2 * i / n;
var xy = new float2(math.sin(angle), math.cos(angle));
cvx.Hull.AddPoint(new float3(xy, -1));
cvx.Hull.AddPoint(new float3(xy, +1));
}
cvx.UpdateMesh = true;
}
if (GUILayout.Button("Cone"))
{
cvx.Reset();
var pi2 = math.acos(0) * 4;
for (int i = 0, n = 32; i < n; ++i)
{
var angle = pi2 * i / n;
var xy = new float2(math.sin(angle), math.cos(angle));
cvx.Hull.AddPoint(new float3(xy, 0));
}
cvx.Hull.AddPoint(new float3(0, 0, 1));
cvx.UpdateMesh = true;
}
if (GUILayout.Button("Circle"))
{
cvx.Reset();
var pi2 = math.acos(0) * 4;
for (int i = 0, n = 32; i < n; ++i)
{
var angle = pi2 * i / n;
var xy = new float2(math.sin(angle), math.cos(angle));
cvx.Hull.AddPoint(new float3(xy, 0));
}
cvx.UpdateMesh = true;
}
GUILayout.EndHorizontal();
SceneView.RepaintAll();
}
//
// Reference implementations of queries using simple brute-force methods
//
static unsafe float RefConvexConvexDistance(ref ConvexHull a, ref ConvexHull b, MTransform aFromB)
{
// Build the minkowski difference in a-space
int maxNumVertices = a.NumVertices * b.NumVertices;
ConvexHullBuilder diff = new ConvexHullBuilder(maxNumVertices, 2 * maxNumVertices, Allocator.Temp);
bool success = true;
Aabb aabb = Aabb.Empty;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
aabb.Include(vertexA - vertexB);
}
}
diff.IntegerSpaceAabb = aabb;
for (int iB = 0; iB < b.NumVertices; iB++)
{
float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
for (int iA = 0; iA < a.NumVertices; iA++)
{
float3 vertexA = a.Vertices[iA];
if (!diff.AddPoint(vertexA - vertexB, (uint)(iA | iB << 16)))
{
// TODO - coplanar vertices are tripping up ConvexHullBuilder, we should fix it but for now fall back to DistanceQueries.ConvexConvex()
success = false;
}
}
}
float distance;
if (!success || diff.Triangles.GetFirstIndex() == -1)
{
// No triangles unless the difference is 3D, fall back to GJK
// Most of the time this happens for cases like sphere-sphere, capsule-capsule, etc. which have special implementations,
// so comparing those to GJK still validates the results of different API queries against each other.
distance = DistanceQueries.ConvexConvex(ref a, ref b, aFromB).Distance;
}
else
{
// Find the closest triangle to the origin
distance = float.MaxValue;
bool penetrating = true;
for (int t = diff.Triangles.GetFirstIndex(); t != -1; t = diff.Triangles.GetNextIndex(t))
{
ConvexHullBuilder.Triangle triangle = diff.Triangles[t];
float3 v0 = diff.Vertices[triangle.GetVertex(0)].Position;
float3 v1 = diff.Vertices[triangle.GetVertex(1)].Position;
float3 v2 = diff.Vertices[triangle.GetVertex(2)].Position;
float3 n = diff.ComputePlane(t).Normal;
DistanceQueries.Result result = DistanceQueries.TriangleSphere(v0, v1, v2, n, float3.zero, 0.0f, MTransform.Identity);
if (result.Distance < distance)
{
distance = result.Distance;
}
penetrating = penetrating & (math.dot(n, -result.NormalInA) < 0.0f); // only penetrating if inside of all planes
}
if (penetrating)
{
distance = -distance;
}
distance -= a.ConvexRadius + b.ConvexRadius;
}
diff.Dispose();
return(distance);
}
public void TestAabb()
{
float3 v0 = float3(100, 200, 300);
float3 v1 = float3(200, 300, 400);
float3 v2 = float3(50, 100, 350);
Aabb a0; a0.Min = float3.zero; a0.Max = v0;
Aabb a1; a1.Min = float3.zero; a1.Max = v1;
Aabb a2; a2.Min = v2; a2.Max = v1;
Aabb a3; a3.Min = v2; a3.Max = v0;
Assert.IsTrue(a0.IsValid);
Assert.IsTrue(a1.IsValid);
Assert.IsTrue(a2.IsValid);
Assert.IsFalse(a3.IsValid);
Assert.IsTrue(a1.Contains(a0));
Assert.IsFalse(a0.Contains(a1));
Assert.IsTrue(a1.Contains(a2));
Assert.IsFalse(a2.Contains(a1));
Assert.IsFalse(a0.Contains(a2));
Assert.IsFalse(a2.Contains(a0));
// Test Union / Intersect
{
Aabb unionAabb = a0;
unionAabb.Include(a1);
Assert.IsTrue(unionAabb.Min.x == 0);
Assert.IsTrue(unionAabb.Min.y == 0);
Assert.IsTrue(unionAabb.Min.z == 0);
Assert.IsTrue(unionAabb.Max.x == a1.Max.x);
Assert.IsTrue(unionAabb.Max.y == a1.Max.y);
Assert.IsTrue(unionAabb.Max.z == a1.Max.z);
Aabb intersectAabb = a2;
intersectAabb.Intersect(a3);
Assert.IsTrue(intersectAabb.Min.x == 50);
Assert.IsTrue(intersectAabb.Min.y == 100);
Assert.IsTrue(intersectAabb.Min.z == 350);
Assert.IsTrue(intersectAabb.Max.x == a3.Max.x);
Assert.IsTrue(intersectAabb.Max.y == a3.Max.y);
Assert.IsTrue(intersectAabb.Max.z == a3.Max.z);
}
// Test Expand / Contains
{
Aabb a5; a5.Min = v2; a5.Max = v1;
float3 testPoint = float3(v2.x - 1.0f, v1.y + 1.0f, .5f * (v2.z + v1.z));
Assert.IsFalse(a5.Contains(testPoint));
a5.Expand(1.5f);
Assert.IsTrue(a5.Contains(testPoint));
}
// Test transform
{
Aabb ut; ut.Min = v0; ut.Max = v1;
// Identity transform should not modify aabb
Aabb outAabb = Unity.Physics.Math.TransformAabb(RigidTransform.identity, ut);
TestUtils.AreEqual(ut.Min, outAabb.Min, 1e-3f);
// Test translation
outAabb = Unity.Physics.Math.TransformAabb(new RigidTransform(quaternion.identity, float3(100.0f, 0.0f, 0.0f)), ut);
Assert.AreEqual(outAabb.Min.x, 200);
Assert.AreEqual(outAabb.Min.y, 200);
Assert.AreEqual(outAabb.Max.x, 300);
Assert.AreEqual(outAabb.Max.z, 400);
// Test rotation
quaternion rot = quaternion.EulerXYZ(0.0f, 0.0f, k_pi2);
outAabb = Unity.Physics.Math.TransformAabb(new RigidTransform(rot, float3.zero), ut);
TestUtils.AreEqual(outAabb.Min, float3(-300.0f, 100.0f, 300.0f), 1e-3f);
TestUtils.AreEqual(outAabb.Max, float3(-200.0f, 200.0f, 400.0f), 1e-3f);
TestUtils.AreEqual(outAabb.SurfaceArea, ut.SurfaceArea, 1e-2f);
}
}