Exemplo n.º 1
0
        internal bool Synchronize(BroadPhase broadPhase, Transform Transform1, Transform Transform2)
        {
            if (_proxyId == PairManager.NullProxy)
            {
                return(false);
            }

            // Compute an AABB that covers the swept shape (may miss some rotation effect).
            AABB aabb1, aabb2;

            _shape.ComputeAABB(out aabb1, Transform1);
            _shape.ComputeAABB(out aabb2, Transform2);

            AABB aabb = new AABB();

            aabb.Combine(aabb1, aabb2);

            if (broadPhase.InRange(aabb))
            {
                broadPhase.MoveProxy(_proxyId, aabb);
                return(true);
            }
            else
            {
                return(false);
            }
        }
Exemplo n.º 2
0
        public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c)
        {
            Vector2 p = xf.TransformPoint(_position);
            float   l = -(Vector2.Dot(normal, p) - offset);

            if (l < -_radius + Box2DNet.Common.Settings.FLT_EPSILON)
            {
                //Completely dry
                c = new Vector2();
                return(0);
            }
            if (l > _radius)
            {
                //Completely wet
                c = p;
                return(Box2DNet.Common.Settings.Pi * _radius * _radius);
            }

            //Magic
            float r2   = _radius * _radius;
            float l2   = l * l;
            float area = r2 * ((float)System.Math.Asin(l / _radius) + Box2DNet.Common.Settings.Pi / 2) +
                         l * Box2DNet.Common.Math.Sqrt(r2 - l2);
            float com = -2.0f / 3.0f * (float)System.Math.Pow(r2 - l2, 1.5f) / area;

            c.X = p.X + normal.X * com;
            c.Y = p.Y + normal.Y * com;

            return(area);
        }
Exemplo n.º 3
0
        public override bool TestPoint(Transform xf, Vector2 p)
        {
            Vector2 center = xf.position + xf.TransformDirection(_position);
            Vector2 d      = p - center;

            return(Vector2.Dot(d, d) <= _radius * _radius);
        }
        internal void Initialize(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB)
        {
            ShapeA = shapeA;
            ShapeB = shapeB;
            int count = cache.Count;

            if (count == 1)
            {
                FaceType = Type.Points;
                Vector2 localPointA = ShapeA.GetVertex(cache.IndexA[0]);
                Vector2 localPointB = ShapeB.GetVertex(cache.IndexB[0]);
                Vector2 pointA      = transformA.TransformPoint(localPointA);
                Vector2 pointB      = transformB.TransformPoint(localPointB);
                Axis = pointB - pointA;
                Axis.Normalize();
            }
            else if (cache.IndexB[0] == cache.IndexB[1])
            {
                // Two points on A and one on B
                FaceType = Type.FaceA;
                Vector2 localPointA1 = ShapeA.GetVertex(cache.IndexA[0]);
                Vector2 localPointA2 = ShapeA.GetVertex(cache.IndexA[1]);
                Vector2 localPointB  = ShapeB.GetVertex(cache.IndexB[0]);
                LocalPoint = 0.5f * (localPointA1 + localPointA2);
                Axis       = (localPointA2 - localPointA1).CrossScalarPostMultiply(1.0f);
                Axis.Normalize();

                Vector2 normal = transformA.TransformDirection(Axis);
                Vector2 pointA = transformA.TransformPoint(LocalPoint);
                Vector2 pointB = transformB.TransformPoint(localPointB);

                float s = Vector2.Dot(pointB - pointA, normal);
                if (s < 0.0f)
                {
                    Axis = -Axis;
                }
            }
            else
            {
                // Two points on B and one or two points on A.
                // We ignore the second point on A.
                FaceType = Type.FaceB;
                Vector2 localPointA  = shapeA.GetVertex(cache.IndexA[0]);
                Vector2 localPointB1 = shapeB.GetVertex(cache.IndexB[0]);
                Vector2 localPointB2 = shapeB.GetVertex(cache.IndexB[1]);
                LocalPoint = 0.5f * (localPointB1 + localPointB2);
                Axis       = (localPointB2 - localPointB1).CrossScalarPostMultiply(1.0f);
                Axis.Normalize();

                Vector2 normal = transformB.TransformDirection(Axis);
                Vector2 pointB = transformB.TransformPoint(LocalPoint);
                Vector2 pointA = transformA.TransformPoint(localPointA);

                float s = Vector2.Dot(pointA - pointB, normal);
                if (s < 0.0f)
                {
                    Axis = -Axis;
                }
            }
        }
Exemplo n.º 5
0
        internal bool SynchronizeFixtures()
        {
            Transform xf1 = new Transform();

            xf1.rotation = Box2DNet.Common.Math.AngleToRotation(_sweep.A0);
            //xf1.R = new Mat22(_sweep.A0);
            xf1.position = _sweep.C0 - xf1.TransformDirection(_sweep.LocalCenter);

            bool inRange = true;

            for (Fixture f = _fixtureList; f != null; f = f.Next)
            {
                inRange = f.Synchronize(_world._broadPhase, xf1, _xf);
                if (inRange == false)
                {
                    break;
                }
            }

            if (inRange == false)
            {
                _flags          |= BodyFlags.Frozen;
                _linearVelocity  = Vector2.Zero;
                _angularVelocity = 0.0f;

                // Failure
                return(false);
            }

            // Success
            return(true);
        }
Exemplo n.º 6
0
        public override void ComputeAABB(out AABB aabb, Transform xf)
        {
            aabb = new AABB();

            Vector2 p = xf.position + xf.TransformDirection(_position);

            aabb.LowerBound = new Vector2(p.X - _radius, p.Y - _radius);
            aabb.UpperBound = new Vector2(p.X + _radius, p.Y + _radius);
        }
Exemplo n.º 7
0
        public override void ComputeAABB(out AABB aabb, Transform xf)
        {
            Vector2 v1 = xf.TransformPoint(_v1);
            Vector2 v2 = xf.TransformPoint(_v2);

            Vector2 r = new Vector2(_radius, _radius);

            aabb.LowerBound = Vector2.Min(v1, v2) - r;
            aabb.UpperBound = Vector2.Max(v1, v2) + r;
        }
Exemplo n.º 8
0
        internal unsafe void ReadCache(SimplexCache *cache, Shape shapeA, Transform TransformA, Shape shapeB, Transform TransformB)
        {
            Box2DNetDebug.Assert(0 <= cache->Count && cache->Count <= 3);

            // Copy data from cache.
            _count = cache->Count;
            SimplexVertex **vertices = stackalloc SimplexVertex *[3];

            fixed(SimplexVertex *v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3)
            {
                vertices[0] = v1Ptr;
                vertices[1] = v2Ptr;
                vertices[2] = v3Ptr;
                for (int i = 0; i < _count; ++i)
                {
                    SimplexVertex *v = vertices[i];
                    v->indexA = cache->IndexA[i];
                    v->indexB = cache->IndexB[i];
                    Vector2 wALocal = shapeA.GetVertex(v->indexA);
                    Vector2 wBLocal = shapeB.GetVertex(v->indexB);
                    v->wA = TransformA.TransformPoint(wALocal);
                    v->wB = TransformB.TransformPoint(wBLocal);
                    v->w  = v->wB - v->wA;
                    v->a  = 0.0f;
                }

                // Compute the new simplex metric, if it is substantially different than
                // old metric then flush the simplex.
                if (_count > 1)
                {
                    float metric1 = cache->Metric;
                    float metric2 = GetMetric();
                    if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Common.Settings.FLT_EPSILON)
                    {
                        // Reset the simplex.
                        _count = 0;
                    }
                }

                // If the cache is empty or invalid ...
                if (_count == 0)
                {
                    SimplexVertex *v = vertices[0];
                    v->indexA = 0;
                    v->indexB = 0;
                    Vector2 wALocal = shapeA.GetVertex(0);
                    Vector2 wBLocal = shapeB.GetVertex(0);
                    v->wA  = TransformA.TransformPoint(wALocal);
                    v->wB  = TransformB.TransformPoint(wBLocal);
                    v->w   = v->wB - v->wA;
                    _count = 1;
                }
            }
        }
        internal float Evaluate(Transform TransformA, Transform TransformB)
        {
            switch (FaceType)
            {
            case Type.Points:
            {
                Vector2 axisA       = TransformA.InverseTransformDirection(Axis);
                Vector2 axisB       = TransformB.InverseTransformDirection(-Axis);
                Vector2 localPointA = ShapeA.GetSupportVertex(axisA);
                Vector2 localPointB = ShapeB.GetSupportVertex(axisB);
                Vector2 pointA      = TransformA.TransformPoint(localPointA);
                Vector2 pointB      = TransformB.TransformPoint(localPointB);
                float   separation  = Vector2.Dot(pointB - pointA, Axis);
                return(separation);
            }

            case Type.FaceA:
            {
                Vector2 normal = TransformA.TransformDirection(Axis);
                Vector2 pointA = TransformA.TransformPoint(LocalPoint);

                Vector2 axisB = TransformB.InverseTransformDirection(-normal);

                Vector2 localPointB = ShapeB.GetSupportVertex(axisB);
                Vector2 pointB      = TransformB.TransformPoint(localPointB);

                float separation = Vector2.Dot(pointB - pointA, normal);
                return(separation);
            }

            case Type.FaceB:
            {
                Vector2 normal = TransformB.TransformDirection(Axis);
                Vector2 pointB = TransformB.TransformPoint(LocalPoint);

                Vector2 axisA = TransformA.InverseTransformDirection(-normal);

                Vector2 localPointA = ShapeA.GetSupportVertex(axisA);
                Vector2 pointA      = TransformA.TransformPoint(localPointA);

                float separation = Vector2.Dot(pointA - pointB, normal);
                return(separation);
            }

            default:
                Box2DNetDebug.Assert(false);
                return(0.0f);
            }
        }
Exemplo n.º 10
0
        public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c)
        {
            //Note that v0 is independent of any details of the specific edge
            //We are relying on v0 being consistent between multiple edges of the same body
            Vector2 v0 = offset * normal;
            //b2Vec2 v0 = xf.position + (offset - b2Dot(normal, xf.position)) * normal;

            Vector2 v1 = xf.TransformPoint(_v1);
            Vector2 v2 = xf.TransformPoint(_v2);

            float d1 = Vector2.Dot(normal, v1) - offset;
            float d2 = Vector2.Dot(normal, v2) - offset;

            if (d1 > 0.0f)
            {
                if (d2 > 0.0f)
                {
                    c = new Vector2();
                    return(0.0f);
                }
                else
                {
                    v1 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2;
                }
            }
            else
            {
                if (d2 > 0.0f)
                {
                    v2 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2;
                }
                else
                {
                    //Nothing
                }
            }

            // v0,v1,v2 represents a fully submerged triangle
            float k_inv3 = 1.0f / 3.0f;

            // Area weighted centroid
            c = k_inv3 * (v0 + v1 + v2);

            Vector2 e1 = v1 - v0;
            Vector2 e2 = v2 - v0;

            return(0.5f * e1.Cross(e2));
        }
Exemplo n.º 11
0
        internal void ReadCache(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB)
        {
            Box2DNetDebug.Assert(0 <= cache.Count && cache.Count <= 3);

            // Copy data from cache.
            _count = cache.Count;
            SimplexVertex[] vertices = new SimplexVertex[] { _v1, _v2, _v3 };
            for (int i = 0; i < _count; ++i)
            {
                SimplexVertex v = vertices[i];
                v.indexA = cache.IndexA[i];
                v.indexB = cache.IndexB[i];
                Vector2 wALocal = shapeA.GetVertex(v.indexA);
                Vector2 wBLocal = shapeB.GetVertex(v.indexB);
                v.wA = transformA.TransformPoint(wALocal);
                v.wB = transformB.TransformPoint(wBLocal);
                v.w  = v.wB - v.wA;
                v.a  = 0.0f;
            }

            // Compute the new simplex metric, if it is substantially different than
            // old metric then flush the simplex.
            if (_count > 1)
            {
                float metric1 = cache.Metric;
                float metric2 = GetMetric();
                if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Common.Settings.FLT_EPSILON)
                {
                    // Reset the simplex.
                    _count = 0;
                }
            }

            // If the cache is empty or invalid ...
            if (_count == 0)
            {
                SimplexVertex v = vertices[0];
                v.indexA = 0;
                v.indexB = 0;
                Vector2 wALocal = shapeA.GetVertex(0);
                Vector2 wBLocal = shapeB.GetVertex(0);
                v.wA   = transformA.TransformPoint(wALocal);
                v.wB   = transformB.TransformPoint(wBLocal);
                v.w    = v.wB - v.wA;
                _count = 1;
            }
        }
Exemplo n.º 12
0
        public static void FindIncidentEdge(out ClipVertex[] c, PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2)
        {
            int count1 = poly1._vertexCount;

            Vector2[] normals1 = poly1._normals;

            int count2 = poly2._vertexCount;

            Vector2[] vertices2 = poly2._vertices;
            Vector2[] normals2  = poly2._normals;

            Box2DNetDebug.Assert(0 <= edge1 && edge1 < count1);

            // Get the normal of the reference edge in poly2's frame.
            Vector2 normal1 = xf2.InverseTransformDirection(xf1.TransformDirection(normals1[edge1]));

            // Find the incident edge on poly2.
            int   index  = 0;
            float minDot = Settings.FLT_MAX;

            for (int i = 0; i < count2; ++i)
            {
                float dot = Vector2.Dot(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 = new ClipVertex[2];

            c[0].V = Common.Math.Mul(xf2, vertices2[i1]);
            c[0].ID.Features.ReferenceEdge  = (byte)edge1;
            c[0].ID.Features.IncidentEdge   = (byte)i1;
            c[0].ID.Features.IncidentVertex = 0;

            c[1].V = Common.Math.Mul(xf2, vertices2[i2]);
            c[1].ID.Features.ReferenceEdge  = (byte)edge1;
            c[1].ID.Features.IncidentEdge   = (byte)i2;
            c[1].ID.Features.IncidentVertex = 1;
        }
Exemplo n.º 13
0
        /// <summary>
        /// Build vertices to represent an oriented box.
        /// </summary>
        /// <param name="hx">The half-width</param>
        /// <param name="hy">The half-height.</param>
        /// <param name="center">The center of the box in local coordinates.</param>
        /// <param name="angle">The rotation of the box in local coordinates.</param>
        public void SetAsBox(float hx, float hy, Vector2 center, float angle)
        {
            SetAsBox(hx, hy);

            Transform xf = new Transform();

            xf.position = center;
            xf.rotation = Box2DNet.Common.Math.AngleToRotation(angle);
            //xf.R = new Mat22(angle);

            //Debug.Log(string.Format("xf.position = ({0},{1}) xf.rotation = ({2},{3},{4},{5})", xf.position.x, xf.position.y, xf.rotation.x, xf.rotation.y, xf.rotation.z, xf.rotation.w));

            for (int i = 0; i < VertexCount; ++i)
            {
                Vertices[i] = xf.TransformPoint(Vertices[i]);
            }
        }
Exemplo n.º 14
0
        public override void ComputeAABB(out AABB aabb, Transform xf)
        {
            Vector2 lower = xf.TransformPoint(_vertices[0]);
            Vector2 upper = lower;

            for (int i = 1; i < _vertexCount; ++i)
            {
                Vector2 v = xf.TransformPoint(_vertices[i]);
                lower = Vector2.Min(lower, v);
                upper = Vector2.Max(upper, v);
            }

            Vector2 r = new Vector2(_radius, _radius);

            aabb.LowerBound = lower - r;
            aabb.UpperBound = upper + r;
        }
Exemplo n.º 15
0
        public override bool TestPoint(Transform xf, Vector2 p)
        {
            Vector2 pLocal = xf.InverseTransformDirection(p - xf.position);

            int vc = _vertexCount;

            for (int i = 0; i < vc; ++i)
            {
                float dot = Vector2.Dot(_normals[i], pLocal - _vertices[i]);
                if (dot > 0.0f)
                {
                    return(false);
                }
            }

            return(true);
        }
Exemplo n.º 16
0
        // 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 SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda)
        {
            lambda = 0f;
            normal = Vector2.Zero;

            Vector2 position = xf.position + xf.TransformDirection(_position);
            Vector2 s        = segment.P1 - position;
            float   b        = Vector2.Dot(s, s) - _radius * _radius;

            // Does the segment start inside the circle?
            if (b < 0.0f)
            {
                lambda = 0f;
                return(SegmentCollide.StartInsideCollide);
            }

            // Solve quadratic equation.
            Vector2 r     = segment.P2 - segment.P1;
            float   c     = Vector2.Dot(s, r);
            float   rr    = Vector2.Dot(r, r);
            float   sigma = c * c - rr * b;

            // Check for negative discriminant and short segment.
            if (sigma < 0.0f || rr < Common.Settings.FLT_EPSILON)
            {
                return(SegmentCollide.MissCollide);
            }

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

            // Is the intersection point on the segment?
            if (0.0f <= a && a <= maxLambda * rr)
            {
                a     /= rr;
                lambda = a;
                normal = s + a * r;
                normal.Normalize();
                return(SegmentCollide.HitCollide);
            }

            return(SegmentCollide.MissCollide);
        }
Exemplo n.º 17
0
        /// <summary>
        /// Find the separation between poly1 and poly2 for a give edge normal on poly1.
        /// </summary>
        public static float EdgeSeparation(PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2)
        {
            int count1 = poly1._vertexCount;

            Vector2[] vertices1 = poly1._vertices;
            Vector2[] normals1  = poly1._normals;

            int count2 = poly2._vertexCount;

            Vector2[] vertices2 = poly2._vertices;

            //This fixed it, not sure why it was broken
            if (normals1.Length - 1 <= edge1 || edge1 < 0)
            {
                return(0);
            }

            // Convert normal from poly1's frame into poly2's frame.
            Vector2 normal1World = xf1.TransformDirection(normals1[edge1]);
            Vector2 normal1      = xf2.InverseTransformDirection(normal1World);

            // Find support vertex on poly2 for -normal.
            int   index  = 0;
            float minDot = Common.Settings.FLT_MAX;

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

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

            return(separation);
        }
Exemplo n.º 18
0
        public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda)
        {
            Vector2 r  = segment.P2 - segment.P1;
            Vector2 v1 = xf.TransformPoint(_v1);
            Vector2 d  = ((Vector2)xf.TransformPoint(_v2)) - v1;
            Vector2 n  = d.CrossScalarPostMultiply(1.0f);

            float k_slop = 100.0f * Common.Settings.FLT_EPSILON;
            float denom  = -Vector2.Dot(r, n);

            // Cull back facing collision and ignore parallel segments.
            if (denom > k_slop)
            {
                // Does the segment intersect the infinite line associated with this segment?
                Vector2 b = segment.P1 - v1;
                float   a = Vector2.Dot(b, n);

                if (0.0f <= a && a <= maxLambda * denom)
                {
                    float mu2 = -r.X * b.Y + r.Y * b.X;

                    // Does the segment intersect this segment?
                    if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop))
                    {
                        a /= denom;
                        n.Normalize();
                        lambda = a;
                        normal = n;
                        return(SegmentCollide.HitCollide);
                    }
                }
            }

            lambda = 0;
            normal = new Vector2();
            return(SegmentCollide.MissCollide);
        }
Exemplo n.º 19
0
        internal void RefilterProxy(BroadPhase broadPhase, Transform Transform)
        {
            if (_proxyId == PairManager.NullProxy)
            {
                return;
            }

            broadPhase.DestroyProxy(_proxyId);

            AABB aabb;

            _shape.ComputeAABB(out aabb, Transform);

            bool inRange = broadPhase.InRange(aabb);

            if (inRange)
            {
                _proxyId = broadPhase.CreateProxy(aabb, this);
            }
            else
            {
                _proxyId = PairManager.NullProxy;
            }
        }
Exemplo n.º 20
0
 /// <summary>
 /// Draw a Transform. Choose your own length scale.
 /// </summary>
 /// <param name="xf">A Transform.</param>
 public abstract void DrawTransform(Transform xf);
Exemplo n.º 21
0
 /// <summary>
 /// Compute the volume and centroid of this shape intersected with a half plane.
 /// </summary>
 /// <param name="normal">Normal the surface normal.</param>
 /// <param name="offset">Offset the surface offset along normal.</param>
 /// <param name="xf">The shape Transform.</param>
 /// <param name="c">Returns the centroid.</param>
 /// <returns>The total volume less than offset along normal.</returns>
 public abstract float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c);
Exemplo n.º 22
0
 /// <summary>
 /// Test a point for containment in this shape. This only works for convex shapes.
 /// </summary>
 /// <param name="xf">The shape world Transform.</param>
 /// <param name="p">A point in world coordinates.</param>
 /// <returns></returns>
 public abstract bool TestPoint(Transform xf, Vector2 p);
Exemplo n.º 23
0
        public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c)
        {
            //Transform plane into shape co-ordinates
            Vector2 normalL = xf.InverseTransformDirection(normal);
            float   offsetL = offset - Vector2.Dot(normal, xf.position);

            float[] depths    = new float[Common.Settings.MaxPolygonVertices];
            int     diveCount = 0;
            int     intoIndex = -1;
            int     outoIndex = -1;

            bool lastSubmerged = false;
            int  i;

            for (i = 0; i < _vertexCount; i++)
            {
                depths[i] = Vector2.Dot(normalL, _vertices[i]) - offsetL;
                bool isSubmerged = depths[i] < -Common.Settings.FLT_EPSILON;
                if (i > 0)
                {
                    if (isSubmerged)
                    {
                        if (!lastSubmerged)
                        {
                            intoIndex = i - 1;
                            diveCount++;
                        }
                    }
                    else
                    {
                        if (lastSubmerged)
                        {
                            outoIndex = i - 1;
                            diveCount++;
                        }
                    }
                }
                lastSubmerged = isSubmerged;
            }
            switch (diveCount)
            {
            case 0:
                if (lastSubmerged)
                {
                    //Completely submerged
                    MassData md;
                    ComputeMass(out md, 1f);
                    c = xf.TransformPoint(md.Center);
                    return(md.Mass);
                }
                else
                {
                    //Completely dry
                    c = new Vector2();
                    return(0);
                }
                break;

            case 1:
                if (intoIndex == -1)
                {
                    intoIndex = _vertexCount - 1;
                }
                else
                {
                    outoIndex = _vertexCount - 1;
                }
                break;
            }
            int intoIndex2 = (intoIndex + 1) % _vertexCount;
            int outoIndex2 = (outoIndex + 1) % _vertexCount;

            float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
            float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]);

            Vector2 intoVec = new Vector2(_vertices[intoIndex].X * (1 - intoLambda) + _vertices[intoIndex2].X * intoLambda,
                                          _vertices[intoIndex].Y * (1 - intoLambda) + _vertices[intoIndex2].Y * intoLambda);
            Vector2 outoVec = new Vector2(_vertices[outoIndex].X * (1 - outoLambda) + _vertices[outoIndex2].X * outoLambda,
                                          _vertices[outoIndex].Y * (1 - outoLambda) + _vertices[outoIndex2].Y * outoLambda);

            //Initialize accumulator
            float   area   = 0;
            Vector2 center = Vector2.Zero;
            Vector2 p2     = _vertices[intoIndex2];
            Vector2 p3;

            const float k_inv3 = 1.0f / 3.0f;

            //An awkward loop from intoIndex2+1 to outIndex2
            i = intoIndex2;
            while (i != outoIndex2)
            {
                i = (i + 1) % _vertexCount;
                if (i == outoIndex2)
                {
                    p3 = outoVec;
                }
                else
                {
                    p3 = _vertices[i];
                }
                //Add the triangle formed by intoVec,p2,p3
                {
                    Vector2 e1 = p2 - intoVec;
                    Vector2 e2 = p3 - intoVec;

                    float D = e1.Cross(e2);

                    float triangleArea = 0.5f * D;

                    area += triangleArea;

                    // Area weighted centroid
                    center += triangleArea * k_inv3 * (intoVec + p2 + p3);
                }
                //
                p2 = p3;
            }

            //Normalize and Transform centroid
            center *= 1.0f / area;

            c = xf.TransformPoint(center);

            return(area);
        }
Exemplo n.º 24
0
        public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda)
        {
            lambda = 0f;
            normal = Vector2.Zero;

            float lower = 0.0f, upper = maxLambda;

            Vector2 p1    = xf.InverseTransformDirection(segment.P1 - xf.position);
            Vector2 p2    = xf.InverseTransformDirection(segment.P2 - xf.position);
            Vector2 d     = p2 - p1;
            int     index = -1;

            for (int i = 0; i < _vertexCount; ++i)
            {
                // p = p1 + a * d
                // dot(normal, p - v) = 0
                // dot(normal, p1 - v) + a * dot(normal, d) = 0
                float numerator   = Vector2.Dot(_normals[i], _vertices[i] - p1);
                float denominator = Vector2.Dot(_normals[i], d);

                if (denominator == 0.0f)
                {
                    if (numerator < 0.0f)
                    {
                        return(SegmentCollide.MissCollide);
                    }
                }
                else
                {
                    // Note: we want this predicate without division:
                    // lower < numerator / denominator, where denominator < 0
                    // Since denominator < 0, we have to flip the inequality:
                    // lower < numerator / denominator <==> denominator * lower > numerator.
                    if (denominator < 0.0f && numerator < lower * denominator)
                    {
                        // Increase lower.
                        // The segment enters this half-space.
                        lower = numerator / denominator;
                        index = i;
                    }
                    else if (denominator > 0.0f && numerator < upper * denominator)
                    {
                        // Decrease upper.
                        // The segment exits this half-space.
                        upper = numerator / denominator;
                    }
                }

                if (upper < lower)
                {
                    return(SegmentCollide.MissCollide);
                }
            }


            if (index >= 0)
            {
                lambda = lower;
                normal = xf.TransformDirection(_normals[index]);
                return(SegmentCollide.HitCollide);
            }

            lambda = 0f;
            return(SegmentCollide.StartInsideCollide);
        }
Exemplo n.º 25
0
		internal void ComputeTransform(ref Transform xf, Vector2 center, Vector2 localCenter, float angle)
		{
			xf.rotation = Box2DNet.Common.Math.AngleToRotation(angle);
			//xf.R = new Mat22(angle);
			xf.position = center - xf.TransformDirection(localCenter);
		}
Exemplo n.º 26
0
 private static void CollidePolyAndEdgeContact(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2)
 {
     Collision.Collision.CollidePolyAndEdge(ref manifold, (PolygonShape)shape1, xf1, (EdgeShape)shape2, xf2);
 }
Exemplo n.º 27
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            _localCenter1 = b1.GetLocalCenter();
            _localCenter2 = b2.GetLocalCenter();

            Transform xf1 = b1.GetTransform();
            Transform xf2 = b2.GetTransform();

            // Compute the effective masses.
            Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1);
            Vector2 r2 = xf2.TransformDirection(_localAnchor2 - _localCenter2);
            Vector2 d  = b2._sweep.C + r2 - b1._sweep.C - r1;

            _invMass1 = b1._invMass;
            _invI1    = b1._invI;
            _invMass2 = b2._invMass;
            _invI2    = b2._invI;

            // Compute motor Jacobian and effective mass.
            {
                _axis = xf1.TransformDirection(_localXAxis1);
                _a1   = (d + r1).Cross(_axis);
                _a2   = r2.Cross(_axis);

                _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
                _motorMass = 1.0f / _motorMass;
            }

            // Prismatic constraint.
            {
                _perp = xf1.TransformDirection(_localYAxis1);

                _s1 = (d + r1).Cross(_perp);
                _s2 = r2.Cross(_perp);

                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
                float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
                float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

                _K.Col1 = new Vector2(k11, k12);
                _K.Col2 = new Vector2(k12, k22);
            }

            // Compute motor and limit terms.
            if (_enableLimit)
            {
                float jointTranslation = Vector2.Dot(_axis, d);
                if (Box2DNet.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointTranslation <= _lowerTranslation)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _limitState = LimitState.AtLowerLimit;
                        _impulse.Y  = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _limitState = LimitState.AtUpperLimit;
                        _impulse.Y  = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.Y  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

            if (_enableMotor == false)
            {
                _motorImpulse = 0.0f;
            }

            if (step.WarmStarting)
            {
                // Account for variable time step.
                _impulse      *= step.DtRatio;
                _motorImpulse *= step.DtRatio;

                Vector2 P  = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
                float   L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
                float   L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;

                b1._linearVelocity  -= _invMass1 * P;
                b1._angularVelocity -= _invI1 * L1;

                b2._linearVelocity  += _invMass2 * P;
                b2._angularVelocity += _invI2 * L2;
            }
            else
            {
                _impulse      = Vector2.Zero;
                _motorImpulse = 0.0f;
            }
        }
Exemplo n.º 28
0
 private static void CollidePolygonCircle(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2)
 {
     Collision.Collision.CollidePolygonAndCircle(ref manifold, (PolygonShape)shape1, xf1, (CircleShape)shape2, xf2);
 }
Exemplo n.º 29
0
        static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
        {
            output = new DistanceOutput();

            Transform transformA = input.TransformA;
            Transform transformB = input.TransformB;

            // Initialize the simplex.
            Simplex simplex = new Simplex();

#if ALLOWUNSAFE
            fixed(SimplexCache *sPtr = &cache)
            {
                simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
            }
#else
            simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB);
#endif

            // Get simplex vertices as an array.
#if ALLOWUNSAFE
            SimplexVertex *vertices = &simplex._v1;
#else
            SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 };
#endif

            // These store the vertices of the last simplex so that we
            // can check for duplicates and prevent cycling.
#if ALLOWUNSAFE
            int *lastA = stackalloc int[4], lastB = stackalloc int[4];
#else
            int[] lastA = new int[4];
            int[] lastB = new int[4];
#endif // ALLOWUNSAFE
            int lastCount;

            // Main iteration loop.
            int       iter = 0;
            const int k_maxIterationCount = 20;
            while (iter < k_maxIterationCount)
            {
                // Copy simplex so we can identify duplicates.
                lastCount = simplex._count;
                int i;
                for (i = 0; i < lastCount; ++i)
                {
                    lastA[i] = vertices[i].indexA;
                    lastB[i] = vertices[i].indexB;
                }

                switch (simplex._count)
                {
                case 1:
                    break;

                case 2:
                    simplex.Solve2();
                    break;

                case 3:
                    simplex.Solve3();
                    break;

                default:
                    break;
                }

                // If we have 3 points, then the origin is in the corresponding triangle.
                if (simplex._count == 3)
                {
                    break;
                }

                // Compute closest point.
                Vector2 p           = simplex.GetClosestPoint();
                float   distanceSqr = p.LengthSquared();

                // Ensure the search direction is numerically fit.
                if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED)
                {
                    // The origin is probably contained by a line segment
                    // or triangle. Thus the shapes are overlapped.

                    // We can't return zero here even though there may be overlap.
                    // In case the simplex is a point, segment, or triangle it is difficult
                    // to determine if the origin is contained in the CSO or very close to it.
                    break;
                }

                // Compute a tentative new simplex vertex using support points.
#if ALLOWUNSAFE
                SimplexVertex *vertex = vertices + simplex._count;
                vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
                vertex->wA     = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA));
                //Vec2 wBLocal;
                vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
                vertex->wB     = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB));
                vertex->w      = vertex->wB - vertex->wA;
#else
                SimplexVertex vertex = vertices[simplex._count - 1];
                vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
                vertex.wA     = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA));
                //Vec2 wBLocal;
                vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
                vertex.wB     = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB));
                vertex.w      = vertex.wB - vertex.wA;
#endif // ALLOWUNSAFE

                // Iteration count is equated to the number of support point calls.
                ++iter;

                // Check for convergence.
#if ALLOWUNSAFE
                float lowerBound = Vector2.Dot(p, vertex->w);
#else
                float lowerBound = Vector2.Dot(p, vertex.w);
#endif
                float       upperBound       = distanceSqr;
                const float k_relativeTolSqr = 0.01f * 0.01f;                   // 1:100
                if (upperBound - lowerBound <= k_relativeTolSqr * upperBound)
                {
                    // Converged!
                    break;
                }

                // Check for duplicate support points.
                bool duplicate = false;
                for (i = 0; i < lastCount; ++i)
                {
#if ALLOWUNSAFE
                    if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
#else
                    if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i])
#endif
                    {
                        duplicate = true;
                        break;
                    }
                }

                // If we found a duplicate support point we must exit to avoid cycling.
                if (duplicate)
                {
                    break;
                }

                // New vertex is ok and needed.
                ++simplex._count;
            }


#if ALLOWUNSAFE
            fixed(DistanceOutput *doPtr = &output)
            {
                // Prepare output.
                simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
                doPtr->Distance   = Vector2.Distance(doPtr->PointA, doPtr->PointB);
                doPtr->Iterations = iter;
            }

            fixed(SimplexCache *sPtr = &cache)
            {
                // Cache the simplex.
                simplex.WriteCache(sPtr);
            }
#else
            // Prepare output.
            simplex.GetWitnessPoints(ref output.PointA, ref output.PointB);
            output.Distance   = Box2DNet.Common.Math.Distance(output.PointA, output.PointB);
            output.Iterations = iter;

            // Cache the simplex.
            simplex.WriteCache(cache);
#endif

            // Apply radii if requested.
            if (input.UseRadii)
            {
                float rA = shapeA._radius;
                float rB = shapeB._radius;

                if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_EPSILON)
                {
                    // Shapes are still no overlapped.
                    // Move the witness points to the outer surface.
                    output.Distance -= rA + rB;
                    Vector2 normal = output.PointB - output.PointA;
                    normal.Normalize();
                    output.PointA += rA * normal;
                    output.PointB -= rB * normal;
                }
                else
                {
                    // Shapes are overlapped when radii are considered.
                    // Move the witness points to the middle.
                    Vector2 p = 0.5f * (output.PointA + output.PointB);
                    output.PointA   = p;
                    output.PointB   = p;
                    output.Distance = 0.0f;
                }
            }
        }
Exemplo n.º 30
0
        public void Create(BroadPhase broadPhase, Body body, Transform xf, FixtureDef def)
        {
            UserData    = def.UserData;
            Friction    = def.Friction;
            Restitution = def.Restitution;
            Density     = def.Density;

            _body = body;
            _next = null;

            Filter = def.Filter;

            _isSensor = def.IsSensor;

            _type = def.Type;

            // Allocate and initialize the child shape.
            switch (_type)
            {
            case ShapeType.CircleShape:
            {
                CircleShape circle    = new CircleShape();
                CircleDef   circleDef = (CircleDef)def;
                circle._position = circleDef.LocalPosition;
                circle._radius   = circleDef.Radius;
                _shape           = circle;
            }
            break;

            case ShapeType.PolygonShape:
            {
                PolygonShape polygon    = new PolygonShape();
                PolygonDef   polygonDef = (PolygonDef)def;
                polygon.Set(polygonDef.Vertices, polygonDef.VertexCount);
                _shape = polygon;
            }
            break;

            case ShapeType.EdgeShape:
            {
                EdgeShape edge    = new EdgeShape();
                EdgeDef   edgeDef = (EdgeDef)def;
                edge.Set(edgeDef.Vertex1, edgeDef.Vertex2);
                _shape = edge;
            }
            break;

            default:
                Box2DNetDebug.Assert(false);
                break;
            }

            // Create proxy in the broad-phase.
            AABB aabb;

            _shape.ComputeAABB(out aabb, xf);

            bool inRange = broadPhase.InRange(aabb);

            // You are creating a shape outside the world box.
            Box2DNetDebug.Assert(inRange);

            if (inRange)
            {
                _proxyId = broadPhase.CreateProxy(aabb, this);
            }
            else
            {
                _proxyId = PairManager.NullProxy;
            }
        }