예제 #1
0
        static void FindIncidentEdge(ClipVertex[] c,
                                     Box2dShape poly1, ref IndexedMatrix xf1, int edge1,
                                     Box2dShape poly2, ref IndexedMatrix xf2)
        {
            IndexedVector3[] normals1 = poly1.GetNormals();

            int count2 = poly2.GetVertexCount();

            IndexedVector3[] vertices2 = poly2.GetVertices();
            IndexedVector3[] normals2  = poly2.GetNormals();

            Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount());

            // Get the normal of the reference edge in poly2's frame.
            IndexedVector3 normal1 = xf2._basis.Transpose() * (xf1._basis * normals1[edge1]);

            // Find the incident edge on poly2.
            int   index  = 0;
            float minDot = MathUtil.BT_LARGE_FLOAT;

            for (int i = 0; i < count2; ++i)
            {
                float dot = normal1.Dot(normals2[i]);
                if (dot < minDot)
                {
                    minDot = dot;
                    index  = i;
                }
            }

            // Build the clip vertices for the incident edge.
            int i1 = index;
            int i2 = i1 + 1 < count2 ? i1 + 1 : 0;

            c[0].v = xf2 * vertices2[i1];
            //	c[0].id.features.referenceEdge = (unsigned char)edge1;
            //	c[0].id.features.incidentEdge = (unsigned char)i1;
            //	c[0].id.features.incidentVertex = 0;

            c[1].v = xf2 * vertices2[i2];
            //	c[1].id.features.referenceEdge = (unsigned char)edge1;
            //	c[1].id.features.incidentEdge = (unsigned char)i2;
            //	c[1].id.features.incidentVertex = 1;
        }
예제 #2
0
        public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
            {
                return;
            }

            CollisionObject col0 = body0;
            CollisionObject col1 = body1;
            Box2dShape      box0 = (Box2dShape)col0.GetCollisionShape();
            Box2dShape      box1 = (Box2dShape)col1.GetCollisionShape();

            resultOut.SetPersistentManifold(m_manifoldPtr);

            B2CollidePolygons(ref resultOut, box0, col0.GetWorldTransform(), box1, col1.GetWorldTransform());

            //  refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
            if (m_ownManifold)
            {
                resultOut.RefreshContactPoints();
            }
        }
예제 #3
0
        // Find the separation between poly1 and poly2 for a give edge normal on poly1.
        static float EdgeSeparation(Box2dShape poly1, ref IndexedMatrix xf1, int edge1,
                                    Box2dShape poly2, ref IndexedMatrix xf2)
        {
            IndexedVector3[] vertices1 = poly1.GetVertices();
            IndexedVector3[] normals1  = poly1.GetNormals();

            int count2 = poly2.GetVertexCount();

            IndexedVector3[] vertices2 = poly2.GetVertices();

            Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount());

            // Convert normal from poly1's frame into poly2's frame.
            IndexedVector3 normal1World = xf1._basis * normals1[edge1];
            IndexedVector3 normal1      = xf1._basis.Transpose() * normal1World;

            // Find support vertex on poly2 for -normal.
            int   index  = 0;
            float minDot = MathUtil.BT_LARGE_FLOAT;

            for (int i = 0; i < count2; ++i)
            {
                float dot = vertices2[i].Dot(normal1);
                if (dot < minDot)
                {
                    minDot = dot;
                    index  = i;
                }
            }

            IndexedVector3 v1         = xf1 * vertices1[edge1];
            IndexedVector3 v2         = xf2 * vertices2[index];
            float          separation = (v2 - v1).Dot(normal1World);

            return(separation);
        }
예제 #4
0
        // Find the max separation between poly1 and poly2 using edge normals from poly1.
        static float FindMaxSeparation(ref int edgeIndex,
                                       Box2dShape poly1, ref IndexedMatrix xf1,
                                       Box2dShape poly2, ref IndexedMatrix xf2)
        {
            int count1 = poly1.GetVertexCount();

            IndexedVector3[] normals1 = poly1.GetNormals();

            // Vector pointing from the centroid of poly1 to the centroid of poly2.
            IndexedVector3 d       = xf2 * poly2.GetCentroid() - xf1 * poly1.GetCentroid();
            IndexedVector3 dLocal1 = xf1._basis.Transpose() * d;


            // Find edge normal on poly1 that has the largest projection onto d.
            int   edge   = 0;
            float maxDot = -MathUtil.BT_LARGE_FLOAT;

            for (int i = 0; i < count1; ++i)
            {
                float dot = normals1[i].Dot(ref dLocal1);
                if (dot > maxDot)
                {
                    maxDot = dot;
                    edge   = i;
                }
            }

            // Get the separation for the edge normal.
            float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);

            if (s > 0.0f)
            {
                return(s);
            }

            // Check the separation for the previous edge normal.
            int   prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
            float sPrev    = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2);

            if (sPrev > 0.0f)
            {
                return(sPrev);
            }

            // Check the separation for the next edge normal.
            int   nextEdge = edge + 1 < count1 ? edge + 1 : 0;
            float sNext    = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2);

            if (sNext > 0.0f)
            {
                return(sNext);
            }

            // Find the best edge and the search direction.
            int   bestEdge;
            float bestSeparation;
            int   increment;

            if (sPrev > s && sPrev > sNext)
            {
                increment      = -1;
                bestEdge       = prevEdge;
                bestSeparation = sPrev;
            }
            else if (sNext > s)
            {
                increment      = 1;
                bestEdge       = nextEdge;
                bestSeparation = sNext;
            }
            else
            {
                edgeIndex = edge;
                return(s);
            }

            // Perform a local search for the best edge normal.
            for ( ; ;)
            {
                if (increment == -1)
                {
                    edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
                }
                else
                {
                    edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
                }

                s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);
                if (s > 0.0f)
                {
                    return(s);
                }

                if (s > bestSeparation)
                {
                    bestEdge       = edge;
                    bestSeparation = s;
                }
                else
                {
                    break;
                }
            }

            edgeIndex = bestEdge;
            return(bestSeparation);
        }
예제 #5
0
        void B2CollidePolygons(ref ManifoldResult manifold,
                               Box2dShape polyA, ref IndexedMatrix xfA,
                               Box2dShape polyB, ref IndexedMatrix xfB)
        {
            int   edgeA       = 0;
            float separationA = FindMaxSeparation(ref edgeA, polyA, ref xfA, polyB, ref xfB);

            if (separationA > 0.0f)
            {
                return;
            }

            int   edgeB       = 0;
            float separationB = FindMaxSeparation(ref edgeB, polyB, ref xfB, polyA, ref xfA);

            if (separationB > 0.0f)
            {
                return;
            }

            Box2dShape    poly1;                // reference poly
            Box2dShape    poly2;                // incident poly
            IndexedMatrix xf1, xf2;
            int           edge1;                // reference edge
            bool          flip;
            const float   k_relativeTol = 0.98f;
            const float   k_absoluteTol = 0.001f;

            // TODO_ERIN use "radius" of poly for absolute tolerance.
            if (separationB > k_relativeTol * separationA + k_absoluteTol)
            {
                poly1 = polyB;
                poly2 = polyA;
                xf1   = xfB;
                xf2   = xfA;
                edge1 = edgeB;
                flip  = true;
            }
            else
            {
                poly1 = polyA;
                poly2 = polyB;
                xf1   = xfA;
                xf2   = xfB;
                edge1 = edgeA;
                flip  = false;
            }

            ClipVertex[] incidentEdge = new ClipVertex[2];
            FindIncidentEdge(incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);

            int count1 = poly1.GetVertexCount();

            IndexedVector3[] vertices1 = poly1.GetVertices();

            IndexedVector3 v11 = vertices1[edge1];
            IndexedVector3 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];

            IndexedVector3 dv         = v12 - v11;
            IndexedVector3 sideNormal = xf1._basis * (v12 - v11);

            sideNormal.Normalize();
            IndexedVector3 frontNormal = CrossS(ref sideNormal, 1.0f);

            v11 = xf1 * v11;
            v12 = xf1 * v12;

            float frontOffset = frontNormal.Dot(ref v11);
            float sideOffset1 = -(sideNormal.Dot(ref v11));
            float sideOffset2 = sideNormal.Dot(ref v12);

            // Clip incident edge against extruded edge1 side edges.
            ClipVertex[] clipPoints1 = new ClipVertex[2];
            clipPoints1[0].v = IndexedVector3.Zero;
            clipPoints1[1].v = IndexedVector3.Zero;

            ClipVertex[] clipPoints2 = new ClipVertex[2];
            clipPoints2[0].v = IndexedVector3.Zero;
            clipPoints2[1].v = IndexedVector3.Zero;


            int np;

            // Clip to box side 1
            np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);

            if (np < 2)
            {
                return;
            }

            // Clip to negative box side 1
            np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);

            if (np < 2)
            {
                return;
            }

            // Now clipPoints2 contains the clipped points.
            IndexedVector3 manifoldNormal = flip ? -frontNormal : frontNormal;

            int pointCount = 0;

            for (int i = 0; i < b2_maxManifoldPoints; ++i)
            {
                float separation = frontNormal.Dot(clipPoints2[i].v) - frontOffset;

                if (separation <= 0.0f)
                {
                    //b2ManifoldPoint* cp = manifold.points + pointCount;
                    //float separation = separation;
                    //cp.localPoint1 = b2MulT(xfA, clipPoints2[i].v);
                    //cp.localPoint2 = b2MulT(xfB, clipPoints2[i].v);

                    manifold.AddContactPoint(-manifoldNormal, clipPoints2[i].v, separation);

                    //			cp.id = clipPoints2[i].id;
                    //			cp.id.features.flip = flip;
                    ++pointCount;
                }
            }

            //	manifold.pointCount = pointCount;}
        }
예제 #6
0
        // Find edge normal of max separation on A - return if separating axis is found
        // Find edge normal of max separation on B - return if separation axis is found
        // Choose reference edge as min(minA, minB)
        // Find incident edge
        // Clip

        // The normal points from 1 to 2

        void B2CollidePolygons(ref ManifoldResult manifold,
                               Box2dShape polyA, IndexedMatrix xfA,
                               Box2dShape polyB, IndexedMatrix xfB)
        {
            B2CollidePolygons(ref manifold, polyA, ref xfA, polyB, ref xfB);
        }