Exemplo n.º 1
0
        /// <summary>Solves the velocity constraints.</summary>
        /// <param name="step">The time step for this update.</param>
        /// <param name="velocities">The velocities of the related bodies.</param>
        internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
        {
            var vA = velocities[_tmp.IndexA].LinearVelocity;
            var wA = velocities[_tmp.IndexA].AngularVelocity;
            var vB = velocities[_tmp.IndexB].LinearVelocity;
            var wB = velocities[_tmp.IndexB].AngularVelocity;

            // cDot = dot(u, v + cross(w, r))
            var vpA  = vA + Vector2Util.Cross(wA, ref _tmp.RotA);
            var vpB  = vB + Vector2Util.Cross(wB, ref _tmp.RotB);
            var cDot = Vector2Util.Dot(_tmp.U, vpB - vpA);

            var impulse = -_tmp.Mass * (cDot + _tmp.Bias + _tmp.Gamma * _impulse);

            _impulse += impulse;

            var p = impulse * _tmp.U;

            vA -= _tmp.InverseMassA * p;
            wA -= _tmp.InverseInertiaA * Vector2Util.Cross(ref _tmp.RotA, ref p);
            vB += _tmp.InverseMassB * p;
            wB += _tmp.InverseInertiaB * Vector2Util.Cross(ref _tmp.RotB, ref p);

            velocities[_tmp.IndexA].LinearVelocity  = vA;
            velocities[_tmp.IndexA].AngularVelocity = wA;
            velocities[_tmp.IndexB].LinearVelocity  = vB;
            velocities[_tmp.IndexB].AngularVelocity = wB;
        }
Exemplo n.º 2
0
        /// <summary>Solves the velocity constraints.</summary>
        /// <param name="step">The time step for this update.</param>
        /// <param name="velocities">The velocities of the related bodies.</param>
        internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
        {
            var vA = velocities[_tmp.IndexA].LinearVelocity;
            var wA = velocities[_tmp.IndexA].AngularVelocity;
            var vB = velocities[_tmp.IndexB].LinearVelocity;
            var wB = velocities[_tmp.IndexB].AngularVelocity;

            var vpA = vA + Vector2Util.Cross(wA, ref _tmp.RotA);
            var vpB = vB + Vector2Util.Cross(wB, ref _tmp.RotB);

            var cdot    = -Vector2Util.Dot(ref _tmp.AxisA, ref vpA) - _ratio * Vector2Util.Dot(ref _tmp.AxisB, ref vpB);
            var impulse = -_tmp.Mass * cdot;

            _impulse += impulse;

            var pA = -impulse * _tmp.AxisA;
            var pB = -_ratio * impulse * _tmp.AxisB;

            vA += _tmp.InverseMassA * pA;
            wA += _tmp.InverseInertiaA * Vector2Util.Cross(ref _tmp.RotA, ref pA);
            vB += _tmp.InverseMassB * pB;
            wB += _tmp.InverseInertiaB * Vector2Util.Cross(ref _tmp.RotB, ref pB);

            velocities[_tmp.IndexA].LinearVelocity  = vA;
            velocities[_tmp.IndexA].AngularVelocity = wA;
            velocities[_tmp.IndexB].LinearVelocity  = vB;
            velocities[_tmp.IndexB].AngularVelocity = wB;
        }
Exemplo n.º 3
0
        /// <summary>
        ///     Get the mass data for this fixture. The mass data is based on the density and the shape. The rotational
        ///     inertia is about the shape's origin. This operation may be expensive.
        /// </summary>
        /// <param name="mass">The mass of this fixture.</param>
        /// <param name="center">The center of mass relative to the local origin.</param>
        /// <param name="inertia">The inertia about the local origin.</param>
        internal override void GetMassData(out float mass, out LocalPoint center, out float inertia)
        {
            mass   = Density * MathHelper.Pi * Radius * Radius;
            center = Center;

            // Inertia about the local origin.
            inertia = mass * (0.5f * Radius * Radius + Vector2Util.Dot(ref Center, ref Center));
        }
Exemplo n.º 4
0
        /// <summary>This returns true if the position errors are within tolerance, allowing an early exit from the iteration loop.</summary>
        /// <param name="positions">The positions of the related bodies.</param>
        /// <returns>
        ///     <c>true</c> if the position errors are within tolerance.
        /// </returns>
        internal override bool SolvePositionConstraints(Position[] positions)
        {
            var cA = positions[_tmp.IndexA].Point;
            var aA = positions[_tmp.IndexA].Angle;
            var cB = positions[_tmp.IndexB].Point;
            var aB = positions[_tmp.IndexB].Angle;

            var qA = new Rotation(aA);
            var qB = new Rotation(aB);

            var rA = qA * (_localAnchorA - _tmp.LocalCenterA);
            var rB = qB * (_localAnchorB - _tmp.LocalCenterB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
            var d = (Vector2)(cB - cA) + (rB - rA);
// ReSharper restore RedundantCast

            var ay = qA * _localYAxisA;

            var sAy = Vector2Util.Cross(d + rA, ay);
            var sBy = Vector2Util.Cross(rB, ay);

            var c = Vector2Util.Dot(ref d, ref ay);

            var k = _tmp.InverseMassA + _tmp.InverseMassB + _tmp.InverseInertiaA * _tmp.SAy * _tmp.SAy +
                    _tmp.InverseInertiaB * _tmp.SBy * _tmp.SBy;

            float impulse;

// ReSharper disable CompareOfFloatsByEqualityOperator Intentional.
            if (k != 0.0f)
// ReSharper restore CompareOfFloatsByEqualityOperator
            {
                impulse = -c / k;
            }
            else
            {
                impulse = 0.0f;
            }

            var p  = impulse * ay;
            var lA = impulse * sAy;
            var lB = impulse * sBy;

            cA -= _tmp.InverseMassA * p;
            aA -= _tmp.InverseInertiaA * lA;
            cB += _tmp.InverseMassB * p;
            aB += _tmp.InverseInertiaB * lB;

            positions[_tmp.IndexA].Point = cA;
            positions[_tmp.IndexA].Angle = aA;
            positions[_tmp.IndexB].Point = cB;
            positions[_tmp.IndexB].Angle = aB;

            return(System.Math.Abs(c) <= Settings.LinearSlop);
        }
Exemplo n.º 5
0
        /// <summary>Test the specified point for containment in this fixture.</summary>
        /// <param name="localPoint">The point in local coordinates.</param>
        /// <returns>Whether the point is contained in this fixture or not.</returns>
        public override bool TestPoint(Vector2 localPoint)
        {
            for (var i = 0; i < Count; ++i)
            {
                if (Vector2Util.Dot(Normals[i], localPoint - Vertices[i]) > 0.0f)
                {
                    return(false);
                }
            }

            return(true);
        }
Exemplo n.º 6
0
        /// Clipping for contact manifolds.
        private static int ClipSegmentToLine(
            out FixedArray2 <ClipVertex> vOut,
            FixedArray2 <ClipVertex> vIn,
            Vector2 normal,
            float offset,
            int vertexIndexA)
        {
            // Satisfy outs.
            vOut = new FixedArray2 <ClipVertex>();

            // Start with no output points
            var numOut = 0;

            // Calculate the distance of end points to the line
            var distance0 = Vector2Util.Dot(ref normal, ref vIn.Item1.Vertex) - offset;
            var distance1 = Vector2Util.Dot(ref normal, ref vIn.Item2.Vertex) - offset;

            // If the points are behind the plane
            if (distance0 <= 0.0f)
            {
                vOut.Item1 = vIn.Item1;
                ++numOut;
                if (distance1 <= 0.0f)
                {
                    vOut.Item2 = vIn.Item2;
                    ++numOut;
                }
            }
            else if (distance1 <= 0.0f)
            {
                vOut.Item1 = vIn.Item2;
                ++numOut;
            }

            // If the points are on different sides of the plane
            if (distance0 * distance1 < 0.0f)
            {
                System.Diagnostics.Debug.Assert(numOut == 1);

                // Find intersection point of edge and plane
                var interp = distance0 / (distance0 - distance1);
                vOut.Item2.Vertex = vIn.Item1.Vertex + interp * (vIn.Item2.Vertex - vIn.Item1.Vertex);

                // VertexA is hitting edgeB.
                vOut.Item2.Id.Feature.IndexA = (byte)vertexIndexA;
                vOut.Item2.Id.Feature.IndexB = vIn.Item1.Id.Feature.IndexB;
                vOut.Item2.Id.Feature.TypeA  = (byte)ContactFeature.FeatureType.Vertex;
                vOut.Item2.Id.Feature.TypeB  = (byte)ContactFeature.FeatureType.Face;
                ++numOut;
            }

            return(numOut);
        }
Exemplo n.º 7
0
            public float Evaluate(int indexA, int indexB, float t)
            {
                WorldTransform xfA, xfB;

                _sweepA.GetTransform(out xfA, t);
                _sweepB.GetTransform(out xfB, t);

                switch (_type)
                {
                case Type.Points:
                {
                    var localPointA = _proxyA.Vertices[indexA];
                    var localPointB = _proxyB.Vertices[indexB];

                    var pointA = xfA.ToGlobal(localPointA);
                    var pointB = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
                    return(Vector2Util.Dot((Vector2)(pointB - pointA), _axis));
// ReSharper restore RedundantCast
                }

                case Type.FaceA:
                {
                    var normal = xfA.Rotation * _axis;
                    var pointA = xfA.ToGlobal(_localPoint);

                    var localPointB = _proxyB.Vertices[indexB];
                    var pointB      = xfB.ToGlobal(localPointB);

// ReSharper disable RedundantCast Necessary for FarPhysics.
                    return(Vector2Util.Dot((Vector2)(pointB - pointA), normal));
// ReSharper restore RedundantCast
                }

                case Type.FaceB:
                {
                    var normal = xfB.Rotation * _axis;
                    var pointB = xfB.ToGlobal(_localPoint);

                    var localPointA = _proxyA.Vertices[indexA];
                    var pointA      = xfA.ToGlobal(localPointA);

// ReSharper disable RedundantCast Necessary for FarPhysics.
                    return(Vector2Util.Dot((Vector2)(pointA - pointB), normal));
// ReSharper restore RedundantCast
                }

                default:
                    throw new ArgumentOutOfRangeException();
                }
            }
Exemplo n.º 8
0
            /// Get the supporting vertex index in the given direction.
            public int GetSupport(Vector2 d)
            {
                var bestIndex = 0;
                var bestValue = Vector2Util.Dot(ref Vertices[0], ref d);

                for (var i = 1; i < _count; ++i)
                {
                    var value = Vector2Util.Dot(ref Vertices[i], ref d);
                    if (value > bestValue)
                    {
                        bestIndex = i;
                        bestValue = value;
                    }
                }

                return(bestIndex);
            }
Exemplo n.º 9
0
        /// <summary>
        /// Returns true if the point is contained by the triangle.
        /// </summary>
        /// <remarks>
        /// <para>
        /// The test is inclusive of the triangle edges.
        /// </para>
        /// </remarks>
        /// <param name="p">The point to test.</param>
        /// <param name="a">Vertex A of triangle ABC.</param>
        /// <param name="b">Vertex B of triangle ABC</param>
        /// <param name="c">Vertex C of triangle ABC</param>
        /// <returns>True if the point is contained by the triangle ABC.</returns>
        public static bool Contains(Vector2 p, Vector2 a, Vector2 b, Vector2 c)
        {
            Vector2 dirAB = b - a;
            Vector2 dirAC = c - a;
            Vector2 dirAP = p - a;

            float dotABAB = Vector2Util.Dot(dirAB, dirAB);
            float dotACAB = Vector2Util.Dot(dirAC, dirAB);
            float dotACAC = Vector2Util.Dot(dirAC, dirAC);
            float dotACAP = Vector2Util.Dot(dirAC, dirAP);
            float dotABAP = Vector2Util.Dot(dirAB, dirAP);

            float invDenom = 1 / (dotACAC * dotABAB - dotACAB * dotACAB);
            float u        = (dotABAB * dotACAP - dotACAB * dotABAP) * invDenom;
            float v        = (dotACAC * dotABAP - dotACAB * dotACAP) * invDenom;

            // Altered this slightly from the reference so that points on the
            // vertices and edges are considered to be inside the triangle.
            return((u >= 0) && (v >= 0) && (u + v <= 1));
        }
Exemplo n.º 10
0
        // Find the separation between poly1 and poly2 for a give edge normal on poly1.
        private static float EdgeSeparation(
            int edge1,
            PolygonFixture poly1,
            WorldTransform xf1,
            PolygonFixture poly2,
            WorldTransform xf2)
        {
            var vertices1 = poly1.Vertices;
            var normals1  = poly1.Normals;

            var count2    = poly2.Count;
            var vertices2 = poly2.Vertices;

            System.Diagnostics.Debug.Assert(0 <= edge1 && edge1 < poly1.Count);

            // Convert normal from poly1's frame into poly2's frame.
            var normal1World = xf1.Rotation * normals1[edge1];
            var normal1      = -xf2.Rotation * normal1World;

            // Find support vertex on poly2 for -normal.
            var index  = 0;
            var minDot = float.MaxValue;

            for (var i = 0; i < count2; ++i)
            {
                var dot = Vector2Util.Dot(ref vertices2[i], ref normal1);
                if (dot < minDot)
                {
                    minDot = dot;
                    index  = i;
                }
            }

            var v1 = xf1.ToGlobal(vertices1[edge1]);
            var v2 = xf2.ToGlobal(vertices2[index]);
// ReSharper disable RedundantCast Necessary for FarPhysics.
            var separation = Vector2Util.Dot((Vector2)(v2 - v1), normal1World);

// ReSharper restore RedundantCast
            return(separation);
        }
Exemplo n.º 11
0
        public static bool CollideCircles(
            Fixture fixtureA,
            WorldTransform xfA,
            Fixture fixtureB,
            WorldTransform xfB,
            out Manifold manifold)
        {
            manifold = new Manifold();

            var circleA = fixtureA as CircleFixture;
            var circleB = fixtureB as CircleFixture;

            System.Diagnostics.Debug.Assert(circleA != null);
            System.Diagnostics.Debug.Assert(circleB != null);

            var pA = xfA.ToGlobal(circleA.Center);
            var pB = xfB.ToGlobal(circleB.Center);

// ReSharper disable RedundantCast Necessary for FarPhysics.
            var d = (Vector2)(pB - pA);
// ReSharper restore RedundantCast

            var distanceSquared = Vector2Util.Dot(ref d, ref d);
            var radius          = circleA.Radius + circleB.Radius;

            if (distanceSquared > radius * radius)
            {
                return(false);
            }

            manifold.Type                    = Manifold.ManifoldType.Circles;
            manifold.LocalPoint              = circleA.Center;
            manifold.LocalNormal             = Vector2.Zero;
            manifold.PointCount              = 1;
            manifold.Points.Item1.LocalPoint = circleB.Center;
            manifold.Points.Item1.Id.Key     = 0;

            return(true);
        }
Exemplo n.º 12
0
        /// <summary>Solves the velocity constraints.</summary>
        /// <param name="step">The time step for this update.</param>
        /// <param name="velocities">The velocities of the related bodies.</param>
        internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
        {
            var vA = velocities[_tmp.IndexA].LinearVelocity;
            var wA = velocities[_tmp.IndexA].AngularVelocity;
            var vB = velocities[_tmp.IndexB].LinearVelocity;
            var wB = velocities[_tmp.IndexB].AngularVelocity;
            var vC = velocities[_tmp.IndexC].LinearVelocity;
            var wC = velocities[_tmp.IndexC].AngularVelocity;
            var vD = velocities[_tmp.IndexD].LinearVelocity;
            var wD = velocities[_tmp.IndexD].AngularVelocity;

            var cDot = Vector2Util.Dot(_tmp.JvAC, vA - vC) + Vector2Util.Dot(_tmp.JvBD, vB - vD);

            cDot += (_tmp.JwA * wA - _tmp.JwC * wC) + (_tmp.JwB * wB - _tmp.JwD * wD);

            var impulse = -_tmp.Mass * cDot;

            _impulse += impulse;

            vA += (_tmp.InverseMassA * impulse) * _tmp.JvAC;
            wA += _tmp.InverseInertiaA * impulse * _tmp.JwA;
            vB += (_tmp.InverseMassB * impulse) * _tmp.JvBD;
            wB += _tmp.InverseInertiaB * impulse * _tmp.JwB;
            vC -= (_tmp.InverseMassC * impulse) * _tmp.JvAC;
            wC -= _tmp.InverseInertiaC * impulse * _tmp.JwC;
            vD -= (_tmp.InverseMassD * impulse) * _tmp.JvBD;
            wD -= _tmp.InverseInertiaD * impulse * _tmp.JwD;

            velocities[_tmp.IndexA].LinearVelocity  = vA;
            velocities[_tmp.IndexA].AngularVelocity = wA;
            velocities[_tmp.IndexB].LinearVelocity  = vB;
            velocities[_tmp.IndexB].AngularVelocity = wB;
            velocities[_tmp.IndexC].LinearVelocity  = vC;
            velocities[_tmp.IndexC].AngularVelocity = wC;
            velocities[_tmp.IndexD].LinearVelocity  = vD;
            velocities[_tmp.IndexD].AngularVelocity = wD;
        }
Exemplo n.º 13
0
        /// <summary>Solves the velocity constraints.</summary>
        /// <param name="step">The time step for this update.</param>
        /// <param name="velocities">The velocities of the related bodies.</param>
        internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
        {
            var vA = velocities[_tmp.IndexA].LinearVelocity;
            var wA = velocities[_tmp.IndexA].AngularVelocity;
            var vB = velocities[_tmp.IndexB].LinearVelocity;
            var wB = velocities[_tmp.IndexB].AngularVelocity;

            // Cdot = dot(u, v + cross(w, r))
            var vpA  = vA + Vector2Util.Cross(wA, ref _tmp.RotA);
            var vpB  = vB + Vector2Util.Cross(wB, ref _tmp.RotB);
            var c    = _length - _maxLength;
            var cdot = Vector2Util.Dot(_tmp.Axis, vpB - vpA);

            // Predictive constraint.
            if (c < 0.0f)
            {
                cdot += step.InverseDeltaT * c;
            }

            var impulse    = -_tmp.Mass * cdot;
            var oldImpulse = _impulse;

            _impulse = System.Math.Min(0.0f, _impulse + impulse);
            impulse  = _impulse - oldImpulse;

            var p = impulse * _tmp.Axis;

            vA -= _tmp.InverseMassA * p;
            wA -= _tmp.InverseInertiaA * Vector2Util.Cross(ref _tmp.RotA, ref p);
            vB += _tmp.InverseMassB * p;
            wB += _tmp.InverseInertiaB * Vector2Util.Cross(ref _tmp.RotB, ref p);

            velocities[_tmp.IndexA].LinearVelocity  = vA;
            velocities[_tmp.IndexA].AngularVelocity = wA;
            velocities[_tmp.IndexB].LinearVelocity  = vB;
            velocities[_tmp.IndexB].AngularVelocity = wB;
        }
Exemplo n.º 14
0
            // Solve a line segment using barycentric coordinates.
            //
            // p = a1 * w1 + a2 * w2
            // a1 + a2 = 1
            //
            // The vector from the origin to the closest point on the line is
            // perpendicular to the line.
            // e12 = w2 - w1
            // dot(p, e) = 0
            // a1 * dot(w1, e) + a2 * dot(w2, e) = 0
            //
            // 2-by-2 linear system
            // [1      1     ][a1] = [1]
            // [w1.e12 w2.e12][a2] = [0]
            //
            // Define
            // d12_1 =  dot(w2, e12)
            // d12_2 = -dot(w1, e12)
            // d12 = d12_1 + d12_2
            //
            // Solution
            // a1 = d12_1 / d12
            // a2 = d12_2 / d12
            public void Solve2()
            {
                var w1  = Vertices.Item1.VertexDelta;
                var w2  = Vertices.Item2.VertexDelta;
                var e12 = w2 - w1;

                // w1 region
                var d12_2 = -Vector2Util.Dot(ref w1, ref e12);

                if (d12_2 <= 0.0f)
                {
                    // a2 <= 0, so we clamp it to 0
                    Vertices.Item1.Alpha = 1.0f;
                    Count = 1;
                    return;
                }

                // w2 region
                var d12_1 = Vector2Util.Dot(ref w2, ref e12);

                if (d12_1 <= 0.0f)
                {
                    // a1 <= 0, so we clamp it to 0
                    Vertices.Item2.Alpha = 1.0f;
                    Count          = 1;
                    Vertices.Item1 = Vertices.Item2;
                    return;
                }

                // Must be in e12 region.
                var inv_d12 = 1.0f / (d12_1 + d12_2);

                Vertices.Item1.Alpha = d12_1 * inv_d12;
                Vertices.Item2.Alpha = d12_2 * inv_d12;
                Count = 2;
            }
Exemplo n.º 15
0
        /// <summary>This returns true if the position errors are within tolerance, allowing an early exit from the iteration loop.</summary>
        /// <param name="positions">The positions of the related bodies.</param>
        /// <returns>
        ///     <c>true</c> if the position errors are within tolerance.
        /// </returns>
        internal override bool SolvePositionConstraints(Position[] positions)
        {
            var cA = positions[_tmp.IndexA].Point;
            var aA = positions[_tmp.IndexA].Angle;
            var cB = positions[_tmp.IndexB].Point;
            var aB = positions[_tmp.IndexB].Angle;
            var cC = positions[_tmp.IndexC].Point;
            var aC = positions[_tmp.IndexC].Angle;
            var cD = positions[_tmp.IndexD].Point;
            var aD = positions[_tmp.IndexD].Angle;

            var qA = new Rotation(aA);
            var qB = new Rotation(aB);
            var qC = new Rotation(aC);
            var qD = new Rotation(aD);

            const float linearError = 0.0f;

            float coordinateA, coordinateB;

            Vector2 JvAC, JvBD;
            float   JwA, JwB, JwC, JwD;
            var     mass = 0.0f;

            if (_typeA == JointType.Revolute)
            {
                JvAC  = Vector2.Zero;
                JwA   = 1.0f;
                JwC   = 1.0f;
                mass += _tmp.InverseInertiaA + _tmp.InverseInertiaC;

                coordinateA = aA - aC - _referenceAngleA;
            }
            else
            {
                var u  = qC * _localAxisC;
                var rC = qC * (_localAnchorC - _tmp.LcC);
                var rA = qA * (_localAnchorA - _tmp.LcA);
                JvAC  = u;
                JwC   = Vector2Util.Cross(ref rC, ref u);
                JwA   = Vector2Util.Cross(ref rA, ref u);
                mass += _tmp.InverseMassC + _tmp.InverseMassA + _tmp.InverseInertiaC * JwC * JwC +
                        _tmp.InverseInertiaA * JwA * JwA;

                var pC = _localAnchorC - _tmp.LcC;
// ReSharper disable RedundantCast Necessary for FarPhysics.
                var pA = -qC * (rA + (Vector2)(cA - cC));
// ReSharper restore RedundantCast
                coordinateA = Vector2Util.Dot(pA - pC, _localAxisC);
            }

            if (_typeB == JointType.Revolute)
            {
                JvBD  = Vector2.Zero;
                JwB   = _ratio;
                JwD   = _ratio;
                mass += _ratio * _ratio * (_tmp.InverseInertiaB + _tmp.InverseInertiaD);

                coordinateB = aB - aD - _referenceAngleB;
            }
            else
            {
                var u  = qD * _localAxisD;
                var rD = qD * (_localAnchorD - _tmp.LcD);
                var rB = qB * (_localAnchorB - _tmp.LcB);
                JvBD  = _ratio * u;
                JwD   = _ratio * Vector2Util.Cross(ref rD, ref u);
                JwB   = _ratio * Vector2Util.Cross(ref rB, ref u);
                mass += _ratio * _ratio * (_tmp.InverseMassD + _tmp.InverseMassB) + _tmp.InverseInertiaD * JwD * JwD +
                        _tmp.InverseInertiaB * JwB * JwB;

                var pD = _localAnchorD - _tmp.LcD;
// ReSharper disable RedundantCast Necessary for FarPhysics.
                var pB = -qD * (rB + (Vector2)(cB - cD));
// ReSharper restore RedundantCast
                coordinateB = Vector2Util.Dot(pB - pD, _localAxisD);
            }

            var c = (coordinateA + _ratio * coordinateB) - _constant;

            var impulse = 0.0f;

            if (mass > 0.0f)
            {
                impulse = -c / mass;
            }

            cA += _tmp.InverseMassA * impulse * JvAC;
            aA += _tmp.InverseInertiaA * impulse * JwA;
            cB += _tmp.InverseMassB * impulse * JvBD;
            aB += _tmp.InverseInertiaB * impulse * JwB;
            cC -= _tmp.InverseMassC * impulse * JvAC;
            aC -= _tmp.InverseInertiaC * impulse * JwC;
            cD -= _tmp.InverseMassD * impulse * JvBD;
            aD -= _tmp.InverseInertiaD * impulse * JwD;

            positions[_tmp.IndexA].Point = cA;
            positions[_tmp.IndexA].Angle = aA;
            positions[_tmp.IndexB].Point = cB;
            positions[_tmp.IndexB].Angle = aB;
            positions[_tmp.IndexC].Point = cC;
            positions[_tmp.IndexC].Angle = aC;
            positions[_tmp.IndexD].Point = cD;
            positions[_tmp.IndexD].Angle = aD;

            // TODO_ERIN not implemented
            return(linearError < Settings.LinearSlop);
        }
Exemplo n.º 16
0
        /// <summary>Initializes this joint with the specified parameters.</summary>
        internal void Initialize(IManager manager, Joint jointA, Joint jointB, Body bodyA, Body bodyB, float ratio)
        {
            Manager = manager;

            _typeA = jointA.Type;
            _typeB = jointB.Type;

            System.Diagnostics.Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic);
            System.Diagnostics.Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic);

            float coordinateA, coordinateB;

            // TODO_ERIN there might be some problem with the joint edges in b2Joint.

            _bodyIdA = bodyA.Id;
            _bodyIdB = bodyB.Id;
            _bodyIdC = jointA.BodyA == bodyA ? jointA.BodyB.Id : jointA.BodyA.Id;
            _bodyIdD = jointB.BodyA == bodyB ? jointB.BodyB.Id : jointB.BodyA.Id;

            // Get geometry of joint1
            var xfA = BodyA.Transform;
            var aA  = BodyA.Sweep.Angle;
            var xfC = BodyC.Transform;
            var aC  = BodyC.Sweep.Angle;

            if (_typeA == JointType.Revolute)
            {
                var revolute = (RevoluteJoint)jointA;
                if (bodyA == jointA.BodyA)
                {
                    _localAnchorA = revolute.LocalAnchorA;
                    _localAnchorC = revolute.LocalAnchorB;
                }
                else
                {
                    _localAnchorA = revolute.LocalAnchorB;
                    _localAnchorC = revolute.LocalAnchorA;
                }
                _referenceAngleA = revolute.ReferenceAngle;
                _localAxisC      = Vector2.Zero;

                coordinateA = aA - aC - _referenceAngleA;
            }
            else
            {
                var prismatic = (PrismaticJoint)jointA;
                if (bodyA == jointA.BodyA)
                {
                    _localAnchorA = prismatic.LocalAnchorA;
                    _localAnchorC = prismatic.LocalAnchorB;
                }
                else
                {
                    _localAnchorA = prismatic.LocalAnchorB;
                    _localAnchorC = prismatic.LocalAnchorA;
                }
                _referenceAngleA = prismatic.ReferenceAngle;
                _localAxisC      = prismatic.LocalAxisA;

                var pC = _localAnchorC;
// ReSharper disable RedundantCast Necessary for FarPhysics.
                var pA = -xfC.Rotation *
                         ((xfA.Rotation * _localAnchorA) + (Vector2)(xfA.Translation - xfC.Translation));
// ReSharper restore RedundantCast
                coordinateA = Vector2Util.Dot(pA - pC, _localAxisC);
            }

            // Get geometry of joint2
            var xfB = BodyB.Transform;
            var aB  = BodyB.Sweep.Angle;
            var xfD = BodyD.Transform;
            var aD  = BodyD.Sweep.Angle;

            if (_typeB == JointType.Revolute)
            {
                var revolute = (RevoluteJoint)jointB;
                if (bodyB == revolute.BodyA)
                {
                    _localAnchorB = revolute.LocalAnchorA;
                    _localAnchorD = revolute.LocalAnchorB;
                }
                else
                {
                    _localAnchorB = revolute.LocalAnchorB;
                    _localAnchorD = revolute.LocalAnchorA;
                }
                _referenceAngleB = revolute.ReferenceAngle;
                _localAxisD      = Vector2.Zero;

                coordinateB = aB - aD - _referenceAngleB;
            }
            else
            {
                var prismatic = (PrismaticJoint)jointB;
                if (bodyB == prismatic.BodyA)
                {
                    _localAnchorB = prismatic.LocalAnchorA;
                    _localAnchorD = prismatic.LocalAnchorB;
                }
                else
                {
                    _localAnchorB = prismatic.LocalAnchorB;
                    _localAnchorD = prismatic.LocalAnchorA;
                }
                _referenceAngleB = prismatic.ReferenceAngle;
                _localAxisD      = prismatic.LocalAxisA;

                var pD = _localAnchorD;
// ReSharper disable RedundantCast Necessary for FarPhysics.
                var pB = -xfD.Rotation *
                         ((xfB.Rotation * _localAnchorB) + (Vector2)(xfB.Translation - xfD.Translation));
// ReSharper restore RedundantCast
                coordinateB = Vector2Util.Dot(pB - pD, _localAxisD);
            }

            _ratio = ratio;

            _constant = coordinateA + _ratio * coordinateB;
            _impulse  = 0.0f;
        }
Exemplo n.º 17
0
        public static bool CollidePolygonAndCircle(
            Fixture fixtureA,
            WorldTransform xfA,
            Fixture fixtureB,
            WorldTransform xfB,
            out Manifold manifold)
        {
            manifold = new Manifold();

            var polygonA = fixtureA as PolygonFixture;
            var circleB  = fixtureB as CircleFixture;

            System.Diagnostics.Debug.Assert(polygonA != null);
            System.Diagnostics.Debug.Assert(circleB != null);

            // Compute circle position in the frame of the polygon.
            var centerInA = xfA.ToLocal(xfB.ToGlobal(circleB.Center));

            var totalRadius  = polygonA.Radius + circleB.Radius;
            var vertexCountA = polygonA.Count;
            var verticesA    = polygonA.Vertices;
            var normalsA     = polygonA.Normals;

            // Find the min separating edge.
            var normalIndexA = 0;
            var separation   = float.MinValue;

            for (var i = 0; i < vertexCountA; ++i)
            {
                var s = Vector2Util.Dot(normalsA[i], centerInA - verticesA[i]);

                if (s > totalRadius)
                {
                    // Early out.
                    manifold = new Manifold();
                    return(false);
                }

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

            // Vertices that subtend the incident face.
            var vertexIndexA1 = normalIndexA;
            var vertexIndexA2 = vertexIndexA1 + 1 < vertexCountA ? vertexIndexA1 + 1 : 0;
            var vertexA1      = verticesA[vertexIndexA1];
            var vertexA2      = verticesA[vertexIndexA2];

            // If the center is inside the polygon ...
            if (separation < Settings.Epsilon)
            {
                manifold.Type       = Manifold.ManifoldType.FaceA;
                manifold.PointCount = 1;
                manifold.Points.Item1.LocalPoint = circleB.Center;
                manifold.Points.Item1.Id.Key     = 0;

                manifold.LocalNormal = normalsA[normalIndexA];
                manifold.LocalPoint  = 0.5f * (vertexA1 + vertexA2);
                return(true);
            }

            // Compute barycentric coordinates
            var u1 = Vector2Util.Dot(centerInA - vertexA1, vertexA2 - vertexA1);
            var u2 = Vector2Util.Dot(centerInA - vertexA2, vertexA1 - vertexA2);

            if (u1 <= 0.0f)
            {
                if (LocalPoint.DistanceSquared(centerInA, vertexA1) <= totalRadius * totalRadius)
                {
                    var normalInA = centerInA - vertexA1;
                    normalInA.Normalize();

                    manifold.Type       = Manifold.ManifoldType.FaceA;
                    manifold.PointCount = 1;
                    manifold.Points.Item1.LocalPoint = circleB.Center;
                    manifold.Points.Item1.Id.Key     = 0;

                    manifold.LocalNormal = normalInA;
                    manifold.LocalPoint  = vertexA1;
                    return(true);
                }
            }
            else if (u2 <= 0.0f)
            {
                if (LocalPoint.DistanceSquared(centerInA, vertexA2) <= totalRadius * totalRadius)
                {
                    manifold.Type       = Manifold.ManifoldType.FaceA;
                    manifold.PointCount = 1;
                    manifold.Points.Item1.LocalPoint = circleB.Center;
                    manifold.Points.Item1.Id.Key     = 0;

                    manifold.LocalNormal = centerInA - vertexA2;
                    manifold.LocalNormal.Normalize();
                    manifold.LocalPoint = vertexA2;
                    return(true);
                }
            }
            else
            {
                var faceCenter = 0.5f * (vertexA1 + vertexA2);
                if (Vector2Util.Dot(centerInA - faceCenter, normalsA[vertexIndexA1]) <= totalRadius)
                {
                    manifold.Type       = Manifold.ManifoldType.FaceA;
                    manifold.PointCount = 1;
                    manifold.Points.Item1.LocalPoint = circleB.Center;
                    manifold.Points.Item1.Id.Key     = 0;

                    manifold.LocalNormal = normalsA[vertexIndexA1];
                    manifold.LocalPoint  = faceCenter;
                    return(true);
                }
            }

            return(false);
        }
Exemplo n.º 18
0
        /// <summary>
        ///     Get the mass data for this fixture. The mass data is based on the density and the shape. The rotational
        ///     inertia is about the shape's origin. This operation may be expensive.
        /// </summary>
        /// <param name="mass">The mass of this fixture.</param>
        /// <param name="center">The center of mass relative to the local origin.</param>
        /// <param name="inertia">The inertia about the local origin.</param>
        internal override void GetMassData(out float mass, out LocalPoint center, out float inertia)
        {
            // Polygon mass, centroid, and inertia.
            // Let rho be the polygon density in mass per unit area.
            // Then:
            // mass = rho * int(dA)
            // centroid.X = (1/mass) * rho * int(x * dA)
            // centroid.Y = (1/mass) * rho * int(y * dA)
            // I = rho * int((x*x + y*y) * dA)
            //
            // We can compute these integrals by summing all the integrals
            // for each triangle of the polygon. To evaluate the integral
            // for a single triangle, we make a change of variables to
            // the (u,v) coordinates of the triangle:
            // x = x0 + e1x * u + e2x * v
            // y = y0 + e1y * u + e2y * v
            // where 0 <= u && 0 <= v && u + v <= 1.
            //
            // We integrate u from [0,1-v] and then v from [0,1].
            // We also need to use the Jacobian of the transformation:
            // D = cross(e1, e2)
            //
            // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
            //
            // The rest of the derivation is handled by computer algebra.

            System.Diagnostics.Debug.Assert(Count >= 3);

            var centroid = LocalPoint.Zero;
            var area     = 0f;
            var I        = 0f;

            // s is the reference point for forming triangles.
            // It's location doesn't change the result (except for rounding error).
            var s = LocalPoint.Zero;

            // This code would put the reference point inside the polygon.
            for (var i = 0; i < Count; ++i)
            {
                s += Vertices[i];
            }
            s /= Count;

            const float inv3 = 1.0f / 3.0f;

            for (var i = 0; i < Count; ++i)
            {
                // Triangle vertices.
                var e1 = Vertices[i] - s;
                var e2 = i + 1 < Count
                             ? Vertices[i + 1] - s
                             : Vertices[0] - s;

                var d = Vector2Util.Cross(ref e1, ref e2);

                var triangleArea = 0.5f * d;
                area += triangleArea;

                // Area weighted centroid
                centroid += triangleArea * inv3 * (e1 + e2);

                var intx2 = e1.X * e1.X + e2.X * e1.X + e2.X * e2.X;
                var inty2 = e1.Y * e1.Y + e2.Y * e1.Y + e2.Y * e2.Y;

                I += (0.25f * inv3 * d) * (intx2 + inty2);
            }

            // Total mass
            mass = Density * area;

            // Center of mass
            System.Diagnostics.Debug.Assert(area > Settings.Epsilon);
            centroid *= 1.0f / area;
            center    = centroid + s;

            // Inertia tensor relative to the local origin (point s).
            inertia = Density * I;

            // Shift to center of mass then to original body origin.
            inertia += mass * (Vector2Util.Dot(ref center, ref center) - Vector2Util.Dot(ref centroid, ref centroid));
        }
Exemplo n.º 19
0
            public static void Initialize(
                out SeparationFunction f,
                SimplexCache cache,
                DistanceProxy proxyA,
                DistanceProxy proxyB,
                Sweep sweepA,
                Sweep sweepB,
                float t1)
            {
                System.Diagnostics.Debug.Assert(0 < cache.Count && cache.Count < 3);

                f._proxyA = proxyA;
                f._proxyB = proxyB;

                f._sweepA = sweepA;
                f._sweepB = sweepB;

                WorldTransform xfA, xfB;

                f._sweepA.GetTransform(out xfA, t1);
                f._sweepB.GetTransform(out xfB, t1);

                if (cache.Count == 1)
                {
                    f._type = Type.Points;
                    var localPointA = f._proxyA.Vertices[cache.IndexA.Item1];
                    var localPointB = f._proxyB.Vertices[cache.IndexB.Item1];
                    var pointA      = xfA.ToGlobal(localPointA);
                    var pointB      = xfB.ToGlobal(localPointB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
                    f._axis = (Vector2)(pointB - pointA);
// ReSharper restore RedundantCast
                    f._axis.Normalize();
                    f._localPoint = LocalPoint.Zero;
                }
                else if (cache.IndexA.Item1 == cache.IndexA.Item2)
                {
                    // Two points on B and one on A.
                    f._type = Type.FaceB;
                    var localPointB1 = proxyB.Vertices[cache.IndexB.Item1];
                    var localPointB2 = proxyB.Vertices[cache.IndexB.Item2];

                    f._axis = Vector2Util.Cross(localPointB2 - localPointB1, 1.0f);
                    f._axis.Normalize();
                    var normal = xfB.Rotation * f._axis;

                    f._localPoint = 0.5f * (localPointB1 + localPointB2);
                    var pointB = xfB.ToGlobal(f._localPoint);

                    var localPointA = proxyA.Vertices[cache.IndexA[0]];
                    var pointA      = xfA.ToGlobal(localPointA);

// ReSharper disable RedundantCast Necessary for FarPhysics.
                    var s = Vector2Util.Dot((Vector2)(pointA - pointB), normal);
// ReSharper restore RedundantCast
                    if (s < 0.0f)
                    {
                        f._axis = -f._axis;
                    }
                }
                else
                {
                    // Two points on A and one or two points on B.
                    f._type = Type.FaceA;
                    var localPointA1 = f._proxyA.Vertices[cache.IndexA.Item1];
                    var localPointA2 = f._proxyA.Vertices[cache.IndexA.Item2];

                    f._axis = Vector2Util.Cross(localPointA2 - localPointA1, 1.0f);
                    f._axis.Normalize();
                    var normal = xfA.Rotation * f._axis;

                    f._localPoint = 0.5f * (localPointA1 + localPointA2);
                    var pointA = xfA.ToGlobal(f._localPoint);

                    var localPointB = f._proxyB.Vertices[cache.IndexB[0]];
                    var pointB      = xfB.ToGlobal(localPointB);

// ReSharper disable RedundantCast Necessary for FarPhysics.
                    var s = Vector2Util.Dot((Vector2)(pointB - pointA), normal);
// ReSharper restore RedundantCast
                    if (s < 0.0f)
                    {
                        f._axis = -f._axis;
                    }
                }
            }
Exemplo n.º 20
0
// ReSharper disable RedundantCast Necessary for FarPhysics.
        private static void InitializePositionSolverManifold(
            ContactPositionConstraint pc,
            WorldTransform xfA,
            WorldTransform xfB,
            int index,
            out Vector2 normal,
            out WorldPoint point,
            out float separation)
        {
            System.Diagnostics.Debug.Assert(pc.PointCount > 0);

            switch (pc.Type)
            {
            case Manifold.ManifoldType.Circles:
            {
                var pointA = xfA.ToGlobal(pc.LocalPoint);
                var pointB = xfB.ToGlobal(pc.LocalPoints[0]);

                normal.X = (float)(pointB.X - pointA.X);
                normal.Y = (float)(pointB.Y - pointA.Y);
                normal.Normalize();

#if FARMATH
                // Avoid multiplication of far values.
                point.X = pointA.X + 0.5f * (float)(pointB.X - pointA.X);
                point.Y = pointA.Y + 0.5f * (float)(pointB.Y - pointA.Y);
#else
                point.X = 0.5f * (pointA.X + pointB.X);
                point.Y = 0.5f * (pointA.Y + pointB.Y);
#endif

                separation = Vector2Util.Dot((Vector2)(pointB - pointA), normal) - pc.RadiusA - pc.RadiusB;

                break;
            }

            case Manifold.ManifoldType.FaceA:
            {
                normal.X = xfA.Rotation.Cos * pc.LocalNormal.X - xfA.Rotation.Sin * pc.LocalNormal.Y;
                normal.Y = xfA.Rotation.Sin * pc.LocalNormal.X + xfA.Rotation.Cos * pc.LocalNormal.Y;

                var planePoint = xfA.ToGlobal(pc.LocalPoint);
                point = xfB.ToGlobal(pc.LocalPoints[index]);

                separation = Vector2Util.Dot((Vector2)(point - planePoint), normal) - pc.RadiusA - pc.RadiusB;

                break;
            }

            case Manifold.ManifoldType.FaceB:
            {
                normal.X = xfB.Rotation.Cos * pc.LocalNormal.X - xfB.Rotation.Sin * pc.LocalNormal.Y;
                normal.Y = xfB.Rotation.Sin * pc.LocalNormal.X + xfB.Rotation.Cos * pc.LocalNormal.Y;

                var planePoint = xfB.ToGlobal(pc.LocalPoint);
                point = xfA.ToGlobal(pc.LocalPoints[index]);

                separation = Vector2Util.Dot((Vector2)(point - planePoint), normal) - pc.RadiusA - pc.RadiusB;

                // Ensure normal points from A to B
                normal = -normal;

                break;
            }

            default:
                throw new InvalidOperationException();
            }
        }
Exemplo n.º 21
0
        /// <summary>Solves the velocity constraints.</summary>
        /// <param name="step">The time step for this update.</param>
        /// <param name="velocities">The velocities of the related bodies.</param>
        internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
        {
            var mA = _tmp.InverseMassA;
            var mB = _tmp.InverseMassB;
            var iA = _tmp.InverseInertiaA;
            var iB = _tmp.InverseInertiaB;

            var vA = velocities[_tmp.IndexA].LinearVelocity;
            var wA = velocities[_tmp.IndexA].AngularVelocity;
            var vB = velocities[_tmp.IndexB].LinearVelocity;
            var wB = velocities[_tmp.IndexB].AngularVelocity;

            // Solve spring constraint
            {
                var cdot    = Vector2Util.Dot(_tmp.Ax, vB - vA) + _tmp.SBx * wB - _tmp.SAx * wA;
                var impulse = -_tmp.SpringMass * (cdot + _tmp.Bias + _tmp.Gamma * _springImpulse);
                _springImpulse += impulse;

                var p  = impulse * _tmp.Ax;
                var lA = impulse * _tmp.SAx;
                var lB = impulse * _tmp.SBx;

                vA -= mA * p;
                wA -= iA * lA;

                vB += mB * p;
                wB += iB * lB;
            }

            // Solve rotational motor constraint
            {
                var cdot    = wB - wA - _motorSpeed;
                var impulse = -_tmp.MotorMass * cdot;

                var oldImpulse = _motorImpulse;
                var maxImpulse = step.DeltaT * _maxMotorTorque;
                _motorImpulse = MathHelper.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse       = _motorImpulse - oldImpulse;

                wA -= iA * impulse;
                wB += iB * impulse;
            }

            // Solve point to line constraint
            {
                var cdot    = Vector2Util.Dot(_tmp.Ay, vB - vA) + _tmp.SBy * wB - _tmp.SAy * wA;
                var impulse = -_tmp.Mass * cdot;
                _impulse += impulse;

                var p  = impulse * _tmp.Ay;
                var lA = impulse * _tmp.SAy;
                var lB = impulse * _tmp.SBy;

                vA -= mA * p;
                wA -= iA * lA;

                vB += mB * p;
                wB += iB * lB;
            }

            velocities[_tmp.IndexA].LinearVelocity  = vA;
            velocities[_tmp.IndexA].AngularVelocity = wA;
            velocities[_tmp.IndexB].LinearVelocity  = vB;
            velocities[_tmp.IndexB].AngularVelocity = wB;
        }
Exemplo n.º 22
0
        /// <summary>Solves the velocity constraints.</summary>
        /// <param name="step">The time step for this update.</param>
        /// <param name="velocities">The velocities of the related bodies.</param>
        internal override void SolveVelocityConstraints(TimeStep step, Velocity[] velocities)
        {
            var vA = velocities[_tmp.IndexA].LinearVelocity;
            var wA = velocities[_tmp.IndexA].AngularVelocity;
            var vB = velocities[_tmp.IndexB].LinearVelocity;
            var wB = velocities[_tmp.IndexB].AngularVelocity;

            var mA = _tmp.InverseMassA;
            var mB = _tmp.InverseMassB;
            var iA = _tmp.InverseInertiaA;
            var iB = _tmp.InverseInertiaB;

            // Solve linear motor constraint.
            if (_enableMotor && _limitState != LimitState.Equal)
            {
                var cdot       = Vector2Util.Dot(_tmp.Axis, vB - vA) + _tmp.A2 * wB - _tmp.A1 * wA;
                var impulse    = _tmp.MotorMass * (_motorSpeed - cdot);
                var oldImpulse = _motorImpulse;
                var maxImpulse = step.DeltaT * _maxMotorForce;
                _motorImpulse = MathHelper.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse       = _motorImpulse - oldImpulse;

                var p  = impulse * _tmp.Axis;
                var lA = impulse * _tmp.A1;
                var lB = impulse * _tmp.A2;

                vA -= mA * p;
                wA -= iA * lA;

                vB += mB * p;
                wB += iB * lB;
            }

            Vector2 cdot1;

            cdot1.X = Vector2Util.Dot(_tmp.Perp, vB - vA) + _tmp.S2 * wB - _tmp.S1 * wA;
            cdot1.Y = wB - wA;

            if (_enableLimit && _limitState != LimitState.Inactive)
            {
                // Solve prismatic and limit constraint in block form.
                var cdot2 = Vector2Util.Dot(_tmp.Axis, vB - vA) + _tmp.A2 * wB - _tmp.A1 * wA;
                var cdot  = new Vector3(cdot1.X, cdot1.Y, cdot2);

                var f1 = _impulse;
                var df = _tmp.K.Solve33(-cdot);
                _impulse += df;

                if (_limitState == LimitState.AtLower)
                {
                    _impulse.Z = System.Math.Max(_impulse.Z, 0.0f);
                }
                else if (_limitState == LimitState.AtUpper)
                {
                    _impulse.Z = System.Math.Min(_impulse.Z, 0.0f);
                }

                // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
                var b    = -cdot1 - (_impulse.Z - f1.Z) * new Vector2(_tmp.K.Column3.X, _tmp.K.Column3.Y);
                var fToR = _tmp.K.Solve22(b) + new Vector2(f1.X, f1.Y);
                _impulse.X = fToR.X;
                _impulse.Y = fToR.Y;

                df = _impulse - f1;

                var p  = df.X * _tmp.Perp + df.Z * _tmp.Axis;
                var lA = df.X * _tmp.S1 + df.Y + df.Z * _tmp.A1;
                var lB = df.X * _tmp.S2 + df.Y + df.Z * _tmp.A2;

                vA -= mA * p;
                wA -= iA * lA;

                vB += mB * p;
                wB += iB * lB;
            }
            else
            {
                // Limit is inactive, just solve the prismatic constraint in block form.
                var df = _tmp.K.Solve22(-cdot1);
                _impulse.X += df.X;
                _impulse.Y += df.Y;

                var p  = df.X * _tmp.Perp;
                var lA = df.X * _tmp.S1 + df.Y;
                var lB = df.X * _tmp.S2 + df.Y;

                vA -= mA * p;
                wA -= iA * lA;

                vB += mB * p;
                wB += iB * lB;
            }

            velocities[_tmp.IndexA].LinearVelocity  = vA;
            velocities[_tmp.IndexA].AngularVelocity = wA;
            velocities[_tmp.IndexB].LinearVelocity  = vB;
            velocities[_tmp.IndexB].AngularVelocity = wB;
        }
Exemplo n.º 23
0
        // Compute contact points for edge versus circle.
        // This accounts for edge connectivity.
        public static bool CollideEdgeAndCircle(
            Fixture fixtureA,
            WorldTransform xfA,
            Fixture fixtureB,
            WorldTransform xfB,
            out Manifold manifold)
        {
            manifold = new Manifold();

            var edgeA   = fixtureA as EdgeFixture;
            var circleB = fixtureB as CircleFixture;

            System.Diagnostics.Debug.Assert(edgeA != null);
            System.Diagnostics.Debug.Assert(circleB != null);

            // Compute circle in frame of edge
            var q = xfA.ToLocal(xfB.ToGlobal(circleB.Center));

            var a = edgeA.Vertex1;
            var b = edgeA.Vertex2;
            var e = b - a;

            // Barycentric coordinates
            var u = Vector2Util.Dot(e, b - q);
            var v = Vector2Util.Dot(e, q - a);

            var radius = edgeA.Radius + circleB.Radius;

            ContactFeature cf;

            cf.IndexB = 0;
            cf.TypeB  = (byte)ContactFeature.FeatureType.Vertex;

            // Region A
            if (v <= 0.0f)
            {
                var p  = a;
                var d  = q - p;
                var dd = Vector2Util.Dot(ref d, ref d);
                if (dd > radius * radius)
                {
                    return(false);
                }

                // Is there an edge connected to A?
                if (edgeA.HasVertex0)
                {
                    var a1 = edgeA.Vertex0;
                    var b1 = a;
                    var e1 = b1 - a1;
                    var u1 = Vector2Util.Dot(e1, b1 - q);

                    // Is the circle in Region AB of the previous edge?
                    if (u1 > 0.0f)
                    {
                        return(false);
                    }
                }

                cf.IndexA = 0;
                cf.TypeA  = (byte)ContactFeature.FeatureType.Vertex;

                manifold.Type                    = Manifold.ManifoldType.Circles;
                manifold.LocalPoint              = p;
                manifold.LocalNormal             = Vector2.Zero;
                manifold.PointCount              = 1;
                manifold.Points.Item1.Id.Key     = 0;
                manifold.Points.Item1.Id.Feature = cf;
                manifold.Points.Item1.LocalPoint = circleB.Center;

                return(true);
            }

            // Region B
            if (u <= 0.0f)
            {
                var p  = b;
                var d  = q - p;
                var dd = Vector2Util.Dot(ref d, ref d);
                if (dd > radius * radius)
                {
                    return(false);
                }

                // Is there an edge connected to B?
                if (edgeA.HasVertex3)
                {
                    var a2 = b;
                    var b2 = edgeA.Vertex3;
                    var e2 = b2 - a2;
                    var v2 = Vector2Util.Dot(e2, q - a2);

                    // Is the circle in Region AB of the next edge?
                    if (v2 > 0.0f)
                    {
                        return(false);
                    }
                }

                cf.IndexA = 1;
                cf.TypeA  = (byte)ContactFeature.FeatureType.Vertex;

                manifold.Type                    = Manifold.ManifoldType.Circles;
                manifold.LocalPoint              = p;
                manifold.LocalNormal             = Vector2.Zero;
                manifold.PointCount              = 1;
                manifold.Points.Item1.Id.Key     = 0;
                manifold.Points.Item1.Id.Feature = cf;
                manifold.Points.Item1.LocalPoint = circleB.Center;

                return(true);
            }

            // Region AB
            var den = Vector2Util.Dot(ref e, ref e);

            System.Diagnostics.Debug.Assert(den > 0.0f);
            {
                var p  = (1.0f / den) * (u * a + v * b);
                var d  = q - p;
                var dd = Vector2Util.Dot(ref d, ref d);
                if (dd > radius * radius)
                {
                    return(false);
                }
            }

            Vector2 n;

            n.X = -e.Y;
            n.Y = e.X;
            if (Vector2Util.Dot(n, q - a) < 0.0f)
            {
                n = -n;
            }
            n.Normalize();

            cf.IndexA = 0;
            cf.TypeA  = (byte)ContactFeature.FeatureType.Face;

            manifold.Type                    = Manifold.ManifoldType.FaceA;
            manifold.LocalPoint              = a;
            manifold.LocalNormal             = n;
            manifold.PointCount              = 1;
            manifold.Points.Item1.Id.Key     = 0;
            manifold.Points.Item1.Id.Feature = cf;
            manifold.Points.Item1.LocalPoint = circleB.Center;

            return(true);
        }
Exemplo n.º 24
0
        /// <summary>
        ///     This resets the mass properties to the sum of the mass properties of the fixtures. This normally does not need
        ///     to be called unless you called SetMassData to override the mass and you later want to reset the mass.
        /// </summary>
        public void ResetMassData()
        {
            // Compute mass data from shapes. Each shape has its own density.
            MassInternal      = 0.0f;
            InverseMass       = 0.0f;
            _inertia          = 0.0f;
            InverseInertia    = 0.0f;
            Sweep.LocalCenter = Vector2.Zero;

            // Static and kinematic bodies have zero mass.
            if (TypeInternal == BodyType.Static || TypeInternal == BodyType.Kinematic)
            {
                Sweep.CenterOfMass0 = Transform.Translation;
                Sweep.CenterOfMass  = Transform.Translation;
                Sweep.Angle0        = Sweep.Angle;
                return;
            }

            System.Diagnostics.Debug.Assert(TypeInternal == BodyType.Dynamic);

            // Accumulate mass over all fixtures.
            var localCenter = Vector2.Zero;

            foreach (Fixture fixture in Fixtures)
            {
// ReSharper disable CompareOfFloatsByEqualityOperator Intentional.
                if (fixture.Density == 0)
// ReSharper restore CompareOfFloatsByEqualityOperator
                {
                    continue;
                }

                float   mass, inertia;
                Vector2 center;
                fixture.GetMassData(out mass, out center, out inertia);
                MassInternal += mass;
                localCenter  += mass * center;
                _inertia     += inertia;
            }

            // Compute center of mass.
            if (MassInternal <= 0)
            {
                // Force all dynamic bodies to have a positive mass.
                MassInternal = 1;
                InverseMass  = 1;
            }
            else
            {
                InverseMass  = 1 / MassInternal;
                localCenter *= InverseMass;
            }

            if (_isRotationFixed || _inertia <= 0)
            {
                _inertia       = 0;
                InverseInertia = 0;
            }
            else
            {
                // Center the inertia about the center of mass.
                _inertia      -= MassInternal * Vector2Util.Dot(ref localCenter, ref localCenter);
                InverseInertia = 1 / _inertia;
            }

            // Move center of mass.
            var oldCenter = Sweep.CenterOfMass;

            Sweep.LocalCenter   = localCenter;
            Sweep.CenterOfMass0 = Sweep.CenterOfMass = Transform.ToGlobal(Sweep.LocalCenter);

            // Update center of mass velocity.
// ReSharper disable RedundantCast Necessary for FarPhysics.
            LinearVelocityInternal += Vector2Util.Cross(
                AngularVelocityInternal, (Vector2)(Sweep.CenterOfMass - oldCenter));
// ReSharper restore RedundantCast
        }
Exemplo n.º 25
0
        /// <summary>
        ///     Computes the world manifold data from this manifold with the specified properties for the two involved
        ///     objects.
        /// </summary>
        /// <param name="xfA">The transform of object A.</param>
        /// <param name="radiusA">The radius of object A.</param>
        /// <param name="xfB">The transform of object B.</param>
        /// <param name="radiusB">The radius of object B.</param>
        /// <param name="normal">The normal.</param>
        /// <param name="points">The world contact points.</param>
        public void ComputeWorldManifold(
            WorldTransform xfA,
            float radiusA,
            WorldTransform xfB,
            float radiusB,
            out Vector2 normal,
            out FixedArray2 <WorldPoint> points)
        {
            points = new FixedArray2 <WorldPoint>(); // satisfy out
            switch (Type)
            {
            case ManifoldType.Circles:
            {
                normal = Vector2.UnitX;
                var pointA = xfA.ToGlobal(LocalPoint);
                var pointB = xfB.ToGlobal(Points[0].LocalPoint);
                if (WorldPoint.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
                {
// ReSharper disable RedundantCast Necessary for FarPhysics.
                    normal = (Vector2)(pointB - pointA);
// ReSharper restore RedundantCast
                    normal.Normalize();
                }

                var cA = pointA + radiusA * normal;
                var cB = pointB - radiusB * normal;
                points.Item1 = 0.5f * (cA + cB);
                break;
            }

            case ManifoldType.FaceA:
            {
                normal = xfA.Rotation * LocalNormal;
                var planePoint = xfA.ToGlobal(LocalPoint);

                for (var i = 0; i < PointCount; ++i)
                {
                    var clipPoint = xfB.ToGlobal(Points[i].LocalPoint);
// ReSharper disable RedundantCast Necessary for FarPhysics.
                    var cA = clipPoint + (radiusA - Vector2Util.Dot((Vector2)(clipPoint - planePoint), normal)) * normal;
// ReSharper restore RedundantCast
                    var cB = clipPoint - radiusB * normal;
                    points[i] = 0.5f * (cA + cB);
                }
                break;
            }

            case ManifoldType.FaceB:
            {
                normal = xfB.Rotation * LocalNormal;
                var planePoint = xfB.ToGlobal(LocalPoint);

                for (var i = 0; i < PointCount; ++i)
                {
                    var clipPoint = xfA.ToGlobal(Points[i].LocalPoint);
// ReSharper disable RedundantCast Necessary for FarPhysics.
                    var cB = clipPoint + (radiusB - Vector2Util.Dot((Vector2)(clipPoint - planePoint), normal)) * normal;
// ReSharper restore RedundantCast
                    var cA = clipPoint - radiusA * normal;
                    points[i] = 0.5f * (cA + cB);
                }

                // Ensure normal points from A to B.
                normal = -normal;
                break;
            }

            default:
                throw new ArgumentOutOfRangeException();
            }
        }
Exemplo n.º 26
0
            // Possible regions:
            // - points[2]
            // - edge points[0]-points[2]
            // - edge points[1]-points[2]
            // - inside the triangle
            public void Solve3()
            {
                var w1 = Vertices.Item1.VertexDelta;
                var w2 = Vertices.Item2.VertexDelta;
                var w3 = Vertices.Item3.VertexDelta;

                // Edge12
                // [1      1     ][a1] = [1]
                // [w1.e12 w2.e12][a2] = [0]
                // a3 = 0
                var e12   = w2 - w1;
                var w1e12 = Vector2Util.Dot(ref w1, ref e12);
                var w2e12 = Vector2Util.Dot(ref w2, ref e12);
                var d12_1 = w2e12;
                var d12_2 = -w1e12;

                // Edge13
                // [1      1     ][a1] = [1]
                // [w1.e13 w3.e13][a3] = [0]
                // a2 = 0
                var e13   = w3 - w1;
                var w1e13 = Vector2Util.Dot(ref w1, ref e13);
                var w3e13 = Vector2Util.Dot(ref w3, ref e13);
                var d13_1 = w3e13;
                var d13_2 = -w1e13;

                // Edge23
                // [1      1     ][a2] = [1]
                // [w2.e23 w3.e23][a3] = [0]
                // a1 = 0
                var e23   = w3 - w2;
                var w2e23 = Vector2Util.Dot(ref w2, ref e23);
                var w3e23 = Vector2Util.Dot(ref w3, ref e23);
                var d23_1 = w3e23;
                var d23_2 = -w2e23;

                // Triangle123
                var n123 = Vector2Util.Cross(ref e12, ref e13);

                var d123_1 = n123 * Vector2Util.Cross(ref w2, ref w3);
                var d123_2 = n123 * Vector2Util.Cross(ref w3, ref w1);
                var d123_3 = n123 * Vector2Util.Cross(ref w1, ref w2);

                // w1 region
                if (d12_2 <= 0.0f && d13_2 <= 0.0f)
                {
                    Vertices.Item1.Alpha = 1.0f;
                    Count = 1;
                    return;
                }

                // e12
                if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
                {
                    var inv_d12 = 1.0f / (d12_1 + d12_2);
                    Vertices.Item1.Alpha = d12_1 * inv_d12;
                    Vertices.Item2.Alpha = d12_2 * inv_d12;
                    Count = 2;
                    return;
                }

                // e13
                if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
                {
                    var inv_d13 = 1.0f / (d13_1 + d13_2);
                    Vertices.Item1.Alpha = d13_1 * inv_d13;
                    Vertices.Item3.Alpha = d13_2 * inv_d13;
                    Count          = 2;
                    Vertices.Item2 = Vertices.Item3;
                    return;
                }

                // w2 region
                if (d12_1 <= 0.0f && d23_2 <= 0.0f)
                {
                    Vertices.Item2.Alpha = 1.0f;
                    Count          = 1;
                    Vertices.Item1 = Vertices.Item2;
                    return;
                }

                // w3 region
                if (d13_1 <= 0.0f && d23_1 <= 0.0f)
                {
                    Vertices.Item3.Alpha = 1.0f;
                    Count          = 1;
                    Vertices.Item1 = Vertices.Item3;
                    return;
                }

                // e23
                if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
                {
                    var inv_d23 = 1.0f / (d23_1 + d23_2);
                    Vertices.Item2.Alpha = d23_1 * inv_d23;
                    Vertices.Item3.Alpha = d23_2 * inv_d23;
                    Count          = 2;
                    Vertices.Item1 = Vertices.Item3;
                    return;
                }

                // Must be in triangle123
                var inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);

                Vertices.Item1.Alpha = d123_1 * inv_d123;
                Vertices.Item2.Alpha = d123_2 * inv_d123;
                Vertices.Item3.Alpha = d123_3 * inv_d123;
                Count = 3;
            }
Exemplo n.º 27
0
        // Linear constraint (point-to-line)
        // d = p2 - p1 = x2 + r2 - x1 - r1
        // C = dot(perp, d)
        // Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
        //      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
        // J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
        //
        // Angular constraint
        // C = a2 - a1 + a_initial
        // Cdot = w2 - w1
        // J = [0 0 -1 0 0 1]
        //
        // K = J * invM * JT
        //
        // J = [-a -s1 a s2]
        //     [0  -1  0  1]
        // a = perp
        // s1 = cross(d + r1, a) = cross(p2 - x1, a)
        // s2 = cross(r2, a) = cross(p2 - x2, a)

        // Motor/Limit linear constraint
        // C = dot(ax1, d)
        // Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
        // J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]

        // Block Solver
        // We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
        // when the mass has poor distribution (leading to large torques about the joint anchor points).
        //
        // The Jacobian has 3 rows:
        // J = [-uT -s1 uT s2] // linear
        //     [0   -1   0  1] // angular
        //     [-vT -a1 vT a2] // limit
        //
        // u = perp
        // v = axis
        // s1 = cross(d + r1, u), s2 = cross(r2, u)
        // a1 = cross(d + r1, v), a2 = cross(r2, v)

        // M * (v2 - v1) = JT * df
        // J * v2 = bias
        //
        // v2 = v1 + invM * JT * df
        // J * (v1 + invM * JT * df) = bias
        // K * df = bias - J * v1 = -Cdot
        // K = J * invM * JT
        // Cdot = J * v1 - bias
        //
        // Now solve for f2.
        // df = f2 - f1
        // K * (f2 - f1) = -Cdot
        // f2 = invK * (-Cdot) + f1
        //
        // Clamp accumulated limit impulse.
        // lower: f2(3) = max(f2(3), 0)
        // upper: f2(3) = min(f2(3), 0)
        //
        // Solve for correct f2(1:2)
        // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
        //                       = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
        // K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
        // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
        //
        // Now compute impulse to be applied:
        // df = f2 - f1

        /// <summary>Initializes the velocity constraints.</summary>
        /// <param name="step">The time step for this update.</param>
        /// <param name="positions">The positions of the related bodies.</param>
        /// <param name="velocities">The velocities of the related bodies.</param>
        internal override void InitializeVelocityConstraints(TimeStep step, Position[] positions, Velocity[] velocities)
        {
            _tmp.IndexA          = BodyA.IslandIndex;
            _tmp.IndexB          = BodyB.IslandIndex;
            _tmp.LocalCenterA    = BodyA.Sweep.LocalCenter;
            _tmp.LocalCenterB    = BodyB.Sweep.LocalCenter;
            _tmp.InverseMassA    = BodyA.InverseMass;
            _tmp.InverseMassB    = BodyB.InverseMass;
            _tmp.InverseInertiaA = BodyA.InverseInertia;
            _tmp.InverseInertiaB = BodyB.InverseInertia;

            var cA = positions[_tmp.IndexA].Point;
            var aA = positions[_tmp.IndexA].Angle;
            var vA = velocities[_tmp.IndexA].LinearVelocity;
            var wA = velocities[_tmp.IndexA].AngularVelocity;

            var cB = positions[_tmp.IndexB].Point;
            var aB = positions[_tmp.IndexB].Angle;
            var vB = velocities[_tmp.IndexB].LinearVelocity;
            var wB = velocities[_tmp.IndexB].AngularVelocity;

            var qA = new Rotation(aA);
            var qB = new Rotation(aB);

            // Compute the effective masses.
            var rA = qA * (_localAnchorA - _tmp.LocalCenterA);
            var rB = qB * (_localAnchorB - _tmp.LocalCenterB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
            var d = (Vector2)(cB - cA) + (rB - rA);
// ReSharper restore RedundantCast

            var mA = _tmp.InverseMassA;
            var mB = _tmp.InverseMassB;
            var iA = _tmp.InverseInertiaA;
            var iB = _tmp.InverseInertiaB;

            // Compute motor Jacobian and effective mass.
            {
                _tmp.Axis = qA * _localXAxisA;
                _tmp.A1   = Vector2Util.Cross(d + rA, _tmp.Axis);
                _tmp.A2   = Vector2Util.Cross(rB, _tmp.Axis);

                _tmp.MotorMass = mA + mB + iA * _tmp.A1 * _tmp.A1 + iB * _tmp.A2 * _tmp.A2;
                if (_tmp.MotorMass > 0.0f)
                {
                    _tmp.MotorMass = 1.0f / _tmp.MotorMass;
                }
            }

            // Prismatic constraint.
            {
                _tmp.Perp = qA * _localYAxisA;

                _tmp.S1 = Vector2Util.Cross(d + rA, _tmp.Perp);
                _tmp.S2 = Vector2Util.Cross(rB, _tmp.Perp);

                var k11 = mA + mB + iA * _tmp.S1 * _tmp.S1 + iB * _tmp.S2 * _tmp.S2;
                var k12 = iA * _tmp.S1 + iB * _tmp.S2;
                var k13 = iA * _tmp.S1 * _tmp.A1 + iB * _tmp.S2 * _tmp.A2;
                var k22 = iA + iB;
// ReSharper disable CompareOfFloatsByEqualityOperator Intentional.
                if (k22 == 0.0f)
// ReSharper restore CompareOfFloatsByEqualityOperator
                {
                    // For bodies with fixed rotation.
                    k22 = 1.0f;
                }
                var k23 = iA * _tmp.A1 + iB * _tmp.A2;
                var k33 = mA + mB + iA * _tmp.A1 * _tmp.A1 + iB * _tmp.A2 * _tmp.A2;

                Vector3Util.Set(out _tmp.K.Column1, k11, k12, k13);
                Vector3Util.Set(out _tmp.K.Column2, k12, k22, k23);
                Vector3Util.Set(out _tmp.K.Column3, k13, k23, k33);
            }

            // Compute motor and limit terms.
            if (_enableLimit)
            {
                var jointTranslation = Vector2Util.Dot(ref _tmp.Axis, ref d);
                if (System.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    _limitState = LimitState.Equal;
                }
                else if (jointTranslation <= _lowerTranslation)
                {
                    if (_limitState != LimitState.AtLower)
                    {
                        _limitState = LimitState.AtLower;
                        _impulse.Z  = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpper)
                    {
                        _limitState = LimitState.AtUpper;
                        _impulse.Z  = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.Inactive;
                    _impulse.Z  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.Inactive;
                _impulse.Z  = 0.0f;
            }

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

            var p  = _impulse.X * _tmp.Perp + (_motorImpulse + _impulse.Z) * _tmp.Axis;
            var lA = _impulse.X * _tmp.S1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _tmp.A1;
            var lB = _impulse.X * _tmp.S2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _tmp.A2;

            vA -= mA * p;
            wA -= iA * lA;
            vB += mB * p;
            wB += iB * lB;

            velocities[_tmp.IndexA].LinearVelocity  = vA;
            velocities[_tmp.IndexA].AngularVelocity = wA;
            velocities[_tmp.IndexB].LinearVelocity  = vB;
            velocities[_tmp.IndexB].AngularVelocity = wB;
        }
Exemplo n.º 28
0
        // Linear constraint (point-to-line)
        // d = pB - pA = xB + rB - xA - rA
        // C = dot(ay, d)
        // Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
        //      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
        // J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]

        // Spring linear constraint
        // C = dot(ax, d)
        // Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
        // J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]

        // Motor rotational constraint
        // Cdot = wB - wA
        // J = [0 0 -1 0 0 1]

        /// <summary>Initializes the velocity constraints.</summary>
        /// <param name="step">The time step for this update.</param>
        /// <param name="positions">The positions of the related bodies.</param>
        /// <param name="velocities">The velocities of the related bodies.</param>
        internal override void InitializeVelocityConstraints(TimeStep step, Position[] positions, Velocity[] velocities)
        {
            _tmp.IndexA          = BodyA.IslandIndex;
            _tmp.IndexB          = BodyB.IslandIndex;
            _tmp.LocalCenterA    = BodyA.Sweep.LocalCenter;
            _tmp.LocalCenterB    = BodyB.Sweep.LocalCenter;
            _tmp.InverseMassA    = BodyA.InverseMass;
            _tmp.InverseMassB    = BodyB.InverseMass;
            _tmp.InverseInertiaA = BodyA.InverseInertia;
            _tmp.InverseInertiaB = BodyB.InverseInertia;

            var mA = _tmp.InverseMassA;
            var mB = _tmp.InverseMassB;
            var iA = _tmp.InverseInertiaA;
            var iB = _tmp.InverseInertiaB;

            var cA = positions[_tmp.IndexA].Point;
            var aA = positions[_tmp.IndexA].Angle;
            var vA = velocities[_tmp.IndexA].LinearVelocity;
            var wA = velocities[_tmp.IndexA].AngularVelocity;

            var cB = positions[_tmp.IndexB].Point;
            var aB = positions[_tmp.IndexB].Angle;
            var vB = velocities[_tmp.IndexB].LinearVelocity;
            var wB = velocities[_tmp.IndexB].AngularVelocity;

            var qA = new Rotation(aA);
            var qB = new Rotation(aB);

            // Compute the effective masses.
            var rA = qA * (_localAnchorA - _tmp.LocalCenterA);
            var rB = qB * (_localAnchorB - _tmp.LocalCenterB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
            var d = (Vector2)(cB - cA) + (rB - rA);

// ReSharper restore RedundantCast

            // Point to line constraint
            {
                _tmp.Ay  = qA * _localYAxisA;
                _tmp.SAy = Vector2Util.Cross(d + rA, _tmp.Ay);
                _tmp.SBy = Vector2Util.Cross(rB, _tmp.Ay);

                _tmp.Mass = mA + mB + iA * _tmp.SAy * _tmp.SAy + iB * _tmp.SBy * _tmp.SBy;

                if (_tmp.Mass > 0.0f)
                {
                    _tmp.Mass = 1.0f / _tmp.Mass;
                }
            }

            // Spring constraint
            _tmp.SpringMass = 0.0f;
            _tmp.Bias       = 0.0f;
            _tmp.Gamma      = 0.0f;
            if (_frequency > 0.0f)
            {
                _tmp.Ax  = qA * _localXAxisA;
                _tmp.SAx = Vector2Util.Cross(d + rA, _tmp.Ax);
                _tmp.SBx = Vector2Util.Cross(rB, _tmp.Ax);

                var invMass = mA + mB + iA * _tmp.SAx * _tmp.SAx + iB * _tmp.SBx * _tmp.SBx;

                if (invMass > 0.0f)
                {
                    _tmp.SpringMass = 1.0f / invMass;

                    var c = Vector2Util.Dot(ref d, ref _tmp.Ax);

                    // Frequency
                    var omega = 2.0f * MathHelper.Pi * _frequency;

                    // Damping coefficient
                    var dc = 2.0f * _tmp.SpringMass * _dampingRatio * omega;

                    // Spring stiffness
                    var k = _tmp.SpringMass * omega * omega;

                    // magic formulas
                    var h = step.DeltaT;
                    _tmp.Gamma = h * (dc + h * k);
                    if (_tmp.Gamma > 0.0f)
                    {
                        _tmp.Gamma = 1.0f / _tmp.Gamma;
                    }

                    _tmp.Bias = c * h * k * _tmp.Gamma;

                    _tmp.SpringMass = invMass + _tmp.Gamma;
                    if (_tmp.SpringMass > 0.0f)
                    {
                        _tmp.SpringMass = 1.0f / _tmp.SpringMass;
                    }
                }
            }
            else
            {
                _springImpulse = 0.0f;
            }

            // Rotational motor
            if (_enableMotor)
            {
                _tmp.MotorMass = iA + iB;
                if (_tmp.MotorMass > 0.0f)
                {
                    _tmp.MotorMass = 1.0f / _tmp.MotorMass;
                }
            }
            else
            {
                _tmp.MotorMass = 0.0f;
                _motorImpulse  = 0.0f;
            }

            var p  = _impulse * _tmp.Ay + _springImpulse * _tmp.Ax;
            var lA = _impulse * _tmp.SAy + _springImpulse * _tmp.SAx + _motorImpulse;
            var lB = _impulse * _tmp.SBy + _springImpulse * _tmp.SBx + _motorImpulse;

            vA -= _tmp.InverseMassA * p;
            wA -= _tmp.InverseInertiaA * lA;

            vB += _tmp.InverseMassB * p;
            wB += _tmp.InverseInertiaB * lB;

            velocities[_tmp.IndexA].LinearVelocity  = vA;
            velocities[_tmp.IndexA].AngularVelocity = wA;
            velocities[_tmp.IndexB].LinearVelocity  = vB;
            velocities[_tmp.IndexB].AngularVelocity = wB;
        }
Exemplo n.º 29
0
        /// <summary>This returns true if the position errors are within tolerance, allowing an early exit from the iteration loop.</summary>
        /// <param name="positions">The positions of the related bodies.</param>
        /// <returns>
        ///     <c>true</c> if the position errors are within tolerance.
        /// </returns>
        internal override bool SolvePositionConstraints(Position[] positions)
        {
            var cA = positions[_tmp.IndexA].Point;
            var aA = positions[_tmp.IndexA].Angle;
            var cB = positions[_tmp.IndexB].Point;
            var aB = positions[_tmp.IndexB].Angle;

            var qA = new Rotation(aA);
            var qB = new Rotation(aB);

            var mA = _tmp.InverseMassA;
            var mB = _tmp.InverseMassB;
            var iA = _tmp.InverseInertiaA;
            var iB = _tmp.InverseInertiaB;

            // Compute fresh Jacobians
            var rA = qA * (_localAnchorA - _tmp.LocalCenterA);
            var rB = qB * (_localAnchorB - _tmp.LocalCenterB);
// ReSharper disable RedundantCast Necessary for FarPhysics.
            var d = (Vector2)(cB - cA) + (rB - rA);
// ReSharper restore RedundantCast

            var axis = qA * _localXAxisA;
            var a1   = Vector2Util.Cross(d + rA, axis);
            var a2   = Vector2Util.Cross(rB, axis);
            var perp = qA * _localYAxisA;

            var s1 = Vector2Util.Cross(d + rA, perp);
            var s2 = Vector2Util.Cross(rB, perp);

            Vector3 impulse;
            Vector2 c1;

            c1.X = Vector2Util.Dot(ref perp, ref d);
            c1.Y = aB - aA - _referenceAngle;

            var linearError  = System.Math.Abs(c1.X);
            var angularError = System.Math.Abs(c1.Y);

            var active = false;
            var c2     = 0.0f;

            if (_enableLimit)
            {
                var translation = Vector2Util.Dot(ref axis, ref d);
                if (System.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    // Prevent large angular corrections
                    c2          = MathHelper.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
                    linearError = System.Math.Max(linearError, System.Math.Abs(translation));
                    active      = true;
                }
                else if (translation <= _lowerTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    c2 = MathHelper.Clamp(
                        translation - _lowerTranslation + Settings.LinearSlop,
                        -Settings.MaxLinearCorrection,
                        0.0f);
                    linearError = System.Math.Max(linearError, _lowerTranslation - translation);
                    active      = true;
                }
                else if (translation >= _upperTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    c2 = MathHelper.Clamp(
                        translation - _upperTranslation - Settings.LinearSlop,
                        0.0f,
                        Settings.MaxLinearCorrection);
                    linearError = System.Math.Max(linearError, translation - _upperTranslation);
                    active      = true;
                }
            }

            if (active)
            {
                var k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
                var k12 = iA * s1 + iB * s2;
                var k13 = iA * s1 * a1 + iB * s2 * a2;
                var k22 = iA + iB;
// ReSharper disable CompareOfFloatsByEqualityOperator Intentional.
                if (k22 == 0.0f)
// ReSharper restore CompareOfFloatsByEqualityOperator
                {
                    // For fixed rotation
                    k22 = 1.0f;
                }
                var k23 = iA * a1 + iB * a2;
                var k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;

                Matrix33 k;
                Vector3Util.Set(out k.Column1, k11, k12, k13);
                Vector3Util.Set(out k.Column2, k12, k22, k23);
                Vector3Util.Set(out k.Column3, k13, k23, k33);

                Vector3 c;
                c.X = c1.X;
                c.Y = c1.Y;
                c.Z = c2;

                impulse = k.Solve33(-c);
            }
            else
            {
                var k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
                var k12 = iA * s1 + iB * s2;
                var k22 = iA + iB;
// ReSharper disable CompareOfFloatsByEqualityOperator Intentional.
                if (k22 == 0.0f)
// ReSharper restore CompareOfFloatsByEqualityOperator
                {
                    k22 = 1.0f;
                }

                Matrix22 k;
                Vector2Util.Set(out k.Column1, k11, k12);
                Vector2Util.Set(out k.Column2, k12, k22);

                var impulse1 = k.Solve(-c1);
                impulse.X = impulse1.X;
                impulse.Y = impulse1.Y;
                impulse.Z = 0.0f;
            }

            var p  = impulse.X * perp + impulse.Z * axis;
            var lA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
            var lB = impulse.X * s2 + impulse.Y + impulse.Z * a2;

            cA -= mA * p;
            aA -= iA * lA;
            cB += mB * p;
            aB += iB * lB;

            positions[_tmp.IndexA].Point = cA;
            positions[_tmp.IndexA].Angle = aA;
            positions[_tmp.IndexB].Point = cB;
            positions[_tmp.IndexB].Angle = aB;

            return(linearError <= (Settings.LinearSlop + Settings.Epsilon) && angularError <= (Settings.AngularSlop + Settings.Epsilon));
        }
Exemplo n.º 30
0
        // This function collides and edge and a polygon.
        // This takes into account edge adjacency.
        // Algorithm:
        // 1. Classify v1 and v2
        // 2. Classify polygon centroid as front or back
        // 3. Flip normal if necessary
        // 4. Initialize normal range to [-pi, pi] about face normal
        // 5. Adjust normal range according to adjacent edges
        // 6. Visit each separating axes, only accept axes within the range
        // 7. Return if _any_ axis indicates separation
        // 8. Clip
        public static bool CollideEdgeAndPolygon(
            Fixture fixtureA,
            WorldTransform xfA,
            Fixture fixtureB,
            WorldTransform xfB,
            out Manifold manifold)
        {
            manifold = new Manifold();

            var edgeA    = fixtureA as EdgeFixture;
            var polygonB = fixtureB as PolygonFixture;

            System.Diagnostics.Debug.Assert(edgeA != null);
            System.Diagnostics.Debug.Assert(polygonB != null);

            // This holds polygon B expressed in frame A.
            var tpv = new Vector2[Settings.MaxPolygonVertices];
            var tpn = new Vector2[Settings.MaxPolygonVertices];

            Vector2 normal0 = Vector2.Zero, normal1, normal2 = Vector2.Zero;

            var xf = xfA.MulT(xfB);

            var centroidB = xf.ToOther(polygonB.Centroid);

            var v0 = edgeA.Vertex0;
            var v1 = edgeA.Vertex1;
            var v2 = edgeA.Vertex2;
            var v3 = edgeA.Vertex3;

            var hasVertex0 = edgeA.HasVertex0;
            var hasVertex3 = edgeA.HasVertex3;

            var edge1 = v2 - v1;

            edge1.Normalize();
            normal1.X = edge1.Y;
            normal1.Y = -edge1.X;
            var offset1 = Vector2Util.Dot(normal1, centroidB - v1);
            var offset0 = 0.0f;
            var offset2 = 0.0f;
            var convex1 = false;
            var convex2 = false;

            // Is there a preceding edge?
            if (hasVertex0)
            {
                var edge0 = v1 - v0;
                edge0.Normalize();
                normal0.X = edge0.Y;
                normal0.Y = -edge0.X;
                convex1   = Vector2Util.Cross(ref edge0, ref edge1) >= 0.0f;
                offset0   = Vector2Util.Dot(normal0, centroidB - v0);
            }

            // Is there a following edge?
            if (hasVertex3)
            {
                var edge2 = v3 - v2;
                edge2.Normalize();
                normal2.X = edge2.Y;
                normal2.Y = -edge2.X;
                convex2   = Vector2Util.Cross(ref edge1, ref edge2) > 0.0f;
                offset2   = Vector2Util.Dot(normal2, centroidB - v2);
            }

            // Determine front or back collision. Determine collision normal limits.
            bool    front;
            Vector2 normal, lowerLimit, upperLimit;

            if (hasVertex0 && hasVertex3)
            {
                if (convex1 && convex2)
                {
                    front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = normal0;
                        upperLimit = normal2;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = -normal1;
                        upperLimit = -normal1;
                    }
                }
                else if (convex1)
                {
                    front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = normal0;
                        upperLimit = normal1;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = -normal2;
                        upperLimit = -normal1;
                    }
                }
                else if (convex2)
                {
                    front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = normal1;
                        upperLimit = normal2;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = -normal1;
                        upperLimit = -normal0;
                    }
                }
                else
                {
                    front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = normal1;
                        upperLimit = normal1;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = -normal2;
                        upperLimit = -normal0;
                    }
                }
            }
            else if (hasVertex0)
            {
                if (convex1)
                {
                    front = offset0 >= 0.0f || offset1 >= 0.0f;
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = normal0;
                        upperLimit = -normal1;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = normal1;
                        upperLimit = -normal1;
                    }
                }
                else
                {
                    front = offset0 >= 0.0f && offset1 >= 0.0f;
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = normal1;
                        upperLimit = -normal1;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = normal1;
                        upperLimit = -normal0;
                    }
                }
            }
            else if (hasVertex3)
            {
                if (convex2)
                {
                    front = offset1 >= 0.0f || offset2 >= 0.0f;
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = -normal1;
                        upperLimit = normal2;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = -normal1;
                        upperLimit = normal1;
                    }
                }
                else
                {
                    front = offset1 >= 0.0f && offset2 >= 0.0f;
                    if (front)
                    {
                        normal     = normal1;
                        lowerLimit = -normal1;
                        upperLimit = normal1;
                    }
                    else
                    {
                        normal     = -normal1;
                        lowerLimit = -normal2;
                        upperLimit = normal1;
                    }
                }
            }
            else
            {
                front = offset1 >= 0.0f;
                if (front)
                {
                    normal     = normal1;
                    lowerLimit = -normal1;
                    upperLimit = -normal1;
                }
                else
                {
                    normal     = -normal1;
                    lowerLimit = normal1;
                    upperLimit = normal1;
                }
            }

            // Get polygonB in frameA.
            var tpc = polygonB.Count;

            for (var i = 0; i < tpc; ++i)
            {
                tpv[i] = xf.ToOther(polygonB.Vertices[i]);
                tpn[i] = xf.Rotation * polygonB.Normals[i];
            }

            const float radius = 2.0f * Settings.PolygonRadius;

            Axis edgeAxis;

            edgeAxis.Type       = Axis.AxisType.EdgeA;
            edgeAxis.Index      = front ? 0 : 1;
            edgeAxis.Separation = float.MaxValue;

            for (var i = 0; i < tpc; ++i)
            {
                var s = Vector2Util.Dot(normal, tpv[i] - v1);
                if (s < edgeAxis.Separation)
                {
                    edgeAxis.Separation = s;
                }
            }

            // If no valid normal can be found than this edge should not collide.
            if (edgeAxis.Type == Axis.AxisType.None)
            {
                return(false);
            }

            if (edgeAxis.Separation > radius)
            {
                return(false);
            }

            Axis polygonAxis;

            polygonAxis.Type       = Axis.AxisType.None;
            polygonAxis.Index      = -1;
            polygonAxis.Separation = float.MinValue;

            Vector2 perp;

            perp.X = -normal.Y;
            perp.Y = normal.X;

            for (var i = 0; i < tpc; ++i)
            {
                var n = -tpn[i];

                var s1 = Vector2Util.Dot(n, tpv[i] - v1);
                var s2 = Vector2Util.Dot(n, tpv[i] - v2);
                var s  = System.Math.Min(s1, s2);

                if (s > radius)
                {
                    // No collision
                    polygonAxis.Type       = Axis.AxisType.EdgeB;
                    polygonAxis.Index      = i;
                    polygonAxis.Separation = s;
                    break;
                }

                // Adjacency
                if (Vector2Util.Dot(ref n, ref perp) >= 0.0f)
                {
                    if (Vector2Util.Dot(n - upperLimit, normal) < -Settings.AngularSlop)
                    {
                        continue;
                    }
                }
                else
                {
                    if (Vector2Util.Dot(n - lowerLimit, normal) < -Settings.AngularSlop)
                    {
                        continue;
                    }
                }

                if (s > polygonAxis.Separation)
                {
                    polygonAxis.Type       = Axis.AxisType.EdgeB;
                    polygonAxis.Index      = i;
                    polygonAxis.Separation = s;
                }
            }

            if (polygonAxis.Type != Axis.AxisType.None && polygonAxis.Separation > radius)
            {
                return(false);
            }

            // Use hysteresis for jitter reduction.
            const float relativeTol = 0.98f;
            const float absoluteTol = 0.001f;

            Axis primaryAxis;

            if (polygonAxis.Type == Axis.AxisType.None)
            {
                primaryAxis = edgeAxis;
            }
            else if (polygonAxis.Separation > relativeTol * edgeAxis.Separation + absoluteTol)
            {
                primaryAxis = polygonAxis;
            }
            else
            {
                primaryAxis = edgeAxis;
            }

            FixedArray2 <ClipVertex> incidentEdge;

            // Reference face used for clipping
            int     rfi1, rfi2;
            Vector2 rfv1, rfv2;
            Vector2 rfnormal;
            Vector2 rfsideNormal1;

            if (primaryAxis.Type == Axis.AxisType.EdgeA)
            {
                manifold.Type = Manifold.ManifoldType.FaceA;

                // Search for the polygon normal that is most anti-parallel to the edge normal.
                var bestIndex = 0;
                var bestValue = Vector2Util.Dot(ref normal, ref tpn[0]);
                for (var i = 1; i < tpc; ++i)
                {
                    var value = Vector2Util.Dot(ref normal, ref tpn[i]);
                    if (value < bestValue)
                    {
                        bestValue = value;
                        bestIndex = i;
                    }
                }

                var i1 = bestIndex;
                var i2 = i1 + 1 < tpc ? i1 + 1 : 0;

                incidentEdge = new FixedArray2 <ClipVertex>
                {
                    Item1 = new ClipVertex
                    {
                        Vertex = tpv[i1],
                        Id     =
                        {
                            Feature    =
                            {
                                IndexA =                                     0,
                                IndexB = (byte)i1,
                                TypeA  = (byte)ContactFeature.FeatureType.Face,
                                TypeB  = (byte)ContactFeature.FeatureType.Vertex
                            }
                        }
                    },
                    Item2 = new ClipVertex
                    {
                        Vertex = tpv[i2],
                        Id     =
                        {
                            Feature    =
                            {
                                IndexA =                                     0,
                                IndexB = (byte)i2,
                                TypeA  = (byte)ContactFeature.FeatureType.Face,
                                TypeB  = (byte)ContactFeature.FeatureType.Vertex
                            }
                        }
                    }
                };

                if (front)
                {
                    rfi1     = 0;
                    rfi2     = 1;
                    rfv1     = v1;
                    rfv2     = v2;
                    rfnormal = normal1;
                }
                else
                {
                    rfi1     = 1;
                    rfi2     = 0;
                    rfv1     = v2;
                    rfv2     = v1;
                    rfnormal = -normal1;
                }
            }
            else
            {
                manifold.Type = Manifold.ManifoldType.FaceB;

                incidentEdge = new FixedArray2 <ClipVertex>
                {
                    Item1 = new ClipVertex
                    {
                        Vertex = v1,
                        Id     =
                        {
                            Feature    =
                            {
                                IndexA =                                       0,
                                IndexB = (byte)primaryAxis.Index,
                                TypeA  = (byte)ContactFeature.FeatureType.Vertex,
                                TypeB  = (byte)ContactFeature.FeatureType.Face
                            }
                        }
                    },
                    Item2 = new ClipVertex
                    {
                        Vertex = v2,
                        Id     =
                        {
                            Feature    =
                            {
                                IndexA =                                       0,
                                IndexB = (byte)primaryAxis.Index,
                                TypeA  = (byte)ContactFeature.FeatureType.Vertex,
                                TypeB  = (byte)ContactFeature.FeatureType.Face
                            }
                        }
                    }
                };

                rfi1     = primaryAxis.Index;
                rfi2     = rfi1 + 1 < tpc ? rfi1 + 1 : 0;
                rfv1     = tpv[rfi1];
                rfv2     = tpv[rfi2];
                rfnormal = tpn[rfi1];
            }

            rfsideNormal1.X = rfnormal.Y;
            rfsideNormal1.Y = -rfnormal.X;
            var rfsideNormal2 = -rfsideNormal1;
            var rfsideOffset1 = Vector2Util.Dot(ref rfsideNormal1, ref rfv1);
            var rfsideOffset2 = Vector2Util.Dot(ref rfsideNormal2, ref rfv2);

            // Clip incident edge against extruded edge1 side edges.
            FixedArray2 <ClipVertex> clipPoints1, clipPoints2;

            // Clip to box side 1
            var np = ClipSegmentToLine(
                out clipPoints1,
                incidentEdge,
                rfsideNormal1,
                rfsideOffset1,
                rfi1);

            if (np < 2)
            {
                return(false);
            }

            // Clip to negative box side 1
            np = ClipSegmentToLine(
                out clipPoints2,
                clipPoints1,
                rfsideNormal2,
                rfsideOffset2,
                rfi2);

            if (np < 2)
            {
                return(false);
            }

            // Now clipPoints2 contains the clipped points.
            if (primaryAxis.Type == Axis.AxisType.EdgeA)
            {
                manifold.LocalPoint  = rfv1;
                manifold.LocalNormal = rfnormal;
            }
            else
            {
                manifold.LocalPoint  = polygonB.Vertices[rfi1];
                manifold.LocalNormal = polygonB.Normals[rfi1];
            }

            var pointCount = 0;

            for (var i = 0; i < 2; ++i)
            {
                if (Vector2Util.Dot(rfnormal, clipPoints2[i].Vertex - rfv1) <= radius)
                {
                    var cp = manifold.Points[pointCount];
                    if (primaryAxis.Type == Axis.AxisType.EdgeA)
                    {
                        cp.LocalPoint = xf.FromOther(clipPoints2[i].Vertex);
                        cp.Id         = clipPoints2[i].Id;
                    }
                    else
                    {
                        cp.LocalPoint        = clipPoints2[i].Vertex;
                        cp.Id.Feature.TypeA  = clipPoints2[i].Id.Feature.TypeB;
                        cp.Id.Feature.TypeB  = clipPoints2[i].Id.Feature.TypeA;
                        cp.Id.Feature.IndexA = clipPoints2[i].Id.Feature.IndexB;
                        cp.Id.Feature.IndexB = clipPoints2[i].Id.Feature.IndexA;
                    }
                    manifold.Points[pointCount] = cp;

                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;

            return(pointCount > 0);
        }