///<summary> /// Gets the local transform of B in the space of A. ///</summary> ///<param name="transformA">First transform.</param> ///<param name="transformB">Second transform.</param> ///<param name="localTransformB">Transform of B in the local space of A.</param> public static void GetLocalTransform(ref RigidTransform transformA, ref RigidTransform transformB, out RigidTransform localTransformB) { //Put B into A's space. Quaternion conjugateOrientationA; Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA); Quaternion.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation); Vector3.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position); Vector3.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position); }
///<summary> /// Gets the extreme point of the shape in world space in a given direction. ///</summary> ///<param name="direction">Direction to find the extreme point in.</param> /// <param name="shapeTransform">Transform to use for the shape.</param> ///<param name="extremePoint">Extreme point on the shape.</param> public void GetExtremePointWithoutMargin(Vector3 direction, ref RigidTransform shapeTransform, out Vector3 extremePoint) { Quaternion conjugate; Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate); Vector3.Transform(ref direction, ref conjugate, out direction); GetLocalExtremePointWithoutMargin(ref direction, out extremePoint); Vector3.Transform(ref extremePoint, ref shapeTransform.Orientation, out extremePoint); Vector3.Add(ref extremePoint, ref shapeTransform.Position, out extremePoint); }
///<summary> /// Gets the extreme point of the shape in world space in a given direction with margin expansion. ///</summary> ///<param name="direction">Direction to find the extreme point in.</param> /// <param name="shapeTransform">Transform to use for the shape.</param> ///<param name="extremePoint">Extreme point on the shape.</param> public void GetExtremePoint(Vector3 direction, ref RigidTransform shapeTransform, out Vector3 extremePoint) { GetExtremePointWithoutMargin(direction, ref shapeTransform, out extremePoint); float directionLength = direction.LengthSquared(); if (directionLength > Toolbox.Epsilon) { Vector3.Multiply(ref direction, collisionMargin / (float)Math.Sqrt(directionLength), out direction); Vector3.Add(ref extremePoint, ref direction, out extremePoint); } }
/// <summary> /// Gets the bounding box of the shape given a transform. /// </summary> /// <param name="shapeTransform">Transform to use.</param> /// <param name="boundingBox">Bounding box of the transformed shape.</param> public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif boundingBox.Min.X = shapeTransform.Position.X - collisionMargin; boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin; boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin; boundingBox.Max.X = shapeTransform.Position.X + collisionMargin; boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin; boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin; }
public override void UpdateCollision(float dt) { WasContaining = Containing; WasTouching = Touching; var transform = new RigidTransform { Orientation = Quaternion.Identity }; DetectorVolume.TriangleMesh.Tree.GetOverlaps(convex.boundingBox, overlaps); for (int i = 0; i < overlaps.count; i++) { DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[i], out triangle.vA, out triangle.vB, out triangle.vC); Vector3.Add(ref triangle.vA, ref triangle.vB, out transform.Position); Vector3.Add(ref triangle.vC, ref transform.Position, out transform.Position); Vector3.Multiply(ref transform.Position, 1 / 3f, out transform.Position); Vector3.Subtract(ref triangle.vA, ref transform.Position, out triangle.vA); Vector3.Subtract(ref triangle.vB, ref transform.Position, out triangle.vB); Vector3.Subtract(ref triangle.vC, ref transform.Position, out triangle.vC); //If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.))) //[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker when objects are overlapping than GJK. The GJK implementation does better on separated objects.] if (MPRToolbox.AreShapesOverlapping(convex.Shape, triangle, ref convex.worldTransform, ref transform)) { Touching = true; //The convex can't be fully contained if it's still touching the surface. Containing = false; overlaps.Clear(); goto events; } } overlaps.Clear(); //If we get here, then there was no shell intersection. //If the convex's center point is contained by the mesh, then the convex is fully contained. //If this is a child pair, the CheckContainment flag may be set to false. This is because the parent has //already determined that it is not contained (another child performed the check and found that it was not contained) //and that it is already touching somehow (either by intersection or by containment). //so further containment tests are unnecessary. if (CheckContainment && DetectorVolume.IsPointContained(ref convex.worldTransform.Position, overlaps)) { Touching = true; Containing = true; goto events; } //If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate! Touching = false; Containing = false; events: NotifyDetectorVolumeOfChanges(); }
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif Vector3 upExtreme = new Vector3(0, halfLength, 0); Vector3 downExtreme = new Vector3(0, -halfLength, 0); Vector3.Transform(ref upExtreme, ref shapeTransform.Orientation, out upExtreme); Vector3.Transform(ref downExtreme, ref shapeTransform.Orientation, out downExtreme); if (upExtreme.X > downExtreme.X) { boundingBox.Max.X = upExtreme.X; boundingBox.Min.X = downExtreme.X; } else { boundingBox.Max.X = downExtreme.X; boundingBox.Min.X = upExtreme.X; } if (upExtreme.Y > downExtreme.Y) { boundingBox.Max.Y = upExtreme.Y; boundingBox.Min.Y = downExtreme.Y; } else { boundingBox.Max.Y = downExtreme.Y; boundingBox.Min.Y = upExtreme.Y; } if (upExtreme.Z > downExtreme.Z) { boundingBox.Max.Z = upExtreme.Z; boundingBox.Min.Z = downExtreme.Z; } else { boundingBox.Max.Z = downExtreme.Z; boundingBox.Min.Z = upExtreme.Z; } boundingBox.Min.X += shapeTransform.Position.X - collisionMargin; boundingBox.Min.Y += shapeTransform.Position.Y - collisionMargin; boundingBox.Min.Z += shapeTransform.Position.Z - collisionMargin; boundingBox.Max.X += shapeTransform.Position.X + collisionMargin; boundingBox.Max.Y += shapeTransform.Position.Y + collisionMargin; boundingBox.Max.Z += shapeTransform.Position.Z + collisionMargin; }
///<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="extremePointA">The extreme point on shapeA.</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 extremePointA, 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 extremePointA); Vector3 v; Vector3.Negate(ref direction, out v); Vector3 extremePointB; shapeB.GetExtremePointWithoutMargin(v, ref localTransformB, out extremePointB); ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, direction, ref extremePointA, ref extremePointB); Vector3.Subtract(ref extremePointA, ref extremePointB, out extremePoint); }
internal void Enable() { //Turn everything on. lock (FlipLocker) { int initialCount = Math.Max(manager.entities.Count, 64); backBuffer = new RigidTransform[initialCount]; states = new RigidTransform[initialCount]; for (int i = 0; i < manager.entities.Count; i++) { Entity entity = manager.entities[i]; backBuffer[i].Position = entity.position; backBuffer[i].Orientation = entity.orientation; } Array.Copy(backBuffer, states, backBuffer.Length); } }
///<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(0, 0, 0), a.Orientation); bTransform = new RigidTransform(new Vector3(0, 10, 0), b.Orientation); game.Camera.Position = new Vector3(0, 5, 17); }
/// <summary> /// Gets the bounding box of the shape given a transform. /// </summary> /// <param name="shapeTransform">Transform to use.</param> /// <param name="boundingBox">Bounding box of the transformed shape.</param> public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif Matrix3X3 o; Matrix3X3.CreateFromQuaternion(ref shapeTransform.Orientation, out o); //Sample the local directions from the orientation matrix, implicitly transposed. //Notice only three directions are used. Due to box symmetry, 'left' is just -right. Vector3 direction = new Vector3(o.M11, o.M21, o.M31); Vector3 right; GetLocalExtremePointWithoutMargin(ref direction, out right); direction = new Vector3(o.M12, o.M22, o.M32); Vector3 up; GetLocalExtremePointWithoutMargin(ref direction, out up); direction = new Vector3(o.M13, o.M23, o.M33); Vector3 backward; GetLocalExtremePointWithoutMargin(ref direction, out backward); Matrix3X3.Transform(ref right, ref o, out right); Matrix3X3.Transform(ref up, ref o, out up); Matrix3X3.Transform(ref backward, ref o, out backward); //These right/up/backward represent the extreme points in world space along the world space axes. boundingBox.Max.X = shapeTransform.Position.X + collisionMargin + right.X; boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin + up.Y; boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin + backward.Z; boundingBox.Min.X = shapeTransform.Position.X - collisionMargin - right.X; boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin - up.Y; boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin - backward.Z; }
///<summary> /// Transforms a rigid transform by another rigid transform. ///</summary> ///<param name="a">The first, "local" rigid transform.</param> ///<param name="b">The second, "world" rigid transform.</param> ///<param name="combined">Combined rigid transform.</param> public static void Transform(ref RigidTransform a, ref RigidTransform b, out RigidTransform combined) { Vector3 intermediate; Vector3.Transform(ref a.Position, ref b.Orientation, out intermediate); Vector3.Add(ref intermediate, ref b.Position, out combined.Position); Quaternion.Concatenate(ref a.Orientation, ref b.Orientation, out combined.Orientation); }
/// <summary> /// Gets the bounding box of the shape given a transform. /// </summary> /// <param name="shapeTransform">Transform to use.</param> /// <param name="boundingBox">Bounding box of the transformed shape.</param> public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif Matrix3X3 o; Matrix3X3.CreateFromQuaternion(ref shapeTransform.Orientation, out o); float minX, maxX; float minY, maxY; float minZ, maxZ; Vector3 right = new Vector3(o.M11, o.M21, o.M31); Vector3 up = new Vector3(o.M12, o.M22, o.M32); Vector3 backward = new Vector3(o.M13, o.M23, o.M33); Vector3.Dot(ref vertices.Elements[0], ref right, out maxX); minX = maxX; Vector3.Dot(ref vertices.Elements[0], ref up, out maxY); minY = maxY; Vector3.Dot(ref vertices.Elements[0], ref backward, out maxZ); minZ = maxZ; int minXIndex = 0; int maxXIndex = 0; int minYIndex = 0; int maxYIndex = 0; int minZIndex = 0; int maxZIndex = 0; for (int i = 1; i < vertices.count; i++) { float dot; Vector3.Dot(ref vertices.Elements[i], ref right, out dot); if (dot < minX) { minX = dot; minXIndex = i; } else if (dot > maxX) { maxX = dot; maxXIndex = i; } Vector3.Dot(ref vertices.Elements[i], ref up, out dot); if (dot < minY) { minY = dot; minYIndex = i; } else if (dot > maxY) { maxY = dot; maxYIndex = i; } Vector3.Dot(ref vertices.Elements[i], ref backward, out dot); if (dot < minZ) { minZ = dot; minZIndex = i; } else if (dot > maxZ) { maxZ = dot; maxZIndex = i; } } Vector3 minXpoint, maxXpoint, minYpoint, maxYpoint, minZpoint, maxZpoint; Matrix3X3.Transform(ref vertices.Elements[minXIndex], ref o, out minXpoint); Matrix3X3.Transform(ref vertices.Elements[maxXIndex], ref o, out maxXpoint); Matrix3X3.Transform(ref vertices.Elements[minYIndex], ref o, out minYpoint); Matrix3X3.Transform(ref vertices.Elements[maxYIndex], ref o, out maxYpoint); Matrix3X3.Transform(ref vertices.Elements[minZIndex], ref o, out minZpoint); Matrix3X3.Transform(ref vertices.Elements[maxZIndex], ref o, out maxZpoint); boundingBox.Max.X = shapeTransform.Position.X + collisionMargin + maxXpoint.X; boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin + maxYpoint.Y; boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin + maxZpoint.Z; boundingBox.Min.X = shapeTransform.Position.X - collisionMargin + minXpoint.X; boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin + minYpoint.Y; boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin + minZpoint.Z; }
/// <summary> /// Updates the collidable's world transform and bounding box. /// This is a convenience method for external modification of the collidable's data. /// </summary> /// <param name="transform">Transform to use for the collidable.</param> public void UpdateBoundingBoxForTransform(ref RigidTransform transform) { UpdateBoundingBoxForTransform(ref transform, 0); }
/// <summary> /// Updates the collidable's world transform and bounding box. The transform provided /// will be offset by the collidable's LocalPosition to get the shape transform. /// This is a convenience method for external modification of the collidable's data. /// </summary> /// <param name="transform">Transform to use for the collidable.</param> /// <param name="dt">Duration of the simulation time step. Used to expand the /// bounding box using the owning entity's velocity. If the collidable /// does not have an owning entity, this must be zero.</param> public void UpdateBoundingBoxForTransform(ref RigidTransform transform, float dt) { UpdateWorldTransform(ref transform.Position, ref transform.Orientation); UpdateBoundingBoxInternal(dt); }
public static void Validate(this RigidTransform r) { r.Position.Validate(); r.Orientation.Validate(); }
/// <summary> /// Gets the intersection between the triangle and the ray. /// </summary> /// <param name="ray">Ray to test against the triangle.</param> /// <param name="transform">Transform to apply to the triangle shape for the test.</param> /// <param name="maximumLength">Maximum distance to travel in units of the direction vector's length.</param> /// <param name="hit">Hit data of the ray cast, if any.</param> /// <returns>Whether or not the ray hit the target.</returns> public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit) { Matrix3X3 orientation; Matrix3X3.CreateFromQuaternion(ref transform.Orientation, out orientation); Ray localRay; Quaternion conjugate; Quaternion.Conjugate(ref transform.Orientation, out conjugate); Vector3.Transform(ref ray.Direction, ref conjugate, out localRay.Direction); Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position); Vector3.Transform(ref localRay.Position, ref conjugate, out localRay.Position); bool toReturn = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref vA, ref vB, ref vC, out hit); //Move the hit back into world space. Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location); Vector3.Add(ref ray.Position, ref hit.Location, out hit.Location); Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); return toReturn; }
/// <summary> /// Gets the bounding box of the shape given a transform. /// </summary> /// <param name="shapeTransform">Transform to use.</param> /// <param name="boundingBox">Bounding box of the transformed shape.</param> public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { Vector3 a, b, c; Vector3.Transform(ref vA, ref shapeTransform.Orientation, out a); Vector3.Transform(ref vB, ref shapeTransform.Orientation, out b); Vector3.Transform(ref vC, ref shapeTransform.Orientation, out c); Vector3.Min(ref a, ref b, out boundingBox.Min); Vector3.Min(ref c, ref boundingBox.Min, out boundingBox.Min); Vector3.Max(ref a, ref b, out boundingBox.Max); Vector3.Max(ref c, ref boundingBox.Max, out boundingBox.Max); boundingBox.Min.X += shapeTransform.Position.X - collisionMargin; boundingBox.Min.Y += shapeTransform.Position.Y - collisionMargin; boundingBox.Min.Z += shapeTransform.Position.Z - collisionMargin; boundingBox.Max.X += shapeTransform.Position.X + collisionMargin; boundingBox.Max.Y += shapeTransform.Position.Y + collisionMargin; boundingBox.Max.Z += shapeTransform.Position.Z + collisionMargin; }
///<summary> /// Transforms a position by a rigid transform's inverse. ///</summary> ///<param name="position">Position to transform.</param> ///<param name="transform">Transform to invert and apply.</param> ///<param name="result">Transformed position.</param> public static void TransformByInverse(ref Vector3 position, ref RigidTransform transform, out Vector3 result) { Quaternion orientation; Vector3 intermediate; Vector3.Subtract(ref position, ref transform.Position, out intermediate); Quaternion.Conjugate(ref transform.Orientation, out orientation); Vector3.Transform(ref intermediate, ref orientation, out result); }
/// <summary> /// Gets the intersection between the sphere and the ray. /// </summary> /// <param name="ray">Ray to test against the sphere.</param> /// <param name="transform">Transform applied to the convex for the test.</param> /// <param name="maximumLength">Maximum distance to travel in units of the ray direction's length.</param> /// <param name="hit">Ray hit data, if any.</param> /// <returns>Whether or not the ray hit the target.</returns> public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit) { return Toolbox.RayCastSphere(ref ray, ref transform.Position, collisionMargin, maximumLength, out hit); //Vector3 normalizedDirection; //float length = ray.Direction.Length(); //Vector3.Divide(ref ray.Direction, length, out normalizedDirection); //maximumLength *= length; //hit = new RayHit(); //Vector3 m; //Vector3.Subtract(ref ray.Position, ref transform.Position, out m); //float b = Vector3.Dot(m, normalizedDirection); //float c = m.LengthSquared() - collisionMargin * collisionMargin; //if (c > 0 && b > 0) // return false; //float discriminant = b * b - c; //if (discriminant < 0) // return false; //hit.T = -b - (float)Math.Sqrt(discriminant); //if (hit.T < 0) // hit.T = 0; //if (hit.T > maximumLength) // return false; //Vector3.Multiply(ref normalizedDirection, hit.T, out hit.Location); //Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); //Vector3.Subtract(ref hit.Location, ref transform.Position, out hit.Normal); //hit.Normal.Normalize(); //return true; }
///<summary> /// Transforms a rigid transform by another rigid transform's inverse. ///</summary> ///<param name="a">The first rigid transform.</param> ///<param name="b">The second rigid transform, to be inverted.</param> ///<param name="combinedTransform">Combined rigid transform.</param> public static void TransformByInverse(ref RigidTransform a, ref RigidTransform b, out RigidTransform combinedTransform) { Invert(ref b, out combinedTransform); Transform(ref a, ref combinedTransform, out combinedTransform); }
///<summary> /// Transforms a position by a rigid transform. ///</summary> ///<param name="position">Position to transform.</param> ///<param name="transform">Transform to apply.</param> ///<param name="result">Transformed position.</param> public static void Transform(ref Vector3 position, ref RigidTransform transform, out Vector3 result) { Vector3 intermediate; Vector3.Transform(ref position, ref transform.Orientation, out intermediate); Vector3.Add(ref intermediate, ref transform.Position, out result); }
/// <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(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit) { hit = new RayHit(); BoundingBox boundingBox; Toolbox.GetExpandedBoundingBox(ref castShape, ref startingTransform, ref sweep, out boundingBox); var hitElements = Resources.GetCompoundChildList(); if (hierarchy.Tree.GetOverlaps(boundingBox, hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.count; i++) { var candidate = hitElements.Elements[i].CollisionInformation; RayHit tempHit; if (candidate.ConvexCast(castShape, ref startingTransform, ref sweep, out tempHit) && tempHit.T < hit.T) { hit = tempHit; } } Resources.GiveBack(hitElements); return hit.T != float.MaxValue; } Resources.GiveBack(hitElements); return false; }
/// <summary> /// Inverts a rigid transform. /// </summary> /// <param name="transform">Transform to invert.</param> /// <param name="inverse">Inverse of the transform.</param> public static void Invert(ref RigidTransform transform, out RigidTransform inverse) { Quaternion.Conjugate(ref transform.Orientation, out inverse.Orientation); Vector3.Transform(ref transform.Position, ref inverse.Orientation, out inverse.Position); Vector3.Negate(ref inverse.Position, out inverse.Position); }
public override bool ConvexCast(ConvexShape castShape, ref MathExtensions.RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit) { hit = new RayHit(); BoundingBox boundingBox; Toolbox.GetExpandedBoundingBox(ref castShape, ref startingTransform, ref sweep, out boundingBox); var tri = Resources.GetTriangle(); var hitElements = Resources.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; Resources.GiveBack(tri); Resources.GiveBack(hitElements); return hit.T != float.MaxValue; } Resources.GiveBack(tri); Resources.GiveBack(hitElements); return false; }
/// <summary> /// Gets the normal of the triangle in world space. /// </summary> /// <param name="transform">World transform.</param> /// <returns>Normal of the triangle in world space.</returns> public Vector3 GetNormal(RigidTransform transform) { Vector3 normal = GetLocalNormal(); Vector3.Transform(ref normal, ref transform.Orientation, out normal); return normal; }
///<summary> /// Computes the bounding box of the transformed mesh shape. ///</summary> ///<param name="shapeTransform">Transform to apply to the shape during the bounding box calculation.</param> ///<param name="boundingBox">Bounding box containing the transformed mesh shape.</param> public void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { ////TODO: Could use an approximate bounding volume. Would be cheaper at runtime and use less memory, though the box would be bigger. //Matrix3X3 o; //Matrix3X3.CreateFromQuaternion(ref shapeTransform.Orientation, out o); ////Sample the local directions from the orientation matrix, implicitly transposed. //Vector3 right = new Vector3(o.M11 * 100000, o.M21 * 100000, o.M31 * 100000); //Vector3 up = new Vector3(o.M12 * 100000, o.M22 * 100000, o.M32 * 100000); //Vector3 backward = new Vector3(o.M13 * 100000, o.M23 * 100000, o.M33 * 100000); //Vector3 left, down, forward; //Vector3.Negate(ref right, out left); //Vector3.Negate(ref up, out down); //Vector3.Negate(ref backward, out forward); //for (int i = 0; i < extents.count; i++) //{ // extents.Elements[i].Clamp(ref right); // extents.Elements[i].Clamp(ref left); // extents.Elements[i].Clamp(ref up); // extents.Elements[i].Clamp(ref down); // extents.Elements[i].Clamp(ref backward); // extents.Elements[i].Clamp(ref forward); //} //Matrix3X3.Transform(ref right, ref o, out right); //Matrix3X3.Transform(ref left, ref o, out left); //Matrix3X3.Transform(ref down, ref o, out down); //Matrix3X3.Transform(ref up, ref o, out up); //Matrix3X3.Transform(ref forward, ref o, out forward); //Matrix3X3.Transform(ref backward, ref o, out backward); //boundingBox.Max.X = shapeTransform.Position.X + right.X; //boundingBox.Max.Y = shapeTransform.Position.Y + up.Y; //boundingBox.Max.Z = shapeTransform.Position.Z + backward.Z; //boundingBox.Min.X = shapeTransform.Position.X + left.X; //boundingBox.Min.Y = shapeTransform.Position.Y + down.Y; //boundingBox.Min.Z = shapeTransform.Position.Z + forward.Z; Matrix3X3 o; Matrix3X3.CreateFromQuaternion(ref shapeTransform.Orientation, out o); GetBoundingBox(ref o, out boundingBox); boundingBox.Max.X += shapeTransform.Position.X; boundingBox.Max.Y += shapeTransform.Position.Y; boundingBox.Max.Z += shapeTransform.Position.Z; boundingBox.Min.X += shapeTransform.Position.X; boundingBox.Min.Y += shapeTransform.Position.Y; boundingBox.Min.Z += shapeTransform.Position.Z; }
/// <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(CollisionShapes.ConvexShapes.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; AffineTransform transform = new AffineTransform(); transform.Translation = worldTransform.Position; Matrix3X3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform); castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox); var tri = Resources.GetTriangle(); var hitElements = Resources.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(); triangleTransform.Orientation = Quaternion.Identity; triangleTransform.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; Resources.GiveBack(tri); Resources.GiveBack(hitElements); return hit.T != float.MaxValue; } Resources.GiveBack(tri); Resources.GiveBack(hitElements); return false; }
/// <summary> /// Gets the bounding box of the mesh transformed first into world space, and then into the local space of another affine transform. /// </summary> /// <param name="shapeTransform">Transform to use to put the shape into world space.</param> /// <param name="spaceTransform">Used as the frame of reference to compute the bounding box. /// In effect, the shape is transformed by the inverse of the space transform to compute its bounding box in local space.</param> /// <param name="boundingBox">Bounding box in the local space.</param> public void GetLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif //TODO: This method peforms quite a few sqrts because the collision margin can get scaled, and so cannot be applied as a final step. //There should be a better way to do this. //Additionally, this bounding box is not consistent in all cases with the post-add version. Adding the collision margin at the end can //slightly overestimate the size of a margin expanded shape at the corners, which is fine (and actually important for the box-box special case). //Move forward into convex's space, backwards into the new space's local space. AffineTransform transform; AffineTransform.Invert(ref spaceTransform, out transform); AffineTransform.Multiply(ref shapeTransform, ref transform, out transform); GetBoundingBox(ref transform.LinearTransform, out boundingBox); boundingBox.Max.X += transform.Translation.X; boundingBox.Max.Y += transform.Translation.Y; boundingBox.Max.Z += transform.Translation.Z; boundingBox.Min.X += transform.Translation.X; boundingBox.Min.Y += transform.Translation.Y; boundingBox.Min.Z += transform.Translation.Z; }
/// <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> /// Gets the bounding box of the mesh transformed first into world space, and then into the local space of another affine transform. /// </summary> /// <param name="shapeTransform">Transform to use to put the shape into world space.</param> /// <param name="spaceTransform">Used as the frame of reference to compute the bounding box. /// In effect, the shape is transformed by the inverse of the space transform to compute its bounding box in local space.</param> /// <param name="sweep">World space sweep direction to transform and add to the bounding box.</param> /// <param name="boundingBox">Bounding box in the local space.</param> public void GetSweptLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, ref Vector3 sweep, out BoundingBox boundingBox) { GetLocalBoundingBox(ref shapeTransform, ref spaceTransform, out boundingBox); Vector3 expansion; Matrix3X3.TransformTranspose(ref sweep, ref spaceTransform.LinearTransform, out expansion); Toolbox.ExpandBoundingBox(ref boundingBox, ref expansion); }