Exemple #1
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 r1 = CommonMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = CommonMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

            if (_state == LimitState.AtUpperLimit)
            {
                Vector2 v1 = b1._linearVelocity + CommonMath.Cross(b1._angularVelocity, r1);
                Vector2 v2 = b2._linearVelocity + CommonMath.Cross(b2._angularVelocity, r2);

                float Cdot       = -Vector2.Dot(_u1, v1) - _ratio * Vector2.Dot(_u2, v2);
                float impulse    = _pulleyMass * (-Cdot);
                float oldImpulse = _impulse;
                _impulse = CommonMath.Max(0.0f, _impulse + impulse);
                impulse  = _impulse - oldImpulse;

                Vector2 P1 = -impulse * _u1;
                Vector2 P2 = -_ratio * impulse * _u2;
                b1._linearVelocity  += b1._invMass * P1;
                b1._angularVelocity += b1._invI * CommonMath.Cross(ref r1, ref P1);
                b2._linearVelocity  += b2._invMass * P2;
                b2._angularVelocity += b2._invI * CommonMath.Cross(ref r2, ref P2);
            }

            if (_limitState1 == LimitState.AtUpperLimit)
            {
                Vector2 v1 = b1._linearVelocity + CommonMath.Cross(b1._angularVelocity, r1);

                float Cdot       = -Vector2.Dot(_u1, v1);
                float impulse    = -_limitMass1 * Cdot;
                float oldImpulse = _limitImpulse1;
                _limitImpulse1 = CommonMath.Max(0.0f, _limitImpulse1 + impulse);
                impulse        = _limitImpulse1 - oldImpulse;

                Vector2 P1 = -impulse * _u1;
                b1._linearVelocity  += b1._invMass * P1;
                b1._angularVelocity += b1._invI * CommonMath.Cross(ref r1, ref P1);
            }

            if (_limitState2 == LimitState.AtUpperLimit)
            {
                Vector2 v2 = b2._linearVelocity + CommonMath.Cross(b2._angularVelocity, r2);

                float Cdot       = -Vector2.Dot(_u2, v2);
                float impulse    = -_limitMass2 * Cdot;
                float oldImpulse = _limitImpulse2;
                _limitImpulse2 = CommonMath.Max(0.0f, _limitImpulse2 + impulse);
                impulse        = _limitImpulse2 - oldImpulse;

                Vector2 P2 = -impulse * _u2;
                b2._linearVelocity  += b2._invMass * P2;
                b2._angularVelocity += b2._invI * CommonMath.Cross(ref r2, ref P2);
            }
        }
        // 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
        public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, XForm xfA, PolygonShape polyB, XForm xfB)
        {
            manifold.PointCount = 0;

            int   edgeA       = 0;
            float separationA = FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);

            if (separationA > 0.0f)
            {
                return;
            }

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

            if (separationB > 0.0f)
            {
                return;
            }

            PolygonShape poly1; // reference poly
            PolygonShape poly2; // incident poly
            XForm        xf1, xf2;
            int          edge1; // reference edge
            byte         flip;
            float        k_relativeTol = 0.98f;
            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  = 1;
            }
            else
            {
                poly1 = polyA;
                poly2 = polyB;
                xf1   = xfA;
                xf2   = xfB;
                edge1 = edgeA;
                flip  = 0;
            }

            ClipVertex[] incidentEdge;
            FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);

            int count1 = poly1.VertexCount;

            Vector2[] vertices1 = poly1.GetVertices();

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

            Vector2 dv         = v12 - v11;
            Vector2 sideNormal = CommonMath.Mul(xf1.R, v12 - v11);

            sideNormal.Normalize();
            Vector2 frontNormal = CommonMath.Cross(sideNormal, 1.0f);

            v11 = CommonMath.Mul(xf1, v11);
            v12 = CommonMath.Mul(xf1, v12);

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

            // Clip incident edge against extruded edge1 side edges.
            ClipVertex[] clipPoints1;
            ClipVertex[] clipPoints2;
            int          np;

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

            if (np < 2)
            {
                return;
            }

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

            if (np < 2)
            {
                return;
            }

            // Now clipPoints2 contains the clipped points.
            manifold.Normal = flip != 0 ? -frontNormal : frontNormal;

            int pointCount = 0;

            for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
            {
                float separation = Vector2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;

                if (separation <= 0.0f)
                {
                    ManifoldPoint cp = manifold.Points[pointCount];
                    cp.Separation       = separation;
                    cp.LocalPoint1      = CommonMath.MulT(xfA, clipPoints2[i].V);
                    cp.LocalPoint2      = CommonMath.MulT(xfB, clipPoints2[i].V);
                    cp.ID               = clipPoints2[i].ID;
                    cp.ID.Features.Flip = flip;
                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;
        }
Exemple #3
0
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 s1 = _ground.GetXForm().Position + _groundAnchor1;
            Vector2 s2 = _ground.GetXForm().Position + _groundAnchor2;

            float linearError = 0.0f;

            if (_state == LimitState.AtUpperLimit)
            {
                Vector2 r1 = CommonMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = CommonMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

                Vector2 p1 = b1._sweep.C + r1;
                Vector2 p2 = b2._sweep.C + r2;

                // Get the pulley axes.
                _u1 = p1 - s1;
                _u2 = p2 - s2;

                float length1 = _u1.Length();
                float length2 = _u2.Length();

                if (length1 > Settings.LinearSlop)
                {
                    _u1 *= 1.0f / length1;
                }
                else
                {
                    _u1 = Vector2.Zero;
                }

                if (length2 > Settings.LinearSlop)
                {
                    _u2 *= 1.0f / length2;
                }
                else
                {
                    _u2 = Vector2.Zero;
                }

                float C = _constant - length1 - _ratio * length2;
                linearError = CommonMath.Max(linearError, -C);

                C = CommonMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                float impulse = -_pulleyMass * C;

                Vector2 P1 = -impulse * _u1;
                Vector2 P2 = -_ratio * impulse * _u2;

                b1._sweep.C += b1._invMass * P1;
                b1._sweep.A += b1._invI * CommonMath.Cross(ref r1, ref P1);
                b2._sweep.C += b2._invMass * P2;
                b2._sweep.A += b2._invI * CommonMath.Cross(ref r2, ref P2);

                b1.SynchronizeTransform();
                b2.SynchronizeTransform();
            }

            if (_limitState1 == LimitState.AtUpperLimit)
            {
                Vector2 r1 = CommonMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
                Vector2 p1 = b1._sweep.C + r1;

                _u1 = p1 - s1;
                float length1 = _u1.Length();

                if (length1 > Settings.LinearSlop)
                {
                    _u1 *= 1.0f / length1;
                }
                else
                {
                    _u1 = Vector2.Zero;
                }

                float C = _maxLength1 - length1;
                linearError = CommonMath.Max(linearError, -C);
                C           = CommonMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                float impulse = -_limitMass1 * C;

                Vector2 P1 = -impulse * _u1;
                b1._sweep.C += b1._invMass * P1;
                b1._sweep.A += b1._invI * CommonMath.Cross(ref r1, ref P1);

                b1.SynchronizeTransform();
            }

            if (_limitState2 == LimitState.AtUpperLimit)
            {
                Vector2 r2 = CommonMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
                Vector2 p2 = b2._sweep.C + r2;

                _u2 = p2 - s2;
                float length2 = _u2.Length();

                if (length2 > Settings.LinearSlop)
                {
                    _u2 *= 1.0f / length2;
                }
                else
                {
                    _u2 = Vector2.Zero;
                }

                float C = _maxLength2 - length2;
                linearError = CommonMath.Max(linearError, -C);
                C           = CommonMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                float impulse = -_limitMass2 * C;

                Vector2 P2 = -impulse * _u2;
                b2._sweep.C += b2._invMass * P2;
                b2._sweep.A += b2._invI * CommonMath.Cross(ref r2, ref P2);

                b2.SynchronizeTransform();
            }

            return(linearError < Settings.LinearSlop);
        }
Exemple #4
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 r1 = CommonMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = CommonMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

            Vector2 p1 = b1._sweep.C + r1;
            Vector2 p2 = b2._sweep.C + r2;

            Vector2 s1 = _ground.GetXForm().Position + _groundAnchor1;
            Vector2 s2 = _ground.GetXForm().Position + _groundAnchor2;

            // Get the pulley axes.
            _u1 = p1 - s1;
            _u2 = p2 - s2;

            float length1 = _u1.Length();
            float length2 = _u2.Length();

            if (length1 > Settings.LinearSlop)
            {
                _u1 *= 1.0f / length1;
            }
            else
            {
                _u1 = Vector2.Zero;
            }

            if (length2 > Settings.LinearSlop)
            {
                _u2 *= 1.0f / length2;
            }
            else
            {
                _u2 = Vector2.Zero;
            }

            float C = _constant - length1 - _ratio * length2;

            if (C > 0.0f)
            {
                _state   = LimitState.InactiveLimit;
                _impulse = 0.0f;
            }
            else
            {
                _state = LimitState.AtUpperLimit;
            }

            if (length1 < _maxLength1)
            {
                _limitState1   = LimitState.InactiveLimit;
                _limitImpulse1 = 0.0f;
            }
            else
            {
                _limitState1 = LimitState.AtUpperLimit;
            }

            if (length2 < _maxLength2)
            {
                _limitState2   = LimitState.InactiveLimit;
                _limitImpulse2 = 0.0f;
            }
            else
            {
                _limitState2 = LimitState.AtUpperLimit;
            }

            // Compute effective mass.
            float cr1u1 = CommonMath.Cross(ref r1, ref _u1);
            float cr2u2 = CommonMath.Cross(ref r2, ref _u2);

            _limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
            _limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
            _pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
            //Box2DXDebug.Assert(_limitMass1 > Settings.FLT_EPSILON);
            //Box2DXDebug.Assert(_limitMass2 > Settings.FLT_EPSILON);
            //Box2DXDebug.Assert(_pulleyMass > Settings.FLT_EPSILON);
            _limitMass1 = 1.0f / _limitMass1;
            _limitMass2 = 1.0f / _limitMass2;
            _pulleyMass = 1.0f / _pulleyMass;

            if (step.WarmStarting)
            {
                // Scale impulses to support variable time steps.
                _impulse       *= step.DtRatio;
                _limitImpulse1 *= step.DtRatio;
                _limitImpulse2 *= step.DtRatio;

                // Warm starting.
                Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
                Vector2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
                b1._linearVelocity  += b1._invMass * P1;
                b1._angularVelocity += b1._invI * CommonMath.Cross(ref r1, ref P1);
                b2._linearVelocity  += b2._invMass * P2;
                b2._angularVelocity += b2._invI * CommonMath.Cross(ref r2, ref P2);
            }
            else
            {
                _impulse       = 0.0f;
                _limitImpulse1 = 0.0f;
                _limitImpulse2 = 0.0f;
            }
        }
        // Possible regions:
        // - points[2]
        // - edge points[0]-points[2]
        // - edge points[1]-points[2]
        // - inside the triangle
        public static int ProcessThree(out Vector2 x1, out Vector2 x2, ref Vector2[] p1s, ref Vector2[] p2s, ref Vector2[] points)
        {
            Vector2 a = points[0];
            Vector2 b = points[1];
            Vector2 c = points[2];

            Vector2 ab = b - a;
            Vector2 ac = c - a;
            Vector2 bc = c - b;

            float sn = -Vector2.Dot(a, ab), sd = Vector2.Dot(b, ab);
            float tn = -Vector2.Dot(a, ac), td = Vector2.Dot(c, ac);
            float un = -Vector2.Dot(b, bc), ud = Vector2.Dot(c, bc);

            // In vertex c region?
            if (td <= 0.0f && ud <= 0.0f)
            {
                // Single point
                x1        = p1s[2];
                x2        = p2s[2];
                p1s[0]    = p1s[2];
                p2s[0]    = p2s[2];
                points[0] = points[2];
                return(1);
            }

            // Should not be in vertex a or b region.

            //B2_NOT_USED(sd);
            //B2_NOT_USED(sn);
            //Box2DXDebug.Assert(sn > 0.0f || tn > 0.0f);
            //Box2DXDebug.Assert(sd > 0.0f || un > 0.0f);

            float n = CommonMath.Cross(ref ab, ref ac);

#if TARGET_FLOAT32_IS_FIXED
            n = (n < 0.0f) ? -1.0f : ((n > 0.0f) ? 1.0f : 0.0f);
#endif

            // Should not be in edge ab region.
            float vc = n * CommonMath.Cross(ref a, ref b);
            //Box2DXDebug.Assert(vc > 0.0f || sn > 0.0f || sd > 0.0f);

            // In edge bc region?
            float va = n * CommonMath.Cross(ref b, ref c);
            if (va <= 0.0f && un >= 0.0f && ud >= 0.0f && (un + ud) > 0.0f)
            {
                //Box2DXDebug.Assert(un + ud > 0.0f);
                float lambda = un / (un + ud);
                x1        = p1s[1] + lambda * (p1s[2] - p1s[1]);
                x2        = p2s[1] + lambda * (p2s[2] - p2s[1]);
                p1s[0]    = p1s[2];
                p2s[0]    = p2s[2];
                points[0] = points[2];
                return(2);
            }

            // In edge ac region?
            float vb = n * CommonMath.Cross(ref c, ref a);
            if (vb <= 0.0f && tn >= 0.0f && td >= 0.0f && (tn + td) > 0.0f)
            {
                //Box2DXDebug.Assert(tn + td > 0.0f);
                float lambda = tn / (tn + td);
                x1        = p1s[0] + lambda * (p1s[2] - p1s[0]);
                x2        = p2s[0] + lambda * (p2s[2] - p2s[0]);
                p1s[1]    = p1s[2];
                p2s[1]    = p2s[2];
                points[1] = points[2];
                return(2);
            }

            // Inside the triangle, compute barycentric coordinates
            float denom = va + vb + vc;
            //Box2DXDebug.Assert(denom > 0.0f);
            denom = 1.0f / denom;
#if TARGET_FLOAT32_IS_FIXED
            x1 = denom * (va * p1s[0] + vb * p1s[1] + vc * p1s[2]);
            x2 = denom * (va * p2s[0] + vb * p2s[1] + vc * p2s[2]);
#else
            float u = va * denom;
            float v = vb * denom;
            float w = 1.0f - u - v;
            x1 = u * p1s[0] + v * p1s[1] + w * p1s[2];
            x2 = u * p2s[0] + v * p2s[1] + w * p2s[2];
#endif
            return(3);
        }