Пример #1
0
        /// <summary>
        /// A fallback for handling degenerate "Star of David" cases.
        /// </summary>
        private static void FindVertsFallback(
            VoltPolygon poly1,
            VoltPolygon poly2,
            Vector2 normal,
            float penetration,
            Manifold manifold)
        {
            for (int i = 0; i < poly1.countWorld; i++)
            {
                Vector2 vertex = poly1.worldVertices[i];
                if (poly2.ContainsPointPartial(vertex, normal) == true)
                {
                    if (manifold.AddContact(vertex, normal, penetration) == false)
                    {
                        return;
                    }
                }
            }

            for (int i = 0; i < poly2.countWorld; i++)
            {
                Vector2 vertex = poly2.worldVertices[i];
                if (poly1.ContainsPointPartial(vertex, -normal) == true)
                {
                    if (manifold.AddContact(vertex, normal, penetration) == false)
                    {
                        return;
                    }
                }
            }
        }
Пример #2
0
        private static bool FindMinSepAxis(
            VoltPolygon poly1,
            VoltPolygon poly2,
            out Axis axis)
        {
            axis = new Axis(Vector2.zero, float.NegativeInfinity);

            for (int i = 0; i < poly1.countWorld; i++)
            {
                Axis  a   = poly1.worldAxes[i];
                float min = float.PositiveInfinity;
                for (int j = 0; j < poly2.countWorld; j++)
                {
                    Vector2 v = poly2.worldVertices[j];
                    min = Mathf.Min(min, Vector2.Dot(a.Normal, v));
                }
                min -= a.Width;

                if (min > 0)
                {
                    return(false);
                }
                if (min > axis.Width)
                {
                    axis = new Axis(a.Normal, min);
                }
            }

            return(true);
        }
Пример #3
0
        private static bool FindMinSepAxis(
            VoltPolygon poly1,
            VoltPolygon poly2,
            out Axis axis)
        {
            axis = new Axis(VoltVector2.zero, Fix64.MinValue);

            for (int i = 0; i < poly1.countWorld; i++)
            {
                Axis  a   = poly1.worldAxes[i];
                Fix64 min = Fix64.MaxValue;
                for (int j = 0; j < poly2.countWorld; j++)
                {
                    VoltVector2 v = poly2.worldVertices[j];
                    min = VoltMath.Min(min, VoltVector2.Dot(a.Normal, v));
                }
                min -= a.Width;

                if (min > Fix64.Zero)
                {
                    return(false);
                }
                if (min > axis.Width)
                {
                    axis = new Axis(a.Normal, min);
                }
            }

            return(true);
        }
Пример #4
0
        /// <summary>
        /// Returns the index of the axis with the max circle penetration depth.
        /// Breaks out if a separating axis is found between the two shapes.
        /// Outputs the penetration depth of the circle in the axis (if any).
        /// </summary>
        internal static int FindAxisMaxPenetration(
            VoltVector2 origin,
            Fix64 radius,
            VoltPolygon poly,
            out Fix64 penetration)
        {
            int index = 0;
            int found = 0;

            penetration = Fix64.MinValue;

            for (int i = 0; i < poly.countWorld; i++)
            {
                Axis  axis = poly.worldAxes[i];
                Fix64 dot  = VoltVector2.Dot(axis.Normal, origin);
                Fix64 dist = dot - axis.Width - radius;

                if (dist > Fix64.Zero)
                {
                    return(-1);
                }

                if (dist > penetration)
                {
                    penetration = dist;
                    found       = index;
                }

                index++;
            }

            return(found);
        }
Пример #5
0
        private static Manifold Polygon_Polygon(
            VoltWorld world,
            VoltPolygon polyA,
            VoltPolygon polyB)
        {
            Axis a1, a2;

            if (Collision.FindMinSepAxis(polyA, polyB, out a1) == false)
            {
                return(null);
            }
            if (Collision.FindMinSepAxis(polyB, polyA, out a2) == false)
            {
                return(null);
            }

            // We will use poly1's axis, so we may need to swap
            if (a2.Width > a1.Width)
            {
                VoltUtil.Swap(ref polyA, ref polyB);
                VoltUtil.Swap(ref a1, ref a2);
            }

            // Build the collision Manifold
            Manifold manifold =
                world.AllocateManifold().Assign(world, polyA, polyB);

            Collision.FindVerts(polyA, polyB, a1.Normal, a1.Width, manifold);
            return(manifold);
        }
Пример #6
0
        /// <summary>
        /// Returns the index of the axis with the max circle penetration depth.
        /// Breaks out if a separating axis is found between the two shapes.
        /// Outputs the penetration depth of the circle in the axis (if any).
        /// </summary>
        internal static int FindAxisMaxPenetration(
            Vector2 origin,
            float radius,
            VoltPolygon poly,
            out float penetration)
        {
            int index = 0;
            int found = 0;

            penetration = float.NegativeInfinity;

            for (int i = 0; i < poly.countWorld; i++)
            {
                Axis  axis = poly.worldAxes[i];
                float dot  = Vector2.Dot(axis.Normal, origin);
                float dist = dot - axis.Width - radius;

                if (dist > 0)
                {
                    return(-1);
                }

                if (dist > penetration)
                {
                    penetration = dist;
                    found       = index;
                }

                index++;
            }

            return(found);
        }
Пример #7
0
 public override VoltShape PrepareShape(VoltWorld world)
 {
     Vector2[] vertices = new Vector2[this.points.Length];
     for (int i = 0; i < this.points.Length; i++)
       vertices[i] = this.points[i].position;
     this.shape = world.CreatePolygonWorldSpace(
       vertices,
       this.density);
     return this.shape;
 }
Пример #8
0
        /// <summary>
        /// Creates a new polygon shape from body-space vertices.
        /// </summary>
        public VoltPolygon CreatePolygonBodySpace(
            VoltVector2[] bodyVertices, Fix64 density, Fix64 friction, Fix64 restitution)
        {
            VoltPolygon polygon = (VoltPolygon)this.polygonPool.Allocate();

            polygon.InitializeFromBodyVertices(
                bodyVertices,
                density,
                friction,
                restitution);
            return(polygon);
        }
Пример #9
0
        protected override void ApplyBodyPosition()
        {
            for (int i = 0; i < this.countWorld; i++)
            {
                this.worldVertices[i] =
                    this.Body.BodyToWorldPointCurrent(this.bodyVertices[i]);
                this.worldAxes[i] =
                    this.Body.BodyToWorldAxisCurrent(this.bodyAxes[i]);
            }

            this.worldSpaceAABB =
                VoltPolygon.ComputeBounds(this.worldVertices, this.countWorld);
        }
Пример #10
0
        /// <summary>
        /// Creates a new polygon shape from body-space vertices.
        /// </summary>
        public VoltPolygon CreatePolygonBodySpace(
            Vector2[] bodyVertices,
            float density     = VoltConfig.DEFAULT_DENSITY,
            float friction    = VoltConfig.DEFAULT_FRICTION,
            float restitution = VoltConfig.DEFAULT_RESTITUTION)
        {
            VoltPolygon polygon = (VoltPolygon)this.polygonPool.Allocate();

            polygon.InitializeFromBodyVertices(
                bodyVertices,
                density,
                friction,
                restitution);
            return(polygon);
        }
Пример #11
0
        /// <summary>
        /// Creates a new polygon shape from body-space vertices.
        /// </summary>
        public VoltPolygon CreatePolygonBodySpace(
            TSVector2[] bodyVertices,
            FP?density     = null,
            FP?friction    = null,
            FP?restitution = null)
        {
            VoltPolygon polygon = (VoltPolygon)this.polygonPool.Allocate();

            polygon.InitializeFromBodyVertices(
                bodyVertices,
                density ?? VoltConfig.DEFAULT_DENSITY,
                friction ?? VoltConfig.DEFAULT_FRICTION,
                restitution ?? VoltConfig.DEFAULT_RESTITUTION);
            return(polygon);
        }
Пример #12
0
        internal void InitializeFromWorldVertices(
            Vector2[] vertices,
            float density,
            float friction,
            float restitution)
        {
            base.Initialize(density, friction, restitution);
            this.UpdateArrays(vertices.Length);

            this.countWorld = vertices.Length;
            Array.Copy(vertices, this.worldVertices, this.countWorld);
            VoltPolygon.ComputeAxes(vertices, this.countWorld, ref this.worldAxes);
            this.worldSpaceAABB =
                VoltPolygon.ComputeBounds(vertices, this.countWorld);

            this.countBody = 0; // Needs to be set on metric compute
        }
Пример #13
0
        private static Manifold Circle_Polygon(
            VoltWorld world,
            VoltCircle circ,
            VoltPolygon poly)
        {
            // Get the axis on the polygon closest to the circle's origin
            float penetration;
            int   index =
                Collision.FindAxisMaxPenetration(
                    circ.worldSpaceOrigin,
                    circ.radius,
                    poly,
                    out penetration);

            if (index < 0)
            {
                return(null);
            }

            Vector2 a, b;

            poly.GetEdge(index, out a, out b);
            Axis axis = poly.GetWorldAxis(index);

            // If the circle is past one of the two vertices, check it like
            // a circle-circle intersection where the vertex has radius 0
            float d = VoltMath.Cross(axis.Normal, circ.worldSpaceOrigin);

            if (d > VoltMath.Cross(axis.Normal, a))
            {
                return(Collision.TestCircles(world, circ, poly, a, 0.0f));
            }
            if (d < VoltMath.Cross(axis.Normal, b))
            {
                return(Collision.TestCircles(world, circ, poly, b, 0.0f));
            }

            // Build the collision Manifold
            Manifold manifold = world.AllocateManifold().Assign(world, circ, poly);
            Vector2  pos      =
                circ.worldSpaceOrigin - (circ.radius + penetration / 2) * axis.Normal;

            manifold.AddContact(pos, -axis.Normal, penetration);
            return(manifold);
        }
Пример #14
0
        internal void InitializeFromBodyVertices(
            Vector2[] vertices,
            float density,
            float friction,
            float restitution)
        {
            base.Initialize(density, friction, restitution);
            this.UpdateArrays(vertices.Length);

            // World vertices will be computed on position update
            this.countWorld = vertices.Length;

            this.countBody = vertices.Length;
            Array.Copy(vertices, this.bodyVertices, vertices.Length);
            VoltPolygon.ComputeAxes(vertices, this.countBody, ref this.bodyAxes);
            this.bodySpaceAABB =
                VoltPolygon.ComputeBounds(vertices, this.countBody);
        }
Пример #15
0
        /// <summary>
        /// Add contacts for penetrating vertices. Note that this does not handle
        /// cases where an overlap was detected, but no vertices fall inside the
        /// opposing polygon (like a Star of David). For this we have a fallback.
        ///
        /// See http://chipmunk-physics.googlecode.com/svn/trunk/src/cpCollision.c
        /// </summary>
        private static void FindVerts(
            VoltPolygon poly1,
            VoltPolygon poly2,
            Vector2 normal,
            float penetration,
            Manifold manifold)
        {
            bool found = false;

            for (int i = 0; i < poly1.countWorld; i++)
            {
                Vector2 vertex = poly1.worldVertices[i];
                if (poly2.ContainsPoint(vertex) == true)
                {
                    if (manifold.AddContact(vertex, normal, penetration) == false)
                    {
                        return;
                    }
                    found = true;
                }
            }

            for (int i = 0; i < poly2.countWorld; i++)
            {
                Vector2 vertex = poly2.worldVertices[i];
                if (poly1.ContainsPoint(vertex) == true)
                {
                    if (manifold.AddContact(vertex, normal, penetration) == false)
                    {
                        return;
                    }
                    found = true;
                }
            }

            // Fallback to check the degenerate "Star of David" case
            if (found == false)
            {
                FindVertsFallback(poly1, poly2, normal, penetration, manifold);
            }
        }
Пример #16
0
        protected override void ComputeMetrics()
        {
            // If we were initialized with world points, we need to compute body
            if (this.countBody == 0)
            {
                // Compute body-space geometry data (only need to do this once)
                VoltPolygon.WorldToBody(
                    this.Body,
                    this.worldVertices,
                    this.bodyVertices,
                    this.countWorld);
                this.countBody = this.countWorld;
                VoltPolygon.ComputeAxes(this.bodyVertices, this.countBody, ref this.bodyAxes);
                this.bodySpaceAABB =
                    VoltPolygon.ComputeBounds(this.bodyVertices, this.countBody);
            }

            this.Area    = this.ComputeArea();
            this.Mass    = this.Area * this.Density * VoltConfig.AreaMassRatio;
            this.Inertia = this.ComputeInertia();
        }
Пример #17
0
        /// <summary>
        /// Returns the index of the axis with the max circle penetration depth.
        /// Breaks out if a separating axis is found between the two shapes.
        /// Outputs the penetration depth of the circle in the axis (if any).
        /// </summary>
        internal static int FindAxisMaxPenetration(
      Vector2 origin,
      float radius,
      VoltPolygon poly,
      out float penetration)
        {
            int index = 0;
              int found = 0;
              penetration = float.NegativeInfinity;

              for (int i = 0; i < poly.countWorld; i++)
              {
            Axis axis = poly.worldAxes[i];
            float dot = Vector2.Dot(axis.Normal, origin);
            float dist = dot - axis.Width - radius;

            if (dist > 0)
              return -1;

            if (dist > penetration)
            {
              penetration = dist;
              found = index;
            }

            index++;
              }

              return found;
        }
Пример #18
0
        private static Manifold Circle_Polygon(
      VoltWorld world,
      VoltCircle circ,
      VoltPolygon poly)
        {
            // Get the axis on the polygon closest to the circle's origin
              float penetration;
              int index =
            Collision.FindAxisMaxPenetration(
              circ.worldSpaceOrigin,
              circ.radius,
              poly,
              out penetration);

              if (index < 0)
            return null;

              Vector2 a, b;
              poly.GetEdge(index, out a, out b);
              Axis axis = poly.GetWorldAxis(index);

              // If the circle is past one of the two vertices, check it like
              // a circle-circle intersection where the vertex has radius 0
              float d = VoltMath.Cross(axis.Normal, circ.worldSpaceOrigin);
              if (d > VoltMath.Cross(axis.Normal, a))
            return Collision.TestCircles(world, circ, poly, a, 0.0f);
              if (d < VoltMath.Cross(axis.Normal, b))
            return Collision.TestCircles(world, circ, poly, b, 0.0f);

              // Build the collision Manifold
              Manifold manifold = world.AllocateManifold().Assign(world, circ, poly);
              Vector2 pos =
            circ.worldSpaceOrigin - (circ.radius + penetration / 2) * axis.Normal;
              manifold.AddContact(pos, -axis.Normal, penetration);
              return manifold;
        }
Пример #19
0
        private static bool FindMinSepAxis(
      VoltPolygon poly1,
      VoltPolygon poly2,
      out Axis axis)
        {
            axis = new Axis(Vector2.zero, float.NegativeInfinity);

              for (int i = 0; i < poly1.countWorld; i++)
              {
            Axis a = poly1.worldAxes[i];
            float min = float.PositiveInfinity;
            for (int j = 0; j < poly2.countWorld; j++)
            {
              Vector2 v = poly2.worldVertices[j];
              min = Mathf.Min(min, Vector2.Dot(a.Normal, v));
            }
            min -= a.Width;

            if (min > 0)
              return false;
            if (min > axis.Width)
              axis = new Axis(a.Normal, min);
              }

              return true;
        }
Пример #20
0
        /// <summary>
        /// Add contacts for penetrating vertices. Note that this does not handle
        /// cases where an overlap was detected, but no vertices fall inside the
        /// opposing polygon (like a Star of David). For this we have a fallback.
        /// 
        /// See http://chipmunk-physics.googlecode.com/svn/trunk/src/cpCollision.c
        /// </summary>
        private static void FindVerts(
      VoltPolygon poly1,
      VoltPolygon poly2,
      Vector2 normal,
      float penetration,
      Manifold manifold)
        {
            bool found = false;

              for (int i = 0; i < poly1.countWorld; i++)
              {
            Vector2 vertex = poly1.worldVertices[i];
            if (poly2.ContainsPoint(vertex) == true)
            {
              if (manifold.AddContact(vertex, normal, penetration) == false)
            return;
              found = true;
            }
              }

              for (int i = 0; i < poly2.countWorld; i++)
              {
            Vector2 vertex = poly2.worldVertices[i];
            if (poly1.ContainsPoint(vertex) == true)
            {
              if (manifold.AddContact(vertex, normal, penetration) == false)
            return;
              found = true;
            }
              }

              // Fallback to check the degenerate "Star of David" case
              if (found == false)
            FindVertsFallback(poly1, poly2, normal, penetration, manifold);
        }
Пример #21
0
        /// <summary>
        /// A fallback for handling degenerate "Star of David" cases.
        /// </summary>
        private static void FindVertsFallback(
      VoltPolygon poly1,
      VoltPolygon poly2,
      Vector2 normal,
      float penetration,
      Manifold manifold)
        {
            for (int i = 0; i < poly1.countWorld; i++)
              {
            Vector2 vertex = poly1.worldVertices[i];
            if (poly2.ContainsPointPartial(vertex, normal) == true)
              if (manifold.AddContact(vertex, normal, penetration) == false)
            return;
              }

              for (int i = 0; i < poly2.countWorld; i++)
              {
            Vector2 vertex = poly2.worldVertices[i];
            if (poly1.ContainsPointPartial(vertex, -normal) == true)
              if (manifold.AddContact(vertex, normal, penetration) == false)
            return;
              }
        }
Пример #22
0
        private static Manifold Polygon_Polygon(
      VoltWorld world,
      VoltPolygon polyA,
      VoltPolygon polyB)
        {
            Axis a1, a2;
              if (Collision.FindMinSepAxis(polyA, polyB, out a1) == false)
            return null;
              if (Collision.FindMinSepAxis(polyB, polyA, out a2) == false)
            return null;

              // We will use poly1's axis, so we may need to swap
              if (a2.Width > a1.Width)
              {
            VoltUtil.Swap(ref polyA, ref polyB);
            VoltUtil.Swap(ref a1, ref a2);
              }

              // Build the collision Manifold
              Manifold manifold =
            world.AllocateManifold().Assign(world, polyA, polyB);
              Collision.FindVerts(polyA, polyB, a1.Normal, a1.Width, manifold);
              return manifold;
        }