A circle shape.
Inheritance: Shape
Exemplo n.º 1
0
        // Compute contact points for edge versus circle.
        // This accounts for edge connectivity.
        public virtual void collideEdgeAndCircle(Manifold manifold, EdgeShape edgeA, Transform xfA, CircleShape circleB, Transform xfB)
        {
            manifold.pointCount = 0;

            // Compute circle in frame of edge
            // Vec2 Q = MulT(xfA, Mul(xfB, circleB.m_p));
            Transform.mulToOutUnsafe(xfB, circleB.m_p, temp);
            Transform.mulTransToOutUnsafe(xfA, temp, Q);

            Vec2 A = edgeA.m_vertex1;
            Vec2 B = edgeA.m_vertex2;
            e.set_Renamed(B).subLocal(A);

            // Barycentric coordinates
            float u = Vec2.dot(e, temp.set_Renamed(B).subLocal(Q));
            float v = Vec2.dot(e, temp.set_Renamed(Q).subLocal(A));

            float radius = edgeA.m_radius + circleB.m_radius;

            // ContactFeature cf;
            cf.indexB = 0;
            cf.typeB = (sbyte)ContactID.Type.VERTEX;

            // Region A
            if (v <= 0.0f)
            {
                Vec2 _P = A;
                d.set_Renamed(Q).subLocal(_P);
                float dd = Vec2.dot(d, d);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to A?
                if (edgeA.m_hasVertex0)
                {
                    Vec2 A1 = edgeA.m_vertex0;
                    Vec2 B1 = A;
                    e1.set_Renamed(B1).subLocal(A1);
                    float u1 = Vec2.dot(e1, temp.set_Renamed(B1).subLocal(Q));

                    // Is the circle in Region AB of the previous edge?
                    if (u1 > 0.0f)
                    {
                        return;
                    }
                }

                cf.indexA = 0;
                cf.typeA = (sbyte)ContactID.Type.VERTEX;
                manifold.pointCount = 1;
                manifold.type = Manifold.ManifoldType.CIRCLES;
                manifold.localNormal.setZero();
                manifold.localPoint.set_Renamed(_P);
                // manifold.points[0].id.key = 0;
                manifold.points[0].id.set_Renamed(cf);
                manifold.points[0].localPoint.set_Renamed(circleB.m_p);
                return;
            }

            // Region B
            if (u <= 0.0f)
            {
                Vec2 _P = B;
                d.set_Renamed(Q).subLocal(_P);
                float dd = Vec2.dot(d, d);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to B?
                if (edgeA.m_hasVertex3)
                {
                    Vec2 B2 = edgeA.m_vertex3;
                    Vec2 A2 = B;
                    Vec2 e2 = e1;
                    e2.set_Renamed(B2).subLocal(A2);
                    float v2 = Vec2.dot(e2, temp.set_Renamed(Q).subLocal(A2));

                    // Is the circle in Region AB of the next edge?
                    if (v2 > 0.0f)
                    {
                        return;
                    }
                }

                cf.indexA = 1;
                cf.typeA = (sbyte)ContactID.Type.VERTEX;
                manifold.pointCount = 1;
                manifold.type = Manifold.ManifoldType.CIRCLES;
                manifold.localNormal.setZero();
                manifold.localPoint.set_Renamed(_P);
                // manifold.points[0].id.key = 0;
                manifold.points[0].id.set_Renamed(cf);
                manifold.points[0].localPoint.set_Renamed(circleB.m_p);
                return;
            }

            // Region AB
            float den = Vec2.dot(e, e);
            Debug.Assert(den > 0.0f);

            // Vec2 P = (1.0f / den) * (u * A + v * B);
            P.set_Renamed(A).mulLocal(u).addLocal(temp.set_Renamed(B).mulLocal(v));
            P.mulLocal(1.0f / den);
            d.set_Renamed(Q).subLocal(P);
            float dd2 = Vec2.dot(d, d);
            if (dd2 > radius * radius)
            {
                return;
            }

            n.x = -e.y;
            n.y = e.x;
            if (Vec2.dot(n, temp.set_Renamed(Q).subLocal(A)) < 0.0f)
            {
                n.set_Renamed(-n.x, -n.y);
            }
            n.normalize();

            cf.indexA = 0;
            cf.typeA = (sbyte)ContactID.Type.FACE;
            manifold.pointCount = 1;
            manifold.type = Manifold.ManifoldType.FACE_A;
            manifold.localNormal.set_Renamed(n);
            manifold.localPoint.set_Renamed(A);
            // manifold.points[0].id.key = 0;
            manifold.points[0].id.set_Renamed(cf);
            manifold.points[0].localPoint.set_Renamed(circleB.m_p);
        }
Exemplo n.º 2
0
        /// <summary>
        /// Compute the collision manifold between a polygon and a circle.
        /// </summary>
        /// <param name="manifold"></param>
        /// <param name="polygon"></param>
        /// <param name="xfA"></param>
        /// <param name="circle"></param>
        /// <param name="xfB"></param>
        public void collidePolygonAndCircle(Manifold manifold, PolygonShape polygon, Transform xfA, CircleShape circle, Transform xfB)
        {
            manifold.pointCount = 0;
            //Vec2 v = circle.m_p;

            // Compute circle position in the frame of the polygon.
            // before inline:
            Transform.mulToOut(xfB, circle.m_p, c);
            Transform.mulTransToOut(xfA, c, cLocal);

            float cLocalx = cLocal.x;
            float cLocaly = cLocal.y;
            // after inline:
            // final float cy = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y;
            // final float cx = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y;
            // final float v1x = cx - xfA.p.x;
            // final float v1y = cy - xfA.p.y;
            // final Vec2 b = xfA.q.ex;
            // final Vec2 b1 = xfA.q.ey;
            // final float cLocaly = v1x * b1.x + v1y * b1.y;
            // final float cLocalx = v1x * b.x + v1y * b.y;
            // end inline

            // Find the min separating edge.
            int normalIndex = 0;
            //UPGRADE_TODO: The equivalent in .NET for field 'java.lang.Float.MIN_VALUE' may return a different value. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1043'"
            float separation = Single.Epsilon;
            float radius = polygon.m_radius + circle.m_radius;
            int vertexCount = polygon.m_count;

            Vec2[] vertices = polygon.m_vertices;
            Vec2[] normals = polygon.m_normals;

            for (int i = 0; i < vertexCount; i++)
            {
                // before inline
                // temp.set(cLocal).subLocal(vertices[i]);
                // float s = Vec2.dot(normals[i], temp);
                // after inline
                Vec2 vertex = vertices[i];
                float tempx = cLocalx - vertex.x;
                float tempy = cLocaly - vertex.y;
                Vec2 normal = normals[i];
                float s = normal.x * tempx + normal.y * tempy;

                if (s > radius)
                {
                    // early out
                    return;
                }

                if (s > separation)
                {
                    separation = s;
                    normalIndex = i;
                }
            }

            // Vertices that subtend the incident face.
            int vertIndex1 = normalIndex;
            int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
            Vec2 v1 = vertices[vertIndex1];
            Vec2 v2 = vertices[vertIndex2];

            // If the center is inside the polygon ...
            if (separation < Settings.EPSILON)
            {
                manifold.pointCount = 1;
                manifold.type = Manifold.ManifoldType.FACE_A;

                // before inline:
                // manifold.localNormal.set(normals[normalIndex]);
                // manifold.localPoint.set(v1).addLocal(v2).mulLocal(.5f);
                // manifold.points[0].localPoint.set(circle.m_p);
                // after inline:
                Vec2 normal = normals[normalIndex];
                manifold.localNormal.x = normal.x;
                manifold.localNormal.y = normal.y;
                manifold.localPoint.x = (v1.x + v2.x) * .5f;
                manifold.localPoint.y = (v1.y + v2.y) * .5f;
                ManifoldPoint mpoint = manifold.points[0];
                mpoint.localPoint.x = circle.m_p.x;
                mpoint.localPoint.y = circle.m_p.y;
                mpoint.id.zero();
                // end inline
                return;
            }

            // Compute barycentric coordinates
            // before inline:
            // temp.set(cLocal).subLocal(v1);
            // temp2.set(v2).subLocal(v1);
            // float u1 = Vec2.dot(temp, temp2);
            // temp.set(cLocal).subLocal(v2);
            // temp2.set(v1).subLocal(v2);
            // float u2 = Vec2.dot(temp, temp2);
            // after inline:
            float tempX = cLocalx - v1.x;
            float tempY = cLocaly - v1.y;
            float temp2X = v2.x - v1.x;
            float temp2Y = v2.y - v1.y;
            float u1 = tempX * temp2X + tempY * temp2Y;

            float temp3X = cLocalx - v2.x;
            float temp3Y = cLocaly - v2.y;
            float temp4X = v1.x - v2.x;
            float temp4Y = v1.y - v2.y;
            float u2 = temp3X * temp4X + temp3Y * temp4Y;
            // end inline

            if (u1 <= 0f)
            {
                // inlined
                float dx = cLocalx - v1.x;
                float dy = cLocaly - v1.y;
                if (dx * dx + dy * dy > radius * radius)
                {
                    return;
                }

                manifold.pointCount = 1;
                manifold.type = Manifold.ManifoldType.FACE_A;
                // before inline:
                // manifold.localNormal.set(cLocal).subLocal(v1);
                // after inline:
                manifold.localNormal.x = cLocalx - v1.x;
                manifold.localNormal.y = cLocaly - v1.y;
                // end inline
                manifold.localNormal.normalize();
                manifold.localPoint.set_Renamed(v1);
                manifold.points[0].localPoint.set_Renamed(circle.m_p);
                manifold.points[0].id.zero();
            }
            else if (u2 <= 0.0f)
            {
                // inlined
                float dx = cLocalx - v2.x;
                float dy = cLocaly - v2.y;
                if (dx * dx + dy * dy > radius * radius)
                {
                    return;
                }

                manifold.pointCount = 1;
                manifold.type = Manifold.ManifoldType.FACE_A;
                // before inline:
                // manifold.localNormal.set(cLocal).subLocal(v2);
                // after inline:
                manifold.localNormal.x = cLocalx - v2.x;
                manifold.localNormal.y = cLocaly - v2.y;
                // end inline
                manifold.localNormal.normalize();
                manifold.localPoint.set_Renamed(v2);
                manifold.points[0].localPoint.set_Renamed(circle.m_p);
                manifold.points[0].id.zero();
            }
            else
            {
                // Vec2 faceCenter = 0.5f * (v1 + v2);
                // (temp is faceCenter)
                // before inline:
                // temp.set(v1).addLocal(v2).mulLocal(.5f);
                //
                // temp2.set(cLocal).subLocal(temp);
                // separation = Vec2.dot(temp2, normals[vertIndex1]);
                // if (separation > radius) {
                // return;
                // }
                // after inline:
                float fcx = (v1.x + v2.x) * .5f;
                float fcy = (v1.y + v2.y) * .5f;

                float tx = cLocalx - fcx;
                float ty = cLocaly - fcy;
                Vec2 normal = normals[vertIndex1];
                separation = tx * normal.x + ty * normal.y;
                if (separation > radius)
                {
                    return;
                }
                // end inline

                manifold.pointCount = 1;
                manifold.type = Manifold.ManifoldType.FACE_A;
                manifold.localNormal.set_Renamed(normals[vertIndex1]);
                manifold.localPoint.x = fcx; // (faceCenter)
                manifold.localPoint.y = fcy;
                manifold.points[0].localPoint.set_Renamed(circle.m_p);
                manifold.points[0].id.zero();
            }
        }
Exemplo n.º 3
0
        /// <summary>
        /// Compute the collision manifold between two circles.
        /// </summary>
        /// <param name="manifold"></param>
        /// <param name="circle1"></param>
        /// <param name="xfA"></param>
        /// <param name="circle2"></param>
        /// <param name="xfB"></param>
        public void collideCircles(Manifold manifold, CircleShape circle1, Transform xfA, CircleShape circle2, Transform xfB)
        {
            manifold.pointCount = 0;

            // before inline:
            Transform.mulToOut(xfA, circle1.m_p, pA);
            Transform.mulToOut(xfB, circle2.m_p, pB);
            d.set_Renamed(pB).subLocal(pA);
            float distSqr = d.x * d.x + d.y * d.y;

            // after inline:
            // final Vec2 v = circle1.m_p;
            // final float pAy = xfA.p.y + xfA.q.ex.y * v.x + xfA.q.ey.y * v.y;
            // final float pAx = xfA.p.x + xfA.q.ex.x * v.x + xfA.q.ey.x * v.y;
            //
            // final Vec2 v1 = circle2.m_p;
            // final float pBy = xfB.p.y + xfB.q.ex.y * v1.x + xfB.q.ey.y * v1.y;
            // final float pBx = xfB.p.x + xfB.q.ex.x * v1.x + xfB.q.ey.x * v1.y;
            //
            // final float dx = pBx - pAx;
            // final float dy = pBy - pAy;
            //
            // final float distSqr = dx * dx + dy * dy;
            // end inline

            float radius = circle1.m_radius + circle2.m_radius;
            if (distSqr > radius * radius)
            {
                return;
            }

            manifold.type = Manifold.ManifoldType.CIRCLES;
            manifold.localPoint.set_Renamed(circle1.m_p);
            manifold.localNormal.setZero();
            manifold.pointCount = 1;

            manifold.points[0].localPoint.set_Renamed(circle2.m_p);
            manifold.points[0].id.zero();
        }
Exemplo n.º 4
0
 public override Shape Clone()
 {
     CircleShape shape = new CircleShape();
     shape.m_p.set_Renamed(m_p);
     shape.m_radius = m_radius;
     return shape;
 }
Exemplo n.º 5
0
 public override Shape Clone()
 {
     CircleShape shape = new CircleShape();
     shape.P.Set(P);
     shape.Radius = Radius;
     return shape;
 }
Exemplo n.º 6
0
        // Compute contact points for edge versus circle.
        // This accounts for edge connectivity.
        public void CollideEdgeAndCircle(Manifold manifold, EdgeShape edgeA, Transform xfA, CircleShape circleB, Transform xfB)
        {
            manifold.PointCount = 0;

            // Compute circle in frame of edge
            // Vec2 Q = MulT(xfA, Mul(xfB, circleB.m_p));
            Transform.MulToOutUnsafe(xfB, circleB.P, temp);
            Transform.MulTransToOutUnsafe(xfA, temp, q);

            Vec2 A = edgeA.Vertex1;
            Vec2 B = edgeA.Vertex2;
            e.Set(B).SubLocal(A);

            // Barycentric coordinates
            float u = Vec2.Dot(e, temp.Set(B).SubLocal(q));
            float v = Vec2.Dot(e, temp.Set(q).SubLocal(A));

            float radius = edgeA.Radius + circleB.Radius;

            // ContactFeature cf;
            cf.IndexB = 0;
            cf.TypeB = (sbyte)ContactID.Type.Vertex;

            // Region A
            if (v <= 0.0f)
            {
                Vec2 P = A;
                D.Set(q).SubLocal(P);
                float dd = Vec2.Dot(D, D);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to A?
                if (edgeA.HasVertex0)
                {
                    Vec2 A1 = edgeA.Vertex0;
                    Vec2 B1 = A;
                    e1.Set(B1).SubLocal(A1);
                    float u1 = Vec2.Dot(e1, temp.Set(B1).SubLocal(q));

                    // Is the circle in Region AB of the previous edge?
                    if (u1 > 0.0f)
                    {
                        return;
                    }
                }

                cf.IndexA = 0;
                cf.TypeA = (sbyte)ContactID.Type.Vertex;
                manifold.PointCount = 1;
                manifold.Type = Manifold.ManifoldType.Circles;
                manifold.LocalNormal.SetZero();
                manifold.LocalPoint.Set(P);
                // manifold.points[0].id.key = 0;
                manifold.Points[0].Id.Set(cf);
                manifold.Points[0].LocalPoint.Set(circleB.P);
                return;
            }

            // Region B
            if (u <= 0.0f)
            {
                Vec2 P = B;
                D.Set(q).SubLocal(P);
                float dd = Vec2.Dot(D, D);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to B?
                if (edgeA.HasVertex3)
                {
                    Vec2 B2 = edgeA.Vertex3;
                    Vec2 A2 = B;
                    Vec2 e2 = e1;
                    e2.Set(B2).SubLocal(A2);
                    float v2 = Vec2.Dot(e2, temp.Set(q).SubLocal(A2));

                    // Is the circle in Region AB of the next edge?
                    if (v2 > 0.0f)
                    {
                        return;
                    }
                }

                cf.IndexA = 1;
                cf.TypeA = (sbyte)ContactID.Type.Vertex;
                manifold.PointCount = 1;
                manifold.Type = Manifold.ManifoldType.Circles;
                manifold.LocalNormal.SetZero();
                manifold.LocalPoint.Set(P);
                // manifold.points[0].id.key = 0;
                manifold.Points[0].Id.Set(cf);
                manifold.Points[0].LocalPoint.Set(circleB.P);
                return;
            }

            // Region AB
            float den = Vec2.Dot(e, e);
            Debug.Assert(den > 0.0f);

            // Vec2 P = (1.0f / den) * (u * A + v * B);
            p.Set(A).MulLocal(u).AddLocal(temp.Set(B).MulLocal(v));
            p.MulLocal(1.0f / den);
            D.Set(q).SubLocal(p);
            float dd2 = Vec2.Dot(D, D);
            if (dd2 > radius * radius)
            {
                return;
            }

            n.X = -e.Y;
            n.Y = e.X;
            if (Vec2.Dot(n, temp.Set(q).SubLocal(A)) < 0.0f)
            {
                n.Set(-n.X, -n.Y);
            }
            n.Normalize();

            cf.IndexA = 0;
            cf.TypeA = (sbyte)ContactID.Type.Face;
            manifold.PointCount = 1;
            manifold.Type = Manifold.ManifoldType.FaceA;
            manifold.LocalNormal.Set(n);
            manifold.LocalPoint.Set(A);
            // manifold.points[0].id.key = 0;
            manifold.Points[0].Id.Set(cf);
            manifold.Points[0].LocalPoint.Set(circleB.P);
        }
Exemplo n.º 7
0
        /// <summary>
        /// Compute the collision manifold between two circles.
        /// </summary>
        /// <param name="manifold"></param>
        /// <param name="circle1"></param>
        /// <param name="xfA"></param>
        /// <param name="circle2"></param>
        /// <param name="xfB"></param>
        public void CollideCircles(Manifold manifold, CircleShape circle1, Transform xfA, CircleShape circle2, Transform xfB)
        {
            manifold.PointCount = 0;

            // before inline:
            Transform.MulToOut(xfA, circle1.P, P_A);
            Transform.MulToOut(xfB, circle2.P, P_B);
            D.Set(P_B).SubLocal(P_A);
            float distSqr = D.X * D.X + D.Y * D.Y;

            // after inline:
            // final Vec2 v = circle1.m_p;
            // final float pAy = xfA.p.y + xfA.q.ex.y * v.x + xfA.q.ey.y * v.y;
            // final float pAx = xfA.p.x + xfA.q.ex.x * v.x + xfA.q.ey.x * v.y;
            //
            // final Vec2 v1 = circle2.m_p;
            // final float pBy = xfB.p.y + xfB.q.ex.y * v1.x + xfB.q.ey.y * v1.y;
            // final float pBx = xfB.p.x + xfB.q.ex.x * v1.x + xfB.q.ey.x * v1.y;
            //
            // final float dx = pBx - pAx;
            // final float dy = pBy - pAy;
            //
            // final float distSqr = dx * dx + dy * dy;
            // end inline

            float radius = circle1.Radius + circle2.Radius;
            if (distSqr > radius * radius)
            {
                return;
            }

            manifold.Type = Manifold.ManifoldType.Circles;
            manifold.LocalPoint.Set(circle1.P);
            manifold.LocalNormal.SetZero();
            manifold.PointCount = 1;

            manifold.Points[0].LocalPoint.Set(circle2.P);
            manifold.Points[0].Id.Zero();
        }