public void GetAxisAlignedBoundingBox()
{
Assert.AreEqual(new Aabb(), new Aabb().GetAabb(Pose.Identity));
Assert.AreEqual(new Aabb(new Vector3F(10, 100, 1000), new Vector3F(10, 100, 1000)),
new Aabb().GetAabb(new Pose(new Vector3F(10, 100, 1000), QuaternionF.Identity)));
Assert.AreEqual(new Aabb(new Vector3F(10, 100, 1000), new Vector3F(10, 100, 1000)),
new Aabb().GetAabb(new Pose(new Vector3F(10, 100, 1000), QuaternionF.CreateRotation(new Vector3F(1, 2, 3), 0.7f))));
Aabb aabb = new Aabb(new Vector3F(1, 10, 100), new Vector3F(2, 20, 200));
Assert.AreEqual(aabb, aabb.GetAabb(Pose.Identity));
Assert.AreEqual(new Aabb(new Vector3F(11, 110, 1100), new Vector3F(12, 120, 1200)),
aabb.GetAabb(new Pose(new Vector3F(10, 100, 1000), QuaternionF.Identity)));
// TODO: Test rotations.
}
public void GetAxisAlignedBoundingBox()
{
Assert.AreEqual(new Aabb(), new Aabb().GetAabb(Pose.Identity));
Assert.AreEqual(new Aabb(new Vector3F(10, 100, 1000), new Vector3F(10, 100, 1000)),
new Aabb().GetAabb(new Pose(new Vector3F(10, 100, 1000), QuaternionF.Identity)));
Assert.AreEqual(new Aabb(new Vector3F(10, 100, 1000), new Vector3F(10, 100, 1000)),
new Aabb().GetAabb(new Pose(new Vector3F(10, 100, 1000), QuaternionF.CreateRotation(new Vector3F(1, 2, 3), 0.7f))));
Aabb aabb = new Aabb(new Vector3F(1, 10, 100), new Vector3F(2, 20, 200));
Assert.AreEqual(aabb, aabb.GetAabb(Pose.Identity));
Assert.AreEqual(new Aabb(new Vector3F(11, 110, 1100), new Vector3F(12, 120, 1200)),
aabb.GetAabb(new Pose(new Vector3F(10, 100, 1000), QuaternionF.Identity)));
// TODO: Test rotations.
}
protected override bool OnNext(out Pair <int> current)
{
while (true)
{
if (_otherCandidates == null)
{
if (_leafNodes.MoveNext())
{
var leaf = _leafNodes.Current;
_leafAabb = _partition.GetAabb(leaf);
_leafAabb = _leafAabb.GetAabb(_scale, _toOther);
_leafAabb.Scale(_otherScaleInverse);
_otherCandidates = _otherPartition.GetOverlaps(_leafAabb).GetEnumerator();
}
else
{
current = default(Pair <int>);
return(false);
}
}
while (_otherCandidates.MoveNext())
{
var leaf = _leafNodes.Current;
var otherCandidate = _otherCandidates.Current;
var overlap = new Pair <int>(leaf.Item, otherCandidate);
if (_partition.Filter == null || _partition.Filter.Filter(overlap))
{
current = overlap;
return(true);
}
}
_otherCandidates.Dispose();
_otherCandidates = null;
}
}
/// <inheritdoc/>
public override Aabb GetAabb(Vector3F scale, Pose pose)
{
// Recompute local cached AABB if it is invalid.
if (Numeric.IsNaN(_minHeight))
{
// Find min and max height.
// TODO: We could cache that beforehand.
_minHeight = float.PositiveInfinity;
_maxHeight = float.NegativeInfinity;
foreach (float height in _samples)
{
if (height < _minHeight)
_minHeight = height;
if (height > _maxHeight)
_maxHeight = height;
}
}
Vector3F minimum = new Vector3F(_originX, _minHeight, _originZ);
Vector3F maximum = new Vector3F(_originX + _widthX, _maxHeight, _originZ + _widthZ);
// Apply scale.
var scaledLocalAabb = new Aabb(minimum, maximum);
scaledLocalAabb.Scale(scale);
// Add depth after scaling because scaleY = 0 makes sense to flatten the height field
// but the bounding box should have a height > 0 to avoid tunneling.
scaledLocalAabb.Minimum.Y = Math.Min(scaledLocalAabb.Minimum.Y - _depth, -_depth);
// Apply pose.
return scaledLocalAabb.GetAabb(pose);
}