///<summary> /// Constructs a new transformable shape. ///</summary> ///<param name="shape">Base shape to transform.</param> ///<param name="transform">Transform to use.</param> public TransformableShape(ConvexShape shape, Matrix3x3 transform) { this.shape = shape; this.transform = transform; UpdateConvexShapeInfo(); }
///<summary> /// Constructs a new transformable shape. ///</summary> /// <param name="shape">Base shape to transform.</param> /// <param name="transform">Transform to use.</param> /// <param name="description">Cached information about the shape. Assumed to be correct; no extra processing or validation is performed.</param> public TransformableShape(ConvexShape shape, Matrix3x3 transform, ConvexShapeDescription description) { this.shape = shape; this.transform = transform; UpdateConvexShapeInfo(description); }
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func<BroadPhaseEntry, bool> filter, out RayHit hit) { Vector3 swp = sweep; double len = swp.Length(); swp /= len; return ConvexCast(castShape, ref startingTransform, ref swp, len, MaterialSolidity.FULLSOLID, out hit); }
/// <summary> /// Casts a convex shape against the collidable. /// </summary> /// <param name="castShape">Shape to cast.</param> /// <param name="startingTransform">Initial transform of the shape.</param> /// <param name="sweep">Sweep to apply to the shape.</param> /// <param name="result">Hit data, if any.</param> /// <returns>Whether or not the cast hit anything.</returns> public bool ConvexCast(ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayCastResult result) { var outputOverlappedElements = Resources.GetCollidableList(); var rayHits = Resources.GetRayHitList(); BoundingBox boundingBox; Toolbox.GetExpandedBoundingBox(ref castShape, ref startingTransform, ref sweep, out boundingBox); CollidableTree.GetOverlaps(boundingBox, outputOverlappedElements); result = new RayCastResult(); result.HitData.T = float.MaxValue; for (int i = 0; i < outputOverlappedElements.count; ++i) { RayHit hit; if (outputOverlappedElements.Elements[i].ConvexCast(castShape, ref startingTransform, ref sweep, out hit)) { if (hit.T < result.HitData.T) { result.HitData = hit; result.HitObject = outputOverlappedElements.Elements[i]; } } } Resources.GiveBack(rayHits); Resources.GiveBack(outputOverlappedElements); return(result.HitData.T < float.MaxValue); }
/// <summary> /// Casts a convex shape against the collidable. /// </summary> /// <param name="castShape">Shape to cast.</param> /// <param name="startingTransform">Initial transform of the shape.</param> /// <param name="sweep">Sweep to apply to the shape.</param> /// <param name="filter">Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts.</param> /// <param name="result">Hit data, if any.</param> /// <returns>Whether or not the cast hit anything.</returns> public bool ConvexCast(ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func <BroadPhaseEntry, bool> filter, out RayCastResult result) { var outputOverlappedElements = PhysicsResources.GetCollidableList(); BoundingBox boundingBox; castShape.GetSweptBoundingBox(ref startingTransform, ref sweep, out boundingBox); CollidableTree.GetOverlaps(boundingBox, outputOverlappedElements); result = new RayCastResult(); result.HitData.T = Fix64.MaxValue; for (int i = 0; i < outputOverlappedElements.Count; ++i) { RayHit hit; if (outputOverlappedElements.Elements[i].ConvexCast(castShape, ref startingTransform, ref sweep, filter, out hit)) { if (hit.T < result.HitData.T) { result.HitData = hit; result.HitObject = outputOverlappedElements.Elements[i]; } } } PhysicsResources.GiveBack(outputOverlappedElements); return(result.HitData.T < Fix64.MaxValue); }
/// <summary> /// Cleans up the pair tester. /// </summary> public override void CleanUp() { convex = null; state = CollisionState.Plane; escapeAttempts = 0; localSeparatingAxis = new System.Numerics.Vector3(); Updated = false; }
///<summary> /// Computes the center and volume of a convex shape. ///</summary> ///<param name="shape">Shape to compute the center of.</param> ///<param name="volume">Volume of the shape.</param> ///<returns>Center of the shape.</returns> public static Vector3 ComputeCenter(ConvexShape shape, out float volume) { var pointContributions = Resources.GetVectorList(); GetPoints(shape, out volume, pointContributions); Vector3 center = AveragePoints(pointContributions); Resources.GiveBack(pointContributions); return center; }
///<summary> /// Computes the volume and volume distribution of a shape. ///</summary> ///<param name="shape">Shape to compute the volume information of.</param> ///<param name="volume">Volume of the shape.</param> ///<returns>Volume distribution of the shape.</returns> public static Matrix3x3 ComputeVolumeDistribution(ConvexShape shape, out float volume) { var pointContributions = CommonResources.GetVectorList(); GetPoints(shape, out volume, pointContributions); Vector3 center = AveragePoints(pointContributions); Matrix3x3 volumeDistribution = ComputeVolumeDistribution(pointContributions, ref center); CommonResources.GiveBack(pointContributions); return volumeDistribution; }
/// <summary> /// Cleans up the pair tester. /// </summary> public override void CleanUp() { triangle = null; convex = null; state = CollisionState.Plane; escapeAttempts = 0; localSeparatingAxis = new Vector3(); Updated = false; }
public bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweepnorm, double slen, MaterialSolidity solidness, out RayHit hit) { RigidTransform rt; RigidTransform.MultiplyByInverse(ref startingTransform, ref worldTransform, out rt); Vector3 swp = Quaternion.Transform(sweepnorm, Quaternion.Inverse(worldTransform.Orientation)); RayHit rh; bool h = ChunkShape.ConvexCast(castShape, ref rt, ref swp, slen, solidness, out rh); RigidTransform.Transform(ref rh.Location, ref worldTransform, out hit.Location); hit.Normal = rh.Normal; hit.T = rh.T; return h; }
///<summary> /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA. ///</summary> ///<param name="shapeA">First shape.</param> ///<param name="shapeB">Second shape.</param> ///<param name="direction">Extreme point direction in local space.</param> ///<param name="localTransformB">Transform of shapeB in the local space of A.</param> ///<param name="extremePoint">The extreme point in the local space of A.</param> public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePoint) { //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D) shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint); Vector3 v; Vector3 negativeN; Vector3.Negate(ref direction, out negativeN); shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out v); Vector3.Subtract(ref extremePoint, ref v, out extremePoint); ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, ref direction, out v); Vector3.Add(ref extremePoint, ref v, out extremePoint); }
public bool SpecialCaseConvexTrace(ConvexShape shape, Location start, Location dir, double len, MaterialSolidity considerSolid, Func<BroadPhaseEntry, bool> filter, out RayCastResult rayHit) { RigidTransform rt = new RigidTransform(start.ToBVector(), BEPUutilities.Quaternion.Identity); BEPUutilities.Vector3 sweep = (dir * len).ToBVector(); RayCastResult best = new RayCastResult(new RayHit() { T = len }, null); bool hA = false; if (considerSolid.HasFlag(MaterialSolidity.FULLSOLID)) { RayCastResult rcr; if (PhysicsWorld.ConvexCast(shape, ref rt, ref sweep, filter, out rcr)) { best = rcr; hA = true; } } sweep = dir.ToBVector(); AABB box = new AABB(); box.Min = start; box.Max = start; box.Include(start + dir * len); foreach (KeyValuePair<Vector3i, Chunk> chunk in LoadedChunks) { if (chunk.Value == null || chunk.Value.FCO == null) { continue; } if (!box.Intersects(new AABB() { Min = chunk.Value.WorldPosition.ToLocation() * Chunk.CHUNK_SIZE, Max = chunk.Value.WorldPosition.ToLocation() * Chunk.CHUNK_SIZE + new Location(Chunk.CHUNK_SIZE, Chunk.CHUNK_SIZE, Chunk.CHUNK_SIZE) })) { continue; } RayHit temp; if (chunk.Value.FCO.ConvexCast(shape, ref rt, ref sweep, len, considerSolid, out temp)) { hA = true; if (temp.T < best.HitData.T) { best.HitData = temp; best.HitObject = chunk.Value.FCO; } } } rayHit = best; return hA; }
///<summary> /// Tests if the pair is intersecting. ///</summary> ///<param name="shapeA">First shape of the pair.</param> ///<param name="shapeB">Second shape of the pair.</param> ///<param name="transformA">Transform to apply to the first shape.</param> ///<param name="transformB">Transform to apply to the second shape.</param> ///<param name="localSeparatingAxis">Warmstartable separating axis used by the method to quickly early-out if possible. Updated to the latest separating axis after each run.</param> ///<returns>Whether or not the objects were intersecting.</returns> public static bool AreShapesIntersecting(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB, ref Vector3 localSeparatingAxis) { RigidTransform localtransformB; MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB); //Warm start the simplex. var simplex = new SimpleSimplex(); Vector3 extremePoint; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref localSeparatingAxis, ref localtransformB, out extremePoint); simplex.AddNewSimplexPoint(ref extremePoint); Vector3 closestPoint; int count = 0; while (count++ < MaximumGJKIterations) { if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex. closestPoint.LengthSquared() <= simplex.GetErrorTolerance() * Toolbox.BigEpsilon) { //Intersecting, or so close to it that it will be difficult/expensive to figure out the separation. return true; } //Use the closest point as a direction. Vector3 direction; Vector3.Negate(ref closestPoint, out direction); MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref direction, ref localtransformB, out extremePoint); //Since this is a boolean test, we don't need to refine the simplex if it becomes apparent that we cannot reach the origin. //If the most extreme point at any given time does not go past the origin, then we can quit immediately. float dot; Vector3.Dot(ref extremePoint, ref closestPoint, out dot); //extreme point dotted against the direction pointing backwards towards the CSO. if (dot > 0) { // If it's positive, that means that the direction pointing towards the origin produced an extreme point 'in front of' the origin, eliminating the possibility of any intersection. localSeparatingAxis = direction; return false; } simplex.AddNewSimplexPoint(ref extremePoint); } return false; }
/// <summary> /// Constructs a new demo. /// </summary> /// <param name="game">Game owning this demo.</param> public MPRCastingDemo(DemosGame game) : base(game) { bShape = new BoxShape(1, 0, 1); //bShape.CollisionMargin = 0; aShape = new ConeShape(1, .4f); //aShape.CollisionMargin = 0; a = new Entity(aShape); b = new Entity(bShape); CollisionRules.AddRule(a, b, CollisionRule.NoSolver); NarrowPhaseHelper.CollisionManagers.Remove(new TypePair(typeof(ConvexCollidable<BoxShape>), typeof(ConvexCollidable<BoxShape>))); Space.Add(a); Space.Add(b); a.Orientation = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver4); b.Orientation = Quaternion.Identity; aTransform = new RigidTransform(new Vector3(-10, -10, -10), a.Orientation); bTransform = new RigidTransform(new Vector3(10, 10, 10), b.Orientation); game.Camera.Position = new Vector3(0, 5, 17); }
/// <summary> /// Sweeps a convex shape against the entry. /// </summary> /// <param name="castShape">Swept shape.</param> /// <param name="startingTransform">Beginning location and orientation of the cast shape.</param> /// <param name="sweep">Sweep motion to apply to the cast shape.</param> /// <param name="filter">Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts.</param> /// <param name="hit">Hit data of the cast on the entry, if any.</param> /// <returns>Whether or not the cast hit the entry.</returns> public virtual bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func<BroadPhaseEntry, bool> filter, out RayHit hit) { if (filter(this)) return ConvexCast(castShape, ref startingTransform, ref sweep, out hit); hit = new RayHit(); return false; }
///<summary> /// Constructs a new transformable shape. ///</summary> ///<param name="shape">Base shape to transform.</param> ///<param name="transform">Transform to use.</param> public TransformableShape(ConvexShape shape, Matrix3X3 transform) { this.shape = shape; Transform = transform; }
/// <summary> /// Constructs a convex shape entry. /// </summary> /// <param name="position">Local position of the entry.</param> /// <param name="shape">Shape of the entry.</param> public ConvexShapeEntry(Vector3 position, ConvexShape shape) { Transform = new RigidTransform(position); CollisionShape = shape; }
///<summary> /// Sweeps a shape against another shape using a given sweep vector. ///</summary> ///<param name="sweptShape">Shape to sweep.</param> ///<param name="target">Shape being swept against.</param> ///<param name="sweep">Sweep vector for the sweptShape.</param> ///<param name="startingSweptTransform">Starting transform of the sweptShape.</param> ///<param name="targetTransform">Transform to apply to the target shape.</param> ///<param name="hit">Hit data of the sweep test, if any.</param> ///<returns>Whether or not the swept shape hit the other shape.</returns> public static bool ConvexCast(ConvexShape sweptShape, ConvexShape target, ref Vector3 sweep, ref RigidTransform startingSweptTransform, ref RigidTransform targetTransform, out RayHit hit) { return ConvexCast(sweptShape, target, ref sweep, ref Toolbox.ZeroVector, ref startingSweptTransform, ref targetTransform, out hit); }
///<summary> /// Tests if the pair is intersecting. ///</summary> ///<param name="shapeA">First shape of the pair.</param> ///<param name="shapeB">Second shape of the pair.</param> ///<param name="transformA">Transform to apply to the first shape.</param> ///<param name="transformB">Transform to apply to the second shape.</param> ///<returns>Whether or not the shapes are intersecting.</returns> public static bool AreShapesIntersecting(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB) { //Zero isn't a very good guess! But it's a cheap guess. Vector3 separatingAxis = Toolbox.ZeroVector; return AreShapesIntersecting(shapeA, shapeB, ref transformA, ref transformB, ref separatingAxis); }
///<summary> /// Gets the closest points between the shapes. ///</summary> ///<param name="shapeA">First shape of the pair.</param> ///<param name="shapeB">Second shape of the pair.</param> ///<param name="transformA">Transform to apply to the first shape.</param> ///<param name="transformB">Transform to apply to the second shape.</param> ///<param name="closestPointA">Closest point on the first shape to the second shape.</param> ///<param name="closestPointB">Closest point on the second shape to the first shape.</param> ///<returns>Whether or not the objects were intersecting. If they are intersecting, then the closest points cannot be identified.</returns> public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB, out Vector3 closestPointA, out Vector3 closestPointB) { //The cached simplex stores locations that are local to the shapes. A fairly decent initial state is between the centroids of the objects. //In local space, the centroids are at the origins. RigidTransform localtransformB; MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB); var simplex = new CachedSimplex {State = SimplexState.Point}; // new CachedSimplex(shapeA, shapeB, ref localtransformB); bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref simplex, out closestPointA, out closestPointB); RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA); RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB); return toReturn; }
private static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB, ref CachedSimplex cachedSimplex, out Vector3 localClosestPointA, out Vector3 localClosestPointB) { var simplex = new PairSimplex(ref cachedSimplex, ref localTransformB); Vector3 closestPoint; int count = 0; while (true) { if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex and computes barycentric coordinates if necessary. closestPoint.LengthSquared() <= Toolbox.Epsilon * simplex.errorTolerance) { //Intersecting. localClosestPointA = Toolbox.ZeroVector; localClosestPointB = Toolbox.ZeroVector; simplex.UpdateCachedSimplex(ref cachedSimplex); return true; } if (++count > MaximumGJKIterations) break; //Must break BEFORE a new vertex is added if we're over the iteration limit. This guarantees final simplex is not a tetrahedron. if (simplex.GetNewSimplexPoint(shapeA, shapeB, count, ref closestPoint)) { //No progress towards origin, not intersecting. break; } } //Compute closest points from the contributing simplexes and barycentric coordinates simplex.GetClosestPoints(out localClosestPointA, out localClosestPointB); //simplex.VerifyContributions(); //if (Vector3.Distance(localClosestPointA - localClosestPointB, closestPoint) > .00001f) // Debug.WriteLine("break."); simplex.UpdateCachedSimplex(ref cachedSimplex); return false; }
///<summary> /// Constructs a new entry. ///</summary> ///<param name="orientation">Orientation of the entry.</param> ///<param name="shape">Shape of the entry.</param> public OrientedConvexShapeEntry(Quaternion orientation, ConvexShape shape) { Orientation = orientation; CollisionShape = shape; }
/// <summary> /// Sweeps a convex shape against the entry. /// </summary> /// <param name="castShape">Swept shape.</param> /// <param name="startingTransform">Beginning location and orientation of the cast shape.</param> /// <param name="sweep">Sweep motion to apply to the cast shape.</param> /// <param name="hit">Hit data of the ray on the entry, if any.</param> /// <returns>Whether or not the ray hit the entry.</returns> public abstract bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit);
///<summary> /// Computes the center of a convex shape. ///</summary> ///<param name="shape">Shape to compute the center of.</param> ///<returns>Center of the shape.</returns> public static Vector3 ComputeCenter(ConvexShape shape) { float volume; return(ComputeCenter(shape, out volume)); }
///<summary> /// Gets the point contributions within a convex shape. ///</summary> ///<param name="shape">Shape to compute the point contributions of.</param> ///<param name="volume">Volume of the shape.</param> ///<param name="outputPointContributions">Point contributions of the shape.</param> public static void GetPoints(ConvexShape shape, out float volume, RawList <Vector3> outputPointContributions) { RigidTransform transform = RigidTransform.Identity; BoundingBox boundingBox; shape.GetBoundingBox(ref transform, out boundingBox); //Find the direction which maximizes the possible hits. Generally, this is the smallest area axis. //Possible options are: //YZ -> use X //XZ -> use Y //XY -> use Z Ray ray; float width = boundingBox.Max.X - boundingBox.Min.X; float height = boundingBox.Max.Y - boundingBox.Min.Y; float length = boundingBox.Max.Z - boundingBox.Min.Z; float yzArea = height * length; float xzArea = width * length; float xyArea = width * height; Vector3 increment1, increment2; float incrementMultiplier = 1f / NumberOfSamplesPerDimension; float maxLength; float rayIncrement; if (yzArea > xzArea && yzArea > xyArea) { //use the x axis as the direction. ray.Direction = Vector3.Right; ray.Position = new Vector3(boundingBox.Min.X, boundingBox.Min.Y + .5f * incrementMultiplier * height, boundingBox.Min.Z + .5f * incrementMultiplier * length); increment1 = new Vector3(0, incrementMultiplier * height, 0); increment2 = new Vector3(0, 0, incrementMultiplier * length); rayIncrement = incrementMultiplier * width; maxLength = width; } else if (xzArea > xyArea) //yz is not the max, given by the previous if. Is xz or xy the max? { //use the y axis as the direction. ray.Direction = Vector3.Up; ray.Position = new Vector3(boundingBox.Min.X + .5f * incrementMultiplier * width, boundingBox.Min.Y, boundingBox.Min.Z + .5f * incrementMultiplier * length); increment1 = new Vector3(incrementMultiplier * width, 0, 0); increment2 = new Vector3(0, 0, incrementMultiplier * height); rayIncrement = incrementMultiplier * height; maxLength = height; } else { //use the z axis as the direction. ray.Direction = Vector3.Backward; ray.Position = new Vector3(boundingBox.Min.X + .5f * incrementMultiplier * width, boundingBox.Min.Y + .5f * incrementMultiplier * height, boundingBox.Min.Z); increment1 = new Vector3(incrementMultiplier * width, 0, 0); increment2 = new Vector3(0, incrementMultiplier * height, 0); rayIncrement = incrementMultiplier * length; maxLength = length; } Ray oppositeRay; volume = 0; for (int i = 0; i < NumberOfSamplesPerDimension; i++) { for (int j = 0; j < NumberOfSamplesPerDimension; j++) { //Ray cast from one direction. If it succeeds, try the other way. This forms an interval in which inertia tensor contributions are contained. RayHit hit; if (shape.RayTest(ref ray, ref transform, maxLength, out hit)) { Vector3.Multiply(ref ray.Direction, maxLength, out oppositeRay.Position); Vector3.Add(ref oppositeRay.Position, ref ray.Position, out oppositeRay.Position); Vector3.Negate(ref ray.Direction, out oppositeRay.Direction); RayHit oppositeHit; if (shape.RayTest(ref oppositeRay, ref transform, maxLength, out oppositeHit)) { //It should always get here if one direction casts, but there may be numerical issues. float scanVolume; ScanObject(rayIncrement, maxLength, ref increment1, ref increment2, ref ray, ref hit, ref oppositeHit, outputPointContributions, out scanVolume); volume += scanVolume; } } Vector3.Add(ref ray.Position, ref increment2, out ray.Position); } Vector3.Add(ref ray.Position, ref increment1, out ray.Position); //Move the ray back to the starting position along the other axis. Vector3 subtract; Vector3.Multiply(ref increment2, NumberOfSamplesPerDimension, out subtract); Vector3.Subtract(ref ray.Position, ref subtract, out ray.Position); } }
///<summary> /// Constructs a convex shape entry with identity transformation. ///</summary> ///<param name="shape">Shape of the entry.</param> public ConvexShapeEntry(ConvexShape shape) { Transform = RigidTransform.Identity; CollisionShape = shape; }
/// <summary> /// Constructs a convex shape entry. /// </summary> /// <param name="transform">Local transform of the entry.</param> /// <param name="shape">Shape of the entry.</param> public ConvexShapeEntry(RigidTransform transform, ConvexShape shape) { Transform = transform; CollisionShape = shape; }
/// <summary> /// Constructs a convex shape entry. /// </summary> /// <param name="orientation">Local orientation of the entry.</param> /// <param name="shape">Shape of the entry.</param> public ConvexShapeEntry(Quaternion orientation, ConvexShape shape) { Transform = new RigidTransform(orientation); CollisionShape = shape; }
///<summary> /// Constructs a new entry with identity orientation. ///</summary> ///<param name="shape">Shape of the entry.</param> public OrientedConvexShapeEntry(ConvexShape shape) { Orientation = Quaternion.Identity; CollisionShape = shape; }
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit) { hit = new RayHit(); BoundingBox boundingBox; castShape.GetSweptBoundingBox(ref startingTransform, ref sweep, out boundingBox); var tri = PhysicsResources.GetTriangle(); var hitElements = CommonResources.GetIntList(); if (triangleMesh.Tree.GetOverlaps(boundingBox, hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { triangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC); Vector3 center; Vector3.Add(ref tri.vA, ref tri.vB, out center); Vector3.Add(ref center, ref tri.vC, out center); Vector3.Multiply(ref center, 1f / 3f, out center); Vector3.Subtract(ref tri.vA, ref center, out tri.vA); Vector3.Subtract(ref tri.vB, ref center, out tri.vB); Vector3.Subtract(ref tri.vC, ref center, out tri.vC); tri.maximumRadius = tri.vA.LengthSquared(); float radius = tri.vB.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; radius = tri.vC.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; tri.maximumRadius = (float)Math.Sqrt(tri.maximumRadius); tri.collisionMargin = 0; var triangleTransform = new RigidTransform { Orientation = Quaternion.Identity, Position = center }; RayHit tempHit; if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T) { hit = tempHit; } } tri.maximumRadius = 0; PhysicsResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return hit.T != float.MaxValue; } PhysicsResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return false; }
///<summary> /// Constructs a new entry with identity orientation. ///</summary> ///<param name="shape">Shape of the entry.</param> public OrientedConvexShapeEntry(ConvexShape shape) { Orientation = System.Numerics.Quaternion.Identity; CollisionShape = shape; }
///<summary> /// Gets the closest points between the shapes. ///</summary> ///<param name="shapeA">First shape of the pair.</param> ///<param name="shapeB">Second shape of the pair.</param> ///<param name="transformA">Transform to apply to the first shape.</param> ///<param name="transformB">Transform to apply to the second shape.</param> /// <param name="cachedSimplex">Simplex from a previous updated used to warmstart the current attempt. Updated after each run.</param> ///<param name="closestPointA">Closest point on the first shape to the second shape.</param> ///<param name="closestPointB">Closest point on the second shape to the first shape.</param> ///<returns>Whether or not the objects were intersecting. If they are intersecting, then the closest points cannot be identified.</returns> public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB, ref CachedSimplex cachedSimplex, out Vector3 closestPointA, out Vector3 closestPointB) { RigidTransform localtransformB; MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB); bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref cachedSimplex, out closestPointA, out closestPointB); RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA); RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB); return toReturn; }
///<summary> /// Gets the point contributions within a convex shape. ///</summary> ///<param name="shape">Shape to compute the point contributions of.</param> ///<param name="volume">Volume of the shape.</param> ///<param name="outputPointContributions">Point contributions of the shape.</param> public static void GetPoints(ConvexShape shape, out float volume, RawList<Vector3> outputPointContributions) { RigidTransform transform = RigidTransform.Identity; BoundingBox boundingBox; shape.GetBoundingBox(ref transform, out boundingBox); //Find the direction which maximizes the possible hits. Generally, this is the smallest area axis. //Possible options are: //YZ -> use X //XZ -> use Y //XY -> use Z Ray ray; float width = boundingBox.Max.X - boundingBox.Min.X; float height = boundingBox.Max.Y - boundingBox.Min.Y; float length = boundingBox.Max.Z - boundingBox.Min.Z; float yzArea = height * length; float xzArea = width * length; float xyArea = width * height; Vector3 increment1, increment2; float incrementMultiplier = 1f / NumberOfSamplesPerDimension; float maxLength; float rayIncrement; if (yzArea > xzArea && yzArea > xyArea) { //use the x axis as the direction. ray.Direction = Vector3.Right; ray.Position = new Vector3(boundingBox.Min.X, boundingBox.Min.Y + .5f * incrementMultiplier * height, boundingBox.Min.Z + .5f * incrementMultiplier * length); increment1 = new Vector3(0, incrementMultiplier * height, 0); increment2 = new Vector3(0, 0, incrementMultiplier * length); rayIncrement = incrementMultiplier * width; maxLength = width; } else if (xzArea > xyArea) //yz is not the max, given by the previous if. Is xz or xy the max? { //use the y axis as the direction. ray.Direction = Vector3.Up; ray.Position = new Vector3(boundingBox.Min.X + .5f * incrementMultiplier * width, boundingBox.Min.Y, boundingBox.Min.Z + .5f * incrementMultiplier * length); increment1 = new Vector3(incrementMultiplier * width, 0, 0); increment2 = new Vector3(0, 0, incrementMultiplier * height); rayIncrement = incrementMultiplier * height; maxLength = height; } else { //use the z axis as the direction. ray.Direction = Vector3.Backward; ray.Position = new Vector3(boundingBox.Min.X + .5f * incrementMultiplier * width, boundingBox.Min.Y + .5f * incrementMultiplier * height, boundingBox.Min.Z); increment1 = new Vector3(incrementMultiplier * width, 0, 0); increment2 = new Vector3(0, incrementMultiplier * height, 0); rayIncrement = incrementMultiplier * length; maxLength = length; } Ray oppositeRay; volume = 0; for (int i = 0; i < NumberOfSamplesPerDimension; i++) { for (int j = 0; j < NumberOfSamplesPerDimension; j++) { //Ray cast from one direction. If it succeeds, try the other way. This forms an interval in which inertia tensor contributions are contained. RayHit hit; if (shape.RayTest(ref ray, ref transform, maxLength, out hit)) { Vector3.Multiply(ref ray.Direction, maxLength, out oppositeRay.Position); Vector3.Add(ref oppositeRay.Position, ref ray.Position, out oppositeRay.Position); Vector3.Negate(ref ray.Direction, out oppositeRay.Direction); RayHit oppositeHit; if (shape.RayTest(ref oppositeRay, ref transform, maxLength, out oppositeHit)) { //It should always get here if one direction casts, but there may be numerical issues. float scanVolume; ScanObject(rayIncrement, maxLength, ref increment1, ref increment2, ref ray, ref hit, ref oppositeHit, outputPointContributions, out scanVolume); volume += scanVolume; } } Vector3.Add(ref ray.Position, ref increment2, out ray.Position); } Vector3.Add(ref ray.Position, ref increment1, out ray.Position); //Move the ray back to the starting position along the other axis. Vector3 subtract; Vector3.Multiply(ref increment2, NumberOfSamplesPerDimension, out subtract); Vector3.Subtract(ref ray.Position, ref subtract, out ray.Position); } }
//TODO: Consider changing the termination epsilons on these casts. Epsilon * Modifier is okay, but there might be better options. ///<summary> /// Tests a ray against a convex shape. ///</summary> ///<param name="ray">Ray to test against the shape.</param> ///<param name="shape">Shape to test.</param> ///<param name="shapeTransform">Transform to apply to the shape for the test.</param> ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param> ///<param name="hit">Hit data of the ray cast, if any.</param> ///<returns>Whether or not the ray hit the shape.</returns> public static bool RayCast(Ray ray, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength, out RayHit hit) { //Transform the ray into the object's local space. Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position); Quaternion conjugate; Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate); Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position); Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction); Vector3 extremePointToRayOrigin, extremePoint; hit.T = 0; hit.Location = ray.Position; hit.Normal = Toolbox.ZeroVector; Vector3 closestOffset = hit.Location; RaySimplex simplex = new RaySimplex(); float vw, closestPointDotDirection; int count = 0; //This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident. while (closestOffset.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position)) { if (++count > MaximumGJKIterations) { //It's taken too long to find a hit. Numerical problems are probable; quit. hit = new RayHit(); return false; } shape.GetLocalExtremePoint(closestOffset, out extremePoint); Vector3.Subtract(ref hit.Location, ref extremePoint, out extremePointToRayOrigin); Vector3.Dot(ref closestOffset, ref extremePointToRayOrigin, out vw); //If the closest offset and the extreme point->ray origin direction point the same way, //then we might be able to conservatively advance the point towards the surface. if (vw > 0) { Vector3.Dot(ref closestOffset, ref ray.Direction, out closestPointDotDirection); if (closestPointDotDirection >= 0) { hit = new RayHit(); return false; } hit.T = hit.T - vw / closestPointDotDirection; if (hit.T > maximumLength) { //If we've gone beyond where the ray can reach, there's obviously no hit. hit = new RayHit(); return false; } //Shift the ray up. Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); hit.Normal = closestOffset; } RaySimplex shiftedSimplex; simplex.AddNewSimplexPoint(ref extremePoint, ref hit.Location, out shiftedSimplex); //Compute the offset from the simplex surface to the origin. shiftedSimplex.GetPointClosestToOrigin(ref simplex, out closestOffset); } //Transform the hit data into world space. Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal); Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location); Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location); return true; }
///<summary> /// Computes the center of a convex shape. ///</summary> ///<param name="shape">Shape to compute the center of.</param> ///<returns>Center of the shape.</returns> public static Vector3 ComputeCenter(ConvexShape shape) { float volume; return ComputeCenter(shape, out volume); }
///<summary> /// Sweeps two shapes against another. ///</summary> ///<param name="shapeA">First shape being swept.</param> ///<param name="shapeB">Second shape being swept.</param> ///<param name="sweepA">Sweep vector for the first shape.</param> ///<param name="sweepB">Sweep vector for the second shape.</param> ///<param name="transformA">Transform to apply to the first shape.</param> ///<param name="transformB">Transform to apply to the second shape.</param> ///<param name="hit">Hit data of the sweep test, if any.</param> ///<returns>Whether or not the swept shapes hit each other..</returns> public static bool ConvexCast(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 sweepA, ref Vector3 sweepB, ref RigidTransform transformA, ref RigidTransform transformB, out RayHit hit) { //Put the velocity into shapeA's local space. Vector3 velocityWorld; Vector3.Subtract(ref sweepB, ref sweepA, out velocityWorld); Quaternion conjugateOrientationA; Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA); Vector3 rayDirection; Quaternion.Transform(ref velocityWorld, ref conjugateOrientationA, out rayDirection); //Transform b into a's local space. RigidTransform localTransformB; Quaternion.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation); Vector3.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position); Quaternion.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position); Vector3 w, p; hit.T = 0; hit.Location = Vector3.Zero; //The ray starts at the origin. hit.Normal = Toolbox.ZeroVector; Vector3 v = hit.Location; RaySimplex simplex = new RaySimplex(); float vw, vdir; int count = 0; do { if (++count > MaximumGJKIterations) { //It's taken too long to find a hit. Numerical problems are probable; quit. hit = new RayHit(); return false; } MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref v, ref localTransformB, out p); Vector3.Subtract(ref hit.Location, ref p, out w); Vector3.Dot(ref v, ref w, out vw); if (vw > 0) { Vector3.Dot(ref v, ref rayDirection, out vdir); if (vdir >= 0) { hit = new RayHit(); return false; } hit.T = hit.T - vw / vdir; if (hit.T > 1) { //If we've gone beyond where the ray can reach, there's obviously no hit. hit = new RayHit(); return false; } //Shift the ray up. Vector3.Multiply(ref rayDirection, hit.T, out hit.Location); //The ray origin is the origin! Don't need to add any ray position. hit.Normal = v; } RaySimplex shiftedSimplex; simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex); shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v); //Could measure the progress of the ray. If it's too little, could early out. //Not used by default since it's biased towards precision over performance. } while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref Toolbox.ZeroVector)); //This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident. //Transform the hit data into world space. Quaternion.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal); Vector3.Multiply(ref velocityWorld, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref transformA.Position, out hit.Location); return true; }
///<summary> /// Constructs a new entry. ///</summary> ///<param name="orientation">Orientation of the entry.</param> ///<param name="shape">Shape of the entry.</param> public OrientedConvexShapeEntry(System.Numerics.Quaternion orientation, ConvexShape shape) { Orientation = orientation; CollisionShape = shape; }
///<summary> /// Casts a fat (sphere expanded) ray against the shape. ///</summary> ///<param name="ray">Ray to test against the shape.</param> ///<param name="radius">Radius of the ray.</param> ///<param name="shape">Shape to test against.</param> ///<param name="shapeTransform">Transform to apply to the shape for the test.</param> ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param> ///<param name="hit">Hit data of the sphere cast, if any.</param> ///<returns>Whether or not the sphere cast hit the shape.</returns> public static bool SphereCast(Ray ray, float radius, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength, out RayHit hit) { //Transform the ray into the object's local space. Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position); Quaternion conjugate; Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate); Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position); Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction); Vector3 w, p; hit.T = 0; hit.Location = ray.Position; hit.Normal = Toolbox.ZeroVector; Vector3 v = hit.Location; RaySimplex simplex = new RaySimplex(); float vw, vdir; int count = 0; //This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident. while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position)) { if (++count > MaximumGJKIterations) { //It's taken too long to find a hit. Numerical problems are probable; quit. hit = new RayHit(); return false; } shape.GetLocalExtremePointWithoutMargin(ref v, out p); Vector3 contribution; MinkowskiToolbox.ExpandMinkowskiSum(shape.collisionMargin, radius, ref v, out contribution); Vector3.Add(ref p, ref contribution, out p); Vector3.Subtract(ref hit.Location, ref p, out w); Vector3.Dot(ref v, ref w, out vw); if (vw > 0) { Vector3.Dot(ref v, ref ray.Direction, out vdir); hit.T = hit.T - vw / vdir; if (vdir >= 0) { //We would have to back up! return false; } if (hit.T > maximumLength) { //If we've gone beyond where the ray can reach, there's obviously no hit. return false; } //Shift the ray up. Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); hit.Normal = v; } RaySimplex shiftedSimplex; simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex); shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v); } //Transform the hit data into world space. Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal); Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location); Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location); return true; }
/// <summary> /// Casts a convex shape against the collidable. /// </summary> /// <param name="castShape">Shape to cast.</param> /// <param name="startingTransform">Initial transform of the shape.</param> /// <param name="sweep">Sweep to apply to the shape.</param> /// <param name="hit">Hit data, if any.</param> /// <returns>Whether or not the cast hit anything.</returns> public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit) { if (Shape.solidity == MobileMeshSolidity.Solid) { //If the convex cast is inside the mesh and the mesh is solid, it should return t = 0. var ray = new Ray() { Position = startingTransform.Position, Direction = Toolbox.UpVector }; if (Shape.IsLocalRayOriginInMesh(ref ray, out hit)) { hit = new RayHit() { Location = startingTransform.Position, Normal = new Vector3(), T = 0 }; return true; } } hit = new RayHit(); BoundingBox boundingBox; var transform = new AffineTransform {Translation = worldTransform.Position}; Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform); castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox); var tri = PhysicsResources.GetTriangle(); var hitElements = CommonResources.GetIntList(); if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC); AffineTransform.Transform(ref tri.vA, ref transform, out tri.vA); AffineTransform.Transform(ref tri.vB, ref transform, out tri.vB); AffineTransform.Transform(ref tri.vC, ref transform, out tri.vC); Vector3 center; Vector3.Add(ref tri.vA, ref tri.vB, out center); Vector3.Add(ref center, ref tri.vC, out center); Vector3.Multiply(ref center, 1f / 3f, out center); Vector3.Subtract(ref tri.vA, ref center, out tri.vA); Vector3.Subtract(ref tri.vB, ref center, out tri.vB); Vector3.Subtract(ref tri.vC, ref center, out tri.vC); tri.maximumRadius = tri.vA.LengthSquared(); float radius = tri.vB.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; radius = tri.vC.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; tri.maximumRadius = (float)Math.Sqrt(tri.maximumRadius); tri.collisionMargin = 0; var triangleTransform = new RigidTransform {Orientation = Quaternion.Identity, Position = center}; RayHit tempHit; if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T) { hit = tempHit; } } tri.maximumRadius = 0; PhysicsResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return hit.T != float.MaxValue; } PhysicsResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return false; }
///<summary> /// Casts a fat (sphere expanded) ray against the shape. If the raycast appears to be stuck in the shape, the cast will be attempted /// with a smaller ray (scaled by the MotionSettings.CoreShapeScaling each time). ///</summary> ///<param name="ray">Ray to test against the shape.</param> ///<param name="radius">Radius of the ray.</param> ///<param name="target">Shape to test against.</param> ///<param name="shapeTransform">Transform to apply to the shape for the test.</param> ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param> ///<param name="hit">Hit data of the sphere cast, if any.</param> ///<returns>Whether or not the sphere cast hit the shape.</returns> public static bool CCDSphereCast( ref Ray ray, float radius, ConvexShape target, ref RigidTransform shapeTransform, float maximumLength, out RayHit hit ) { int iterations = 0; while( true ) { if( GJKToolbox.SphereCast( ray, radius, target, ref shapeTransform, maximumLength, out hit ) && hit.T > 0 ) { //The ray cast isn't embedded in the shape, and it's less than maximum length away! return true; } if( hit.T > maximumLength || hit.T < 0 ) return false; //Failure showed it was too far, or behind. radius *= MotionSettings.CoreShapeScaling; iterations++; if( iterations > 3 ) //Limit could be configurable. { //It's iterated too much, let's just do a last ditch attempt using a raycast and hope that can help. return GJKToolbox.RayCast( ray, target, ref shapeTransform, maximumLength, out hit ) && hit.T > 0; } } }
///<summary> /// Initializes the pair tester. ///</summary> ///<param name="convex">Convex shape to use.</param> ///<param name="triangle">Triangle shape to use.</param> public override void Initialize(ConvexShape convex, TriangleShape triangle) { this.convex = convex; this.triangle = triangle; }