コード例 #1
0
        public override void HandleCollision(Manifold m, Body a, Body b)
        {
            Circle  A = (Circle)a.shape;
            Polygon B = (Polygon)b.shape;

            m.contactCount = 0;

            // Transform circle center to Polygon model space
            // Vec2 center = a->position;
            // center = B->u.Transpose( ) * (center - b->position);
            Vec2 center = B.u.Transpose().Muli(a.position.Sub(b.position));

            // Find edge with minimum penetration
            // Exact concept as using support points in Polygon vs Polygon
            float separation = -float.MaxValue;
            int   faceNormal = 0;

            for (int i = 0; i < B.vertexCount; ++i)
            {
                // real s = Dot( B->m_normals[i], center - B->m_vertices[i] );
                float s = Vec2.Dot(B.normals[i], center.Sub(B.vertices[i]));

                if (s > A.radius)
                {
                    return;
                }

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

            // Grab face's vertices
            Vec2 v1 = B.vertices[faceNormal];
            int  i2 = faceNormal + 1 < B.vertexCount ? faceNormal + 1 : 0;
            Vec2 v2 = B.vertices[i2];

            // Check to see if center is within polygon
            if (separation < ImpulseMath.EPSILON)
            {
                // m->contact_count = 1;
                // m->normal = -(B->u * B->m_normals[faceNormal]);
                // m->contacts[0] = m->normal * A->radius + a->position;
                // m->penetration = A->radius;

                m.contactCount = 1;
                B.u.Mul(B.normals[faceNormal], m.normal).Negi();
                m.contacts[0].Set(m.normal).Muli(A.radius).Addi(a.position);
                m.penetration = A.radius;
                return;
            }

            // Determine which voronoi region of the edge center of circle lies within
            // real dot1 = Dot( center - v1, v2 - v1 );
            // real dot2 = Dot( center - v2, v1 - v2 );
            // m->penetration = A->radius - separation;
            float dot1 = Vec2.Dot(center.Sub(v1), v2.Sub(v1));
            float dot2 = Vec2.Dot(center.Sub(v2), v1.Sub(v2));

            m.penetration = A.radius - separation;

            // Closest to v1
            if (dot1 <= 0.0f)
            {
                if (Vec2.DistanceSq(center, v1) > A.radius * A.radius)
                {
                    return;
                }

                // m->contact_count = 1;
                // Vec2 n = v1 - center;
                // n = B->u * n;
                // n.Normalize( );
                // m->normal = n;
                // v1 = B->u * v1 + b->position;
                // m->contacts[0] = v1;

                m.contactCount = 1;
                B.u.Muli(m.normal.Set(v1).Subi(center)).Normalize();
                B.u.Mul(v1, m.contacts[0]).Addi(b.position);
            }

            // Closest to v2
            else if (dot2 <= 0.0f)
            {
                if (Vec2.DistanceSq(center, v2) > A.radius * A.radius)
                {
                    return;
                }

                // m->contact_count = 1;
                // Vec2 n = v2 - center;
                // v2 = B->u * v2 + b->position;
                // m->contacts[0] = v2;
                // n = B->u * n;
                // n.Normalize( );
                // m->normal = n;

                m.contactCount = 1;
                B.u.Muli(m.normal.Set(v2).Subi(center)).Normalize();
                B.u.Mul(v2, m.contacts[0]).Addi(b.position);
            }

            // Closest to face
            else
            {
                Vec2 n = B.normals[faceNormal];

                if (Vec2.Dot(center.Sub(v1), n) > A.radius)
                {
                    return;
                }

                // n = B->u * n;
                // m->normal = -n;
                // m->contacts[0] = m->normal * A->radius + a->position;
                // m->contact_count = 1;

                m.contactCount = 1;
                B.u.Mul(n, m.normal).Negi();
                m.contacts[0].Set(a.position).Addsi(m.normal, A.radius);
            }
        }