Ejemplo n.º 1
0
        /// <summary>
        /// Discrete Box vs Box test. Very fast. No contact info. NOTE: ensure UpdateAxes is called for each BoxShape prior.
        /// </summary>
        /// <param name="A">BoxShape A.</param>
        /// <param name="PA">BoxShape A's position.</param>
        /// <param name="B">BoxShape B.</param>
        /// <param name="PB">BoxShape B's position.</param>
        /// <returns></returns>
        public static bool BoxBoxTest(ref BoxShape A, ref JVector PA, ref BoxShape B, ref JVector PB)
        {
            JVector T = PB - PA;

            // cache scaled local axes
            JVector Ax = A.halfSize.X * A.xAxis;
            JVector Ay = A.halfSize.Y * A.yAxis;
            JVector Bx = B.halfSize.X * B.xAxis;
            JVector By = B.halfSize.Y * B.yAxis;

            // axis to test
            JVector L = A.xAxis;
            float TL = Math.Abs(T * L);

            float a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
            float b = Math.Abs((Bx) * L) + Math.Abs((By) * L);

            if (TL > (a + b))
                return false;

            // axis to test
            L = A.yAxis;
            TL = Math.Abs(T * L);

            a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
            b = Math.Abs((Bx) * L) + Math.Abs((By) * L);

            if (TL > (a + b))
                return false;

            // axis to test
            L = B.xAxis;
            TL = Math.Abs(T * L);

            a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
            b = Math.Abs((Bx) * L) + Math.Abs((By) * L);

            if (TL > (a + b))
                return false;

            // axis to test
            L = B.yAxis;
            TL = Math.Abs(T * L);

            a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
            b = Math.Abs((Bx) * L) + Math.Abs((By) * L);

            if (TL > (a + b))
                return false;

            // if all axes overlap collision exists
            return true;
        }
Ejemplo n.º 2
0
        protected override void Initialize()
        {
            DebugDrawer = new DebugDrawer(this);
            this.Components.Add(DebugDrawer);

            Camera = new Camera(this);
            this.Components.Add(Camera);

            A = new BoxShape(4, 1);
            PA = new JVector(-1, 2);
            VA = new JVector(0, 0);
            OA = MathHelper.ToRadians(0);

            B = new BoxShape(1, 4);
            PB = new JVector(-3, 0);
            VB = new JVector(0, 0);
            OB = 0;

            base.Initialize();
        }
Ejemplo n.º 3
0
        void DrawBox(BoxShape box, JVector pos, float orientation, Color color)
        {
            List<JVector> poly = new List<JVector>
            {
                new JVector(-box.Size.X * 0.5f, -box.Size.Y * 0.5f),
                new JVector(box.Size.X * 0.5f, -box.Size.Y * 0.5f),
                new JVector(box.Size.X * 0.5f, box.Size.Y * 0.5f),
                new JVector(-box.Size.X * 0.5f,box.Size.Y * 0.5f)
            };

            DrawPoly(poly, pos, JMatrix.CreateRotationZ(orientation), color);
        }
Ejemplo n.º 4
0
        /// <summary>
        /// Discrete Box vs Box test. Very fast. Generates contact info. NOTE: ensure UpdateAxes is called for each BoxShape prior.
        /// </summary>
        /// <param name="A">BoxShape A.</param>
        /// <param name="PA">BoxShape A's position.</param>
        /// <param name="OA">BoxShape A's orientation.</param>
        /// <param name="B">BoxShape B.</param>
        /// <param name="PB">BoxShape B's position.</param>
        /// <param name="OB">BoxShape B's orientation.</param>
        /// <param name="normal">Normal of collision.</param>
        /// <param name="t">Amount of penetration.</param>
        /// <param name="CA">Contacts found on BoxShape A.</param>
        /// <param name="CB">Contacts found on BoxShape B.</param>
        /// <param name="NumContacts">Number of contacts found.</param>
        /// <returns>True if collision exists.</returns>
        public static bool BoxBoxTestContact(ref BoxShape A, ref JVector PA, ref JMatrix OA,
            ref BoxShape B, ref JVector PB, ref JMatrix OB,
            out JVector normal, out float t, out JVector[] CA, out JVector[] CB, out int NumContacts)
        {
            float overlap = 0;
            normal = A.xAxis;
            t = 0.0f;
            // TODO in future to save a few bytes of garbage make these
            // SA1, SA2 and SB1, SB2 (no arrays)
            JVector[] SA = new JVector[2], SB = new JVector[2];
            int NumSA = 0, NumSB = 0;

            CA = new JVector[2]; CB = new JVector[2];
            NumContacts = 0;

            JVector T = PB - PA;

            // cache scaled local axes
            JVector Ax = A.halfSize.X * A.xAxis;
            JVector Ay = A.halfSize.Y * A.yAxis;
            JVector Bx = B.halfSize.X * B.xAxis;
            JVector By = B.halfSize.Y * B.yAxis;

            // axis to test
            JVector L = A.xAxis;
            float TL = Math.Abs(T * L);

            float a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
            float b = Math.Abs((Bx) * L) + Math.Abs((By) * L);

            if (TL > (a + b))
                return false;

            // cache overlap
            // just set first overlap
            overlap = TL - (b + a);

            // axis to test
            L = A.yAxis;
            TL = Math.Abs(T * L);

            a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
            b = Math.Abs((Bx) * L) + Math.Abs((By) * L);

            if (TL > (a + b))
                return false;

            // cache overlap
            var o = TL - (b + a);
            if (o > overlap)
            {
                overlap = o;
                normal = A.yAxis;
            }

            // axis to test
            L = B.xAxis;
            TL = Math.Abs(T * L);

            a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
            b = Math.Abs((Bx) * L) + Math.Abs((By) * L);

            if (TL > (a + b))
                return false;

            // cache overlap
            o = TL - (b + a);
            if (o > overlap)
            {
                overlap = o;
                normal = B.xAxis;
            }

            // axis to test
            L = B.yAxis;
            TL = Math.Abs(T * L);

            a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
            b = Math.Abs((Bx) * L) + Math.Abs((By) * L);

            if (TL > (a + b))
                return false;

            // cache overlap
            o = TL - (b + a);
            if (o > overlap)
            {
                overlap = o;
                normal = B.yAxis;
            }

            // make sure the polygons gets pushed away from each other.
            if (normal * T < 0.0f)
                normal = -normal;

            t = overlap;

            // now to find a contact point or edge!
            var mn = -normal;

            NumSA = A.FindSupportPoints(ref mn, ref PA, ref OA, out SA);
            NumSB = B.FindSupportPoints(ref normal, ref PB, ref OB, out SB);

            // now refine contact points from support points
            // edge/vertex case
            if (NumSA == 2 && NumSB == 1)
            {
                ProjectPointOnLine(ref SB[0], ref SA[0], ref SA[1], out CA[NumContacts]);
                CB[NumContacts] = SB[0];
                NumContacts++;
                return true;
            }
            // vertex/edge case
            if (NumSA == 1 && NumSB == 2)
            {
                ProjectPointOnLine(ref SA[0], ref SB[0], ref SB[1], out CB[NumContacts]);
                CA[NumContacts] = SA[0];
                NumContacts++;
                return true;
            }
            // edge/edge case
            if (NumSA == 2 && NumSB == 2)
            {
                // clip contacts
                JVector perp = new JVector(-normal.Y, normal.X);
                // project first 2 contacts to axis perpendicular to normal
                float min0 = SA[0] * perp;
                float max0 = min0;
                float min1 = SB[0] * perp;
                float max1 = min1;
                // project next point from A
                max0 = SA[1] * perp;

                if (max0 < min0)
                {
                    JMath.Swapf(ref min0, ref max0);
                    JVector.Swap(ref SA[0], ref SA[1]);
                }

                max1 = SB[1] * perp;

                if (max1 < min1)
                {
                    JMath.Swapf(ref min1, ref max1);
                    JVector.Swap(ref SB[0], ref SB[1]);
                }

                if (min0 > min1)
                {
                    JVector Pseg;
                    ProjectPointOnLine(ref SA[0], ref SB[0], ref SB[1], out Pseg);
                    CA[NumContacts] = SA[0];
                    CB[NumContacts] = Pseg;
                    NumContacts++;
                }
                else
                {
                    JVector Pseg;
                    ProjectPointOnLine(ref SB[0], ref SA[0], ref SA[1], out Pseg);
                    CA[NumContacts] = Pseg;
                    CB[NumContacts] = SB[0];
                    NumContacts++;
                }

                if (max0 < max1)
                {
                    JVector Pseg;
                    ProjectPointOnLine(ref SA[1], ref SB[0], ref SB[1], out Pseg);
                    CA[NumContacts] = SA[1];
                    CB[NumContacts] = Pseg;
                    NumContacts++;
                }
                else
                {
                    JVector Pseg;
                    ProjectPointOnLine(ref SB[1], ref SA[0], ref SA[1], out Pseg);
                    CA[NumContacts] = Pseg;
                    CB[NumContacts] = SB[1];
                    NumContacts++;
                }

                return true;
            }

            // if all axes overlap collision exists
            return true;
        }
Ejemplo n.º 5
0
        internal static bool CircleBoxTest(ref CircleShape A, ref JVector PA, ref BoxShape B, ref JVector PB, ref JMatrix OB)
        {
            // find vertex closest to circles center
            // move circle into boxes space
            var pa = JVector.TransposedTransform(PA - PB, OB);
            // find normal from box to circles center
            var axis = pa;
            JVector closestVertex;
            // find closest vertex
            B.SupportMapping(ref axis, out closestVertex);
            // do a SAT test on that axis

            // axis to test
            JVector T = pa - closestVertex;
            float TL = Math.Abs(T * axis);

            float a = Math.Abs(pa * axis);
            float b = Math.Abs(closestVertex * axis);

            if (TL > (a + b + A.Radius))
                return false;

            return true;
        }