Example #1
0
        public TraceResult Trace(JVector origin, JVector direction, TraceOptions options)
        {
            if (direction.Length() < 0.1f)
            {
                return(null);
            }

            RaycastCallback callback = (b, n, f) =>
            {
                if ((b.IsStatic == true) && (options.HasFlag(TraceOptions.IgnoreStatic)))
                {
                    return(false);
                }
                if ((b.IsStatic == false) && (options.HasFlag(TraceOptions.IgnoreDynamic)))
                {
                    return(false);
                }
                return(true);
            };
            RigidBody body;
            JVector   normal;
            float     friction;

            if (this.CollisionSystem.Raycast(
                    origin,
                    direction,
                    callback,
                    out body,
                    out normal,
                    out friction))
            {
                return(new TraceResult(body, friction, origin + friction * direction, normal));
            }
            return(null);
        }
Example #2
0
        public override void PrepareForIteration(float timestep)
        {
            var delta  = Target - Body1.Position;
            var length = delta.Length();

            if (length > 1f * Velocity)
            {
                if (Velocity > targetVelocity.Length())
                {
                    targetVelocity = JVector.Normalize(delta) * (targetVelocity.Length() + Velocity * timestep);
                }
                else
                {
                    targetVelocity = JVector.Normalize(delta) * Velocity;
                }
            }
            else
            {
                targetVelocity = JVector.Normalize(delta) * (float)Math.Sqrt(length);
            }
        }
Example #3
0
        public virtual void SetScale(JVector p_scale, bool p_transform = true)
        {
            _scale = p_scale;

            _boundingSphere.Radius = p_scale.Length() / 2.9f;

            if (p_transform)
            {
                _matWorld = Matrix.CreateScale(p_scale.X, p_scale.Y, p_scale.Z)
                            * Matrix.CreateFromYawPitchRoll(_rotation.Y, _rotation.X, _rotation.Z)
                            * Matrix.CreateTranslation(_position.X, _position.Y, _position.Z);
            }
        }
Example #4
0
        public virtual void SetScaleRotationPosition(JVector p_scale, JVector p_rotation, JVector p_position, bool p_transform = true)
        {
            _scale    = p_scale;
            _rotation = p_rotation;
            _position = p_position;

            _boundingSphere.Radius = p_scale.Length() / 2.9f;

            _boundingSphere.Center.X = p_position.X;
            _boundingSphere.Center.Y = p_position.Y;
            _boundingSphere.Center.Z = p_position.Z;

            if (p_transform)
            {
                _matWorld = Matrix.CreateScale(_scale.X, _scale.Y, _scale.Z)
                            * Matrix.CreateFromYawPitchRoll(_rotation.Y, _rotation.X, _rotation.Z)
                            * Matrix.CreateTranslation(_position.X, _position.Y, _position.Z);
            }
        }
Example #5
0
        /// <summary>
        /// SupportMapping. Finds the point in the shape furthest away from the given direction.
        /// Imagine a plane with a normal in the search direction. Now move the plane along the normal
        /// until the plane does not intersect the shape. The last intersection point is the result.
        /// </summary>
        /// <param name="direction">The direction.</param>
        /// <param name="result">The result.</param>
        public override void SupportMapping(ref JVector direction, out JVector result)
        {
            float sigma = (float)Math.Sqrt((float)(direction.X * direction.X + direction.Z * direction.Z));

            if (direction.Y > direction.Length() * sina)
            {
                result.X = 0.0f;
                result.Y = (2.0f / 3.0f) * height;
                result.Z = 0.0f;
            }
            else if (sigma > 0.0f)
            {
                result.X = radius * direction.X / sigma;
                result.Y = -(1.0f / 3.0f) * height;
                result.Z = radius * direction.Z / sigma;
            }
            else
            {
                result.X = 0.0f;
                result.Y = -(1.0f / 3.0f) * height;
                result.Z = 0.0f;
            }
        }
Example #6
0
        // see: btSubSimplexConvexCast.cpp

        /// <summary>
        /// Checks if a ray definied through it's origin and direction collides
        /// with a shape.
        /// </summary>
        /// <param name="support">The supportmap implementation representing the shape.</param>
        /// <param name="orientation">The orientation of the shape.</param>
        /// <param name="invOrientation">The inverse orientation of the shape.</param>
        /// <param name="position">The position of the shape.</param>
        /// <param name="origin">The origin of the ray.</param>
        /// <param name="direction">The direction of the ray.</param>
        /// <param name="fraction">The fraction which gives information where at the
        /// ray the collision occured. The hitPoint is calculated by: origin+friction*direction.</param>
        /// <param name="normal">The normal from the ray collision.</param>
        /// <returns>Returns true if the ray hit the shape, false otherwise.</returns>
        public static bool Raycast(ISupportMappable support, ref JMatrix orientation, ref JMatrix invOrientation,
                                   ref JVector position, ref JVector origin, ref JVector direction, out float fraction, out JVector normal)
        {
            VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();

            simplexSolver.Reset();

            normal   = JVector.Zero;
            fraction = float.MaxValue;

            float lambda = 0.0f;

            JVector r = direction;
            JVector x = origin;
            JVector w, p, v;

            JVector arbitraryPoint;

            SupportMapTransformed(support, ref orientation, ref position, ref r, out arbitraryPoint);
            JVector.Subtract(ref x, ref arbitraryPoint, out v);

            int maxIter = MaxIterations;

            float distSq  = v.LengthSquared();
            float epsilon = 0.000001f;

            float VdotR;

            while ((distSq > epsilon) && (maxIter-- != 0))
            {
                SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
                JVector.Subtract(ref x, ref p, out w);

                float VdotW = JVector.Dot(ref v, ref w);

                if (VdotW > 0.0f)
                {
                    VdotR = JVector.Dot(ref v, ref r);

                    if (VdotR >= -JMath.Epsilon)
                    {
                        simplexSolverPool.GiveBack(simplexSolver);
                        return(false);
                    }
                    else
                    {
                        lambda = lambda - VdotW / VdotR;
                        JVector.Multiply(ref r, lambda, out x);
                        JVector.Add(ref origin, ref x, out x);
                        JVector.Subtract(ref x, ref p, out w);
                        normal = v;
                    }
                }
                if (!simplexSolver.InSimplex(w))
                {
                    simplexSolver.AddVertex(w, x, p);
                }
                if (simplexSolver.Closest(out v))
                {
                    distSq = v.LengthSquared();
                }
                else
                {
                    distSq = 0.0f;
                }
            }

            #region Retrieving hitPoint

            // Giving back the fraction like this *should* work
            // but is inaccurate against large objects:
            // fraction = lambda;

            JVector p1, p2;
            simplexSolver.ComputePoints(out p1, out p2);

            p2       = p2 - origin;
            fraction = p2.Length() / direction.Length();

            #endregion

            if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
            {
                normal.Normalize();
            }

            simplexSolverPool.GiveBack(simplexSolver);

            return(true);
        }
Example #7
0
        /// <summary>
        /// Checks two shapes for collisions.
        /// </summary>
        /// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
        /// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
        /// <param name="orientation1">The orientation of the first shape.</param>
        /// <param name="orientation2">The orientation of the second shape.</param>
        /// <param name="position1">The position of the first shape.</param>
        /// <param name="position2">The position of the second shape</param>
        /// <param name="point">The pointin world coordinates, where collision occur.</param>
        /// <param name="normal">The normal pointing from body2 to body1.</param>
        /// <param name="penetration">Estimated penetration depth of the collision.</param>
        /// <returns>Returns true if there is a collision, false otherwise.</returns>
        public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
                                  ref JMatrix orientation2, ref JVector position1, ref JVector position2,
                                  out JVector point, out JVector normal, out float penetration)
        {
            // Used variables
            JVector v01, v02, v0;
            JVector v11, v12, v1;
            JVector v21, v22, v2;
            JVector v31, v32, v3;
            JVector mn;

            // Initialization of the output
            point       = normal = JVector.Zero;
            penetration = 0.0f;

            // Get the center of shape1 in world coordinates -> v01
            support1.SupportCenter(out v01);
            JVector.Transform(ref v01, ref orientation1, out v01);
            JVector.Add(ref position1, ref v01, out v01);

            // Get the center of shape2 in world coordinates -> v02
            support2.SupportCenter(out v02);
            JVector.Transform(ref v02, ref orientation2, out v02);
            JVector.Add(ref position2, ref v02, out v02);

            // v0 is the center of the minkowski difference
            JVector.Subtract(ref v02, ref v01, out v0);

            // Avoid case where centers overlap -- any direction is fine in this case
            if (v0.IsNearlyZero())
            {
                v0 = new JVector(0.00001f, 0);
            }

            // v1 = support in direction of origin
            mn = v0;
            JVector.Negate(ref v0, out normal);

            SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
            SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
            JVector.Subtract(ref v12, ref v11, out v1);

            if (JVector.Dot(ref v1, ref normal) <= 0.0f)
            {
                return(false);
            }

            // v2 = support perpendicular to v1,v0
            normal = OutsidePortal(v1, v0);

            JVector.Negate(ref normal, out mn);
            SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
            SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
            JVector.Subtract(ref v22, ref v21, out v2);

            //LD.Draw(Conversion.ToXNAVector2(v1), Conversion.ToXNAVector2(v0), Color.Blue);
            //LD.Draw(Conversion.ToXNAVector2(v2), Conversion.ToXNAVector2(v0), Color.Blue);

            if (JVector.Dot(ref v2, ref normal) <= 0.0f)
            {
                return(false);
            }

            // phase two: portal refinement
            int maxIterations = 0;

            while (true)
            {
                // find normal direction
                if (!IntersectPortal(v0, v2, v1))
                {
                    normal = InsidePortal(v2, v1);
                }
                else
                {
                    // origin ray crosses the portal
                    normal = OutsidePortal(v2, v1);
                }


                // obtain the next support point
                JVector.Negate(ref normal, out mn);
                SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
                SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
                JVector.Subtract(ref v32, ref v31, out v3);

                //LD.Draw(Conversion.ToXNAVector2(v3), Conversion.ToXNAVector2(v0), Color.Green);

                if (JVector.Dot(v3, normal) <= 0)
                {
                    JVector ab = v3 - v2;
                    float   t  = -(JVector.Dot(v2, ab)) / (JVector.Dot(ab, ab));
                    normal = (v2 + (t * ab));
                    return(false);
                }

                // Portal lies on the outside edge of the Minkowski Hull.
                // Return contact information
                if (JVector.Dot((v3 - v2), normal) <= CollideEpsilon || ++maxIterations > MaximumIterations)
                {
                    JVector ab = v2 - v1;
                    float   t  = JVector.Dot(JVector.Negate(v1), ab);
                    if (t <= 0.0f)
                    {
                        t      = 0.0f;
                        normal = v1;
                    }
                    else
                    {
                        float denom = JVector.Dot(ab, ab);
                        if (t >= denom)
                        {
                            normal = v2;
                            t      = 1.0f;
                        }
                        else
                        {
                            t     /= denom;
                            normal = v1 + t * ab;
                        }
                    }

                    float s = 1 - t;

                    point = s * v11 + t * v21;
                    var point2 = s * v12 + t * v22;

                    // this  causes a sq root = bad!
                    penetration = normal.Length();
                    normal.Normalize();
                    return(true);
                }

                // if origin is inside (v1, v0, v3), refine portal
                if (OriginInTriangle(v0, v1, v3))
                {
                    v2  = v3;
                    v21 = v31;
                    v22 = v32;
                    continue;
                }
                // if origin is inside (v3, v0, v2), refine portal
                else if (OriginInTriangle(v0, v2, v3))
                {
                    v1  = v3;
                    v11 = v31;
                    v12 = v32;
                    continue;
                }
                return(false);
            }
        }