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 bool Query(Aabb bounds, QueryCallbcak callback = null)
{
m_stack.Clear();
m_stack.Push(m_root);
bool touchedAnyBounds = false;
while (m_stack.Count > 0)
{
int index = m_stack.Pop();
if (index == Null)
{
continue;
}
Aabb tightBounds = m_nodes[index].Bounds;
tightBounds.Expand(-FatBoundsRadius);
if (!Aabb.Intersects(bounds, tightBounds))
{
continue;
}
if (m_nodes[index].IsLeaf)
{
touchedAnyBounds = true;
bool proceed =
callback != null
? callback(m_nodes[index].UserData)
: true;
if (!proceed)
{
return(true);
}
}
else
{
m_stack.Push(m_nodes[index].ChildA);
m_stack.Push(m_nodes[index].ChildB);
}
}
return(touchedAnyBounds);
}
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 int Fill(ComputeBuffer buffer, float aabbTightenRadius = 0.0f)
{
if (m_root == Null)
{
return(Null);
}
if (m_pods.Length != m_nodes.Length)
{
m_pods = new NodePod[m_nodes.Length];
}
for (int i = 0; i < m_nodes.Length; ++i)
{
if (m_nodes[i].IsFree)
{
continue;
}
Aabb tightBounds = m_nodes[i].Bounds;
tightBounds.Expand(-aabbTightenRadius);
m_pods[i].Bounds = tightBounds;
m_pods[i].Parent = m_nodes[i].Parent;
m_pods[i].ChildA = m_nodes[i].ChildA;
m_pods[i].ChildB = m_nodes[i].ChildB;
var shape = m_nodes[i].UserData as RayMarchedShape;
if (shape != null)
{
m_pods[i].UserDataIndex = shape.ShapeIndex;
}
}
buffer.SetData(m_pods);
return(m_root);
}
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);
}