Пример #1
0
 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);
 }
Пример #2
0
        /// <summary>
        /// Computes the intersection, if any, between a ray and the objects in the character's bounding box.
        /// </summary>
        /// <param name="ray">Ray to test.</param>
        /// <param name="length">Length of the ray to use in units of the ray's length.</param>
        /// <param name="earliestHit">Earliest intersection location and information.</param>
        /// <param name="hitObject">Collidable intersected by the ray, if any.</param>
        /// <returns>Whether or not the ray hit anything.</returns>
        public bool RayCast(Ray ray, float length, out RayHit earliestHit, out Collidable hitObject)
        {
            earliestHit = new RayHit();
            earliestHit.T = float.MaxValue;
            hitObject = null;
            foreach (var collidable in characterBody.CollisionInformation.OverlappedCollidables)
            {
                //Check to see if the collidable is hit by the ray.
                float t;
                if (ray.Intersects(ref collidable.boundingBox, out t) && t < length)
                {
                    //Is it an earlier hit than the current earliest?
                    RayHit hit;
                    if (collidable.RayCast(ray, length, SupportRayFilter, out hit) && hit.T < earliestHit.T)
                    {
                        earliestHit = hit;
                        hitObject = collidable;
                    }
                }
            }
            if (earliestHit.T == float.MaxValue)
                return false;
            return true;

        }
Пример #3
0
 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>
 /// Tests a ray against the entry.
 /// </summary>
 /// <param name="ray">Ray to test.</param>
 /// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</param>
 /// <param name="rayHit">Hit location of the ray on the entry, if any.</param>
 /// <returns>Whether or not the ray hit the entry.</returns>
 public abstract bool RayCast(Ray ray, float maximumLength, out RayHit rayHit);
 /// <summary>
 /// Tests a ray against the entry.
 /// </summary>
 /// <param name="ray">Ray to test.</param>
 /// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</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="rayHit">Hit location of the ray on the entry, if any.</param>
 /// <returns>Whether or not the ray hit the entry.</returns>
 public virtual bool RayCast(Ray ray, float maximumLength, Func<BroadPhaseEntry, bool> filter, out RayHit rayHit)
 {
     if (filter(this))
         return RayCast(ray, maximumLength, out rayHit);
     rayHit = new RayHit();
     return false;
 }
Пример #6
0
		///<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;

				}
			}
		}
Пример #7
0
 ///<summary>
 /// Tests a ray against the triangle mesh.
 ///</summary>
 ///<param name="ray">Ray to test against the mesh.</param>
 /// <param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
 /// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
 ///<param name="rayHit">Hit data for the ray, if any.</param>
 ///<returns>Whether or not the ray hit the mesh.</returns>
 public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit)
 {
     var rayHits = CommonResources.GetRayHitList();
     bool toReturn = RayCast(ray, maximumLength, sidedness, rayHits);
     if (toReturn)
     {
         rayHit = rayHits[0];
         for (int i = 1; i < rayHits.Count; i++)
         {
             RayHit hit = rayHits[i];
             if (hit.T < rayHit.T)
                 rayHit = hit;
         }
     }
     else
         rayHit = new RayHit();
     CommonResources.GiveBack(rayHits);
     return toReturn;
 }
Пример #8
0
        private static void ScanObject(float rayIncrement, float maxLength, ref Vector3 increment1, ref Vector3 increment2, ref Ray ray, ref RayHit startHit, ref RayHit endHit, RawList<Vector3> pointContributions, out float volume)
        {
            Vector3 cell;
            Vector3.Multiply(ref ray.Direction, rayIncrement, out cell);
            Vector3.Add(ref increment1, ref cell, out cell);
            Vector3.Add(ref increment2, ref cell, out cell);
            float perCellVolume = cell.X * cell.Y * cell.Z;

            volume = 0;

            for (int i = (int)(startHit.T / rayIncrement); i <= (int)((maxLength - endHit.T) / rayIncrement); i++)
            {
                Vector3 position;
                Vector3.Multiply(ref ray.Direction, (i + .5f) * rayIncrement, out position);
                Vector3.Add(ref position, ref ray.Position, out position);
                pointContributions.Add(position);
                volume += perCellVolume;
            }
        }
Пример #9
0
        ///<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>
        /// Tests a ray against the instance.
        ///</summary>
        ///<param name="ray">Ray to test.</param>
        ///<param name="maximumLength">Maximum length of the ray to test; in units of the ray's direction's length.</param>
        ///<param name="sidedness">Sidedness to use during the ray cast.  This does not have to be the same as the mesh's sidedness.</param>
        ///<param name="rayHit">The hit location of the ray on the mesh, if any.</param>
        ///<returns>Whether or not the ray hit the mesh.</returns>
        public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit)
        {
            //Put the ray into local space.
            Ray localRay;
            AffineTransform inverse;
            AffineTransform.Invert(ref worldTransform, out inverse);
            Matrix3x3.Transform(ref ray.Direction, ref inverse.LinearTransform, out localRay.Direction);
            AffineTransform.Transform(ref ray.Position, ref inverse, out localRay.Position);

            if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
            {
                //Transform the hit into world space.
                Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
                Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
                Matrix3x3.TransformTranspose(ref rayHit.Normal, ref inverse.LinearTransform, out rayHit.Normal);
                return true;
            }
            rayHit = new RayHit();
            return false;
        }
Пример #11
0
        /// <summary>
        /// Gets the intersection between the box and the ray.
        /// </summary>
        /// <param name="ray">Ray to test against the box.</param>
        /// <param name="transform">Transform of the shape.</param>
        /// <param name="maximumLength">Maximum distance to travel in units of the direction vector's length.</param>
        /// <param name="hit">Hit data for the raycast, 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)
        {
            hit = new RayHit();

            System.Numerics.Quaternion conjugate;
            QuaternionEx.Conjugate(ref transform.Orientation, out conjugate);
            System.Numerics.Vector3 localOrigin;
            Vector3Ex.Subtract(ref ray.Position, ref transform.Position, out localOrigin);
            QuaternionEx.Transform(ref localOrigin, ref conjugate, out localOrigin);
            System.Numerics.Vector3 localDirection;
            QuaternionEx.Transform(ref ray.Direction, ref conjugate, out localDirection);
            System.Numerics.Vector3 normal = Toolbox.ZeroVector;
            float temp, tmin = 0, tmax = maximumLength;

            if (Math.Abs(localDirection.X) < Toolbox.Epsilon && (localOrigin.X < -halfWidth || localOrigin.X > halfWidth))
                return false;
            float inverseDirection = 1 / localDirection.X;
            float t1 = (-halfWidth - localOrigin.X) * inverseDirection;
            float t2 = (halfWidth - localOrigin.X) * inverseDirection;
            var tempNormal = new System.Numerics.Vector3(-1, 0, 0);
            if (t1 > t2)
            {
                temp = t1;
                t1 = t2;
                t2 = temp;
                tempNormal *= -1;
            }
            temp = tmin;
            tmin = Math.Max(tmin, t1);
            if (temp != tmin)
                normal = tempNormal;
            tmax = Math.Min(tmax, t2);
            if (tmin > tmax)
                return false;
            if (Math.Abs(localDirection.Y) < Toolbox.Epsilon && (localOrigin.Y < -halfHeight || localOrigin.Y > halfHeight))
                return false;
            inverseDirection = 1 / localDirection.Y;
            t1 = (-halfHeight - localOrigin.Y) * inverseDirection;
            t2 = (halfHeight - localOrigin.Y) * inverseDirection;
            tempNormal = new System.Numerics.Vector3(0, -1, 0);
            if (t1 > t2)
            {
                temp = t1;
                t1 = t2;
                t2 = temp;
                tempNormal *= -1;
            }
            temp = tmin;
            tmin = Math.Max(tmin, t1);
            if (temp != tmin)
                normal = tempNormal;
            tmax = Math.Min(tmax, t2);
            if (tmin > tmax)
                return false;
            if (Math.Abs(localDirection.Z) < Toolbox.Epsilon && (localOrigin.Z < -halfLength || localOrigin.Z > halfLength))
                return false;
            inverseDirection = 1 / localDirection.Z;
            t1 = (-halfLength - localOrigin.Z) * inverseDirection;
            t2 = (halfLength - localOrigin.Z) * inverseDirection;
            tempNormal = new System.Numerics.Vector3(0, 0, -1);
            if (t1 > t2)
            {
                temp = t1;
                t1 = t2;
                t2 = temp;
                tempNormal *= -1;
            }
            temp = tmin;
            tmin = Math.Max(tmin, t1);
            if (temp != tmin)
                normal = tempNormal;
            tmax = Math.Min(tmax, t2);
            if (tmin > tmax)
                return false;
            hit.T = tmin;
            Vector3Ex.Multiply(ref ray.Direction, tmin, out hit.Location);
            Vector3Ex.Add(ref hit.Location, ref ray.Position, out hit.Location);
            QuaternionEx.Transform(ref normal, ref transform.Orientation, out normal);
            hit.Normal = normal;
            return true;
        }
Пример #12
0
        /// <summary>
        /// Tests a ray against the entry.
        /// </summary>
        /// <param name="ray">Ray to test.</param>
        /// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</param>
        /// <param name="rayHit">Hit location of the ray on the entry, if any.</param>
        /// <returns>Whether or not the ray hit the entry.</returns>
        public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit)
        {
            //Put the ray into local space.
            Ray localRay;
            Matrix3x3 orientation;
            Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation);
            Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction);
            Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position);
            Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position);


            if (Shape.solidity == MobileMeshSolidity.Solid)
            {
                //Find all hits.  Use the count to determine the ray started inside or outside.
                //If it starts inside and we're in 'solid' mode, then return the ray start.
                //The raycast must be of infinite length at first.  This allows it to determine
                //if it is inside or outside.
                if (Shape.IsLocalRayOriginInMesh(ref localRay, out rayHit))
                {
                    //It was inside!
                    rayHit = new RayHit() { Location = ray.Position, Normal = Vector3.Zero, T = 0 };
                    return true;

                }
                else
                {
                    if (rayHit.T < maximumLength)
                    {
                        //Transform the hit into world space.
                        Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
                        Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
                        Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
                    }
                    else
                    {
                        //The hit was too far away, or there was no hit (in which case T would be float.MaxValue).
                        return false;
                    }
                    return true;
                }
            }
            else
            {
                //Just do a normal raycast since the object isn't solid.
                TriangleSidedness sidedness;
                switch (Shape.solidity)
                {
                    case MobileMeshSolidity.Clockwise:
                        sidedness = TriangleSidedness.Clockwise;
                        break;
                    case MobileMeshSolidity.Counterclockwise:
                        sidedness = TriangleSidedness.Counterclockwise;
                        break;
                    default:
                        sidedness = TriangleSidedness.DoubleSided;
                        break;
                }
                if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
                {
                    //Transform the hit into world space.
                    Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
                    Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
                    Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
                    return true;
                }
            }
            rayHit = new RayHit();
            return false;
        }
 /// <summary>
 /// Tests a ray against the entry.
 /// </summary>
 /// <param name="ray">Ray to test.</param>
 /// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</param>
 /// <param name="rayHit">Hit location of the ray on the entry, if any.</param>
 /// <returns>Whether or not the ray hit the entry.</returns>
 public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit)
 {
     return RayCast(ray, maximumLength, sidedness, out rayHit);
 }
Пример #14
0
        /// <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;
        }
Пример #15
0
        ///<summary>
        /// Tests a ray against the surface of the mesh.  This does not take into account solidity.
        ///</summary>
        ///<param name="ray">Ray to test.</param>
        ///<param name="maximumLength">Maximum length of the ray to test; in units of the ray's direction's length.</param>
        ///<param name="sidedness">Sidedness to use during the ray cast.  This does not have to be the same as the mesh's sidedness.</param>
        ///<param name="rayHit">The hit location of the ray on the mesh, if any.</param>
        ///<returns>Whether or not the ray hit the mesh.</returns>
        public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit)
        {
            //Put the ray into local space.
            Ray localRay;
            Matrix3x3 orientation;
            Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation);
            Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction);
            Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position);
            Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position);

            if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
            {
                //Transform the hit into world space.
                Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
                Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
                Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
                return true;
            }
            rayHit = new RayHit();
            return false;
        }
Пример #16
0
        /// <summary>
        /// Gets the intersection between the convex shape and the ray.
        /// </summary>
        /// <param name="ray">Ray to test.</param>
        /// <param name="transform">Transform of the convex shape.</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)
        {
            //Put the ray into local space.
            Quaternion conjugate;
            Quaternion.Conjugate(ref transform.Orientation, out conjugate);
            Ray localRay;
            Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
            Quaternion.Transform(ref localRay.Position, ref conjugate, out localRay.Position);
            Quaternion.Transform(ref ray.Direction, ref conjugate, out localRay.Direction);

            //Check for containment.
            if (localRay.Position.Y >= -halfHeight && localRay.Position.Y <= halfHeight && localRay.Position.X * localRay.Position.X + localRay.Position.Z * localRay.Position.Z <= radius * radius)
            {
                //It's inside!
                hit.T = 0;
                hit.Location = localRay.Position;
                hit.Normal = new Vector3(hit.Location.X, 0, hit.Location.Z);
                float normalLengthSquared = hit.Normal.LengthSquared();
                if (normalLengthSquared > 1e-9f)
                    Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
                else
                    hit.Normal = new Vector3();
                //Pull the hit into world space.
                Quaternion.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
                RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
                return true;
            }

            //Project the ray direction onto the plane where the cylinder is a circle.
            //The projected ray is then tested against the circle to compute the time of impact.
            //That time of impact is used to compute the 3d hit location.
            Vector2 planeDirection = new Vector2(localRay.Direction.X, localRay.Direction.Z);
            float planeDirectionLengthSquared = planeDirection.LengthSquared();

            if (planeDirectionLengthSquared < Toolbox.Epsilon)
            {
                //The ray is nearly parallel with the axis.
                //Skip the cylinder-sides test.  We're either inside the cylinder and won't hit the sides, or we're outside
                //and won't hit the sides.  
                if (localRay.Position.Y > halfHeight)
                    goto upperTest;
                if (localRay.Position.Y < -halfHeight)
                    goto lowerTest;


                hit = new RayHit();
                return false;

            }
            Vector2 planeOrigin = new Vector2(localRay.Position.X, localRay.Position.Z);
            float dot;
            Vector2.Dot(ref planeDirection, ref planeOrigin, out dot);
            float closestToCenterT = -dot / planeDirectionLengthSquared;

            Vector2 closestPoint;
            Vector2.Multiply(ref planeDirection, closestToCenterT, out closestPoint);
            Vector2.Add(ref planeOrigin, ref closestPoint, out closestPoint);
            //How close does the ray come to the circle?
            float squaredDistance = closestPoint.LengthSquared();
            if (squaredDistance > radius * radius)
            {
                //It's too far!  The ray cannot possibly hit the capsule.
                hit = new RayHit();
                return false;
            }



            //With the squared distance, compute the distance backward along the ray from the closest point on the ray to the axis.
            float backwardsDistance = radius * (float)Math.Sqrt(1 - squaredDistance / (radius * radius));
            float tOffset = backwardsDistance / (float)Math.Sqrt(planeDirectionLengthSquared);

            hit.T = closestToCenterT - tOffset;

            //Compute the impact point on the infinite cylinder in 3d local space.
            Vector3.Multiply(ref localRay.Direction, hit.T, out hit.Location);
            Vector3.Add(ref hit.Location, ref localRay.Position, out hit.Location);

            //Is it intersecting the cylindrical portion of the capsule?
            if (hit.Location.Y <= halfHeight && hit.Location.Y >= -halfHeight && hit.T < maximumLength)
            {
                //Yup!
                hit.Normal = new Vector3(hit.Location.X, 0, hit.Location.Z);
                float normalLengthSquared = hit.Normal.LengthSquared();
                if (normalLengthSquared > 1e-9f)
                    Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
                else
                    hit.Normal = new Vector3();
                //Pull the hit into world space.
                Quaternion.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
                RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
                return true;
            }

            if (hit.Location.Y < halfHeight)
                goto lowerTest;
        upperTest:
            //Nope! It may be intersecting the ends of the cylinder though.
            //We're above the cylinder, so cast a ray against the upper cap.
            if (localRay.Direction.Y > -1e-9)
            {
                //Can't hit the upper cap if the ray isn't pointing down.
                hit = new RayHit();
                return false;
            }
            float t = (halfHeight - localRay.Position.Y) / localRay.Direction.Y;
            Vector3 planeIntersection;
            Vector3.Multiply(ref localRay.Direction, t, out planeIntersection);
            Vector3.Add(ref localRay.Position, ref planeIntersection, out planeIntersection);
            if(planeIntersection.X * planeIntersection.X + planeIntersection.Z * planeIntersection.Z < radius * radius + 1e-9 && t < maximumLength)
            {
                //Pull the hit into world space.
                Quaternion.Transform(ref Toolbox.UpVector, ref transform.Orientation, out hit.Normal);
                RigidTransform.Transform(ref planeIntersection, ref transform, out hit.Location);
                hit.T = t;
                return true;
            }
            //No intersection! We can't be hitting the other sphere, so it's over!
            hit = new RayHit();
            return false;

        lowerTest:
            //Is it intersecting the bottom cap?
            if (localRay.Direction.Y < 1e-9)
            {
                //Can't hit the bottom cap if the ray isn't pointing up.
                hit = new RayHit();
                return false;
            }
            t = (-halfHeight - localRay.Position.Y) / localRay.Direction.Y;
            Vector3.Multiply(ref localRay.Direction, t, out planeIntersection);
            Vector3.Add(ref localRay.Position, ref planeIntersection, out planeIntersection);
            if (planeIntersection.X * planeIntersection.X + planeIntersection.Z * planeIntersection.Z < radius * radius + 1e-9 && t < maximumLength)
            {
                //Pull the hit into world space.
                Quaternion.Transform(ref Toolbox.DownVector, ref transform.Orientation, out hit.Normal);
                RigidTransform.Transform(ref planeIntersection, ref transform, out hit.Location);
                hit.T = t;
                return true;
            }
            //No intersection! We can't be hitting the other sphere, so it's over!
            hit = new RayHit();
            return false;

        }
Пример #17
0
 ///<summary>
 /// Tests a ray against the terrain shape.
 ///</summary>
 ///<param name="ray">Ray to test against the shape.</param>
 ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
 ///<param name="transform">Transform to apply to the terrain shape during the test.</param>
 ///<param name="hit">Hit data of the ray cast, if any.</param>
 ///<returns>Whether or not the ray hit the transformed terrain shape.</returns>
 public bool RayCast(ref Ray ray, float maximumLength, ref AffineTransform transform, out RayHit hit)
 {
     return RayCast(ref ray, maximumLength, ref transform, TriangleSidedness.Counterclockwise, out hit);
 }
Пример #18
0
 public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit)
 {
     return triangleMesh.RayCast(ray, maximumLength, TriangleSidedness.DoubleSided, out rayHit);
 }
Пример #19
0
        ///<summary>
        /// Tests a ray against the terrain shape.
        ///</summary>
        ///<param name="ray">Ray to test against the shape.</param>
        ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
        ///<param name="transform">Transform to apply to the terrain shape during the test.</param>
        ///<param name="sidedness">Sidedness of the triangles to use when raycasting.</param>
        ///<param name="hit">Hit data of the ray cast, if any.</param>
        ///<returns>Whether or not the ray hit the transformed terrain shape.</returns>
        public bool RayCast(ref Ray ray, float maximumLength, ref AffineTransform transform, TriangleSidedness sidedness, out RayHit hit)
        {
            hit = new RayHit();
            //Put the ray into local space.
            Ray localRay;
            AffineTransform inverse;
            AffineTransform.Invert(ref transform, out inverse);
            Matrix3x3.Transform(ref ray.Direction, ref inverse.LinearTransform, out localRay.Direction);
            AffineTransform.Transform(ref ray.Position, ref inverse, out localRay.Position);

            //Use rasterizey traversal.
            //The origin is at 0,0,0 and the map goes +X, +Y, +Z.
            //if it's before the origin and facing away, or outside the max and facing out, early out.
            float maxX = heights.GetLength(0) - 1;
            float maxZ = heights.GetLength(1) - 1;

            Vector3 progressingOrigin = localRay.Position;
            float distance = 0;
            //Check the outside cases first.
            if (progressingOrigin.X < 0)
            {
                if (localRay.Direction.X > 0)
                {
                    //Off the left side.
                    float timeToMinX = -progressingOrigin.X / localRay.Direction.X;
                    distance += timeToMinX;
                    Vector3 increment;
                    Vector3.Multiply(ref localRay.Direction, timeToMinX, out increment);
                    Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
                }
                else
                    return false; //Outside and pointing away from the terrain.
            }
            else if (progressingOrigin.X > maxX)
            {
                if (localRay.Direction.X < 0)
                {
                    //Off the left side.
                    float timeToMinX = -(progressingOrigin.X - maxX) / localRay.Direction.X;
                    distance += timeToMinX;
                    Vector3 increment;
                    Vector3.Multiply(ref localRay.Direction, timeToMinX, out increment);
                    Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
                }
                else
                    return false; //Outside and pointing away from the terrain.
            }

            if (progressingOrigin.Z < 0)
            {
                if (localRay.Direction.Z > 0)
                {
                    float timeToMinZ = -progressingOrigin.Z / localRay.Direction.Z;
                    distance += timeToMinZ;
                    Vector3 increment;
                    Vector3.Multiply(ref localRay.Direction, timeToMinZ, out increment);
                    Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
                }
                else
                    return false;
            }
            else if (progressingOrigin.Z > maxZ)
            {
                if (localRay.Direction.Z < 0)
                {
                    float timeToMinZ = -(progressingOrigin.Z - maxZ) / localRay.Direction.Z;
                    distance += timeToMinZ;
                    Vector3 increment;
                    Vector3.Multiply(ref localRay.Direction, timeToMinZ, out increment);
                    Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
                }
                else
                    return false;
            }

            if (distance > maximumLength)
                return false;

            //By now, we should be entering the main body of the terrain.

            int xCell = (int)progressingOrigin.X;
            int zCell = (int)progressingOrigin.Z;
            //If it's hitting the border and going in, then correct the index
            //so that it will initially target a valid quad.
            //Without this, a quad beyond the border would be tried and failed.
            if (xCell == heights.GetLength(0) - 1 && localRay.Direction.X < 0)
                xCell = heights.GetLength(0) - 2;
            if (zCell == heights.GetLength(1) - 1 && localRay.Direction.Z < 0)
                zCell = heights.GetLength(1) - 2;

            while (true)
            {
                //Check for a miss.
                if (xCell < 0 ||
                    zCell < 0 ||
                    xCell >= heights.GetLength(0) - 1 ||
                    zCell >= heights.GetLength(1) - 1)
                    return false;

                //Test the triangles of this cell.
                Vector3 v1, v2, v3, v4;
                // v3 v4
                // v1 v2
                GetLocalPosition(xCell, zCell, out v1);
                GetLocalPosition(xCell + 1, zCell, out v2);
                GetLocalPosition(xCell, zCell + 1, out v3);
                GetLocalPosition(xCell + 1, zCell + 1, out v4);
                RayHit hit1, hit2;
                bool didHit1;
                bool didHit2;

                //Don't bother doing ray intersection tests if the ray can't intersect it.

                float highest = v1.Y;
                float lowest = v1.Y;
                if (v2.Y > highest)
                    highest = v2.Y;
                else if (v2.Y < lowest)
                    lowest = v2.Y;
                if (v3.Y > highest)
                    highest = v3.Y;
                else if (v3.Y < lowest)
                    lowest = v3.Y;
                if (v4.Y > highest)
                    highest = v4.Y;
                else if (v4.Y < lowest)
                    lowest = v4.Y;

                if (!(progressingOrigin.Y > highest && localRay.Direction.Y > 0 ||
                    progressingOrigin.Y < lowest && localRay.Direction.Y < 0))
                {

                    if (quadTriangleOrganization == QuadTriangleOrganization.BottomLeftUpperRight)
                    {
                        //Always perform the raycast as if Y+ in local space is the way the triangles are facing.
                        didHit1 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v2, ref v3, out hit1);
                        didHit2 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v2, ref v4, ref v3, out hit2);
                    }
                    else //if (quadTriangleOrganization == CollisionShapes.QuadTriangleOrganization.BottomRightUpperLeft)
                    {
                        didHit1 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v2, ref v4, out hit1);
                        didHit2 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v4, ref v3, out hit2);
                    }
                    if (didHit1 && didHit2)
                    {
                        if (hit1.T < hit2.T)
                        {
                            Vector3.Multiply(ref ray.Direction, hit1.T, out hit.Location);
                            Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
                            Matrix3x3.TransformTranspose(ref hit1.Normal, ref inverse.LinearTransform, out hit.Normal);
                            hit.T = hit1.T;
                            return true;
                        }
                        Vector3.Multiply(ref ray.Direction, hit2.T, out hit.Location);
                        Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
                        Matrix3x3.TransformTranspose(ref hit2.Normal, ref inverse.LinearTransform, out hit.Normal);
                        hit.T = hit2.T;
                        return true;
                    }
                    else if (didHit1)
                    {
                        Vector3.Multiply(ref ray.Direction, hit1.T, out hit.Location);
                        Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
                        Matrix3x3.TransformTranspose(ref hit1.Normal, ref inverse.LinearTransform, out hit.Normal);
                        hit.T = hit1.T;
                        return true;
                    }
                    else if (didHit2)
                    {
                        Vector3.Multiply(ref ray.Direction, hit2.T, out hit.Location);
                        Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
                        Matrix3x3.TransformTranspose(ref hit2.Normal, ref inverse.LinearTransform, out hit.Normal);
                        hit.T = hit2.T;
                        return true;
                    }
                }

                //Move to the next cell.

                float timeToX;
                if (localRay.Direction.X < 0)
                    timeToX = -(progressingOrigin.X - xCell) / localRay.Direction.X;
                else if (ray.Direction.X > 0)
                    timeToX = (xCell + 1 - progressingOrigin.X) / localRay.Direction.X;
                else
                    timeToX = float.MaxValue;

                float timeToZ;
                if (localRay.Direction.Z < 0)
                    timeToZ = -(progressingOrigin.Z - zCell) / localRay.Direction.Z;
                else if (localRay.Direction.Z > 0)
                    timeToZ = (zCell + 1 - progressingOrigin.Z) / localRay.Direction.Z;
                else
                    timeToZ = float.MaxValue;

                //Move to the next cell.
                if (timeToX < timeToZ)
                {
                    if (localRay.Direction.X < 0)
                        xCell--;
                    else
                        xCell++;

                    distance += timeToX;
                    if (distance > maximumLength)
                        return false;

                    Vector3 increment;
                    Vector3.Multiply(ref localRay.Direction, timeToX, out increment);
                    Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
                }
                else
                {
                    if (localRay.Direction.Z < 0)
                        zCell--;
                    else
                        zCell++;

                    distance += timeToZ;
                    if (distance > maximumLength)
                        return false;

                    Vector3 increment;
                    Vector3.Multiply(ref localRay.Direction, timeToZ, out increment);
                    Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin);
                }

            }
        }
Пример #20
0
        //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;
        }
Пример #21
0
        ///<summary>
        /// Tests a ray against a sphere.
        ///</summary>
        ///<param name="ray">Ray to test.</param>
        ///<param name="spherePosition">Position of the sphere.</param>
        ///<param name="radius">Radius of the sphere.</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, if any.</param>
        ///<returns>Whether or not the ray hits the sphere.</returns>
        public static bool RayCastSphere(ref Ray ray, ref Vector3 spherePosition, float radius, float maximumLength, out RayHit 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 spherePosition, out m);
            float b = Vector3.Dot(m, normalizedDirection);
            float c = m.LengthSquared() - radius * radius;

            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;
            hit.T /= length;
            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 spherePosition, out hit.Normal);
            hit.Normal.Normalize();
            return true;
        }
Пример #22
0
 ///<summary>
 /// Tests a ray against the triangle mesh.
 ///</summary>
 ///<param name="ray">Ray to test against the mesh.</param>
 /// <param name="sidedness">Sidedness to apply to the mesh for the ray cast.</param>
 ///<param name="rayHit">Hit data for the ray, if any.</param>
 ///<returns>Whether or not the ray hit the mesh.</returns>
 public bool RayCast(Ray ray, TriangleSidedness sidedness, out RayHit rayHit)
 {
     return RayCast(ray, float.MaxValue, sidedness, out rayHit);
 }
Пример #23
0
 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;
 }
Пример #24
0
        /// <summary>
        /// Determines the intersection between a ray and a triangle.
        /// </summary>
        /// <param name="ray">Ray to test.</param>
        /// <param name="maximumLength">Maximum length to travel in units of the direction's length.</param>
        /// <param name="sidedness">Sidedness of the triangle to test.</param>
        /// <param name="a">First vertex of the triangle.</param>
        /// <param name="b">Second vertex of the triangle.</param>
        /// <param name="c">Third vertex of the triangle.</param>
        /// <param name="hit">Hit data of the ray, if any</param>
        /// <returns>Whether or not the ray and triangle intersect.</returns>
        public static bool FindRayTriangleIntersection(ref Ray ray, float maximumLength, TriangleSidedness sidedness, ref Vector3 a, ref Vector3 b, ref Vector3 c, out RayHit hit)
        {
            hit = new RayHit();
            Vector3 ab, ac;
            Vector3.Subtract(ref b, ref a, out ab);
            Vector3.Subtract(ref c, ref a, out ac);

            Vector3.Cross(ref ab, ref ac, out hit.Normal);
            if (hit.Normal.LengthSquared() < Epsilon)
                return false; //Degenerate triangle!

            float d;
            Vector3.Dot(ref ray.Direction, ref hit.Normal, out d);
            d = -d;
            switch (sidedness)
            {
                case TriangleSidedness.DoubleSided:
                    if (d <= 0) //Pointing the wrong way.  Flip the normal.
                    {
                        Vector3.Negate(ref hit.Normal, out hit.Normal);
                        d = -d;
                    }
                    break;
                case TriangleSidedness.Clockwise:
                    if (d <= 0) //Pointing the wrong way.  Can't hit.
                        return false;

                    break;
                case TriangleSidedness.Counterclockwise:
                    if (d >= 0) //Pointing the wrong way.  Can't hit.
                        return false;

                    Vector3.Negate(ref hit.Normal, out hit.Normal);
                    d = -d;
                    break;
            }

            Vector3 ap;
            Vector3.Subtract(ref ray.Position, ref a, out ap);

            Vector3.Dot(ref ap, ref hit.Normal, out hit.T);
            hit.T /= d;
            if (hit.T < 0 || hit.T > maximumLength)
                return false;//Hit is behind origin, or too far away.

            Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
            Vector3.Add(ref ray.Position, ref hit.Location, out hit.Location);

            // Compute barycentric coordinates
            Vector3.Subtract(ref hit.Location, ref a, out ap);
            float ABdotAB, ABdotAC, ABdotAP;
            float ACdotAC, ACdotAP;
            Vector3.Dot(ref ab, ref ab, out ABdotAB);
            Vector3.Dot(ref ab, ref ac, out ABdotAC);
            Vector3.Dot(ref ab, ref ap, out ABdotAP);
            Vector3.Dot(ref ac, ref ac, out ACdotAC);
            Vector3.Dot(ref ac, ref ap, out ACdotAP);

            float denom = 1 / (ABdotAB * ACdotAC - ABdotAC * ABdotAC);
            float u = (ACdotAC * ABdotAP - ABdotAC * ACdotAP) * denom;
            float v = (ABdotAB * ACdotAP - ABdotAC * ABdotAP) * denom;

            return (u >= -Toolbox.BigEpsilon) && (v >= -Toolbox.BigEpsilon) && (u + v <= 1 + Toolbox.BigEpsilon);

        }
Пример #25
0
 ///<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);
 }
Пример #26
0
        /// <summary>
        /// Determines the intersection between a ray and a triangle.
        /// </summary>
        /// <param name="ray">Ray to test.</param>
        /// <param name="maximumLength">Maximum length to travel in units of the direction's length.</param>
        /// <param name="a">First vertex of the triangle.</param>
        /// <param name="b">Second vertex of the triangle.</param>
        /// <param name="c">Third vertex of the triangle.</param>
        /// <param name="hitClockwise">True if the the triangle was hit on the clockwise face, false otherwise.</param>
        /// <param name="hit">Hit data of the ray, if any</param>
        /// <returns>Whether or not the ray and triangle intersect.</returns>
        public static bool FindRayTriangleIntersection(ref Ray ray, float maximumLength, ref Vector3 a, ref Vector3 b, ref Vector3 c, out bool hitClockwise, out RayHit hit)
        {
            hitClockwise = false;
            hit = new RayHit();
            Vector3 ab, ac;
            Vector3.Subtract(ref b, ref a, out ab);
            Vector3.Subtract(ref c, ref a, out ac);

            Vector3.Cross(ref ab, ref ac, out hit.Normal);
            if (hit.Normal.LengthSquared() < Epsilon)
                return false; //Degenerate triangle!

            float d;
            Vector3.Dot(ref ray.Direction, ref hit.Normal, out d);
            d = -d;

            hitClockwise = d >= 0;

            Vector3 ap;
            Vector3.Subtract(ref ray.Position, ref a, out ap);

            Vector3.Dot(ref ap, ref hit.Normal, out hit.T);
            hit.T /= d;
            if (hit.T < 0 || hit.T > maximumLength)
                return false;//Hit is behind origin, or too far away.

            Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
            Vector3.Add(ref ray.Position, ref hit.Location, out hit.Location);

            // Compute barycentric coordinates
            Vector3.Subtract(ref hit.Location, ref a, out ap);
            float ABdotAB, ABdotAC, ABdotAP;
            float ACdotAC, ACdotAP;
            Vector3.Dot(ref ab, ref ab, out ABdotAB);
            Vector3.Dot(ref ab, ref ac, out ABdotAC);
            Vector3.Dot(ref ab, ref ap, out ABdotAP);
            Vector3.Dot(ref ac, ref ac, out ACdotAC);
            Vector3.Dot(ref ac, ref ap, out ACdotAP);

            float denom = 1 / (ABdotAB * ACdotAC - ABdotAC * ABdotAC);
            float u = (ACdotAC * ABdotAP - ABdotAC * ACdotAP) * denom;
            float v = (ABdotAB * ACdotAP - ABdotAC * ABdotAP) * denom;

            return (u >= -Toolbox.BigEpsilon) && (v >= -Toolbox.BigEpsilon) && (u + v <= 1 + Toolbox.BigEpsilon);

        }
Пример #27
0
        ///<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>
 /// 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 cast on the entry, if any.</param>
 /// <returns>Whether or not the cast hit the entry.</returns>
 public abstract bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit);
Пример #29
0
 /// <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);
 }
 /// <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;
 }