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); } }