internal void Initialize(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB)
        {
            ShapeA = shapeA;
            ShapeB = shapeB;
            int count = cache.Count;

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

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

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

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

                float s = Vector2.Dot(pointA - pointB, normal);
                if (s < 0.0f)
                {
                    Axis = -Axis;
                }
            }
        }
Exemplo n.º 2
0
        public override bool TestPoint(Transform xf, Vector2 p)
        {
            Vector2 center = xf.position + xf.TransformDirection(_position);
            Vector2 d      = p - center;

            return(Vector2.Dot(d, d) <= _radius * _radius);
        }
Exemplo n.º 3
0
        internal bool SynchronizeFixtures()
        {
            Transform xf1 = new Transform();

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

            bool inRange = true;

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

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

                // Failure
                return(false);
            }

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

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

            aabb.LowerBound = new Vector2(p.X - _radius, p.Y - _radius);
            aabb.UpperBound = new Vector2(p.X + _radius, p.Y + _radius);
        }
        internal float Evaluate(Transform TransformA, Transform TransformB)
        {
            switch (FaceType)
            {
            case Type.Points:
            {
                Vector2 axisA       = TransformA.InverseTransformDirection(Axis);
                Vector2 axisB       = TransformB.InverseTransformDirection(-Axis);
                Vector2 localPointA = ShapeA.GetSupportVertex(axisA);
                Vector2 localPointB = ShapeB.GetSupportVertex(axisB);
                Vector2 pointA      = TransformA.TransformPoint(localPointA);
                Vector2 pointB      = TransformB.TransformPoint(localPointB);
                float   separation  = Vector2.Dot(pointB - pointA, Axis);
                return(separation);
            }

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

                Vector2 axisB = TransformB.InverseTransformDirection(-normal);

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

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

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

                Vector2 axisA = TransformA.InverseTransformDirection(-normal);

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

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

            default:
                Box2DNetDebug.Assert(false);
                return(0.0f);
            }
        }
Exemplo n.º 6
0
        public static void FindIncidentEdge(out ClipVertex[] c, PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2)
        {
            int count1 = poly1._vertexCount;

            Vector2[] normals1 = poly1._normals;

            int count2 = poly2._vertexCount;

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

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

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

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

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

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

            c = new ClipVertex[2];

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

            c[1].V = Common.Math.Mul(xf2, vertices2[i2]);
            c[1].ID.Features.ReferenceEdge  = (byte)edge1;
            c[1].ID.Features.IncidentEdge   = (byte)i2;
            c[1].ID.Features.IncidentVertex = 1;
        }
Exemplo n.º 7
0
        // Collision Detection in Interactive 3D Environments by Gino van den Bergen
        // From Section 3.1.2
        // x = s + a * r
        // norm(x) = radius
        public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda)
        {
            lambda = 0f;
            normal = Vector2.Zero;

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

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

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

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

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

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

            return(SegmentCollide.MissCollide);
        }
Exemplo n.º 8
0
        /// <summary>
        /// Build vertices to represent an oriented box.
        /// </summary>
        /// <param name="hx">The half-width</param>
        /// <param name="hy">The half-height.</param>
        /// <param name="center">The center of the box in local coordinates.</param>
        /// <param name="angle">The rotation of the box in local coordinates.</param>
        public void SetAsBox(float hx, float hy, Vector2 center, float angle)
        {
            SetAsBox(hx, hy);

            Transform xf = new Transform();

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

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

            // Transform vertices and normals.
            for (int i = 0; i < _vertexCount; ++i)
            {
                _vertices[i] = xf.TransformPoint(_vertices[i]);
                _normals[i]  = xf.TransformDirection(_normals[i]);
            }
        }
Exemplo n.º 9
0
        /// <summary>
        /// Find the separation between poly1 and poly2 for a give edge normal on poly1.
        /// </summary>
        public static float EdgeSeparation(PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2)
        {
            int count1 = poly1._vertexCount;

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

            int count2 = poly2._vertexCount;

            Vector2[] vertices2 = poly2._vertices;

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

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

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

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

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

            return(separation);
        }
Exemplo n.º 10
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

                b2._linearVelocity  += _invMass2 * P;
                b2._angularVelocity += _invI2 * L2;
            }
            else
            {
                _impulse      = Vector2.Zero;
                _motorImpulse = 0.0f;
            }
        }
Exemplo n.º 11
0
        public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda)
        {
            lambda = 0f;
            normal = Vector2.Zero;

            float lower = 0.0f, upper = maxLambda;

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

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

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

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


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

            lambda = 0f;
            return(SegmentCollide.StartInsideCollide);
        }
Exemplo n.º 12
0
		internal void ComputeTransform(ref Transform xf, Vector2 center, Vector2 localCenter, float angle)
		{
			xf.rotation = Box2DNet.Common.Math.AngleToRotation(angle);
			//xf.R = new Mat22(angle);
			xf.position = center - xf.TransformDirection(localCenter);
		}
Exemplo n.º 13
0
        /// Evaluate the manifold with supplied Transforms. This assumes
        /// modest motion from the original state. This does not change the
        /// point count, impulses, etc. The radii must come from the shapes
        /// that generated the manifold.
        public void Initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB)
        {
            if (manifold.PointCount == 0)
            {
                return;
            }

            switch (manifold.Type)
            {
            case ManifoldType.Circles:
            {
                Vector2 pointA = xfA.TransformPoint(manifold.LocalPoint);
                Vector2 pointB = xfB.TransformPoint(manifold.Points[0].LocalPoint);
                Vector2 normal = new Vector2(1.0f, 0.0f);
                if ((pointA - pointB).LengthSquared() > (Box2DNet.Common.Math.Epsilon * Box2DNet.Common.Math.Epsilon))
                {
                    normal = pointB - pointA;
                    normal.Normalize();
                }

                Normal = normal;

                Vector2 cA = pointA + radiusA * normal;
                Vector2 cB = pointB - radiusB * normal;
                Points[0] = 0.5f * (cA + cB);
            }
            break;

            case ManifoldType.FaceA:
            {
                Vector2 normal     = xfA.TransformDirection(manifold.LocalPlaneNormal);
                Vector2 planePoint = xfA.TransformPoint(manifold.LocalPoint);

                // Ensure normal points from A to B.
                Normal = normal;

                for (int i = 0; i < manifold.PointCount; ++i)
                {
                    Vector2 clipPoint = xfB.TransformPoint(manifold.Points[i].LocalPoint);
                    Vector2 cA        = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
                    Vector2 cB        = clipPoint - radiusB * normal;
                    Points[i] = 0.5f * (cA + cB);
                }
            }
            break;

            case ManifoldType.FaceB:
            {
                Vector2 normal     = xfB.TransformDirection(manifold.LocalPlaneNormal);
                Vector2 planePoint = xfB.TransformPoint(manifold.LocalPoint);

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

                for (int i = 0; i < manifold.PointCount; ++i)
                {
                    Vector2 clipPoint = xfA.TransformPoint(manifold.Points[i].LocalPoint);
                    Vector2 cA        = clipPoint - radiusA * normal;
                    Vector2 cB        = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
                    Points[i] = 0.5f * (cA + cB);
                }
            }
            break;
            }
        }
Exemplo n.º 14
0
 /// <summary>
 /// Get the world coordinates of a vector given the local coordinates.
 /// </summary>
 /// <param name="localVector">A vector fixed in the body.</param>
 /// <returns>Return the same vector expressed in world coordinates.</returns>
 public Vector2 GetWorldVector(Vector2 localVector)
 {
     return(_xf.TransformDirection(localVector));
 }