C# Common.b2Transform类代码示例

 public static b2Transform Create()
 {
     var transform = new b2Transform();
     transform.p = b2Vec2.Zero;
     transform.q = b2Rot.Create();
     return transform;
 }

 public override void Evaluate(ref b2Manifold manifold, ref b2Transform xfA, ref b2Transform xfB)
 {
     b2ChainShape chain = (b2ChainShape)m_fixtureA.Shape;
     b2EdgeShape edge;
     edge = chain.GetChildEdge(m_indexA);
     b2Collision.b2CollideEdgeAndCircle(ref manifold, edge, ref xfA,
                             (b2CircleShape)m_fixtureB.Shape, ref xfB);
 }

 public override b2AABB ComputeAABB(b2Transform transform, int childIndex)
 {
     b2Vec2 p = transform.p + b2Math.b2Mul(transform.q, m_p);
     b2AABB aabb = new b2AABB();
     aabb.lowerBound.Set(p.x - m_radius, p.y - m_radius);
     aabb.upperBound.Set(p.x + m_radius, p.y + m_radius);
     return (aabb);
 }

        /*public b2Sweep()
        {
            localCenter = new b2Vec2();
            c0 = new b2Vec2();
            c = new b2Vec2();
            a0 = 0f;
            a = 0f;
            alpha0 = 0f;
        }*/
        /// Get the interpolated transform at a specific time.
        /// @param beta is a factor in [0,1], where 0 indicates alpha0.
        public void GetTransform(b2Transform xfb, float beta)
        {
            xfb.p = (1.0f - beta) * c0 + beta * c;
            float angle = (1.0f - beta) * a0 + beta * a;
            xfb.q.Set(angle);

            // Shift to origin
            xfb.p -= b2Math.b2Mul(xfb.q, localCenter);
        }

        public override bool TestPoint(ref b2Transform transform, b2Vec2 p)
        {
            b2Vec2 tx = b2Math.b2Mul(ref transform.q, ref Position);
            b2Vec2 center;
            center.x = transform.p.x + tx.x;
            center.y = transform.p.y + tx.y;
//            b2Vec2 center = transform.p + tx;
            b2Vec2 d; // = p - center;
            d.x = p.x - center.x;
            d.y = p.y - center.y;
            return d.LengthSquared <= Radius * Radius;
        }

        public override bool TestPoint(b2Transform transform, b2Vec2 p)
        {
            b2Vec2 tx = b2Math.b2Mul(transform.q, m_p);
            b2Vec2 center;
            center.x = transform.p.x + tx.x;
            center.y = transform.p.y + tx.y;
//            b2Vec2 center = transform.p + tx;
            b2Vec2 d; // = p - center;
            d.x = p.x - center.x;
            d.y = p.y - center.y;
            return d.LengthSquared <= m_radius * m_radius;
        }

        public override void DrawTransform(b2Transform xf)
        {
            float axisScale = 0.4f;
            b2Vec2 p1 = xf.p;

            b2Vec2 col1 = new b2Vec2(xf.q.c, xf.q.s);
            b2Vec2 col2 = new b2Vec2(-xf.q.s, xf.q.c);

            b2Vec2 p2 = p1 + axisScale * col1;
            DrawSegment(p1, p2, new b2Color(1f, 0f, 0f));

            p2 = p1 + axisScale * col2; 
            DrawSegment(p1, p2, new b2Color(0f, 0f, 1f));
        }

        // Collision Detection in Interactive 3D Environments by Gino van den Bergen
        // From Section 3.1.2
        // x = s + a * r
        // norm(x) = radius
        public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
                                    b2Transform transform, int childIndex)
        {
            output = b2RayCastOutput.Zero;

            b2Vec2 tx = b2Math.b2Mul(transform.q, m_p);
//            b2Vec2 position = transform.p + tx;
            b2Vec2 position;
            position.x = transform.p.x + tx.x;
            position.y = transform.p.y + tx.y;
//            b2Vec2 s = input.p1 - position;
            b2Vec2 s;
            s.x = input.p1.x - position.x;
            s.y = input.p1.y - position.y;
            float b = s.LengthSquared - m_radius * m_radius;

            // Solve quadratic equation.
            b2Vec2 r;
            r.x = input.p2.x - input.p1.x;
            r.y = input.p2.y - input.p1.y;
//            b2Vec2 r = input.p2 - input.p1;
            float c = s.x * r.x + s.y * r.y; // b2Math.b2Dot(ref s, ref r);
            float rr = r.LengthSquared; //  b2Math.b2Dot(r, r);
            float sigma = c * c - rr * b;

            // Check for negative discriminant and short segment.
            if (sigma < 0.0f || rr < b2Settings.b2_epsilon)
            {
                return false;
            }

            // Find the point of intersection of the line with the circle.
            float a = -(c + b2Math.b2Sqrt(sigma));

            // Is the intersection point on the segment?
            if (0.0f <= a && a <= input.maxFraction * rr)
            {
                a /= rr;
                output.fraction = a;
                output.normal.x = s.x + a * r.x;
                output.normal.y = s.y + a * r.y;
//                output.normal = s + a * r;
                output.normal.Normalize();
                return true;
            }

            return false;
        }

        /// Get the interpolated transform at a specific time.
        /// @param beta is a factor in [0,1], where 0 indicates alpha0.
        public void GetTransform(out b2Transform xfb, float beta)
        {
            float x = (1.0f - beta) * c0.x + beta * c.x;
            float y = (1.0f - beta) * c0.y + beta * c.y;

            float angle = (1.0f - beta) * a0 + beta * a;

            float sin = (float)Math.Sin(angle);
            float cos = (float)Math.Cos(angle);

            // Shift to origin
            xfb.p.x = x - (cos * localCenter.x - sin * localCenter.y);
            xfb.p.y = y - (sin * localCenter.x + cos * localCenter.y);
            
            xfb.q.s = sin;
            xfb.q.c = cos;
        }

        // Collision Detection in Interactive 3D Environments by Gino van den Bergen
        // From Section 3.1.2
        // x = s + a * r
        // norm(x) = radius
        public virtual bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
                                    b2Transform transform, int childIndex)
        {
            output = b2RayCastOutput.Zero;

            b2Vec2 position = transform.p + b2Math.b2Mul(transform.q, m_p);
            b2Vec2 s = input.p1 - position;
            float b = b2Math.b2Dot(s, s) - m_radius * m_radius;

            // Solve quadratic equation.
            b2Vec2 r = input.p2 - input.p1;
            float c = b2Math.b2Dot(s, r);
            float rr = b2Math.b2Dot(r, r);
            float sigma = c * c - rr * b;

            // Check for negative discriminant and short segment.
            if (sigma < 0.0f || rr < b2Settings.b2_epsilon)
            {
                return false;
            }

            // Find the point of intersection of the line with the circle.
            float a = -(c + b2Math.b2Sqrt(sigma));

            // Is the intersection point on the segment?
            if (0.0f <= a && a <= input.maxFraction * rr)
            {
                a /= rr;
                output.fraction = a;
                output.normal = s + a * r;
                output.normal.Normalize();
                return true;
            }

            return false;
        }

 public override bool TestPoint(b2Transform xf, b2Vec2 p)
 {
     return false;
 }

        public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
                                    b2Transform xf, int childIndex)
        {
            b2EdgeShape edgeShape = new b2EdgeShape();
            output = b2RayCastOutput.Zero;

            int i1 = childIndex;
            int i2 = childIndex + 1;
            if (i2 == m_count)
            {
                i2 = 0;
            }

            edgeShape.Vertex1 = m_vertices[i1];
            edgeShape.Vertex2 = m_vertices[i2];

            b2RayCastOutput co = b2RayCastOutput.Zero;
            bool b = edgeShape.RayCast(out co, input, xf, 0);
            output = co;
            return (b);
        }

        public override b2AABB ComputeAABB(b2Transform xf, int childIndex)
        {
            int i1 = childIndex;
            int i2 = childIndex + 1;
            if (i2 == m_count)
            {
                i2 = 0;
            }

            b2Vec2 v1 = b2Math.b2Mul(xf, m_vertices[i1]);
            b2Vec2 v2 = b2Math.b2Mul(xf, m_vertices[i2]);

            b2AABB aabb = b2AABB.Default;
            aabb.Set(b2Math.b2Min(v1, v2),b2Math.b2Max(v1, v2));
            return (aabb);
        }

        public static void b2FindIncidentEdge(b2ClipVertex[] c,
                                     b2PolygonShape poly1, b2Transform xf1, int edge1,
                                     b2PolygonShape poly2, b2Transform xf2)
        {
            b2Vec2[] normals1 = poly1.Normals;

            int count2 = poly2.VertexCount;
            b2Vec2[] vertices2 = poly2.Vertices;
            b2Vec2[] normals2 = poly2.Normals;

            // Get the normal of the reference edge in poly2's frame.
            b2Vec2 normal1 = b2Math.b2MulT(xf2.q, b2Math.b2Mul(xf1.q, normals1[edge1]));

            // Find the incident edge on poly2.
            int index = 0;
            float minDot = b2Settings.b2_maxFloat;
            for (int i = 0; i < count2; ++i)
            {
                float dot = b2Math.b2Dot(normal1, 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 = b2Math.b2Mul(xf2, vertices2[i1]);
            c[0].id.indexA = (byte)edge1;
            c[0].id.indexB = (byte)i1;
            c[0].id.typeA = b2ContactFeatureType.e_face;
            c[0].id.typeB = b2ContactFeatureType.e_vertex;

            c[1].v = b2Math.b2Mul(xf2, vertices2[i2]);
            c[1].id.indexA = (byte)edge1;
            c[1].id.indexB = (byte)i2;
            c[1].id.typeA = b2ContactFeatureType.e_face;
            c[1].id.typeB = b2ContactFeatureType.e_vertex;
        }

        // Find the max separation between poly1 and poly2 using edge normals from poly1.
        public static float b2FindMaxSeparation(out int edgeIndex,
                                         b2PolygonShape poly1, ref b2Transform xf1,
                                         b2PolygonShape poly2, ref b2Transform xf2)
        {
            int count1 = poly1.VertexCount;
            b2Vec2[] normals1 = poly1.Normals;

            // Vector pointing from the centroid of poly1 to the centroid of poly2.
            b2Vec2 d = b2Math.b2Mul(xf2, poly2.Centroid) - b2Math.b2Mul(xf1, poly1.Centroid);
            b2Vec2 dLocal1 = b2Math.b2MulT(xf1.q, d);

            // Find edge normal on poly1 that has the largest projection onto d.
            int edge = 0;
            float maxDot = -b2Settings.b2_maxFloat;
            for (int i = 0; i < count1; ++i)
            {
                float dot = b2Math.b2Dot(normals1[i], dLocal1);
                if (dot > maxDot)
                {
                    maxDot = dot;
                    edge = i;
                }
            }

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

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

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

            // 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 = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);

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

            edgeIndex = bestEdge;
            return bestSeparation;
        }