public void GetBoundsOrthographic()
{
// Get bounds of AABB in clip space. (Used in OcclusionCulling.fx.)
Vector3F cameraPosition = new Vector3F(100, -200, 345);
Vector3F cameraForward = new Vector3F(1, 2, 3).Normalized;
Vector3F cameraUp = new Vector3F(-1, 0.5f, -2).Normalized;
Matrix44F view = Matrix44F.CreateLookAt(cameraPosition, cameraPosition + cameraForward, cameraUp);
Matrix44F proj = Matrix44F.CreateOrthographic(16, 9, -10, 100);
Matrix44F viewProj = proj * view;
Vector3F center = new Vector3F();
Vector3F halfExtent = new Vector3F();
Aabb aabb = new Aabb(center - halfExtent, center + halfExtent);
Aabb aabb0, aabb1;
GetBoundsOrtho(aabb, viewProj, out aabb0);
GetBoundsOrthoSmart(aabb, viewProj, out aabb1);
Assert.IsTrue(Aabb.AreNumericallyEqual(aabb0, aabb1));
center = new Vector3F(-9, 20, -110);
halfExtent = new Vector3F(5, 2, 10);
aabb = new Aabb(center - halfExtent, center + halfExtent);
GetBoundsOrtho(aabb, viewProj, out aabb0);
GetBoundsOrthoSmart(aabb, viewProj, out aabb1);
Assert.IsTrue(Aabb.AreNumericallyEqual(aabb0, aabb1));
}
//--------------------------------------------------------------
public CDIslandLink(CDIsland islandA, CDIsland islandB, float allowedConcavity, float smallIslandBoost, int vertexLimit, bool sampleVertices, bool sampleCenters)
{
IslandA = islandA;
IslandB = islandB;
Aabb aabb = IslandA.Aabb;
aabb.Grow(IslandB.Aabb);
Aabb = aabb;
float aabbExtentLength = aabb.Extent.Length;
Concavity = GetConcavity(vertexLimit, sampleVertices, sampleCenters);
float alpha = allowedConcavity / (10 * aabbExtentLength);
// Other options for alpha that we could evaluate:
//float alpha = 0.03f / aabbExtentLength; // Independent from concavity.
//alpha = 0.001f;
float aspectRatio = GetAspectRatio();
float term1 = Concavity / aabbExtentLength;
float term2 = alpha * aspectRatio;
// This third term is not in the original paper.
// The goal is to encourage the merging of two small islands. Without this factor
// it can happen that there are few large islands and in each step a single triangle
// is merged into a large island. Resulting in approx. O(n) speed.
// It is much faster if small islands merge first to target an O(log n) speed.
float term3 = smallIslandBoost * Math.Max(IslandA.Triangles.Length, IslandB.Triangles.Length);
DecimationCost = term1 + term2 + term3;
}
public bool BoundingBoxInside(Aabb<Vec3> bb)
{
foreach (var p in bb.Corners)
if (DistanceFromPoint (p) >= 0f)
return true;
return false;
}
public static bool HaveContact(Aabb aabb, Ray ray, float epsilon)
{
if (Numeric.IsZero(ray.Length))
return HaveContact(aabb, ray.Origin);
var rayDirectionInverse = new Vector3F(
1 / ray.Direction.X,
1 / ray.Direction.Y,
1 / ray.Direction.Z);
return HaveContact(aabb, ray.Origin, rayDirectionInverse, ray.Length, epsilon);
}
//--------------------------------------------------------------
/// <summary>
/// Initializes a new instance of the <see cref="Terrain"/> class.
/// </summary>
public Terrain()
{
InvalidBaseRegions = new List<Aabb>();
InvalidDetailRegions = new List<Aabb>();
Shape = Shape.Infinite;
BaseClearValues = new Vector4F[4];
DetailClearValues = new Vector4F[4];
BaseClearValues[0] = new Vector4F(-10000, 0, 0, 1);
Tiles = new TerrainTileCollection(this);
Aabb = new Aabb();
}
/// <inheritdoc/>
public float GetClosestPointCandidates(Aabb aabb, float maxDistanceSquared, Func<int, float> callback)
{
if (callback == null)
throw new ArgumentNullException("callback");
Update(false);
if (_numberOfItems == 0)
return -1;
float closestPointDistanceSquared = maxDistanceSquared;
GetClosestPointCandidatesImpl(0, aabb, callback, ref closestPointDistanceSquared);
return closestPointDistanceSquared;
}
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 GetBoundsPerspective()
{
// Get bounds of AABB in clip space. (Used in OcclusionCulling.fx.)
// Note: Z = 0 or negative is handled conservatively. Point (0, 0, 0) is returned.
Vector3F cameraPosition = new Vector3F(100, -200, 345);
Vector3F cameraForward = new Vector3F(1, 2, 3).Normalized;
Vector3F cameraUp = new Vector3F(-1, 0.5f, -2).Normalized;
Matrix44F view = Matrix44F.CreateLookAt(cameraPosition, cameraPosition + cameraForward, cameraUp);
Matrix44F proj = Matrix44F.CreatePerspectiveFieldOfView(MathHelper.ToRadians(90), 16.0f / 9.0f, 1, 100);
Matrix44F viewProj = proj * view;
// Empty AABB at center of near plane.
Vector3F center = cameraPosition + cameraForward;
Vector3F halfExtent = new Vector3F();
Aabb aabb = new Aabb(center - halfExtent, center + halfExtent);
Aabb aabb0, aabb1;
GetBoundsPersp(aabb, viewProj, out aabb0);
GetBoundsPerspSmart(aabb, viewProj, out aabb1);
Assert.IsTrue(Aabb.AreNumericallyEqual(aabb0, aabb1));
// AABB inside frustum.
center = view.Inverse.TransformPosition(new Vector3F(2, -3, -50));
halfExtent = new Vector3F(1, 6, 10);
aabb = new Aabb(center - halfExtent, center + halfExtent);
GetBoundsPersp(aabb, viewProj, out aabb0);
GetBoundsPerspSmart(aabb, viewProj, out aabb1);
Assert.IsTrue(Aabb.AreNumericallyEqual(aabb0, aabb1));
// Behind camera.
center = view.Inverse.TransformPosition(new Vector3F(2, -3, 50));
halfExtent = new Vector3F(1, 6, 10);
aabb = new Aabb(center - halfExtent, center + halfExtent);
GetBoundsPersp(aabb, viewProj, out aabb0);
GetBoundsPerspSmart(aabb, viewProj, out aabb1);
Assert.IsTrue(Aabb.AreNumericallyEqual(aabb0, aabb1));
// Camera inside AABB.
center = view.Inverse.TransformPosition(new Vector3F(2, -3, -50));
halfExtent = new Vector3F(100, 100, 100);
aabb = new Aabb(center - halfExtent, center + halfExtent);
GetBoundsPersp(aabb, viewProj, out aabb0);
GetBoundsPerspSmart(aabb, viewProj, out aabb1);
Assert.IsTrue(Aabb.AreNumericallyEqual(aabb0, aabb1));
}
public void GetClosestPointAabbPoint()
{
Aabb aabb = new Aabb(new Vector3F(1, 2, 3), new Vector3F(4, 5, 6));
// Touching corner contacts.
TestGetClosestPointAabbPoint(aabb, new Vector3F(1, 2, 3), new Vector3F(1, 2, 3), true);
TestGetClosestPointAabbPoint(aabb, new Vector3F(1, 2, 6), new Vector3F(1, 2, 6), true);
TestGetClosestPointAabbPoint(aabb, new Vector3F(1, 5, 3), new Vector3F(1, 5, 3), true);
TestGetClosestPointAabbPoint(aabb, new Vector3F(1, 5, 6), new Vector3F(1, 5, 6), true);
TestGetClosestPointAabbPoint(aabb, new Vector3F(4, 2, 3), new Vector3F(4, 2, 3), true);
TestGetClosestPointAabbPoint(aabb, new Vector3F(4, 2, 6), new Vector3F(4, 2, 6), true);
TestGetClosestPointAabbPoint(aabb, new Vector3F(4, 5, 3), new Vector3F(4, 5, 3), true);
TestGetClosestPointAabbPoint(aabb, new Vector3F(4, 5, 6), new Vector3F(4, 5, 6), true);
// Touching face contacts.
TestGetClosestPointAabbPoint(aabb, new Vector3F(2, 2, 4), new Vector3F(2, 2, 4), true);
TestGetClosestPointAabbPoint(aabb, new Vector3F(2, 3, 4), new Vector3F(2, 3, 4), true);
TestGetClosestPointAabbPoint(aabb, new Vector3F(2, 5, 4), new Vector3F(2, 5, 4), true);
TestGetClosestPointAabbPoint(aabb, new Vector3F(1, 3, 4), new Vector3F(1, 3, 4), true);
TestGetClosestPointAabbPoint(aabb, new Vector3F(2, 3, 3), new Vector3F(2, 3, 3), true);
TestGetClosestPointAabbPoint(aabb, new Vector3F(2, 3, 6), new Vector3F(2, 3, 6), true);
// Intersection
TestGetClosestPointAabbPoint(aabb, new Vector3F(2, 3, 4), new Vector3F(2, 3, 4), true);
// Separated contacts
TestGetClosestPointAabbPoint(aabb, new Vector3F(0, 0, 0), new Vector3F(1, 2, 3), false);
TestGetClosestPointAabbPoint(aabb, new Vector3F(10, 10, 10), new Vector3F(4, 5, 6), false);
// Separated contacts (in Voronoi regions of faces).
TestGetClosestPointAabbPoint(aabb, new Vector3F(2, 1, 4), new Vector3F(2, 2, 4), false);
TestGetClosestPointAabbPoint(aabb, new Vector3F(5, 3, 4), new Vector3F(4, 3, 4), false);
TestGetClosestPointAabbPoint(aabb, new Vector3F(2, 7, 4), new Vector3F(2, 5, 4), false);
TestGetClosestPointAabbPoint(aabb, new Vector3F(-1, 3, 4), new Vector3F(1, 3, 4), false);
TestGetClosestPointAabbPoint(aabb, new Vector3F(2, 3, 2), new Vector3F(2, 3, 3), false);
TestGetClosestPointAabbPoint(aabb, new Vector3F(2, 3, 7), new Vector3F(2, 3, 6), false);
// We could also check separated contacts in the Voronoi regions of the edges and corners.
// But this should be enough.
}
public void HaveContactAabbRayFast()
{
var aabb = new Aabb(new Vector3F(-1), new Vector3F(1));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(-2, 2, 0), new Vector3F(1, 0, 0), 10)));
Assert.AreEqual(true, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(-2, 0, 0), new Vector3F(1, 0, 0), 10)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(-2, -2, 0), new Vector3F(1, 0, 0), 10)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(-2, 2, 0), new Vector3F(-1, 0, 0), 10)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(-2, 0, 0), new Vector3F(-1, 0, 0), 10)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(-2, -2, 0), new Vector3F(-1, 0, 0), 10)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, -2, -2), new Vector3F(0, 1, 0), 10)));
Assert.AreEqual(true, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, -2, 0), new Vector3F(0, 1, 0), 10)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, -2, 2), new Vector3F(0, 1, 0), 10)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, -2, 0), new Vector3F(0, -1, 0), 10)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, -2, 0), new Vector3F(0, -1, 0), 10)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, -2, 0), new Vector3F(0, -1, 0), 10)));
Assert.AreEqual(true, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, 2f, 0), new Vector3F(1, -1, 0).Normalized, 10)));
Assert.AreEqual(true, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, 2f, 0), new Vector3F(1, -1.1f, 0).Normalized, 10)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, 2f, 0), new Vector3F(1, -0.9f, 0).Normalized, 10)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, 1.5f, 0), -new Vector3F(1, -1, 0).Normalized, 10)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, 1.5f, 0), -new Vector3F(1, -1.1f, 0).Normalized, 10)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, 1.5f, 0), -new Vector3F(1, -0.9f, 0).Normalized, 10)));
Assert.AreEqual(true, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, 0, 0), new Vector3F(1, 0, 0).Normalized, 0)));
Assert.AreEqual(true, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(0, 0, 0), new Vector3F(1, 0, 0).Normalized, 0)));
Assert.AreEqual(false, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(-1.1f, 0, 0), -new Vector3F(1, 0, 0).Normalized, 10)));
// Ray is parallel to one AABB side but not touching. Method returns false positive!
Assert.AreEqual(true, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(1, -2, 0), new Vector3F(0, -1, 0).Normalized, 10)));
// Ray is parallel to one AABB side and touching.
Assert.AreEqual(true, GeometryHelper.HaveContactFast(aabb, new Ray(new Vector3F(1, 0, 0), new Vector3F(0, -1, 0).Normalized, 10)));
}
public void HaveContactAabbBox2()
{
var box = new Vector3F(1, 1, 1);
var aabb = new Aabb(new Vector3F(-1, -1, -1), new Vector3F(1, 1, 1));
Assert.IsTrue(GeometryHelper.HaveContact(aabb, box, new Pose(new Vector3F(0, 0, 0)), true));
Assert.IsTrue(GeometryHelper.HaveContact(aabb, box, new Pose(new Vector3F(1, 0, 0)), true));
Assert.IsFalse(GeometryHelper.HaveContact(aabb, box, new Pose(new Vector3F(-2, 0, 0)), true));
Assert.IsFalse(GeometryHelper.HaveContact(aabb, box, new Pose(new Vector3F(0, -2, 0)), true));
Assert.IsFalse(GeometryHelper.HaveContact(aabb, box, new Pose(new Vector3F(0, 0, -2)), true));
Assert.IsFalse(GeometryHelper.HaveContact(aabb, box, new Pose(new Vector3F(2, 0, 0)), true));
Assert.IsFalse(GeometryHelper.HaveContact(aabb, box, new Pose(new Vector3F(0, 2, 0)), true));
Assert.IsFalse(GeometryHelper.HaveContact(aabb, box, new Pose(new Vector3F(0, 0, 2)), true));
Assert.IsFalse(GeometryHelper.HaveContact(aabb, box, new Pose(new Vector3F(1.5f, 1.5f, 0), QuaternionF.CreateRotationZ(MathHelper.ToRadians(45))), true));
Assert.IsFalse(GeometryHelper.HaveContact(aabb, box, new Pose(new Vector3F(1.5f, 1.5f, 0), QuaternionF.CreateRotationZ(MathHelper.ToRadians(-45))), true));
Assert.IsFalse(GeometryHelper.HaveContact(aabb, box, new Pose(new Vector3F(1.5f, 1.5f, 0), QuaternionF.CreateRotationZ(MathHelper.ToRadians(45)) * QuaternionF.CreateRotationY(MathHelper.ToRadians(90))), true));
// Edge test case where MakeEdgeTest makes a difference.
Assert.IsFalse(GeometryHelper.HaveContact(aabb, box, new Pose(new Vector3F(1.6f, 1.6f, 0), QuaternionF.CreateRotationY(MathHelper.ToRadians(45)) * QuaternionF.CreateRotationZ(MathHelper.ToRadians(45))), true));
Assert.IsTrue(GeometryHelper.HaveContact(aabb, box, new Pose(new Vector3F(1.6f, 1.6f, 0), QuaternionF.CreateRotationY(MathHelper.ToRadians(45)) * QuaternionF.CreateRotationZ(MathHelper.ToRadians(45))), false));
}
/// <summary> /// Given a transform, compute the associated axis aligned bounding box for this shape. /// </summary> /// <param name="aabb">Returns the axis aligned box.</param> /// <param name="xf">The world transform of the shape.</param> public abstract void ComputeAabb(out Aabb aabb, XForm xf);
public static bool HaveContact(Aabb aabb, Plane plane)
{
// Get support point of AABB nearest to the plane.
Vector3F support = new Vector3F(
plane.Normal.X > 0 ? aabb.Minimum.X : aabb.Maximum.X,
plane.Normal.Y > 0 ? aabb.Minimum.Y : aabb.Maximum.Y,
plane.Normal.Z > 0 ? aabb.Minimum.Z : aabb.Maximum.Z);
float projectedLength = Vector3F.Dot(support, plane.Normal);
return projectedLength <= plane.DistanceFromOrigin;
}
public static float GetDistance(Aabb aabbA, Aabb aabbB)
{
return (float)Math.Sqrt(GetDistanceSquared(aabbA, aabbB));
}
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();
}
public static ViewingFrustum FromBBox(Aabb <Vec3> bbox)
{
return(new ViewingFrustum(FrustumKind.Orthographic, bbox.Left, bbox.Right, bbox.Bottom, bbox.Top,
bbox.Front, bbox.Back));
}
private static Mat4 GetStackingMatrix(Axis axis, AxisDirection direction, Aabb <Vec3> previous, Aabb <Vec3> current)
{
switch (axis)
{
case Axis.X:
return(Mat.Translation <Mat4> (direction == AxisDirection.Positive ?
previous.Right - current.Left : previous.Left - current.Right, 0f, 0f));
case Axis.Y:
return(Mat.Translation <Mat4> (0f, direction == AxisDirection.Positive ?
previous.Top - current.Bottom : previous.Bottom - current.Top, 0f));
default:
return(Mat.Translation <Mat4> (0f, 0f, direction == AxisDirection.Positive ?
previous.Front - current.Back : previous.Back - current.Front));
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
CollisionObject collisionObjectA = contactSet.ObjectA;
CollisionObject collisionObjectB = contactSet.ObjectB;
IGeometricObject geometricObjectA = collisionObjectA.GeometricObject;
IGeometricObject geometricObjectB = collisionObjectB.GeometricObject;
// Object A should be the composite, swap objects if necessary.
// When testing CompositeShape vs. CompositeShape with BVH, object A should be the
// CompositeShape with BVH.
CompositeShape compositeShapeA = geometricObjectA.Shape as CompositeShape;
CompositeShape compositeShapeB = geometricObjectB.Shape as CompositeShape;
bool swapped = false;
if (compositeShapeA == null)
{
// Object A is something else. Object B must be a composite shape.
swapped = true;
}
else if (compositeShapeA.Partition == null &&
compositeShapeB != null &&
compositeShapeB.Partition != null)
{
// Object A has no BVH, object B is CompositeShape with BVH.
swapped = true;
}
if (swapped)
{
MathHelper.Swap(ref collisionObjectA, ref collisionObjectB);
MathHelper.Swap(ref geometricObjectA, ref geometricObjectB);
MathHelper.Swap(ref compositeShapeA, ref compositeShapeB);
}
// Check if collision objects shapes are correct.
if (compositeShapeA == null)
{
throw new ArgumentException("The contact set must contain a composite shape.", "contactSet");
}
// Assume no contact.
contactSet.HaveContact = false;
Vector3 scaleA = geometricObjectA.Scale;
Vector3 scaleB = geometricObjectB.Scale;
// Check if transforms are supported.
if (compositeShapeA != null && // When object A is a CompositeShape
(scaleA.X != scaleA.Y || scaleA.Y != scaleA.Z) && // non-uniform scaling is not supported
compositeShapeA.Children.Any(child => child.Pose.HasRotation)) // when a child has a local rotation.
{ // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
}
// Same check for object B.
if (compositeShapeB != null &&
(scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z) &&
compositeShapeB.Children.Any(child => child.Pose.HasRotation)) // Note: Any() creates garbage, but non-uniform scalings should not be used anyway.
{
throw new NotSupportedException("Computing collisions for composite shapes with non-uniform scaling and rotated children is not supported.");
}
// ----- A few fixed objects which are reused to avoid GC garbage.
var testCollisionObjectA = ResourcePools.TestCollisionObjects.Obtain();
var testCollisionObjectB = ResourcePools.TestCollisionObjects.Obtain();
// Create a test contact set and initialize with dummy objects.
// (The actual collision objects are set below.)
var testContactSet = ContactSet.Create(testCollisionObjectA, testCollisionObjectB);
var testGeometricObjectA = TestGeometricObject.Create();
var testGeometricObjectB = TestGeometricObject.Create();
try
{
if (compositeShapeA.Partition != null &&
(type != CollisionQueryType.ClosestPoints ||
compositeShapeA.Partition is ISupportClosestPointQueries <int>))
{
if (compositeShapeB != null && compositeShapeB.Partition != null)
{
Debug.Assert(swapped == false, "Why did we swap the objects? Order of objects is fine.");
if (type != CollisionQueryType.ClosestPoints)
{
// ----- Boolean or Contact Query
// Heuristic: Test large BVH vs. small BVH.
Aabb aabbOfA = geometricObjectA.Aabb;
Aabb aabbOfB = geometricObjectB.Aabb;
float largestExtentA = aabbOfA.Extent.LargestComponent;
float largestExtentB = aabbOfB.Extent.LargestComponent;
IEnumerable <Pair <int> > overlaps;
bool overlapsSwapped = largestExtentA < largestExtentB;
if (overlapsSwapped)
{
overlaps = compositeShapeB.Partition.GetOverlaps(
scaleB,
geometricObjectB.Pose,
compositeShapeA.Partition,
scaleA,
geometricObjectA.Pose);
}
else
{
overlaps = compositeShapeA.Partition.GetOverlaps(
scaleA,
geometricObjectA.Pose,
compositeShapeB.Partition,
scaleB,
geometricObjectB.Pose);
}
foreach (var overlap in overlaps)
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddChildChildContacts(
contactSet,
overlapsSwapped ? overlap.Second : overlap.First,
overlapsSwapped ? overlap.First : overlap.Second,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA,
testCollisionObjectB,
testGeometricObjectB);
}
}
else
{
// Closest-Point Query
var callback = ClosestPointsCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = false;
callback.ContactSet = contactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestContactSet = testContactSet;
((ISupportClosestPointQueries <int>)compositeShapeA.Partition)
.GetClosestPointCandidates(
scaleA,
geometricObjectA.Pose,
compositeShapeB.Partition,
scaleB,
geometricObjectB.Pose,
callback.HandlePair);
ClosestPointsCallbacks.Recycle(callback);
}
}
else
{
// Compute AABB of B in local space of the CompositeShape.
Aabb aabbBInA = geometricObjectB.Shape.GetAabb(
scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose);
// Apply inverse scaling to do the AABB checks in the unscaled local space of A.
aabbBInA.Scale(Vector3.One / scaleA);
if (type != CollisionQueryType.ClosestPoints)
{
// Boolean or Contact Query
foreach (var childIndex in compositeShapeA.Partition.GetOverlaps(aabbBInA))
{
if (type == CollisionQueryType.Boolean && contactSet.HaveContact)
{
break; // We can abort early.
}
AddChildContacts(
contactSet,
swapped,
childIndex,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA);
}
}
else if (type == CollisionQueryType.ClosestPoints)
{
// Closest-Point Query
var callback = ClosestPointsCallbacks.Obtain();
callback.CollisionAlgorithm = this;
callback.Swapped = swapped;
callback.ContactSet = contactSet;
callback.TestCollisionObjectA = testCollisionObjectA;
callback.TestCollisionObjectB = testCollisionObjectB;
callback.TestGeometricObjectA = testGeometricObjectA;
callback.TestGeometricObjectB = testGeometricObjectB;
callback.TestContactSet = testContactSet;
((ISupportClosestPointQueries <int>)compositeShapeA.Partition)
.GetClosestPointCandidates(
aabbBInA,
float.PositiveInfinity,
callback.HandleItem);
ClosestPointsCallbacks.Recycle(callback);
}
}
}
else
{
// Compute AABB of B in local space of the composite.
Aabb aabbBInA = geometricObjectB.Shape.GetAabb(scaleB, geometricObjectA.Pose.Inverse * geometricObjectB.Pose);
// Apply inverse scaling to do the AABB checks in the unscaled local space of A.
aabbBInA.Scale(Vector3.One / scaleA);
// Go through list of children and find contacts.
int numberOfChildGeometries = compositeShapeA.Children.Count;
for (int i = 0; i < numberOfChildGeometries; i++)
{
IGeometricObject child = compositeShapeA.Children[i];
// NOTE: For closest-point queries we could be faster estimating a search space.
// See TriangleMeshAlgorithm or BVH queries.
// But the current implementation is sufficient. If the CompositeShape is more complex
// the user should be using spatial partitions anyway.
// For boolean or contact queries, we make an AABB test first.
// For closest points where we have not found a contact yet, we have to search
// all children.
if ((type == CollisionQueryType.ClosestPoints && !contactSet.HaveContact) ||
GeometryHelper.HaveContact(aabbBInA, child.Shape.GetAabb(child.Scale, child.Pose)))
{
// TODO: We could compute the minDistance of the child AABB and the AABB of the
// other shape. If the minDistance is greater than the current closestPairDistance
// we can ignore this pair. - This could be a performance boost.
// Get contacts/closest pairs of this child.
AddChildContacts(
contactSet,
swapped,
i,
type,
testContactSet,
testCollisionObjectA,
testGeometricObjectA);
// We have contact and stop for boolean queries.
if (contactSet.HaveContact && type == CollisionQueryType.Boolean)
{
break;
}
}
}
}
}
finally
{
Debug.Assert(collisionObjectA.GeometricObject.Shape == compositeShapeA, "Shape was altered and not restored.");
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectA);
ResourcePools.TestCollisionObjects.Recycle(testCollisionObjectB);
testGeometricObjectB.Recycle();
testGeometricObjectA.Recycle();
}
}
public static bool HaveContact(Aabb aabb, Vector3 boxExtent, Pose boxPose, bool makeEdgeTests)
{
Debug.Assert(boxExtent >= Vector3.Zero, "Box extent must be positive.");
// The following variables are in local space of a centered AABB.
Vector3 cB = boxPose.Position - aabb.Center; // Center of box.
Matrix mB = boxPose.Orientation; // Orientation matrix of box.
Matrix aMB = Matrix.Absolute(mB); // Absolute of mB.
// Half extent vectors of AABB and box
Vector3 eA = 0.5f * aabb.Extent;
Vector3 eB = 0.5f * boxExtent;
// ----- Separating Axis tests
// See also: BoxBoxAlgorithm
float separation; // Separation distance.
// Case 1: Separating Axis: (1, 0, 0)
separation = Math.Abs(cB.X) - (eA.X + eB.X * aMB.M00 + eB.Y * aMB.M01 + eB.Z * aMB.M02);
if (separation > 0)
{
return(false);
}
// Case 2: Separating Axis: (0, 1, 0)
separation = Math.Abs(cB.Y) - (eA.Y + eB.X * aMB.M10 + eB.Y * aMB.M11 + eB.Z * aMB.M12);
if (separation > 0)
{
return(false);
}
// Case 3: Separating Axis: (0, 0, 1)
separation = Math.Abs(cB.Z) - (eA.Z + eB.X * aMB.M20 + eB.Y * aMB.M21 + eB.Z * aMB.M22);
if (separation > 0)
{
return(false);
}
float ex; // Variable for an expression part.
// Case 4: Separating Axis: OrientationB * (1, 0, 0)
ex = cB.X * mB.M00 + cB.Y * mB.M10 + cB.Z * mB.M20;
separation = Math.Abs(ex) - (eB.X + eA.X * aMB.M00 + eA.Y * aMB.M10 + eA.Z * aMB.M20);
if (separation > 0)
{
return(false);
}
// Case 5: Separating Axis: OrientationB * (0, 1, 0) -----
ex = cB.X * mB.M01 + cB.Y * mB.M11 + cB.Z * mB.M21;
separation = Math.Abs(ex) - (eB.Y + eA.X * aMB.M01 + eA.Y * aMB.M11 + eA.Z * aMB.M21);
if (separation > 0)
{
return(false);
}
// Case 6: Separating Axis: OrientationB * (0, 0, 1) -----
ex = cB.X * mB.M02 + cB.Y * mB.M12 + cB.Z * mB.M22;
separation = Math.Abs(ex) - (eB.Z + eA.X * aMB.M02 + eA.Y * aMB.M12 + eA.Z * aMB.M22);
if (separation > 0)
{
return(false);
}
// ----- The next 9 tests are edge-edge cases.
if (makeEdgeTests == false)
{
return(true);
}
// Case 7: Separating Axis: (1, 0, 0) x (OrientationB * (1, 0, 0))
ex = cB.Z * mB.M10 - cB.Y * mB.M20;
separation = Math.Abs(ex) - (eA.Y * aMB.M20 + eA.Z * aMB.M10 + eB.Y * aMB.M02 + eB.Z * aMB.M01);
if (separation > 0)
{
return(false);
}
// Case 8: Separating Axis: (1, 0, 0) x (OrientationB * (0, 1, 0))
ex = cB.Z * mB.M11 - cB.Y * mB.M21;
separation = Math.Abs(ex) - (eA.Y * aMB.M21 + eA.Z * aMB.M11 + eB.X * aMB.M02 + eB.Z * aMB.M00);
if (separation > 0)
{
return(false);
}
// Case 9: Separating Axis: (1, 0, 0) x (OrientationB * (0, 0, 1))
ex = cB.Z * mB.M12 - cB.Y * mB.M22;
separation = Math.Abs(ex) - (eA.Y * aMB.M22 + eA.Z * aMB.M12 + eB.X * aMB.M01 + eB.Y * aMB.M00);
if (separation > 0)
{
return(false);
}
// Case 10: Separating Axis: (0, 1, 0) x (OrientationB * (1, 0, 0))
ex = cB.X * mB.M20 - cB.Z * mB.M00;
separation = Math.Abs(ex) - (eA.X * aMB.M20 + eA.Z * aMB.M00 + eB.Y * aMB.M12 + eB.Z * aMB.M11);
if (separation > 0)
{
return(false);
}
// Case 11: Separating Axis: (0, 1, 0) x (OrientationB * (0, 1, 0))
ex = cB.X * mB.M21 - cB.Z * mB.M01;
separation = Math.Abs(ex) - (eA.X * aMB.M21 + eA.Z * aMB.M01 + eB.X * aMB.M12 + eB.Z * aMB.M10);
if (separation > 0)
{
return(false);
}
// Case 12: Separating Axis: (0, 1, 0) x (OrientationB * (0, 0, 1))
ex = cB.X * mB.M22 - cB.Z * mB.M02;
separation = Math.Abs(ex) - (eA.X * aMB.M22 + eA.Z * aMB.M02 + eB.X * aMB.M11 + eB.Y * aMB.M10);
if (separation > 0)
{
return(false);
}
// Case 13: Separating Axis: (0, 0, 1) x (OrientationB * (1, 0, 0))
ex = cB.Y * mB.M00 - cB.X * mB.M10;
separation = Math.Abs(ex) - (eA.X * aMB.M10 + eA.Y * aMB.M00 + eB.Y * aMB.M22 + eB.Z * aMB.M21);
if (separation > 0)
{
return(false);
}
// Case 14: Separating Axis: (0, 0, 1) x (OrientationB * (0, 1, 0))
ex = cB.Y * mB.M01 - cB.X * mB.M11;
separation = Math.Abs(ex) - (eA.X * aMB.M11 + eA.Y * aMB.M01 + eB.X * aMB.M22 + eB.Z * aMB.M20);
if (separation > 0)
{
return(false);
}
// Case 15: Separating Axis: (0, 0, 1) x (OrientationB * (0, 0, 1))
ex = cB.Y * mB.M02 - cB.X * mB.M12;
separation = Math.Abs(ex) - (eA.X * aMB.M12 + eA.Y * aMB.M02 + eB.X * aMB.M21 + eB.Y * aMB.M20);
return(separation <= 0);
}
public static float GetDistance(Aabb aabbA, Aabb aabbB)
{
return((float)Math.Sqrt(GetDistanceSquared(aabbA, aabbB)));
}
public static IEnumerable <Node> Create(CompressedAabbTree compressedAabbTree, ref Aabb aabb)
{
var enumerable = Pool.Obtain();
enumerable._compressedAabbTree = compressedAabbTree;
enumerable._aabb = aabb;
enumerable._index = 0;
return(enumerable);
}
/// <summary>
/// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB, then the proxy is
/// removed from the tree and re-inserted. Otherwise the function returns immediately.
/// </summary>
/// <param name="proxyId">The proxy id.</param>
/// <param name="aabb">The AABB.</param>
/// <param name="displacement">The displacement.</param>
/// <returns>true if the proxy was re-inserted.</returns>
public bool MoveProxy(int proxyId, ref Aabb aabb, Vector2 displacement)
{
Debug.Assert(0 <= proxyId && proxyId < nodeCapacity);
Debug.Assert(nodes[proxyId].IsLeaf());
// Extend AABB
Aabb fatAabb = new Aabb();
Vector2 r = new Vector2(Settings.AabbExtension, Settings.AabbExtension);
fatAabb.LowerBound = aabb.LowerBound - r;
fatAabb.UpperBound = aabb.UpperBound + r;
// Predict AABB movement
Vector2 d = Settings.AabbMultiplier * displacement;
if (d.X < 0.0f)
{
fatAabb.LowerBound.X += d.X;
}
else
{
fatAabb.UpperBound.X += d.X;
}
if (d.Y < 0.0f)
{
fatAabb.LowerBound.Y += d.Y;
}
else
{
fatAabb.UpperBound.Y += d.Y;
}
Aabb treeAabb = nodes[proxyId].Aabb;
if (treeAabb.Contains(ref aabb))
{
// The tree AABB still contains the object, but it might be too large.
// Perhaps the object was moving fast but has since gone to sleep.
// The huge AABB is larger than the new fat AABB.
Aabb hugeAabb = new Aabb
{
LowerBound = fatAabb.LowerBound - 4.0f * r,
UpperBound = fatAabb.UpperBound + 4.0f * r
};
if (hugeAabb.Contains(ref treeAabb))
{
// The tree AABB contains the object AABB and the tree AABB is
// not too large. No tree update needed.
return(false);
}
// Otherwise the tree AABB is huge and needs to be shrunk
}
RemoveLeaf(proxyId);
nodes[proxyId].Aabb = fatAabb;
InsertLeaf(proxyId);
nodes[proxyId].Moved = true;
return(true);
}
/// <summary>
/// Ray-cast against the proxies in the tree. This relies on the callback to perform a exact ray-cast in the case
/// were the proxy contains a Shape. The callback also performs the any collision filtering. This has performance
/// roughly
/// equal to k * log(n), where k is the number of collisions and n is the number of proxies in the tree.
/// </summary>
/// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
/// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
public void RayCast(Func <RayCastInput, int, float> callback, ref RayCastInput input)
{
Vector2 p1 = input.Point1;
Vector2 p2 = input.Point2;
Vector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r = Vector2.Normalize(r);
// v is perpendicular to the segment.
Vector2 absV = MathUtils.Abs(new Vector2(-r.Y, r.X)); //Velcro: Inlined the 'v' variable
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.MaxFraction;
// Build a bounding box for the segment.
Aabb segmentAabb = new Aabb();
{
Vector2 t = p1 + maxFraction * (p2 - p1);
segmentAabb.LowerBound = Vector2.Min(p1, t);
segmentAabb.UpperBound = Vector2.Max(p1, t);
}
raycastStack.Clear();
raycastStack.Push(root);
while (raycastStack.Count > 0)
{
int nodeId = raycastStack.Pop();
if (nodeId == NullNode)
{
continue;
}
TreeNode <T> node = nodes[nodeId];
if (!Aabb.TestOverlap(ref node.Aabb, ref segmentAabb))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Vector2 c = node.Aabb.Center;
Vector2 h = node.Aabb.Extents;
float separation = Math.Abs(Vector2.Dot(new Vector2(-r.Y, r.X), p1 - c)) - Vector2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
float value = callback(subInput, nodeId);
if (value == 0.0f)
{
// the client has terminated the raycast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
Vector2 t = p1 + maxFraction * (p2 - p1);
segmentAabb.LowerBound = Vector2.Min(p1, t);
segmentAabb.UpperBound = Vector2.Max(p1, t);
}
}
else
{
raycastStack.Push(node.Child1);
raycastStack.Push(node.Child2);
}
}
}
/// <inheritdoc/>
public IEnumerable<int> GetOverlaps(Aabb aabb)
{
Update(false);
return GetOverlapsImpl(aabb);
}
/// <summary>
/// Gets a bounding shape that matches the specified AABB.
/// </summary>
/// <param name="aabb">The AABB.</param>
/// <returns>A box or transformed box that matches the specified AABB.</returns>
private Shape GetBoundingShape(Aabb aabb)
{
// Get existing shape objects to avoid unnecessary memory allocation.
BoxShape boxShape;
GeometricObject geometricObject = null;
TransformedShape transformedShape = null;
if (Shape is BoxShape)
{
boxShape = (BoxShape)Shape;
}
else if (Shape is TransformedShape)
{
transformedShape = (TransformedShape)Shape;
geometricObject = (GeometricObject)transformedShape.Child;
boxShape = (BoxShape)geometricObject.Shape;
}
else
{
boxShape = new BoxShape();
}
// Make box the size of the AABB.
boxShape.Extent = aabb.Extent;
if (aabb.Center.IsNumericallyZero)
{
// Bounding box is centered at origin.
return boxShape;
}
// Apply offset to bounding box.
if (transformedShape == null)
{
geometricObject = new GeometricObject(boxShape, new Pose(aabb.Center));
transformedShape = new TransformedShape(geometricObject);
}
else
{
geometricObject.Shape = boxShape;
geometricObject.Pose = new Pose(aabb.Center);
}
return transformedShape;
}
// Recursive traversal of tree.
private void GetClosestPointCandidatesImpl(int index, Aabb aabb, Func<int, float> callback, ref float closestPointDistanceSquared)
{
// closestPointDistanceSquared == -1 can be returned by callback to abort the query.
if (closestPointDistanceSquared < 0)
{
// Abort.
return;
}
Node node = _nodes[index];
// If we have a contact, it is not necessary to examine nodes with no AABB contact
// because they cannot give a closer point pair.
if (closestPointDistanceSquared == 0 && !GeometryHelper.HaveContact(GetAabb(node), aabb))
return;
if (node.IsLeaf)
{
// Node is leaf - call callback and updated closest-point distance.
var leafDistanceSquared = callback(node.Item);
closestPointDistanceSquared = Math.Min(leafDistanceSquared, closestPointDistanceSquared);
return;
}
int leftIndex = index + 1;
Node leftChild = _nodes[leftIndex];
int rightIndex = (leftChild.IsLeaf) ? leftIndex + 1 : leftIndex + leftChild.EscapeOffset;
Node rightChild = _nodes[rightIndex];
if (closestPointDistanceSquared == 0)
{
// We have contact, so we must examine all children.
GetClosestPointCandidatesImpl(leftIndex, aabb, callback, ref closestPointDistanceSquared);
GetClosestPointCandidatesImpl(rightIndex, aabb, callback, ref closestPointDistanceSquared);
return;
}
// No contact. Use lower bound estimates to search the best nodes first.
float minDistanceLeft = GeometryHelper.GetDistanceSquared(GetAabb(leftChild), aabb);
float minDistanceRight = GeometryHelper.GetDistanceSquared(GetAabb(rightChild), aabb);
if (minDistanceLeft < minDistanceRight)
{
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN.
if (minDistanceLeft > closestPointDistanceSquared)
return;
// Handle left first.
GetClosestPointCandidatesImpl(leftIndex, aabb, callback, ref closestPointDistanceSquared);
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN.
if (minDistanceRight > closestPointDistanceSquared)
return;
GetClosestPointCandidatesImpl(rightIndex, aabb, callback, ref closestPointDistanceSquared);
}
else
{
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN.
if (minDistanceRight > closestPointDistanceSquared)
return;
// Handle right first.
GetClosestPointCandidatesImpl(rightIndex, aabb, callback, ref closestPointDistanceSquared);
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN.
if (minDistanceLeft > closestPointDistanceSquared)
return;
GetClosestPointCandidatesImpl(leftIndex, aabb, callback, ref closestPointDistanceSquared);
}
}
public static bool HaveContact(Aabb aabb, Vector3F boxExtent, Pose boxPose, bool makeEdgeTests)
{
Debug.Assert(boxExtent >= Vector3F.Zero, "Box extent must be positive.");
// The following variables are in local space of a centered AABB.
Vector3F cB = boxPose.Position - aabb.Center; // Center of box.
Matrix33F mB = boxPose.Orientation; // Orientation matrix of box.
Matrix33F aMB = Matrix33F.Absolute(mB); // Absolute of mB.
// Half extent vectors of AABB and box
Vector3F eA = 0.5f * aabb.Extent;
Vector3F eB = 0.5f * boxExtent;
// ----- Separating Axis tests
// See also: BoxBoxAlgorithm
float separation; // Separation distance.
// Case 1: Separating Axis: (1, 0, 0)
separation = Math.Abs(cB.X) - (eA.X + eB.X * aMB.M00 + eB.Y * aMB.M01 + eB.Z * aMB.M02);
if (separation > 0)
return false;
// Case 2: Separating Axis: (0, 1, 0)
separation = Math.Abs(cB.Y) - (eA.Y + eB.X * aMB.M10 + eB.Y * aMB.M11 + eB.Z * aMB.M12);
if (separation > 0)
return false;
// Case 3: Separating Axis: (0, 0, 1)
separation = Math.Abs(cB.Z) - (eA.Z + eB.X * aMB.M20 + eB.Y * aMB.M21 + eB.Z * aMB.M22);
if (separation > 0)
return false;
float ex; // Variable for an expression part.
// Case 4: Separating Axis: OrientationB * (1, 0, 0)
ex = cB.X * mB.M00 + cB.Y * mB.M10 + cB.Z * mB.M20;
separation = Math.Abs(ex) - (eB.X + eA.X * aMB.M00 + eA.Y * aMB.M10 + eA.Z * aMB.M20);
if (separation > 0)
return false;
// Case 5: Separating Axis: OrientationB * (0, 1, 0) -----
ex = cB.X * mB.M01 + cB.Y * mB.M11 + cB.Z * mB.M21;
separation = Math.Abs(ex) - (eB.Y + eA.X * aMB.M01 + eA.Y * aMB.M11 + eA.Z * aMB.M21);
if (separation > 0)
return false;
// Case 6: Separating Axis: OrientationB * (0, 0, 1) -----
ex = cB.X * mB.M02 + cB.Y * mB.M12 + cB.Z * mB.M22;
separation = Math.Abs(ex) - (eB.Z + eA.X * aMB.M02 + eA.Y * aMB.M12 + eA.Z * aMB.M22);
if (separation > 0)
return false;
// ----- The next 9 tests are edge-edge cases.
if (makeEdgeTests == false)
return true;
// Case 7: Separating Axis: (1, 0, 0) x (OrientationB * (1, 0, 0))
ex = cB.Z * mB.M10 - cB.Y * mB.M20;
separation = Math.Abs(ex) - (eA.Y * aMB.M20 + eA.Z * aMB.M10 + eB.Y * aMB.M02 + eB.Z * aMB.M01);
if (separation > 0)
return false;
// Case 8: Separating Axis: (1, 0, 0) x (OrientationB * (0, 1, 0))
ex = cB.Z * mB.M11 - cB.Y * mB.M21;
separation = Math.Abs(ex) - (eA.Y * aMB.M21 + eA.Z * aMB.M11 + eB.X * aMB.M02 + eB.Z * aMB.M00);
if (separation > 0)
return false;
// Case 9: Separating Axis: (1, 0, 0) x (OrientationB * (0, 0, 1))
ex = cB.Z * mB.M12 - cB.Y * mB.M22;
separation = Math.Abs(ex) - (eA.Y * aMB.M22 + eA.Z * aMB.M12 + eB.X * aMB.M01 + eB.Y * aMB.M00);
if (separation > 0)
return false;
// Case 10: Separating Axis: (0, 1, 0) x (OrientationB * (1, 0, 0))
ex = cB.X * mB.M20 - cB.Z * mB.M00;
separation = Math.Abs(ex) - (eA.X * aMB.M20 + eA.Z * aMB.M00 + eB.Y * aMB.M12 + eB.Z * aMB.M11);
if (separation > 0)
return false;
// Case 11: Separating Axis: (0, 1, 0) x (OrientationB * (0, 1, 0))
ex = cB.X * mB.M21 - cB.Z * mB.M01;
separation = Math.Abs(ex) - (eA.X * aMB.M21 + eA.Z * aMB.M01 + eB.X * aMB.M12 + eB.Z * aMB.M10);
if (separation > 0)
return false;
// Case 12: Separating Axis: (0, 1, 0) x (OrientationB * (0, 0, 1))
ex = cB.X * mB.M22 - cB.Z * mB.M02;
separation = Math.Abs(ex) - (eA.X * aMB.M22 + eA.Z * aMB.M02 + eB.X * aMB.M11 + eB.Y * aMB.M10);
if (separation > 0)
return false;
// Case 13: Separating Axis: (0, 0, 1) x (OrientationB * (1, 0, 0))
ex = cB.Y * mB.M00 - cB.X * mB.M10;
separation = Math.Abs(ex) - (eA.X * aMB.M10 + eA.Y * aMB.M00 + eB.Y * aMB.M22 + eB.Z * aMB.M21);
if (separation > 0)
return false;
// Case 14: Separating Axis: (0, 0, 1) x (OrientationB * (0, 1, 0))
ex = cB.Y * mB.M01 - cB.X * mB.M11;
separation = Math.Abs(ex) - (eA.X * aMB.M11 + eA.Y * aMB.M01 + eB.X * aMB.M22 + eB.Z * aMB.M20);
if (separation > 0)
return false;
// Case 15: Separating Axis: (0, 0, 1) x (OrientationB * (0, 0, 1))
ex = cB.Y * mB.M02 - cB.X * mB.M12;
separation = Math.Abs(ex) - (eA.X * aMB.M12 + eA.Y * aMB.M02 + eB.X * aMB.M21 + eB.Y * aMB.M20);
return (separation <= 0);
}
private Ragdoll CreateRagdoll(MeshNode meshNode)
{
var mesh = meshNode.Mesh;
var skeleton = mesh.Skeleton;
// Extract the vertices from the mesh sorted per bone.
var verticesPerBone = new List <Vector3F> [skeleton.NumberOfBones];
// Also get the AABB of the model.
Aabb?aabb = null;
foreach (var submesh in mesh.Submeshes)
{
// Get vertex element info.
var vertexDeclaration = submesh.VertexBuffer.VertexDeclaration;
var vertexElements = vertexDeclaration.GetVertexElements();
// Get the vertex positions.
var positionElement = vertexElements.First(e => e.VertexElementUsage == VertexElementUsage.Position);
if (positionElement.VertexElementFormat != VertexElementFormat.Vector3)
{
throw new NotSupportedException("For vertex positions only VertexElementFormat.Vector3 is supported.");
}
var positions = new Vector3[submesh.VertexCount];
submesh.VertexBuffer.GetData(
submesh.StartVertex * vertexDeclaration.VertexStride + positionElement.Offset,
positions,
0,
submesh.VertexCount,
vertexDeclaration.VertexStride);
// Get the bone indices.
var boneIndexElement = vertexElements.First(e => e.VertexElementUsage == VertexElementUsage.BlendIndices);
if (boneIndexElement.VertexElementFormat != VertexElementFormat.Byte4)
{
throw new NotSupportedException();
}
var boneIndicesArray = new Byte4[submesh.VertexCount];
submesh.VertexBuffer.GetData(
submesh.StartVertex * vertexDeclaration.VertexStride + boneIndexElement.Offset,
boneIndicesArray,
0,
submesh.VertexCount,
vertexDeclaration.VertexStride);
// Get the bone weights.
var boneWeightElement = vertexElements.First(e => e.VertexElementUsage == VertexElementUsage.BlendWeight);
if (boneWeightElement.VertexElementFormat != VertexElementFormat.Vector4)
{
throw new NotSupportedException();
}
var boneWeightsArray = new Vector4[submesh.VertexCount];
submesh.VertexBuffer.GetData(
submesh.StartVertex * vertexDeclaration.VertexStride + boneWeightElement.Offset,
boneWeightsArray,
0,
submesh.VertexCount,
vertexDeclaration.VertexStride);
// Sort the vertices per bone.
for (int i = 0; i < submesh.VertexCount; i++)
{
var vertex = (Vector3F)positions[i];
// Here, we only check the first bone index. We could also check the
// bone weights to add the vertex to all bone vertex lists where the
// weight is high...
Vector4 boneIndices = boneIndicesArray[i].ToVector4();
//Vector4 boneWeights = boneWeightsArray[i];
int boneIndex = (int)boneIndices.X;
if (verticesPerBone[boneIndex] == null)
{
verticesPerBone[boneIndex] = new List <Vector3F>();
}
verticesPerBone[boneIndex].Add(vertex);
// Add vertex to AABB.
if (aabb == null)
{
aabb = new Aabb(vertex, vertex);
}
else
{
aabb.Value.Grow(vertex);
}
}
}
// We create a body for each bone with vertices.
int numberOfBodies = verticesPerBone.Count(vertices => vertices != null);
// We use the same mass properties for all bodies. This is not realistic but more stable
// because large mass differences or thin bodies (arms!) are less stable.
// We use the mass properties of sphere proportional to the size of the model.
const float totalMass = 80; // The total mass of the ragdoll.
var massFrame = MassFrame.FromShapeAndMass(new SphereShape(aabb.Value.Extent.Y / 8), Vector3F.One, totalMass / numberOfBodies, 0.1f, 1);
var material = new UniformMaterial();
Ragdoll ragdoll = new Ragdoll();
for (int boneIndex = 0; boneIndex < skeleton.NumberOfBones; boneIndex++)
{
var boneVertices = verticesPerBone[boneIndex];
if (boneVertices != null)
{
var bindPoseInverse = (Pose)skeleton.GetBindPoseAbsoluteInverse(boneIndex);
// Compute bounding capsule.
//float radius;
//float height;
//Pose pose;
//GeometryHelper.ComputeBoundingCapsule(boneVertices, out radius, out height, out pose);
//Shape shape = new TransformedShape(new GeometricObject(new CapsuleShape(radius, height), pose));
// Compute convex hull.
var points = GeometryHelper.CreateConvexHull(boneVertices, 32, 0).ToTriangleMesh().Vertices;
Shape shape = new ConvexHullOfPoints(points.Count > 0 ? points : boneVertices);
ragdoll.Bodies.Add(new RigidBody(shape, massFrame, material));
ragdoll.BodyOffsets.Add(bindPoseInverse);
}
else
{
ragdoll.Bodies.Add(null);
ragdoll.BodyOffsets.Add(Pose.Identity);
}
}
return(ragdoll);
}
//betauser public extern static void dGeomGetAABB( dGeomID geom, ref Aabb aabb );
/// <summary>
/// Gets all items that are candidates for the smallest closest-point distance to items in a
/// given partition. (Internal, recursive.)
/// </summary>
/// <param name="nodeA">The first AABB node.</param>
/// <param name="scaleA">The scale of the first AABB node.</param>
/// <param name="nodeB">The second AABB node.</param>
/// <param name="scaleB">The scale of the second AABB node.</param>
/// <param name="poseB">The pose of the second AABB node relative to the first.</param>
/// <param name="callback">
/// The callback that is called with each found candidate item. The method must compute the
/// closest-point on the candidate item and return the squared closest-point distance.
/// </param>
/// <param name="closestPointDistanceSquared">
/// The squared of the current closest-point distance.
/// </param>
private void GetClosestPointCandidatesImpl(Node nodeA, Vector3F scaleA, Node nodeB, Vector3F scaleB, Pose poseB, Func <T, T, float> callback, ref float closestPointDistanceSquared)
{
// closestPointDistanceSquared == -1 indicates early exit.
if (nodeA == null || nodeB == null || closestPointDistanceSquared < 0)
{
// Abort.
return;
}
nodeA.IsActive = true;
nodeB.IsActive = true;
// If we have a contact, it is not necessary to examine nodes with no AABB contact
// because they cannot give a closer point pair.
if (closestPointDistanceSquared == 0 && !HaveAabbContact(nodeA, scaleA, nodeB, scaleB, poseB))
{
return;
}
if (nodeA.IsLeaf && nodeB.IsLeaf)
{
// Leaf vs leaf.
if (Filter == null || Filter.Filter(new Pair <T>(nodeA.Item, nodeB.Item)))
{
var leafDistanceSquared = callback(nodeA.Item, nodeB.Item);
closestPointDistanceSquared = Math.Min(leafDistanceSquared, closestPointDistanceSquared);
}
return;
}
// Determine which one to split:
// If B is a leaf, we have to split A. OR
// If A can be split and is bigger than B, we split A.
if (nodeB.IsLeaf || (!nodeA.IsLeaf && IsABiggerThanB(nodeA, scaleA, nodeB, scaleB)))
{
#region ----- Split A -----
SplitIfNecessary(nodeA);
Node leftChild = nodeA.LeftChild;
Node rightChild = nodeA.RightChild;
if (closestPointDistanceSquared == 0)
{
// We have contact, so we must examine all children.
GetClosestPointCandidatesImpl(leftChild, scaleA, nodeB, scaleB, poseB, callback, ref closestPointDistanceSquared);
GetClosestPointCandidatesImpl(rightChild, scaleA, nodeB, scaleB, poseB, callback, ref closestPointDistanceSquared);
return;
}
// No contact. Use lower bound estimates to search the best nodes first.
// TODO: Optimize: We do not need to call GeometryHelper.GetDistanceLowerBoundSquared for OBBs. We have AABB + OBB.
// Scale AABB of B.
Aabb aabbB = nodeB.Aabb;
aabbB.Scale(scaleB);
// Convert AABB of B to OBB in local space of A.
Vector3F boxExtentB = aabbB.Extent;
Pose poseBoxB = poseB * new Pose(aabbB.Center);
// Scale left child AABB of A.
Aabb leftChildAabb = leftChild.Aabb;
leftChildAabb.Scale(scaleA);
// Convert left child AABB of A to OBB in local space of A.
Vector3F leftChildBoxExtent = leftChildAabb.Extent;
Pose leftChildBoxPose = new Pose(leftChildAabb.Center);
// Compute lower bound for distance to left child.
float minDistanceLeft = GeometryHelper.GetDistanceLowerBoundSquared(leftChildBoxExtent, leftChildBoxPose, boxExtentB, poseBoxB);
// Scale right child AABB of A.
Aabb rightChildAabb = rightChild.Aabb;
rightChildAabb.Scale(scaleA);
// Convert right child AABB of A to OBB in local space of A.
Vector3F rightChildBoxExtent = rightChildAabb.Extent;
Pose rightChildBoxPose = new Pose(rightChildAabb.Center);
// Compute lower bound for distance to right child.
float minDistanceRight = GeometryHelper.GetDistanceLowerBoundSquared(rightChildBoxExtent, rightChildBoxPose, boxExtentB, poseBoxB);
if (minDistanceLeft < minDistanceRight)
{
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN.
if (minDistanceLeft > closestPointDistanceSquared)
{
return;
}
// Handle left first.
GetClosestPointCandidatesImpl(leftChild, scaleA, nodeB, scaleB, poseB, callback, ref closestPointDistanceSquared);
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN.
if (minDistanceRight > closestPointDistanceSquared)
{
return;
}
GetClosestPointCandidatesImpl(rightChild, scaleA, nodeB, scaleB, poseB, callback, ref closestPointDistanceSquared);
}
else
{
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN.
if (minDistanceRight > closestPointDistanceSquared)
{
return;
}
// Handle right first.
GetClosestPointCandidatesImpl(rightChild, scaleA, nodeB, scaleB, poseB, callback, ref closestPointDistanceSquared);
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN.
if (minDistanceLeft > closestPointDistanceSquared)
{
return;
}
GetClosestPointCandidatesImpl(leftChild, scaleA, nodeB, scaleB, poseB, callback, ref closestPointDistanceSquared);
}
#endregion
}
else
{
#region ----- Split B -----
SplitIfNecessary(nodeB);
Node leftChildB = nodeB.LeftChild;
Node rightChildB = nodeB.RightChild;
if (closestPointDistanceSquared == 0)
{
// We have contact, so we must examine all children.
GetClosestPointCandidatesImpl(nodeA, scaleA, leftChildB, scaleB, poseB, callback, ref closestPointDistanceSquared);
GetClosestPointCandidatesImpl(nodeA, scaleA, rightChildB, scaleB, poseB, callback, ref closestPointDistanceSquared);
}
else
{
// No contact. Use lower bound estimates to search the best nodes first.
// TODO: Optimize: We do not need to call GeometryHelper.GetDistanceLowerBoundSquared for OBBs. We have AABB + OBB.
// Scale AABB of A.
Aabb aabbA = nodeA.Aabb;
aabbA.Scale(scaleA);
// Convert AABB of A to OBB in local space of A.
Vector3F boxExtentA = aabbA.Extent;
Pose poseBoxA = new Pose(aabbA.Center);
// Scale left child AABB of B.
Aabb leftChildAabb = leftChildB.Aabb;
leftChildAabb.Scale(scaleB);
// Convert left child AABB of B to OBB in local space of A.
Vector3F childBoxExtent = leftChildAabb.Extent;
Pose poseLeft = poseB * new Pose(leftChildAabb.Center);
// Compute lower bound for distance to left child.
float minDistanceLeft = GeometryHelper.GetDistanceLowerBoundSquared(childBoxExtent, poseLeft, boxExtentA, poseBoxA);
// Scale right child AABB of B.
Aabb rightChildAabb = rightChildB.Aabb;
rightChildAabb.Scale(scaleB);
// Convert right child AABB of B to OBB in local space of A.
childBoxExtent = rightChildAabb.Extent;
Pose poseRight = poseB * new Pose(rightChildAabb.Center);
// Compute lower bound for distance to right child.
float minDistanceRight = GeometryHelper.GetDistanceLowerBoundSquared(childBoxExtent, poseRight, boxExtentA, poseBoxA);
if (minDistanceLeft < minDistanceRight)
{
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN.
if (minDistanceLeft > closestPointDistanceSquared)
{
return;
}
// Handle left first.
GetClosestPointCandidatesImpl(nodeA, scaleA, leftChildB, scaleB, poseB, callback, ref closestPointDistanceSquared);
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN.
if (minDistanceRight > closestPointDistanceSquared)
{
return;
}
GetClosestPointCandidatesImpl(nodeA, scaleA, rightChildB, scaleB, poseB, callback, ref closestPointDistanceSquared);
}
else
{
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN.
if (minDistanceRight > closestPointDistanceSquared)
{
return;
}
// Handle right first.
GetClosestPointCandidatesImpl(nodeA, scaleA, rightChildB, scaleB, poseB, callback, ref closestPointDistanceSquared);
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN.
if (minDistanceLeft > closestPointDistanceSquared)
{
return;
}
GetClosestPointCandidatesImpl(nodeA, scaleA, leftChildB, scaleB, poseB, callback, ref closestPointDistanceSquared);
}
}
#endregion
}
}
/// <summary>
/// Builds the AABB tree.
/// </summary>
private void Build()
{
_root = null;
_leaves = null;
_height = -1;
int numberOfItems = Count;
// No items?
if (numberOfItems == 0)
{
// Nothing to do.
return;
}
if (numberOfItems == 1)
{
// AABB tree contains exactly one item. (One leaf, no internal nodes.)
T item = Items.First();
_root = new Node
{
Aabb = GetAabbForItem(item),
Item = item,
};
_leaves = new[] { _root };
_height = 0;
}
else
{
// Default case: Several items. (Data is stored in the leaves.)
// (Fix for Xbox 360: Create a temporary list of leaves and pass the temporary list to the
// AabbTreeBuilder. Cannot use the leaves array directly, because, the .NET CF has troubles
// with arrays that are cast to IList<T>.)
var leaves = DigitalRune.ResourcePools <IAabbTreeNode <T> > .Lists.Obtain();
foreach (T item in Items)
{
Aabb aabb = GetAabbForItem(item);
leaves.Add(new Node {
Aabb = aabb, Item = item
});
}
// Build tree.
_root = (Node)AabbTreeBuilder.Build(leaves, () => new Node(), BottomUpBuildThreshold);
//_root = CompactNodes(_root, null);
//GC.Collect(0);
// Copy leaves from temporary list.
_leaves = new Node[numberOfItems];
for (int i = 0; i < numberOfItems; i++)
{
_leaves[i] = (Node)leaves[i];
}
_height = GetHeight(_root);
// Recycle temporary list.
DigitalRune.ResourcePools <IAabbTreeNode <T> > .Lists.Recycle(leaves);
}
}
public override void Update(GameTime gameTime)
{
float deltaTime = (float)gameTime.ElapsedGameTime.TotalSeconds;
var debugRenderer = _graphicsScreen.DebugRenderer;
debugRenderer.Clear();
// Change wave height.
if (InputService.IsDown(Keys.H))
{
bool isShiftDown = (InputService.ModifierKeys & ModifierKeys.Shift) != 0;
float sign = isShiftDown ? +1 : -1;
float delta = sign * deltaTime * 0.01f;
var oceanWaves = ((OceanWaves)_waterNode.Waves);
oceanWaves.HeightScale = Math.Max(0, oceanWaves.HeightScale + delta);
}
// Switch water color.
if (InputService.IsPressed(Keys.J, true))
{
if (_waterColorType == 0)
{
_waterColorType = 1;
_waterNode.Water.UnderwaterFogDensity = new Vector3F(12, 8, 8) * 0.04f;
_waterNode.Water.WaterColor = new Vector3F(10, 30, 79) * 0.002f;
}
else
{
_waterColorType = 0;
_waterNode.Water.UnderwaterFogDensity = new Vector3F(1, 0.8f, 0.6f);
_waterNode.Water.WaterColor = new Vector3F(0.2f, 0.4f, 0.5f);
}
}
// Toggle reflection.
if (InputService.IsPressed(Keys.K, true))
{
_waterNode.PlanarReflection.IsEnabled = !_waterNode.PlanarReflection.IsEnabled;
}
// Switch caustics.
if (InputService.IsPressed(Keys.L, true))
{
if (_causticType == 0)
{
_causticType = 1;
_waterNode.Water.CausticsSampleCount = 5;
_waterNode.Water.CausticsIntensity = 10;
_waterNode.Water.CausticsPower = 200;
}
else if (_causticType == 1)
{
// Disable caustics
_causticType = 2;
_waterNode.Water.CausticsIntensity = 0;
}
else
{
_causticType = 0;
_waterNode.Water.CausticsSampleCount = 3;
_waterNode.Water.CausticsIntensity = 3;
_waterNode.Water.CausticsPower = 100;
}
}
// Move rigid bodies with the waves:
// The Buoyancy force effect is only designed for a flat water surface.
// This code applies some impulses to move the bodies. It is not physically
// correct but looks ok.
// The code tracks 3 arbitrary positions on each body. Info for the positions
// are stored in RigidBody.UserData. The wave displacements of the previous
// frame and the current frame are compared an impulse proportional to the
// displacement change is applied.
foreach (var body in Simulation.RigidBodies)
{
if (body.MotionType != MotionType.Dynamic)
{
continue;
}
// Check how much the body penetrates the water using a simple AABB check.
Aabb aabb = body.Aabb;
float waterPenetration = (float)Math.Pow(
MathHelper.Clamp((_waterNode.PoseWorld.Position.Y - aabb.Minimum.Y) / aabb.Extent.Y, 0, 1),
3);
if (waterPenetration < 0)
{
body.UserData = null;
continue;
}
// 3 displacement vectors are stored in the UserData.
var previousDisplacements = body.UserData as Vector3F[];
if (previousDisplacements == null)
{
previousDisplacements = new Vector3F[3];
body.UserData = previousDisplacements;
}
for (int i = 0; i < 3; i++)
{
// Get an arbitrary position on or near the body.
Vector3F position = new Vector3F(
(i < 2) ? aabb.Minimum.X : aabb.Maximum.X,
aabb.Minimum.Y,
(i % 2 == 0) ? aabb.Minimum.Z : aabb.Maximum.Z);
// Get wave displacement of this position.
var waves = (OceanWaves)_waterNode.Waves;
Vector3F displacement, normal;
waves.GetDisplacement(position.X, position.Z, out displacement, out normal);
// Compute velocity from displacement change.
Vector3F currentVelocity = body.GetVelocityOfWorldPoint(position);
Vector3F desiredVelocity = (displacement - previousDisplacements[i]) / deltaTime;
// Apply impulse proportional to the velocity change of the water.
Vector3F velocityDelta = desiredVelocity - currentVelocity;
body.ApplyImpulse(
velocityDelta * body.MassFrame.Mass * waterPenetration * 0.1f,
position);
previousDisplacements[i] = displacement;
}
}
}
public void HaveContactAabbPoint()
{
var p = new Vector3F(10, 10, 10);
var aabb1 = new Aabb(new Vector3F(10, 10, 10), new Vector3F(10, 10, 10));
var aabb2 = new Aabb(new Vector3F(10, 10, 10), new Vector3F(20, 20, 20));
var aabb3 = new Aabb(new Vector3F(30, 10, 10), new Vector3F(40, 20, 20));
var aabbInf = new Aabb(new Vector3F(float.NegativeInfinity), new Vector3F(float.PositiveInfinity));
var aabbNaN = new Aabb(new Vector3F(float.NaN), new Vector3F(float.NaN));
Assert.IsTrue(GeometryHelper.HaveContact(aabb1, p));
Assert.IsTrue(GeometryHelper.HaveContact(aabb2, p));
Assert.IsFalse(GeometryHelper.HaveContact(aabb3, p));
Assert.IsTrue(GeometryHelper.HaveContact(aabbInf, p));
Assert.IsFalse(GeometryHelper.HaveContact(aabbNaN, p));
}
/// <inheritdoc/> public abstract IEnumerable<T> GetOverlaps(Aabb aabb);
private void TestGetClosestPointAabbPoint(Aabb aabb, Vector3F point, Vector3F expectedPoint, bool expectedResult)
{
Vector3F pointOnAabb;
bool result = GeometryHelper.GetClosestPoint(aabb, point, out pointOnAabb);
Assert.AreEqual(expectedPoint, pointOnAabb);
Assert.AreEqual(expectedResult, result);
}
/// <summary>
/// Gets the leaf nodes that touch the given AABB. (Same as
/// <see cref="GetOverlaps(Shapes.Aabb)"/> except we directly return the AABB tree node.
/// </summary>
/// <param name="aabb">The axis-aligned bounding box.</param>
/// <returns>All items that touch the given AABB.</returns>
/// <remarks>
/// Filtering (see <see cref="Filter"/>) is not applied.
/// </remarks>
private IEnumerable<Node> GetLeafNodes(Aabb aabb)
{
// Note: This methods is the same as GetOverlaps(Aabb), but instead of returning items we
// return the nodes directly. This is used in tree vs. tree tests, so we do not have to
// recompute the AABBs of each leaf node.
Update(false);
#if !POOL_ENUMERABLES
if (_numberOfItems == 0)
yield break;
// Stackless traversal of tree.
// The AABB tree nodes are stored in preorder traversal order. We can visit them in linear
// order. The EscapeOffset of each node can be used to skip a subtree.
int index = 0;
while (index < _nodes.Length)
{
Node node = _nodes[index];
bool haveContact = GeometryHelper.HaveContact(GetAabb(node), aabb);
if (haveContact && node.IsLeaf)
yield return node;
if (haveContact || node.IsLeaf)
{
// Given AABB intersects the internal AABB tree node or the node is a leaf.
// Continue with next item in preorder traversal order.
index++;
}
else
{
// Given AABB does not touch the internal AABB tree node.
// --> Skip the subtree.
index += node.EscapeOffset;
}
}
#else
// Avoiding garbage:
return GetLeafNodesWork.Create(this, ref aabb);
#endif
}
/// <summary>Build an optimal tree. Very expensive. For testing.</summary>
public void RebuildBottomUp()
{
int[] nodes = new int[nodeCount];
int count = 0;
// Build array of leaves. Free the rest.
for (int i = 0; i < nodeCapacity; ++i)
{
if (this.nodes[i].Height < 0)
{
// free node in pool
continue;
}
if (this.nodes[i].IsLeaf())
{
this.nodes[i].ParentOrNext = NullNode;
nodes[count] = i;
++count;
}
else
{
FreeNode(i);
}
}
while (count > 1)
{
float minCost = MathConstants.MaxFloat;
int iMin = -1, jMin = -1;
for (int i = 0; i < count; ++i)
{
Aabb aabBi = this.nodes[nodes[i]].Aabb;
for (int j = i + 1; j < count; ++j)
{
Aabb aabBj = this.nodes[nodes[j]].Aabb;
Aabb b = new Aabb();
b.Combine(ref aabBi, ref aabBj);
float cost = b.Perimeter;
if (cost < minCost)
{
iMin = i;
jMin = j;
minCost = cost;
}
}
}
int index1 = nodes[iMin];
int index2 = nodes[jMin];
TreeNode <T> child1 = this.nodes[index1];
TreeNode <T> child2 = this.nodes[index2];
int parentIndex = AllocateNode();
TreeNode <T> parent = this.nodes[parentIndex];
parent.Child1 = index1;
parent.Child2 = index2;
parent.Height = 1 + Math.Max(child1.Height, child2.Height);
parent.Aabb.Combine(ref child1.Aabb, ref child2.Aabb);
parent.ParentOrNext = NullNode;
child1.ParentOrNext = parentIndex;
child2.ParentOrNext = parentIndex;
nodes[jMin] = nodes[count - 1];
nodes[iMin] = parentIndex;
--count;
}
root = nodes[0];
Validate();
}
/// <summary>
/// Gets a bounding shape that matches the specified AABB.
/// </summary>
/// <param name="aabb">The AABB.</param>
/// <returns>A box or transformed box that matches the specified AABB.</returns>
private Shape GetBoundingShape(Aabb aabb)
{
// The AABB of the LOD is real world size including scaling. We have to undo
// the scale because this LodGroupNode also applies the same scale.
var unscaledCenter = aabb.Center / ScaleWorld;
var unscaledExtent = aabb.Extent / ScaleWorld;
// Get existing shape objects to avoid unnecessary memory allocation.
BoxShape boxShape;
GeometricObject geometricObject = null;
TransformedShape transformedShape = null;
if (Shape is BoxShape)
{
boxShape = (BoxShape)Shape;
}
else if (Shape is TransformedShape)
{
transformedShape = (TransformedShape)Shape;
geometricObject = (GeometricObject)transformedShape.Child;
boxShape = (BoxShape)geometricObject.Shape;
}
else
{
boxShape = new BoxShape();
}
// Make bounding box the size of the unscaled AABB.
boxShape.Extent = unscaledExtent;
if (unscaledCenter.IsNumericallyZero)
{
// Bounding box is centered at origin.
return boxShape;
}
// Apply offset to bounding box.
if (transformedShape == null)
{
geometricObject = new GeometricObject(boxShape, new Pose(unscaledCenter));
transformedShape = new TransformedShape(geometricObject);
}
else
{
geometricObject.Shape = boxShape;
geometricObject.Pose = new Pose(unscaledCenter);
}
return transformedShape;
}
/// <summary>Get the fat AABB for a proxy.</summary>
/// <param name="proxyId">The proxy id.</param>
/// <param name="fatAabb">The fat AABB.</param>
public void GetFatAabb(int proxyId, out Aabb fatAabb)
{
Debug.Assert(0 <= proxyId && proxyId < nodeCapacity);
fatAabb = nodes[proxyId].Aabb;
}
/// <inheritdoc/>
public override IEnumerable <T> GetOverlaps(Aabb aabb)
{
return(Items.Where(item => GeometryHelper.HaveContact(GetAabbForItem(item), aabb)));
}
/// <summary>
/// Inserts the leaf using the specified leaf
/// </summary>
/// <param name="leaf">The leaf</param>
private void InsertLeaf(int leaf)
{
if (root == NullNode)
{
root = leaf;
nodes[root].ParentOrNext = NullNode;
return;
}
// Find the best sibling for this node
Aabb leafAabb = nodes[leaf].Aabb;
int index = root;
while (!nodes[index].IsLeaf())
{
int child1 = nodes[index].Child1;
int child2 = nodes[index].Child2;
float area = nodes[index].Aabb.Perimeter;
Aabb combinedAabb = new Aabb();
combinedAabb.Combine(ref nodes[index].Aabb, ref leafAabb);
float combinedArea = combinedAabb.Perimeter;
// Cost of creating a new parent for this node and the new leaf
float cost = 2.0f * combinedArea;
// Minimum cost of pushing the leaf further down the tree
float inheritanceCost = 2.0f * (combinedArea - area);
// Cost of descending into child1
float cost1;
if (nodes[child1].IsLeaf())
{
Aabb aabb = new Aabb();
aabb.Combine(ref leafAabb, ref nodes[child1].Aabb);
cost1 = aabb.Perimeter + inheritanceCost;
}
else
{
Aabb aabb = new Aabb();
aabb.Combine(ref leafAabb, ref nodes[child1].Aabb);
float oldArea = nodes[child1].Aabb.Perimeter;
float newArea = aabb.Perimeter;
cost1 = newArea - oldArea + inheritanceCost;
}
// Cost of descending into child2
float cost2;
if (nodes[child2].IsLeaf())
{
Aabb aabb = new Aabb();
aabb.Combine(ref leafAabb, ref nodes[child2].Aabb);
cost2 = aabb.Perimeter + inheritanceCost;
}
else
{
Aabb aabb = new Aabb();
aabb.Combine(ref leafAabb, ref nodes[child2].Aabb);
float oldArea = nodes[child2].Aabb.Perimeter;
float newArea = aabb.Perimeter;
cost2 = newArea - oldArea + inheritanceCost;
}
// Descend according to the minimum cost.
if (cost < cost1 && cost1 < cost2)
{
break;
}
// Descend
if (cost1 < cost2)
{
index = child1;
}
else
{
index = child2;
}
}
int sibling = index;
// Create a new parent.
int oldParent = nodes[sibling].ParentOrNext;
int newParent = AllocateNode();
nodes[newParent].ParentOrNext = oldParent;
nodes[newParent].UserData = default(T?);
nodes[newParent].Aabb.Combine(ref leafAabb, ref nodes[sibling].Aabb);
nodes[newParent].Height = nodes[sibling].Height + 1;
if (oldParent != NullNode)
{
// The sibling was not the root.
if (nodes[oldParent].Child1 == sibling)
{
nodes[oldParent].Child1 = newParent;
}
else
{
nodes[oldParent].Child2 = newParent;
}
nodes[newParent].Child1 = sibling;
nodes[newParent].Child2 = leaf;
nodes[sibling].ParentOrNext = newParent;
nodes[leaf].ParentOrNext = newParent;
}
else
{
// The sibling was the root.
nodes[newParent].Child1 = sibling;
nodes[newParent].Child2 = leaf;
nodes[sibling].ParentOrNext = newParent;
nodes[leaf].ParentOrNext = newParent;
root = newParent;
}
// Walk back up the tree fixing heights and AABBs
index = nodes[leaf].ParentOrNext;
while (index != NullNode)
{
index = Balance(index);
int child1 = nodes[index].Child1;
int child2 = nodes[index].Child2;
Debug.Assert(child1 != NullNode);
Debug.Assert(child2 != NullNode);
nodes[index].Height = 1 + Math.Max(nodes[child1].Height, nodes[child2].Height);
nodes[index].Aabb.Combine(ref nodes[child1].Aabb, ref nodes[child2].Aabb);
index = nodes[index].ParentOrNext;
}
//Validate();
}
public static void MakeExplosion(ref PhysicsWorld physicsWorld,
ComponentDataFromEntity <Translation> translationGroup, ComponentDataFromEntity <Rotation> rotationGroup,
ComponentDataFromEntity <PhysicsVelocity> physicsVelocityGroup, ComponentDataFromEntity <PhysicsMass> physicsMassGroup,
ComponentDataFromEntity <PlayerHealth> healthGroup,
float3 origin, float explosionRadius, float explosionForce, float explosionMaxDamage = 0f)
{
float3 min = new float3(origin.x - explosionRadius, origin.y - explosionRadius, origin.z - explosionRadius);
float3 max = new float3(origin.x + explosionRadius, origin.y + explosionRadius, origin.z + explosionRadius);
Aabb explosionBounds = new Aabb
{
Min = min,
Max = max,
};
OverlapAabbInput input = new OverlapAabbInput
{
Aabb = explosionBounds,
Filter = CollisionFilter.Default,
};
NativeList <int> rigidBodyIndexs = new NativeList <int>(256, Allocator.Temp); // indexes into world.Bodies[]
if (physicsWorld.CollisionWorld.OverlapAabb(input, ref rigidBodyIndexs))
{
for (int i = 0; i < rigidBodyIndexs.Length; i++)
{
RigidBody body = physicsWorld.Bodies[rigidBodyIndexs[i]];
if (physicsVelocityGroup.Exists(body.Entity) && physicsMassGroup.Exists(body.Entity))
{
float distance = math.distance(body.WorldFromBody.pos, origin);
if (distance < explosionRadius)
{
float gain = math.clamp((explosionRadius - distance) / explosionRadius, 0, 1);
float force = gain * explosionForce;
float3 toBody = body.WorldFromBody.pos - origin;
float3 impulse = math.normalize(toBody) * force;
//impulse.y = explosionForceY;
var velocity = physicsVelocityGroup[body.Entity];
var mass = physicsMassGroup[body.Entity];
var translation = translationGroup[body.Entity];
var rotation = rotationGroup[body.Entity];
velocity.ApplyImpulse(mass, translation, rotation, impulse, origin);
physicsVelocityGroup[body.Entity] = velocity;
if (healthGroup.Exists(body.Entity))
{
var health = healthGroup[body.Entity];
health.Health -= explosionMaxDamage * gain;
healthGroup[body.Entity] = health;
}
}
}
}
}
rigidBodyIndexs.Dispose();
}
// Recursive traversal of tree.
private void GetClosestPointCandidatesImpl(int index, Aabb aabb, Func<int, float> callback, ref float closestPointDistanceSquared)
{
// closestPointDistanceSquared == -1 can be returned by callback to abort the query.
if (closestPointDistanceSquared < 0)
{
// Abort.
return;
}
Node node = _nodes[index];
// If we have a contact, it is not necessary to examine nodes with no AABB contact
// because they cannot give a closer point pair.
if (closestPointDistanceSquared == 0 && !GeometryHelper.HaveContact(GetAabb(node), aabb))
return;
if (node.IsLeaf)
{
// Node is leaf - call callback and updated closest-point distance.
var leafDistanceSquared = callback(node.Item);
closestPointDistanceSquared = Math.Min(leafDistanceSquared, closestPointDistanceSquared);
return;
}
int leftIndex = index + 1;
Node leftChild = _nodes[leftIndex];
int rightIndex = (leftChild.IsLeaf) ? leftIndex + 1 : leftIndex + leftChild.EscapeOffset;
Node rightChild = _nodes[rightIndex];
if (closestPointDistanceSquared == 0)
{
// We have contact, so we must examine all children.
GetClosestPointCandidatesImpl(leftIndex, aabb, callback, ref closestPointDistanceSquared);
GetClosestPointCandidatesImpl(rightIndex, aabb, callback, ref closestPointDistanceSquared);
return;
}
// No contact. Use lower bound estimates to search the best nodes first.
float minDistanceLeft = GeometryHelper.GetDistanceSquared(GetAabb(leftChild), aabb);
float minDistanceRight = GeometryHelper.GetDistanceSquared(GetAabb(rightChild), aabb);
if (minDistanceLeft < minDistanceRight)
{
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN.
if (minDistanceLeft > closestPointDistanceSquared)
return;
// Handle left first.
GetClosestPointCandidatesImpl(leftIndex, aabb, callback, ref closestPointDistanceSquared);
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN.
if (minDistanceRight > closestPointDistanceSquared)
return;
GetClosestPointCandidatesImpl(rightIndex, aabb, callback, ref closestPointDistanceSquared);
}
else
{
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN.
if (minDistanceRight > closestPointDistanceSquared)
return;
// Handle right first.
GetClosestPointCandidatesImpl(rightIndex, aabb, callback, ref closestPointDistanceSquared);
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN.
if (minDistanceLeft > closestPointDistanceSquared)
return;
GetClosestPointCandidatesImpl(leftIndex, aabb, callback, ref closestPointDistanceSquared);
}
}
/// <inheritdoc/>
public override Aabb GetAabb(Vector3F scale, Pose pose)
{
// Recompute local cached AABB if it is invalid.
if (Numeric.IsNaN(_aabbLocal.Minimum.X))
_aabbLocal = base.GetAabb(Vector3F.One, Pose.Identity);
// Apply scale and pose to AABB.
return _aabbLocal.GetAabb(scale, pose);
}
private IEnumerable<int> GetOverlapsImpl(Aabb aabb)
{
// This method avoids the Update() call!
#if !POOL_ENUMERABLES
if (_numberOfItems == 0)
yield break;
// ----- Stackless traversal of tree:
// The AABB tree nodes are stored in preorder traversal order. We can visit them in linear
// order. The EscapeOffset of each node can be used to skip a subtree.
int index = 0;
while (index < _nodes.Length)
{
Node node = _nodes[index];
bool haveContact = GeometryHelper.HaveContact(GetAabb(node), aabb);
if (haveContact && node.IsLeaf)
yield return node.Item;
if (haveContact || node.IsLeaf)
{
// Given AABB intersects the internal AABB tree node or the node is a leaf.
// Continue with next item in preorder traversal order.
index++;
}
else
{
// Given AABB does not touch the internal AABB tree node.
// --> Skip the subtree.
index += node.EscapeOffset;
}
}
#else
// Avoiding garbage:
return GetOverlapsWork.Create(this, ref aabb);
#endif
}
private static float DistanceToBBox(V pos, Aabb <V> bbox, Func <V, V, float> distance)
{
return(distance(pos, pos.Clamp(bbox.Min, bbox.Max)));
}
/// <inheritdoc/>
public IEnumerable<int> GetOverlaps(Aabb aabb)
{
Update(false);
return GetOverlapsImpl(aabb);
}
public IEnumerable <KeyValuePair <V, T> > Overlap(Aabb <V> bbox)
{
return(OverlappingNodes(_root, bbox, 0)
.Select(node => new KeyValuePair <V, T> (node.Position, node.Data)));
}
/// <inheritdoc/>
protected override void OnChanged(ShapeChangedEventArgs eventArgs)
{
// Set cached AABB to "invalid".
_aabbLocal = new Aabb(new Vector3F(float.NaN), new Vector3F(float.NaN));
_innerPoint = new Vector3F(float.NaN);
base.OnChanged(eventArgs);
}
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 static bool GetClosestPoint(Aabb aabb, Vector3F point, out Vector3F pointOnAabb)
{
// Short version: Fast when using SIMD instructions.
//pointOnAabb = point;
//pointOnAabb = Vector3F.Max(pointOnAabb, aabb.Minimum);
//pointOnAabb = Vector3F.Min(pointOnAabb, aabb.Maximum);
//return (point == pointOnAabb);
bool haveContact = true;
pointOnAabb = point;
if (pointOnAabb.X < aabb.Minimum.X)
{
pointOnAabb.X = aabb.Minimum.X;
haveContact = false;
}
else if (pointOnAabb.X > aabb.Maximum.X)
{
pointOnAabb.X = aabb.Maximum.X;
haveContact = false;
}
if (pointOnAabb.Y < aabb.Minimum.Y)
{
pointOnAabb.Y = aabb.Minimum.Y;
haveContact = false;
}
else if (pointOnAabb.Y > aabb.Maximum.Y)
{
pointOnAabb.Y = aabb.Maximum.Y;
haveContact = false;
}
if (pointOnAabb.Z < aabb.Minimum.Z)
{
pointOnAabb.Z = aabb.Minimum.Z;
haveContact = false;
}
else if (pointOnAabb.Z > aabb.Maximum.Z)
{
pointOnAabb.Z = aabb.Maximum.Z;
haveContact = false;
}
return haveContact;
}
private void InsertLeaf(int leaf)
{
if (m_root == Null)
{
m_root = leaf;
m_nodes[m_root].Parent = Null;
return;
}
// find best sibling
Aabb leafBounds = m_nodes[leaf].Bounds;
int index = m_root;
while (!m_nodes[index].IsLeaf)
{
int childA = m_nodes[index].ChildA;
int childB = m_nodes[index].ChildB;
float area = m_nodes[index].Bounds.HalfArea;
Aabb combinedBounds = Aabb.Union(m_nodes[index].Bounds, leafBounds);
float combinedArea = combinedBounds.HalfArea;
// cost of creating a new parent for this node and the new leaf
float cost = 2.0f * combinedArea;
// minimum cost of pushing the leaf further down the tree
float inheritanceCost = 2.0f * (combinedArea - area);
// cost of descending into child A
float costA;
if (m_nodes[childA].IsLeaf)
{
Aabb bounds;
bounds = Aabb.Union(leafBounds, m_nodes[childA].Bounds);
costA = bounds.HalfArea + inheritanceCost;
}
else
{
Aabb bounds;
bounds = Aabb.Union(leafBounds, m_nodes[childA].Bounds);
float oldArea = m_nodes[childA].Bounds.HalfArea;
float newArea = bounds.HalfArea;
costA = (newArea - oldArea) + inheritanceCost;
}
// cost of descending into child B
float costB;
if (m_nodes[childB].IsLeaf)
{
Aabb bounds;
bounds = Aabb.Union(leafBounds, m_nodes[childB].Bounds);
costB = bounds.HalfArea + inheritanceCost;
}
else
{
Aabb bounds;
bounds = Aabb.Union(leafBounds, m_nodes[childB].Bounds);
float oldArea = m_nodes[childB].Bounds.HalfArea;
float newArea = bounds.HalfArea;
costB = (newArea - oldArea) + inheritanceCost;
}
// descend according to the minimum cost
if (cost < costA && cost < costB)
{
break;
}
//descend
index = (costA < costB) ? childA : childB;
}
int sibling = index;
// create a new parent
int oldParent = m_nodes[sibling].Parent;
int newParent = AllocateNode();
m_nodes[newParent].Parent = oldParent;
m_nodes[newParent].Bounds = Aabb.Union(leafBounds, m_nodes[sibling].Bounds);
m_nodes[newParent].Height = m_nodes[sibling].Height + 1;
if (oldParent != Null)
{
// sibling was not the root
if (m_nodes[oldParent].ChildA == sibling)
{
m_nodes[oldParent].ChildA = newParent;
}
else
{
m_nodes[oldParent].ChildB = newParent;
}
m_nodes[newParent].ChildA = sibling;
m_nodes[newParent].ChildB = leaf;
m_nodes[sibling].Parent = newParent;
m_nodes[leaf].Parent = newParent;
}
else
{
// sibling was the root
m_nodes[newParent].ChildA = sibling;
m_nodes[newParent].ChildB = leaf;
m_nodes[sibling].Parent = newParent;
m_nodes[leaf].Parent = newParent;
m_root = newParent;
}
// walk back up to re-balance heights
index = m_nodes[leaf].Parent;
while (index != Null)
{
index = Balance(index);
int childA = m_nodes[index].ChildA;
int childB = m_nodes[index].ChildB;
m_nodes[index].Height = 1 + Mathf.Max(m_nodes[childA].Height, m_nodes[childB].Height);
m_nodes[index].Bounds = Aabb.Union(m_nodes[childA].Bounds, m_nodes[childB].Bounds);
index = m_nodes[index].Parent;
}
}
public static bool HaveContact(Aabb aabb, Vector3F point)
{
// Note: The following check is safe if one AABB is undefined (NaN).
// Do not change the comparison operator!
return aabb.Minimum.X <= point.X
&& aabb.Maximum.X >= point.X
&& aabb.Minimum.Y <= point.Y
&& aabb.Maximum.Y >= point.Y
&& aabb.Minimum.Z <= point.Z
&& aabb.Maximum.Z >= point.Z;
}
private int Balance(int a)
{
if (m_nodes[a].IsLeaf || m_nodes[a].Height < 2)
{
return(a);
}
int b = m_nodes[a].ChildA;
int c = m_nodes[a].ChildB;
int balance = m_nodes[c].Height - m_nodes[b].Height;
// rotate C up
if (balance > 1)
{
int f = m_nodes[c].ChildA;
int g = m_nodes[c].ChildB;
// swap A and C
m_nodes[c].ChildA = a;
m_nodes[c].Parent = m_nodes[a].Parent;
m_nodes[a].Parent = c;
// A's old parent should point to C
if (m_nodes[c].Parent != Null)
{
if (m_nodes[m_nodes[c].Parent].ChildA == a)
{
m_nodes[m_nodes[c].Parent].ChildA = c;
}
else
{
m_nodes[m_nodes[c].Parent].ChildB = c;
}
}
else
{
m_root = c;
}
// rotate
if (m_nodes[f].Height > m_nodes[g].Height)
{
m_nodes[c].ChildB = f;
m_nodes[a].ChildB = g;
m_nodes[g].Parent = a;
m_nodes[a].Bounds = Aabb.Union(m_nodes[b].Bounds, m_nodes[g].Bounds);
m_nodes[c].Bounds = Aabb.Union(m_nodes[a].Bounds, m_nodes[f].Bounds);
m_nodes[a].Height = 1 + Mathf.Max(m_nodes[b].Height, m_nodes[g].Height);
m_nodes[c].Height = 1 + Mathf.Max(m_nodes[a].Height, m_nodes[f].Height);
}
else
{
m_nodes[c].ChildB = g;
m_nodes[a].ChildB = f;
m_nodes[f].Parent = a;
m_nodes[a].Bounds = Aabb.Union(m_nodes[b].Bounds, m_nodes[f].Bounds);
m_nodes[c].Bounds = Aabb.Union(m_nodes[a].Bounds, m_nodes[g].Bounds);
m_nodes[a].Height = 1 + Mathf.Max(m_nodes[b].Height, m_nodes[f].Height);
m_nodes[c].Height = 1 + Mathf.Max(m_nodes[a].Height, m_nodes[g].Height);
}
return(c);
}
// rotate B up
if (balance < -1)
{
int d = m_nodes[b].ChildA;
int e = m_nodes[b].ChildB;
// swap A and B
m_nodes[b].ChildA = a;
m_nodes[b].Parent = m_nodes[a].Parent;
m_nodes[a].Parent = b;
// A's old parent should point to B
if (m_nodes[b].Parent != Null)
{
if (m_nodes[m_nodes[b].Parent].ChildA == a)
{
m_nodes[m_nodes[b].Parent].ChildA = b;
}
else
{
m_nodes[m_nodes[b].Parent].ChildB = b;
}
}
else
{
m_root = b;
}
// rotate
if (m_nodes[d].Height > m_nodes[e].Height)
{
m_nodes[b].ChildB = d;
m_nodes[a].ChildA = e;
m_nodes[e].Parent = a;
m_nodes[a].Bounds = Aabb.Union(m_nodes[c].Bounds, m_nodes[e].Bounds);
m_nodes[b].Bounds = Aabb.Union(m_nodes[a].Bounds, m_nodes[d].Bounds);
m_nodes[a].Height = 1 + Mathf.Max(m_nodes[c].Height, m_nodes[e].Height);
m_nodes[b].Height = 1 + Mathf.Max(m_nodes[a].Height, m_nodes[d].Height);
}
else
{
m_nodes[b].ChildB = e;
m_nodes[a].ChildA = d;
m_nodes[d].Parent = a;
m_nodes[a].Bounds = Aabb.Union(m_nodes[c].Bounds, m_nodes[d].Bounds);
m_nodes[b].Bounds = Aabb.Union(m_nodes[a].Bounds, m_nodes[e].Bounds);
m_nodes[a].Height = 1 + Mathf.Max(m_nodes[c].Height, m_nodes[d].Height);
m_nodes[b].Height = 1 + Mathf.Max(m_nodes[a].Height, m_nodes[e].Height);
}
return(b);
}
return(a);
}
/// <summary>
/// Computes the squared distance between the two AABBs.
/// </summary>
/// <param name="aabbA">The first AABB.</param>
/// <param name="aabbB">The second AABB.</param>
/// <returns>
/// The squared distance between the two AABBs.
/// </returns>
internal static float GetDistanceSquared(Aabb aabbA, Aabb aabbB)
{
float distanceSquared = 0;
if (aabbA.Minimum.X > aabbB.Maximum.X)
{
float delta = aabbA.Minimum.X - aabbB.Maximum.X;
distanceSquared += delta * delta;
}
else if (aabbB.Minimum.X > aabbA.Maximum.X)
{
float delta = aabbB.Minimum.X - aabbA.Maximum.X;
distanceSquared += delta * delta;
}
if (aabbA.Minimum.Y > aabbB.Maximum.Y)
{
float delta = aabbA.Minimum.Y - aabbB.Maximum.Y;
distanceSquared += delta * delta;
}
else if (aabbB.Minimum.Y > aabbA.Maximum.Y)
{
float delta = aabbB.Minimum.Y - aabbA.Maximum.Y;
distanceSquared += delta * delta;
}
if (aabbA.Minimum.Z > aabbB.Maximum.Z)
{
float delta = aabbA.Minimum.Z - aabbB.Maximum.Z;
distanceSquared += delta * delta;
}
else if (aabbB.Minimum.Z > aabbA.Maximum.Z)
{
float delta = aabbB.Minimum.Z - aabbA.Maximum.Z;
distanceSquared += delta * delta;
}
return distanceSquared;
}
public void DrwaGizmos(int isolateDepth = -1)
{
// TODO height is inverted depth
#if UNITY_EDITOR
if (m_root == Null)
{
return;
}
Color prevColor = Handles.color;
int isolateHeight = m_nodes[m_root].Height - isolateDepth;
var aNodeVisited = new bool[m_nodes.Length];
for (int i = 0; i < aNodeVisited.Length; ++i)
{
aNodeVisited[i] = false;
}
for (int i = 0; i < m_nodes.Length; ++i)
{
if (m_nodes[i].IsFree)
{
continue;
}
if (aNodeVisited[i])
{
continue;
}
Aabb bounds = m_nodes[i].Bounds;
if (isolateDepth >= 0)
{
if (m_nodes[i].Height != isolateHeight)
{
continue;
}
Gizmos.color =
m_nodes[i].Height == isolateHeight - 1
? Color.gray
: Color.white;
Handles.color = Color.gray;
DebugDrawNode(m_nodes[i].Parent, false, true, false);
DebugDrawLink(i, m_nodes[i].Parent);
DebugDrawNode(m_nodes[i].ChildA, true, true, false);
DebugDrawLink(i, m_nodes[i].ChildA);
DebugDrawNode(m_nodes[i].ChildB, true, true, false);
DebugDrawLink(i, m_nodes[i].ChildB);
if (m_nodes[i].ChildA != Null)
{
aNodeVisited[m_nodes[i].ChildA] = true;
}
if (m_nodes[i].ChildB != Null)
{
aNodeVisited[m_nodes[i].ChildB] = true;
}
Handles.color = Color.white;
DebugDrawNode(i, true, true, false);
aNodeVisited[i] = true;
continue;
}
Handles.color = Color.white;
DebugDrawLink(i, m_nodes[i].Parent);
DebugDrawNode(i, true, true, true);
aNodeVisited[i] = true;
}
Gizmos.color = prevColor;
#endif
}
private void UpdateBoundingShape()
{
// ----- Update BoundingShape
// Compute a transformed shape with a box from the minimal AABB.
var boxObject = (GeometricObject)BoundingShape.Child;
var boxShape = (BoxShape)boxObject.Shape;
var vertexArray = (Vertices != null) ? Vertices.Array : null;
if (vertexArray != null && vertexArray.Length > 0)
{
Aabb aabb = new Aabb(vertexArray[0], vertexArray[0]);
var numberOfVertices = vertexArray.Length;
for (int i = 1; i < numberOfVertices; i++)
aabb.Grow(vertexArray[i]);
// Update existing shape.
boxObject.Pose = new Pose(aabb.Center);
boxShape.Extent = aabb.Extent;
}
else
{
// Set an "empty" shape.
boxObject.Pose = Pose.Identity;
boxShape.Extent = Vector3F.Zero;
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Object A should be the height field.
CollisionObject heightFieldCollisionObject = contactSet.ObjectA;
CollisionObject otherCollisionObject = contactSet.ObjectB;
// Swap objects if necessary.
bool swapped = !(heightFieldCollisionObject.GeometricObject.Shape is HeightField);
if (swapped)
{
MathHelper.Swap(ref heightFieldCollisionObject, ref otherCollisionObject);
}
IGeometricObject heightFieldGeometricObject = heightFieldCollisionObject.GeometricObject;
IGeometricObject otherGeometricObject = otherCollisionObject.GeometricObject;
HeightField heightField = heightFieldGeometricObject.Shape as HeightField;
Shape otherShape = otherGeometricObject.Shape;
// Check if collision object shapes are correct.
if (heightField == null)
{
throw new ArgumentException("The contact set must contain a height field.", "contactSet");
}
if (heightField.UseFastCollisionApproximation && type != CollisionQueryType.ClosestPoints)
{
// If other object is convex, use the new fast collision detection algorithm.
ConvexShape convex = otherShape as ConvexShape;
if (convex != null)
{
ComputeCollisionFast(
contactSet,
type,
heightFieldGeometricObject,
otherGeometricObject,
heightField,
convex,
swapped);
return;
}
}
Vector3 scaleHeightField = heightFieldGeometricObject.Scale;
Vector3 scaleOther = otherGeometricObject.Scale;
Pose heightFieldPose = heightFieldGeometricObject.Pose;
// We do not support negative scaling. It is not clear what should happen when y is
// scaled with a negative factor and triangle orders would be wrong... Not worth the trouble.
if (scaleHeightField.X < 0 || scaleHeightField.Y < 0 || scaleHeightField.Z < 0)
{
throw new NotSupportedException("Computing collisions for height fields with a negative scaling is not supported.");
}
// Get height field and basic info.
Vector3 heightFieldUpAxis = heightFieldPose.ToWorldDirection(Vector3.UnitY);
int arrayLengthX = heightField.NumberOfSamplesX;
int arrayLengthZ = heightField.NumberOfSamplesZ;
Debug.Assert(arrayLengthX > 1 && arrayLengthZ > 1, "A height field should contain at least 2 x 2 elements (= 1 cell).");
float cellWidthX = heightField.WidthX * scaleHeightField.X / (arrayLengthX - 1);
float cellWidthZ = heightField.WidthZ * scaleHeightField.Z / (arrayLengthZ - 1);
// The search-space is the rectangular region on the height field where the closest points
// must lie in. For contacts we do not have to search neighbor cells. For closest-point
// queries and separation we have to search neighbor cells.
// We compute the search-space using a current maximum search distance.
float currentSearchDistance = 0;
Contact guessedClosestPair = null;
if (!contactSet.HaveContact && type == CollisionQueryType.ClosestPoints)
{
// Make a guess for the closest pair using SupportMapping or InnerPoints.
bool isOverHole;
guessedClosestPair = GuessClosestPair(contactSet, swapped, out isOverHole);
if (isOverHole)
{
// Guesses over holes are useless. --> Check the whole terrain.
currentSearchDistance = heightFieldGeometricObject.Aabb.Extent.Length;
}
else if (guessedClosestPair.PenetrationDepth < 0)
{
currentSearchDistance = -guessedClosestPair.PenetrationDepth;
}
else
{
contactSet.HaveContact = true;
}
}
else
{
// Assume no contact.
contactSet.HaveContact = false;
}
// Get AABB of the other object in local space of the height field.
Aabb aabbOfOther = otherShape.GetAabb(scaleOther, heightFieldPose.Inverse * otherGeometricObject.Pose);
float originX = heightField.OriginX * scaleHeightField.X;
float originZ = heightField.OriginZ * scaleHeightField.Z;
// ----- Compute the cell indices of the search-space.
// Estimate start and end indices from our search distance.
int xIndexStartEstimated = (int)((aabbOfOther.Minimum.X - currentSearchDistance - originX) / cellWidthX);
int xIndexEndEstimated = (int)((aabbOfOther.Maximum.X + currentSearchDistance - originX) / cellWidthX);
int zIndexStartEstimated = (int)((aabbOfOther.Minimum.Z - currentSearchDistance - originZ) / cellWidthZ);
int zIndexEndEstimated = (int)((aabbOfOther.Maximum.Z + currentSearchDistance - originZ) / cellWidthZ);
// Clamp indices to valid range.
int xIndexMax = arrayLengthX - 2;
int zIndexMax = arrayLengthZ - 2;
int xIndexStart = Math.Max(xIndexStartEstimated, 0);
int xIndexEnd = Math.Min(xIndexEndEstimated, xIndexMax);
int zIndexStart = Math.Max(zIndexStartEstimated, 0);
int zIndexEnd = Math.Min(zIndexEndEstimated, zIndexMax);
// Find collision algorithm for MinkowskiSum vs. other object's shape.
CollisionAlgorithm collisionAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(ConvexShape), otherShape.GetType()];
int numberOfContactsInLastFrame = contactSet.Count;
// Create several temporary test objects:
// Instead of the original height field geometric object, we test against a shape for each
// height field triangle. For the test shape we "extrude" the triangle under the height field.
// To create the extrusion we "add" a line segment to the triangle using a Minkowski sum.
// TODO: We can make this faster with a special shape that knows that the child poses are Identity (instead of the standard MinkowskiSumShape).
// This special shape could compute its InnerPoint without applying the poses.
var triangleShape = ResourcePools.TriangleShapes.Obtain();
// (Vertices will be set in the loop below.)
var triangleGeometricObject = TestGeometricObject.Create();
triangleGeometricObject.Shape = triangleShape;
var lineSegment = ResourcePools.LineSegmentShapes.Obtain();
lineSegment.Start = Vector3.Zero;
lineSegment.End = -heightField.Depth * Vector3.UnitY;
var lineSegmentGeometricObject = TestGeometricObject.Create();
lineSegmentGeometricObject.Shape = lineSegment;
var extrudedTriangleShape = TestMinkowskiSumShape.Create();
extrudedTriangleShape.ObjectA = triangleGeometricObject;
extrudedTriangleShape.ObjectB = lineSegmentGeometricObject;
var extrudedTriangleGeometricObject = TestGeometricObject.Create();
extrudedTriangleGeometricObject.Shape = extrudedTriangleShape;
extrudedTriangleGeometricObject.Pose = heightFieldPose;
var testCollisionObject = ResourcePools.TestCollisionObjects.Obtain();
testCollisionObject.SetInternal(heightFieldCollisionObject, extrudedTriangleGeometricObject);
var testContactSet = swapped ? ContactSet.Create(contactSet.ObjectA, testCollisionObject)
: ContactSet.Create(testCollisionObject, contactSet.ObjectB);
testContactSet.IsPerturbationTestAllowed = false;
// We compute closest points with a preferred normal direction: the height field up-axis.
// Loop over the cells in the search space.
// (The inner loop can reduce the search space. Therefore, when we increment the indices
// xIndex and zIndex we also check if the start indices have changed.)
for (int xIndex = xIndexStart; xIndex <= xIndexEnd; xIndex = Math.Max(xIndexStart, xIndex + 1))
{
for (int zIndex = zIndexStart; zIndex <= zIndexEnd; zIndex = Math.Max(zIndexStart, zIndex + 1))
{
// Test the two cell triangles.
for (int triangleIndex = 0; triangleIndex < 2; triangleIndex++)
{
// Get triangle 0 or 1.
var triangle = heightField.GetTriangle(xIndex, zIndex, triangleIndex != 0);
var triangleIsHole = Numeric.IsNaN(triangle.Vertex0.Y * triangle.Vertex1.Y * triangle.Vertex2.Y);
if (triangleIsHole)
{
continue;
}
triangleShape.Vertex0 = triangle.Vertex0 * scaleHeightField;
triangleShape.Vertex1 = triangle.Vertex1 * scaleHeightField;
triangleShape.Vertex2 = triangle.Vertex2 * scaleHeightField;
if (type == CollisionQueryType.Boolean)
{
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Boolean);
contactSet.HaveContact = contactSet.HaveContact || testContactSet.HaveContact;
if (contactSet.HaveContact)
{
// We can stop tests here for boolean queries.
// Update end indices to exit the outer loops.
xIndexEnd = -1;
zIndexEnd = -1;
break;
}
}
else
{
Debug.Assert(testContactSet.Count == 0, "testContactSet needs to be cleared.");
// If we know that we have a contact, then we can make a faster contact query
// instead of a closest-point query.
CollisionQueryType queryType = (contactSet.HaveContact) ? CollisionQueryType.Contacts : type;
collisionAlgorithm.ComputeCollision(testContactSet, queryType);
if (testContactSet.HaveContact)
{
contactSet.HaveContact = true;
if (testContactSet.Count > 0)
{
// Get neighbor triangle.
// To compute the triangle normal we take the normal of the unscaled triangle and transform
// the normal with: (M^-1)^T = 1 / scale
// Note: We cannot use the scaled vertices because negative scalings change the
// face-order of the vertices.
Vector3 triangleNormal = triangle.Normal / scaleHeightField;
triangleNormal = heightFieldPose.ToWorldDirection(triangleNormal);
triangleNormal.TryNormalize();
// Assuming the last contact is the newest. (With closest-point queries
// and the CombinedCollisionAlgo, testContactSet[0] could be a (not so useful)
// closest-point result, and testContactSet[1] the better contact query result.)
var testContact = testContactSet[testContactSet.Count - 1];
var contactNormal = swapped ? -testContact.Normal : testContact.Normal;
if (Vector3.Dot(contactNormal, triangleNormal) < WeldingLimit)
{
// Contact normal deviates by more than the welding limit. --> Check the contact.
// If we do not find a neighbor, we assume the neighbor has the same normal.
var neighborNormal = triangleNormal;
// Get barycentric coordinates of contact position.
Vector3 contactPositionOnHeightField = swapped ? testContact.PositionBLocal / scaleHeightField : testContact.PositionALocal / scaleHeightField;
float u, v, w;
// TODO: GetBaryCentricFromPoint computes the triangle normal, which we already know - optimize.
GeometryHelper.GetBarycentricFromPoint(triangle, contactPositionOnHeightField, out u, out v, out w);
// If one coordinate is near 0, the contact is near an edge.
if (u < 0.05f || v < 0.05f || w < 0.05f)
{
if (triangleIndex == 0)
{
if (u < v && u < w)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex, true).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else if (v < w)
{
if (zIndex > 0)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex - 1, true).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
// The contact is at the border of the whole height field. Set a normal which disables all bad contact filtering.
neighborNormal = new Vector3(float.NaN);
}
}
else
{
if (xIndex > 0)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex - 1, zIndex, true).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
neighborNormal = new Vector3(float.NaN);
}
}
}
else
{
if (u < v && u < w)
{
if (xIndex + 2 < arrayLengthX)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex + 1, zIndex, false).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
neighborNormal = new Vector3(float.NaN);
}
}
else if (v < w)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex, false).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
if (zIndex + 2 < arrayLengthZ)
{
neighborNormal = heightFieldPose.ToWorldDirection(heightField.GetTriangle(xIndex, zIndex + 1, true).Normal / scaleHeightField);
neighborNormal.TryNormalize();
}
else
{
neighborNormal = new Vector3(float.NaN);
}
}
}
}
// Contact normals in the range triangleNormal - neighborNormal are allowed.
// Others, especially vertical contacts in slopes or horizontal normals are not
// allowed.
var cosMinAngle = Vector3.Dot(neighborNormal, triangleNormal) - CollisionDetection.Epsilon;
RemoveBadContacts(swapped, testContactSet, triangleNormal, cosMinAngle);
// If we have no contact yet, we retry with a preferred normal identical to the up axis.
// (Note: contactSet.Count will be > 0 for closest-point queries but will
// probably constraint separated contacts and not real contacts.)
if (testContactSet.Count == 0 &&
(contactSet.Count == 0 || type == CollisionQueryType.ClosestPoints))
{
testContactSet.PreferredNormal = (swapped) ? -heightFieldUpAxis : heightFieldUpAxis;
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Contacts);
testContactSet.PreferredNormal = Vector3.Zero;
RemoveBadContacts(swapped, testContactSet, triangleNormal, cosMinAngle);
}
// If we have no contact yet, we retry with a preferred normal identical to the triangle normal.
// But only if the triangle normal differs significantly from the up axis.
if (testContactSet.Count == 0 &&
(contactSet.Count == 0 || type == CollisionQueryType.ClosestPoints) &&
Vector3.Dot(heightFieldUpAxis, triangleNormal) < WeldingLimit)
{
testContactSet.PreferredNormal = (swapped) ? -triangleNormal : triangleNormal;
collisionAlgorithm.ComputeCollision(testContactSet, CollisionQueryType.Contacts);
testContactSet.PreferredNormal = Vector3.Zero;
RemoveBadContacts(swapped, testContactSet, triangleNormal, cosMinAngle);
}
}
}
}
if (testContactSet.Count > 0)
{
// Remember separation distance for later.
float separationDistance = -testContactSet[0].PenetrationDepth;
// Set the shape feature of the new contacts.
// The features is the height field triangle index (see HeightField documentation).
int numberOfContacts = testContactSet.Count;
for (int i = 0; i < numberOfContacts; i++)
{
Contact contact = testContactSet[i];
int featureIndex = (zIndex * (arrayLengthX - 1) + xIndex) * 2 + triangleIndex;
if (swapped)
{
contact.FeatureB = featureIndex;
}
else
{
contact.FeatureA = featureIndex;
}
}
// Merge the contact info. (Contacts in testContactSet are recycled!)
ContactHelper.Merge(contactSet, testContactSet, type, CollisionDetection.ContactPositionTolerance);
// Update search space if possible.
// The best search distance is 0. For separation we can use the current smallest
// separation as search distance. As soon as we have a contact, we set the
// search distance to 0.
if (currentSearchDistance > 0 && // No need to update search space if search distance is already 0.
(contactSet.HaveContact || // If we have a contact, we set the search distance to 0.
separationDistance < currentSearchDistance)) // If we have closer separation, we use this.
{
// Note: We only check triangleContactSet[0] in the if condition.
// triangleContactSet could contain several contacts, but we don't bother with
// this special case.
// Update search distance.
if (contactSet.HaveContact)
{
currentSearchDistance = 0;
}
else
{
currentSearchDistance = Math.Max(0, separationDistance);
}
// Update search space indices.
xIndexStartEstimated = (int)((aabbOfOther.Minimum.X - currentSearchDistance - originX) / cellWidthX);
xIndexEndEstimated = (int)((aabbOfOther.Maximum.X + currentSearchDistance - originX) / cellWidthX);
zIndexStartEstimated = (int)((aabbOfOther.Minimum.Z - currentSearchDistance - originZ) / cellWidthZ);
zIndexEndEstimated = (int)((aabbOfOther.Maximum.Z + currentSearchDistance - originZ) / cellWidthZ);
xIndexStart = Math.Max(xIndexStart, xIndexStartEstimated);
xIndexEnd = Math.Min(xIndexEndEstimated, xIndexMax);
zIndexStart = Math.Max(zIndexStart, zIndexStartEstimated);
zIndexEnd = Math.Min(zIndexEndEstimated, zIndexMax);
}
}
}
}
}
}
// Recycle temporary objects.
testContactSet.Recycle();
ResourcePools.TestCollisionObjects.Recycle(testCollisionObject);
extrudedTriangleGeometricObject.Recycle();
extrudedTriangleShape.Recycle();
lineSegmentGeometricObject.Recycle();
ResourcePools.LineSegmentShapes.Recycle(lineSegment);
triangleGeometricObject.Recycle();
ResourcePools.TriangleShapes.Recycle(triangleShape);
if (contactSet.Count == 0 &&
(contactSet.HaveContact && type == CollisionQueryType.Contacts || type == CollisionQueryType.ClosestPoints))
{
// ----- Bad contact info:
// We should have contact data because this is either a contact query and the objects touch
// or this is a closest-point query.
// Use our guess as the contact info.
Contact closestPair = guessedClosestPair;
bool isOverHole = false;
if (closestPair == null)
{
closestPair = GuessClosestPair(contactSet, swapped, out isOverHole);
}
// Guesses over holes are useless. :-(
if (!isOverHole)
{
ContactHelper.Merge(contactSet, closestPair, type, CollisionDetection.ContactPositionTolerance);
}
}
if (CollisionDetection.FullContactSetPerFrame &&
type == CollisionQueryType.Contacts &&
numberOfContactsInLastFrame == 0 &&
contactSet.Count > 0 &&
contactSet.Count < 4)
{
// Try to find full contact set.
// TODO: This can be optimized by not doing the whole overhead of ComputeCollision again.
ContactHelper.TestWithPerturbations(
CollisionDetection,
contactSet,
!swapped, // Perturb objectB not the height field.
_computeContactsMethod);
}
}
public extern static void dInfiniteAABB( dGeomID geom, ref Aabb aabb );
private static void GetClosestPointCandidatesImpl(Node node, Aabb aabb, Func <T, float> callback, ref float closestPointDistanceSquared)
{
// closestPointDistanceSquared == -1 indicates early exit.
if (closestPointDistanceSquared < 0)
{
// Abort.
return;
}
node.IsActive = true;
// If we have a contact, it is not necessary to examine nodes with no AABB contact
// because they cannot give a closer point pair.
if (closestPointDistanceSquared == 0 && !GeometryHelper.HaveContact(aabb, node.Aabb))
{
return;
}
if (node.IsLeaf)
{
// Node is leaf - call callback and updated closest-point distance.
var leafDistanceSquared = callback(node.Item);
closestPointDistanceSquared = Math.Min(leafDistanceSquared, closestPointDistanceSquared);
return;
}
SplitIfNecessary(node);
Node leftChild = node.LeftChild;
Node rightChild = node.RightChild;
if (closestPointDistanceSquared == 0)
{
// We have contact, so we must examine all children.
GetClosestPointCandidatesImpl(leftChild, aabb, callback, ref closestPointDistanceSquared);
GetClosestPointCandidatesImpl(rightChild, aabb, callback, ref closestPointDistanceSquared);
return;
}
// No contact. Use lower bound estimates to search the best nodes first.
float minDistanceLeft = GeometryHelper.GetDistanceSquared(aabb, leftChild.Aabb);
float minDistanceRight = GeometryHelper.GetDistanceSquared(aabb, rightChild.Aabb);
if (minDistanceLeft < minDistanceRight)
{
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN.
if (minDistanceLeft > closestPointDistanceSquared)
{
return;
}
// Handle left first.
GetClosestPointCandidatesImpl(leftChild, aabb, callback, ref closestPointDistanceSquared);
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN.
if (minDistanceRight > closestPointDistanceSquared)
{
return;
}
GetClosestPointCandidatesImpl(rightChild, aabb, callback, ref closestPointDistanceSquared);
}
else
{
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceRight is NaN.
if (minDistanceRight > closestPointDistanceSquared)
{
return;
}
// Handle right first.
GetClosestPointCandidatesImpl(rightChild, aabb, callback, ref closestPointDistanceSquared);
// Stop if other child cannot improve result.
// Note: Do not invert the "if" because this way it is safe if minDistanceLeft is NaN.
if (minDistanceLeft > closestPointDistanceSquared)
{
return;
}
GetClosestPointCandidatesImpl(leftChild, aabb, callback, ref closestPointDistanceSquared);
}
}