Esempio n. 1
0
		/// <summary>
		/// tests for an overlap of shapeA and shapeB. Returns false if both Shapes are chains or if they are not overlapping.
		/// </summary>
		/// <returns><c>true</c>, if overlap was tested, <c>false</c> otherwise.</returns>
		/// <param name="shapeA">Shape a.</param>
		/// <param name="shapeB">Shape b.</param>
		/// <param name="transformA">Transform a.</param>
		/// <param name="transformB">Transform b.</param>
		public static bool testOverlap( Shape shapeA, Shape shapeB, ref FSTransform transformA, ref FSTransform transformB )
		{
			if( shapeA.childCount == 1 && shapeB.childCount == 1 )
				return Collision.testOverlap( shapeA, 0, shapeB, 0, ref transformA, ref transformB );

			if( shapeA.childCount > 1 )
			{
				for( var i = 0; i < shapeA.childCount; i++ )
				{
					if( Collision.testOverlap( shapeA, i, shapeB, 0, ref transformA, ref transformB ) )
						return true;
				}
			}

			if( shapeB.childCount > 1 )
			{
				for( var i = 0; i < shapeB.childCount; i++ )
				{
					if( Collision.testOverlap( shapeA, 0, shapeB, i, ref transformA, ref transformB ) )
						return true;
				}
			}

			return false;
		}
Esempio n. 2
0
        /// <summary>
        /// tests for an overlap of shapeA and shapeB. Returns false if both Shapes are chains or if they are not overlapping.
        /// </summary>
        /// <returns><c>true</c>, if overlap was tested, <c>false</c> otherwise.</returns>
        /// <param name="shapeA">Shape a.</param>
        /// <param name="shapeB">Shape b.</param>
        /// <param name="transformA">Transform a.</param>
        /// <param name="transformB">Transform b.</param>
        public static bool testOverlap(Shape shapeA, Shape shapeB, ref FSTransform transformA, ref FSTransform transformB)
        {
            if (shapeA.childCount == 1 && shapeB.childCount == 1)
            {
                return(Collision.testOverlap(shapeA, 0, shapeB, 0, ref transformA, ref transformB));
            }

            if (shapeA.childCount > 1)
            {
                for (var i = 0; i < shapeA.childCount; i++)
                {
                    if (Collision.testOverlap(shapeA, i, shapeB, 0, ref transformA, ref transformB))
                    {
                        return(true);
                    }
                }
            }

            if (shapeB.childCount > 1)
            {
                for (var i = 0; i < shapeB.childCount; i++)
                {
                    if (Collision.testOverlap(shapeA, 0, shapeB, i, ref transformA, ref transformB))
                    {
                        return(true);
                    }
                }
            }

            return(false);
        }
Esempio n. 3
0
        private void DrawShape(Fixture fixture, Transform xf, Color color)
        {
            switch (fixture.Shape.ShapeType)
            {
            case ShapeType.Circle:
            {
                var circle = (FarseerCircleShape)fixture.Shape;

                Vector2 center = MathUtils.Mul(ref xf, circle.Position);
                float   radius = circle.Radius;
                Vector2 axis   = xf.q.GetXAxis();

                DrawSolidCircle(center, radius, axis, color.R, color.G, color.B);
            }
            break;

            case ShapeType.Polygon:
            {
                var poly        = (PolygonShape)fixture.Shape;
                int vertexCount = poly.Vertices.Count;
                var vertices    = new Vector2[vertexCount];

                for (int i = 0; i < vertexCount; ++i)
                {
                    vertices[i] = MathUtils.Mul(ref xf, poly.Vertices[i]);
                }

                DrawSolidPolygon(vertices, vertexCount, color.R, color.G, color.B);
            }
            break;

            default:
                throw new NotImplementedException();
            }
        }
Esempio n. 4
0
        /// <summary>
        /// Given a transform, compute the associated axis aligned bounding box for a child shape.
        /// </summary>
        /// <param name="aabb">The aabb results.</param>
        /// <param name="transform">The world transform of the shape.</param>
        /// <param name="childIndex">The child shape index.</param>
        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
        {
            Vector2 p = transform.Position + MathUtils.Multiply(ref transform.R, Position);

            aabb.LowerBound = new Vector2(p.x - Radius, p.y - Radius);
            aabb.UpperBound = new Vector2(p.x + Radius, p.y + Radius);
        }
Esempio n. 5
0
        static bool collideCircles(CircleShape circleA, ref FSTransform firstTransform, CircleShape circleB, ref FSTransform secondTransform, out FSCollisionResult result)
        {
            result = new FSCollisionResult();
            Collision.collideCircles(ref _manifold, circleA, ref firstTransform, circleB, ref secondTransform);
            if (_manifold.pointCount > 0)
            {
                // this is essentically directly from ContactSolver.WorldManifold.Initialize. To avoid doing the extra math twice we duplicate this code
                // here because it doesnt return some values we need to calculate separation
                var pointA = MathUtils.mul(ref firstTransform, _manifold.localPoint);
                var pointB = MathUtils.mul(ref secondTransform, _manifold.points[0].localPoint);

                result.normal = pointA - pointB;
                result.normal.Normalize();

                var cA = pointA - circleA.radius * result.normal;
                var cB = pointB + circleB.radius * result.normal;
                result.point  = 0.5f * (cA + cB);
                result.point *= FSConvert.simToDisplay;

                var separation = Vector2.Dot(pointA - pointB, result.normal) - circleA.radius - circleB.radius;
                result.minimumTranslationVector = result.normal * Math.Abs(separation);

                                #if DEBUG_FSCOLLISIONS
                Debug.drawPixel(result.point, 5, Color.Red, 0.2f);
                Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);
                                #endif

                return(true);
            }

            return(false);
        }
Esempio n. 6
0
        /// <summary>
        /// Test a point for containment in this shape. This only works for convex shapes.
        /// </summary>
        /// <param name="transform">The shape world transform.</param>
        /// <param name="point">a point in world coordinates.</param>
        /// <returns>True if the point is inside the shape</returns>
        public override bool TestPoint(ref Transform transform, ref Vector2 point)
        {
            Vector2 center = transform.Position + MathUtils.Multiply(ref transform.R, Position);
            Vector2 d      = point - center;

            return(Vector2.Dot(d, d) <= Radius * Radius);
        }
Esempio n. 7
0
        public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc)
        {
            sc = Vector2.zero;

            Vector2 p = MathUtils.Multiply(ref xf, Position);
            float   l = -(Vector2.Dot(normal, p) - offset);

            if (l < -Radius + Settings.Epsilon)
            {
                //Completely dry
                return(0);
            }
            if (l > Radius)
            {
                //Completely wet
                sc = p;
                return(Settings.Pi * Radius * Radius);
            }

            //Magic
            float r2   = Radius * Radius;
            float l2   = l * l;
            float area = r2 * (float)((Mathf.Asin(l / Radius) + Settings.Pi / 2) + l * Mathf.Sqrt(r2 - l2));
            float com  = -2.0f / 3.0f * Mathf.Pow(r2 - l2, 1.5f) / area;

            sc.x = p.x + normal.x * com;
            sc.y = p.y + normal.y * com;

            return(area);
        }
Esempio n. 8
0
        /// <summary>
        /// Performs a global physical picking operation and returns the <see cref="ShapeInfo">shapes</see> that
        /// intersect the specified world coordinate area.
        /// </summary>
        /// <param name="worldCoord"></param>
        /// <param name="size"></param>
        /// <param name="pickedShapes">
        /// A list that will be filled with all shapes that were found.
        /// The list will not be cleared before adding items.
        /// </param>
        /// <returns>Returns whether any new shape was found.</returns>
        public bool PickShapes(Vector2 worldCoord, Vector2 size, List <ShapeInfo> pickedShapes)
        {
            List <RigidBody> potentialBodies = new List <RigidBody>();

            this.QueryRect(worldCoord, size, potentialBodies);
            if (potentialBodies.Count == 0)
            {
                return(false);
            }

            PolygonShape boxShape = new PolygonShape(new Vertices(new List <Vector2>
            {
                PhysicsUnit.LengthToPhysical *worldCoord,
                PhysicsUnit.LengthToPhysical *new Vector2(worldCoord.X + size.X, worldCoord.Y),
                PhysicsUnit.LengthToPhysical * (worldCoord + size),
                PhysicsUnit.LengthToPhysical * new Vector2(worldCoord.X, worldCoord.Y + size.Y)
            }), 1);

            FarseerPhysics.Common.Transform boxTransform = new FarseerPhysics.Common.Transform();
            boxTransform.SetIdentity();

            int oldResultCount = pickedShapes.Count;

            foreach (RigidBody body in potentialBodies)
            {
                body.PickShapes(boxShape, boxTransform, pickedShapes);
            }

            return(pickedShapes.Count > oldResultCount);
        }
Esempio n. 9
0
        static bool collidePolygonCircle(PolygonShape polygon, ref FSTransform polyTransform, CircleShape circle, ref FSTransform circleTransform, out FSCollisionResult result)
        {
            result = new FSCollisionResult();
            Collision.collidePolygonAndCircle(ref _manifold, polygon, ref polyTransform, circle, ref circleTransform);
            if (_manifold.pointCount > 0)
            {
                FixedArray2 <Vector2> points;
                ContactSolver.WorldManifold.initialize(ref _manifold, ref polyTransform, polygon.radius, ref circleTransform, circle.radius, out result.normal, out points);

                //var circleInPolySpace = polygonFixture.Body.GetLocalPoint( circleFixture.Body.Position );
                var circleInPolySpace = MathUtils.mulT(ref polyTransform, circleTransform.p);
                var value1            = circleInPolySpace - _manifold.localPoint;
                var value2            = _manifold.localNormal;
                var separation        = circle.radius - (value1.X * value2.X + value1.Y * value2.Y);

                if (separation <= 0)
                {
                    return(false);
                }

                result.point = points[0] * FSConvert.simToDisplay;
                result.minimumTranslationVector = result.normal * separation;

                                #if DEBUG_FSCOLLISIONS
                Debug.drawPixel(result.point, 2, Color.Red, 0.2f);
                Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);
                                #endif

                return(true);
            }

            return(false);
        }
Esempio n. 10
0
        private void DrawShape(Fixture fixture, Transform xf, Color color)
        {
            switch (fixture.ShapeType)
            {
            case ShapeType.Circle:
            {
                CircleShape circle = (CircleShape)fixture.Shape;

                Vector2 center = MathUtils.M
                                     (ref xf, circle.Position);
                float   radius = circle.Radius;
                Vector2 axis   = MathUtils.Multiply(xf.q, new Vector2(1.0f, 0.0f));

                DrawSolidCircle(center, radius, axis, color);
            }
            break;

            case ShapeType.Polygon:
            {
                PolygonShape poly        = (PolygonShape)fixture.Shape;
                int          vertexCount = poly.Vertices.Count;
                Debug.Assert(vertexCount <= Settings.MaxPolygonVertices);
                Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices];

                for (int i = 0; i < vertexCount; ++i)
                {
                    vertices[i] = MathUtils.Multiply(ref xf, poly.Vertices[i]);
                }

                DrawSolidPolygon(vertices, vertexCount, color);
            }
            break;


            case ShapeType.Edge:
            {
                EdgeShape edge = (EdgeShape)fixture.Shape;
                Vector2   v1   = MathUtils.Multiply(ref xf, edge.Vertex1);
                Vector2   v2   = MathUtils.Multiply(ref xf, edge.Vertex2);
                DrawSegment(v1, v2, color);
            }
            break;

            case ShapeType.Chain:
            {
                ChainShape chain = (ChainShape)fixture.Shape;
                int        count = chain.Vertices.Count;

                Vector2 v1 = MathUtils.Multiply(ref xf, chain.Vertices[count - 1]);
                for (int i = 0; i < count; ++i)
                {
                    Vector2 v2 = MathUtils.Multiply(ref xf, chain.Vertices[i]);
                    DrawSegment(v1, v2, color);
                    v1 = v2;
                }
            }
            break;
            }
        }
Esempio n. 11
0
        public void DrawShape(Fixture fixture, FarseerPhysics.Common.Transform xf, Color color)
        {
            switch (fixture.Shape.ShapeType)
            {
            case ShapeType.Circle:
            {
                var circle = (CircleShape)fixture.Shape;

                System.Numerics.Vector2 center = MathUtils.Mul(ref xf, circle.Position);
                float radius = circle.Radius;
                System.Numerics.Vector2 axis = MathUtils.Mul(xf.Q, new System.Numerics.Vector2(1.0f, 0.0f));

                DrawSolidCircle(center, radius, axis, color);
            }
            break;

            case ShapeType.Polygon:
            {
                var poly        = (PolygonShape)fixture.Shape;
                int vertexCount = poly.Vertices.Count;
                System.Diagnostics.Debug.Assert(vertexCount <= Settings.MaxPolygonVertices);

                if (vertexCount > _tempVertices.Length)
                {
                    _tempVertices = new Vector2[vertexCount];
                }

                for (int i = 0; i < vertexCount; ++i)
                {
                    _tempVertices[i] = MathUtils.Mul(ref xf, poly.Vertices[i]).ToXna();
                }

                DrawSolidPolygon(_tempVertices, vertexCount, color);
            }
            break;

            case ShapeType.Edge:
            {
                var edge = (EdgeShape)fixture.Shape;
                var v1   = MathUtils.Mul(ref xf, edge.Vertex1);
                var v2   = MathUtils.Mul(ref xf, edge.Vertex2);
                DrawSegment(v1, v2, color);
            }
            break;

            case ShapeType.Chain:
            {
                var chain = (ChainShape)fixture.Shape;
                for (int i = 0; i < chain.Vertices.Count - 1; ++i)
                {
                    var v1 = MathUtils.Mul(ref xf, chain.Vertices[i]);
                    var v2 = MathUtils.Mul(ref xf, chain.Vertices[i + 1]);
                    DrawSegment(v1, v2, color);
                }
            }
            break;
            }
        }
Esempio n. 12
0
        public void DrawShape(Fixture fixture, FP.Transform xf, Color color)
        {
            switch (fixture.Shape.ShapeType)
            {
            case ShapeType.Circle:
            {
                CircleShape circle = (CircleShape)fixture.Shape;

                FP.Vector2 center = FP.MathUtils.Mul(ref xf, circle.Position);
                float      radius = circle.Radius;
                FP.Vector2 axis   = FP.MathUtils.Mul(xf.q, new FP.Vector2(1.0f, 0.0f));

                DrawSolidCircle(center, radius, axis, color);
            }
            break;

            case ShapeType.Polygon:
            {
                PolygonShape poly        = (PolygonShape)fixture.Shape;
                int          vertexCount = poly.Vertices.Count;
                Debug.Assert(vertexCount <= Settings.MaxPolygonVertices);

                for (int i = 0; i < vertexCount; ++i)
                {
                    _tempVertices[i] = FP.MathUtils.Mul(ref xf, poly.Vertices[i]);
                }

                DrawSolidPolygon(_tempVertices, vertexCount, color);
            }
            break;


            case ShapeType.Edge:
            {
                EdgeShape  edge = (EdgeShape)fixture.Shape;
                FP.Vector2 v1   = FP.MathUtils.Mul(ref xf, edge.Vertex1);
                FP.Vector2 v2   = FP.MathUtils.Mul(ref xf, edge.Vertex2);
                DrawSegment(v1, v2, color);
            }
            break;

            case ShapeType.Chain:
            {
                ChainShape chain = (ChainShape)fixture.Shape;

                for (int i = 0; i < chain.Vertices.Count - 1; ++i)
                {
                    FP.Vector2 v1 = FP.MathUtils.Mul(ref xf, chain.Vertices[i]);
                    FP.Vector2 v2 = FP.MathUtils.Mul(ref xf, chain.Vertices[i + 1]);
                    DrawSegment(v1, v2, color);
                }
            }
            break;
            }
        }
Esempio n. 13
0
        public override void DrawTransform(ref Transform transform)
        {
            const float axisScale = 0.4f;
            Vector2     p1        = transform.Position;

            Vector2 p2 = p1 + axisScale * transform.R.Col1;

            DrawSegment(p1, p2, Colors.Red);

            p2 = p1 + axisScale * transform.R.Col2;
            DrawSegment(p1, p2, Colors.Green);
        }
        public void drawTransform(ref FarseerPhysics.Common.Transform transform)
        {
            const float axisScale = 0.4f;
            Vector2     p1        = transform.p;

            Vector2 p2 = p1 + axisScale * transform.q.GetXAxis();

            drawSegment(p1, p2, Color.Red);

            p2 = p1 + axisScale * transform.q.GetYAxis();
            drawSegment(p1, p2, Color.Green);
        }
Esempio n. 15
0
        public override void DrawTransform(ref Transform transform)
        {
            const float axisScale = 0.4f;
            Vector2     p1        = transform.p;

            Vector2 p2 = p1 + axisScale * transform.q.GetXAxis();

            DrawSegment(p1, p2, Colors.Red);

            p2 = p1 + axisScale * transform.q.GetYAxis();
            DrawSegment(p1, p2, Colors.Green);
        }
Esempio n. 16
0
        public override void DrawTransform(ref FP.Transform transform)
        {
            const float axisScale = 0.4f;

            FP.Vector2 p1 = transform.p;

            FP.Vector2 p2 = p1 + axisScale * transform.q.GetXAxis();
            debugRenderer.DrawSegment(p1, p2, Color.Red);

            p2 = p1 + axisScale * transform.q.GetYAxis();
            debugRenderer.DrawSegment(p1, p2, Color.Green);
        }
Esempio n. 17
0
        public void DrawTransform(ref FarseerPhysics.Common.Transform transform)
        {
            const float AxisScale = 0.4f;
            Vector2     p1        = transform.Position;

            Vector2 p2 = p1 + AxisScale * transform.R.Col1;

            this.DrawSegment(p1, p2, Color.Red);

            p2 = p1 + AxisScale * transform.R.Col2;
            this.DrawSegment(p1, p2, Color.Green);
        }
Esempio n. 18
0
        public void DrawTransform(ref FarseerPhysics.Common.Transform transform)
        {
            const float axisScale = 0.4f;

            System.Numerics.Vector2 p1 = transform.P;

            System.Numerics.Vector2 p2 = p1 + axisScale * transform.Q.GetXAxis();
            DrawSegment(p1, p2, Color.Red);

            p2 = p1 + axisScale * transform.Q.GetYAxis();
            DrawSegment(p1, p2, Color.Green);
        }
Esempio n. 19
0
        static bool CollidePolygons(PolygonShape polygonA, ref FSTransform transformA, PolygonShape polygonB,
                                    ref FSTransform transformB, out FSCollisionResult result)
        {
            result = new FSCollisionResult();

            Collision.CollidePolygons(ref _manifold, polygonA as PolygonShape, ref transformA, polygonB as PolygonShape,
                                      ref transformB);
            if (_manifold.PointCount > 0)
            {
                FixedArray2 <Vector2> points;
                ContactSolver.WorldManifold.Initialize(ref _manifold, ref transformA, polygonA.Radius, ref transformB,
                                                       polygonB.Radius, out result.Normal, out points);

                // code adapted from PositionSolverManifold.Initialize
                if (_manifold.Type == ManifoldType.FaceA)
                {
                    result.Normal = MathUtils.Mul(transformA.Q, _manifold.LocalNormal);
                    var planePoint = MathUtils.Mul(ref transformA, _manifold.LocalPoint);

                    var clipPoint  = MathUtils.Mul(ref transformB, _manifold.Points[0].LocalPoint);
                    var separation = Vector2.Dot(clipPoint - planePoint, result.Normal) - polygonA.Radius -
                                     polygonB.Radius;
                    result.Point = clipPoint * FSConvert.SimToDisplay;

                    // Ensure normal points from A to B
                    Vector2.Negate(ref result.Normal, out result.Normal);

                    result.MinimumTranslationVector = result.Normal * -separation;
                }
                else
                {
                    result.Normal = MathUtils.Mul(transformB.Q, _manifold.LocalNormal);
                    var planePoint = MathUtils.Mul(ref transformB, _manifold.LocalPoint);

                    var clipPoint  = MathUtils.Mul(ref transformA, _manifold.Points[0].LocalPoint);
                    var separation = Vector2.Dot(clipPoint - planePoint, result.Normal) - polygonA.Radius -
                                     polygonB.Radius;
                    result.Point = clipPoint * FSConvert.SimToDisplay;

                    result.MinimumTranslationVector = result.Normal * -separation;
                }

#if DEBUG_FSCOLLISIONS
                Debug.drawPixel(result.point, 2, Color.Red, 0.2f);
                Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);
#endif

                return(true);
            }

            return(false);
        }
Esempio n. 20
0
        /// <summary>
        /// Given a transform, compute the associated axis aligned bounding box for a child shape.
        /// </summary>
        /// <param name="aabb">The aabb results.</param>
        /// <param name="transform">The world transform of the shape.</param>
        /// <param name="childIndex">The child shape index.</param>
        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
        {
            Vector2 v1 = MathUtils.Multiply(ref transform, _vertex1);
            Vector2 v2 = MathUtils.Multiply(ref transform, _vertex2);

            Vector2 lower = Vector2.Min(v1, v2);
            Vector2 upper = Vector2.Max(v1, v2);

            Vector2 r = new Vector2(Radius, Radius);

            aabb.LowerBound = lower - r;
            aabb.UpperBound = upper + r;
        }
Esempio n. 21
0
        static bool CollideEdgeAndCircle(EdgeShape edge, ref FSTransform edgeTransform, CircleShape circle,
                                         ref FSTransform circleTransform, out FSCollisionResult result)
        {
            result = new FSCollisionResult();
            Collision.CollideEdgeAndCircle(ref _manifold, edge, ref edgeTransform, circle, ref circleTransform);
            if (_manifold.PointCount > 0)
            {
                // code adapted from PositionSolverManifold.Initialize
                if (_manifold.Type == ManifoldType.Circles)
                {
                    // this is essentically directly from ContactSolver.WorldManifold.Initialize. To avoid doing the extra math twice we duplicate this code
                    // here because it doesnt return some values we need to calculate separation
                    var pointA = MathUtils.Mul(ref edgeTransform, _manifold.LocalPoint);
                    var pointB = MathUtils.Mul(ref circleTransform, _manifold.Points[0].LocalPoint);

                    result.Normal = pointA - pointB;
                    Vector2Ext.Normalize(ref result.Normal);

                    var cA = pointA - edge.Radius * result.Normal;
                    var cB = pointB + circle.Radius * result.Normal;
                    result.Point  = 0.5f * (cA + cB);
                    result.Point *= FSConvert.SimToDisplay;

                    var separation = Vector2.Dot(pointA - pointB, result.Normal) - edge.Radius - circle.Radius;

                    // Ensure normal points from A to B
                    Vector2.Negate(ref result.Normal, out result.Normal);
                    result.MinimumTranslationVector = result.Normal * Math.Abs(separation);
                }
                else                 // FaceA
                {
                    result.Normal = MathUtils.Mul(edgeTransform.Q, _manifold.LocalNormal);
                    var planePoint = MathUtils.Mul(ref edgeTransform, _manifold.LocalPoint);

                    var clipPoint  = MathUtils.Mul(ref circleTransform, _manifold.Points[0].LocalPoint);
                    var separation = Vector2.Dot(clipPoint - planePoint, result.Normal) - edge.Radius - circle.Radius;
                    result.Point = (clipPoint - result.Normal * circle.Radius) * FSConvert.SimToDisplay;

                    result.MinimumTranslationVector = result.Normal * -separation;
                }

#if DEBUG_FSCOLLISIONS
                Debug.drawPixel(result.point, 5, Color.Red, 0.2f);
                Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);
#endif

                return(true);
            }

            return(false);
        }
Esempio n. 22
0
        internal void ReadCache(ref SimplexCache cache,
                                DistanceProxy proxyA, ref Transform transformA,
                                DistanceProxy proxyB, ref Transform transformB)
        {
            //Debug.Assert(cache.Count <= 3);

            // Copy data from cache.
            Count = cache.Count;
            for (int i = 0; i < Count; ++i)
            {
                SimplexVertex v = V[i];
                v.IndexA = cache.IndexA[i];
                v.IndexB = cache.IndexB[i];
                Vector2 wALocal = proxyA.Vertices[v.IndexA];
                Vector2 wBLocal = proxyB.Vertices[v.IndexB];
                v.WA = MathUtils.Multiply(ref transformA, wALocal);
                v.WB = MathUtils.Multiply(ref transformB, wBLocal);
                v.W  = v.WB - v.WA;
                v.A  = 0.0f;
                V[i] = v;
            }

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

            // If the cache is empty or invalid ...
            if (Count == 0)
            {
                SimplexVertex v = V[0];
                v.IndexA = 0;
                v.IndexB = 0;
                Vector2 wALocal = proxyA.Vertices[0];
                Vector2 wBLocal = proxyB.Vertices[0];
                v.WA  = MathUtils.Multiply(ref transformA, wALocal);
                v.WB  = MathUtils.Multiply(ref transformB, wBLocal);
                v.W   = v.WB - v.WA;
                V[0]  = v;
                Count = 1;
            }
        }
Esempio n. 23
0
        /// <summary>
        /// Evaluate this contact with your own manifold and transforms.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="transformA">The first transform.</param>
        /// <param name="transformB">The second transform.</param>
        private void Evaluate(ref Manifold manifold, ref Transform transformA, ref Transform transformB)
        {
            switch (_type)
            {
            case ContactType.Polygon:
                Collision.Collision.CollidePolygons(ref manifold,
                                                    (PolygonShape)FixtureA.Shape, ref transformA,
                                                    (PolygonShape)FixtureB.Shape, ref transformB);
                break;

            case ContactType.PolygonAndCircle:
                Collision.Collision.CollidePolygonAndCircle(ref manifold,
                                                            (PolygonShape)FixtureA.Shape, ref transformA,
                                                            (CircleShape)FixtureB.Shape, ref transformB);
                break;

            case ContactType.EdgeAndCircle:
                Collision.Collision.CollideEdgeAndCircle(ref manifold,
                                                         (EdgeShape)FixtureA.Shape, ref transformA,
                                                         (CircleShape)FixtureB.Shape, ref transformB);
                break;

            case ContactType.EdgeAndPolygon:
                Collision.Collision.CollideEdgeAndPolygon(ref manifold,
                                                          (EdgeShape)FixtureA.Shape, ref transformA,
                                                          (PolygonShape)FixtureB.Shape, ref transformB);
                break;

            case ContactType.LoopAndCircle:
                LoopShape loop = (LoopShape)FixtureA.Shape;
                loop.GetChildEdge(ref _edge, ChildIndexA);
                Collision.Collision.CollideEdgeAndCircle(ref manifold, _edge, ref transformA,
                                                         (CircleShape)FixtureB.Shape, ref transformB);
                break;

            case ContactType.LoopAndPolygon:
                LoopShape loop2 = (LoopShape)FixtureA.Shape;
                loop2.GetChildEdge(ref _edge, ChildIndexA);
                Collision.Collision.CollideEdgeAndPolygon(ref manifold, _edge, ref transformA,
                                                          (PolygonShape)FixtureB.Shape, ref transformB);
                break;

            case ContactType.Circle:
                Collision.Collision.CollideCircles(ref manifold,
                                                   (CircleShape)FixtureA.Shape, ref transformA,
                                                   (CircleShape)FixtureB.Shape, ref transformB);
                break;
            }
        }
Esempio n. 24
0
        /// <summary>
        /// Test a point for containment in this shape. This only works for convex shapes.
        /// </summary>
        /// <param name="transform">The shape world transform.</param>
        /// <param name="point">a point in world coordinates.</param>
        /// <returns>True if the point is inside the shape</returns>
        public override bool TestPoint(ref Transform transform, ref Vector2 point)
        {
            Vector2 pLocal = MathUtils.MultiplyT(ref transform.R, point - transform.Position);

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

            return(true);
        }
Esempio n. 25
0
        private static bool CollideEdgeAndPolygon(EdgeShape edge, ref FSTransform edgeTransform, PolygonShape polygon,
                                                  ref FSTransform polygonTransform, out FSCollisionResult result)
        {
            result = new FSCollisionResult();
            Collision.CollideEdgeAndPolygon(ref _manifold, edge, ref edgeTransform, polygon, ref polygonTransform);
            if (_manifold.PointCount > 0)
            {
                FixedArray2 <Vector2> points;
                ContactSolver.WorldManifold.Initialize(ref _manifold, ref edgeTransform, edge.Radius,
                                                       ref polygonTransform, polygon.Radius, out result.Normal, out points);

                // code adapted from PositionSolverManifold.Initialize
                if (_manifold.Type == ManifoldType.FaceA)
                {
                    result.Normal = MathUtils.Mul(edgeTransform.Q, _manifold.LocalNormal);
                    Vector2 planePoint = MathUtils.Mul(ref edgeTransform, _manifold.LocalPoint);

                    Vector2 clipPoint  = MathUtils.Mul(ref polygonTransform, _manifold.Points[0].LocalPoint);
                    float   separation = Vector2.Dot(clipPoint - planePoint, result.Normal) - edge.Radius - polygon.Radius;
                    result.Point = clipPoint * FSConvert.SimToDisplay;

                    result.MinimumTranslationVector = result.Normal * -separation;
                }
                else
                {
                    result.Normal = MathUtils.Mul(polygonTransform.Q, _manifold.LocalNormal);
                    Vector2 planePoint = MathUtils.Mul(ref polygonTransform, _manifold.LocalPoint);

                    Vector2 clipPoint  = MathUtils.Mul(ref edgeTransform, _manifold.Points[0].LocalPoint);
                    float   separation = Vector2.Dot(clipPoint - planePoint, result.Normal) - edge.Radius - polygon.Radius;
                    result.Point = clipPoint * FSConvert.SimToDisplay;

                    // Ensure normal points from A to B
                    Vector2.Negate(ref result.Normal, out result.Normal);

                    result.MinimumTranslationVector = result.Normal * -separation;
                }

#if DEBUG_FSCOLLISIONS
                Debug.drawPixel(result.point, 5, Color.Red, 0.2f);
                Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);
#endif

                return(true);
            }

            return(false);
        }
Esempio n. 26
0
        private bool collideWithHole(Fixture fixtureA, ref FSTransform transformA, Fixture fixtureB, ref FSTransform transformB, out FSCollisionResult result)
        {
            result         = new FSCollisionResult();
            result.fixture = fixtureB;

            if (!ContactManager.shouldCollide(fixtureA, fixtureB))
            {
                return(false);
            }

            if (fixtureA.body.world.contactManager.onContactFilter != null && !fixtureA.body.world.contactManager.onContactFilter(fixtureA, fixtureB))
            {
                return(false);
            }

            return(collidePolygonCircle(fixtureB.shape as PolygonShape, ref transformB, fixtureA.shape as CircleShape, ref transformA, out result));
        }
Esempio n. 27
0
        /// <summary>
        /// Given a transform, compute the associated axis aligned bounding box for a child shape.
        /// </summary>
        /// <param name="aabb">The aabb results.</param>
        /// <param name="transform">The world transform of the shape.</param>
        /// <param name="childIndex">The child shape index.</param>
        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
        {
            Vector2 lower = MathUtils.Multiply(ref transform, Vertices[0]);
            Vector2 upper = lower;

            for (int i = 1; i < Vertices.Count; ++i)
            {
                Vector2 v = MathUtils.Multiply(ref transform, Vertices[i]);
                lower = Vector2.Min(lower, v);
                upper = Vector2.Max(upper, v);
            }

            Vector2 r = new Vector2(Radius, Radius);

            aabb.LowerBound = lower - r;
            aabb.UpperBound = upper + r;
        }
Esempio n. 28
0
        /// <summary>
        /// Cast a ray against a child shape.
        /// </summary>
        /// <param name="output">The ray-cast results.</param>
        /// <param name="input">The ray-cast input parameters.</param>
        /// <param name="transform">The transform to be applied to the shape.</param>
        /// <param name="childIndex">The child shape index.</param>
        /// <returns>True if the ray-cast hits the shape</returns>
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
                                     ref Transform transform, int childIndex)
        {
            //Debug.Assert(childIndex < Vertices.Count);

            int i1 = childIndex;
            int i2 = childIndex + 1;

            if (i2 == Vertices.Count)
            {
                i2 = 0;
            }

            _edgeShape.Vertex1 = Vertices[i1];
            _edgeShape.Vertex2 = Vertices[i2];

            return(_edgeShape.RayCast(out output, ref input, ref transform, 0));
        }
Esempio n. 29
0
        /// <summary>
        /// Given a transform, compute the associated axis aligned bounding box for a child shape.
        /// </summary>
        /// <param name="aabb">The aabb results.</param>
        /// <param name="transform">The world transform of the shape.</param>
        /// <param name="childIndex">The child shape index.</param>
        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
        {
            //Debug.Assert(childIndex < Vertices.Count);

            int i1 = childIndex;
            int i2 = childIndex + 1;

            if (i2 == Vertices.Count)
            {
                i2 = 0;
            }

            Vector2 v1 = MathUtils.Multiply(ref transform, Vertices[i1]);
            Vector2 v2 = MathUtils.Multiply(ref transform, Vertices[i2]);

            aabb.LowerBound = Vector2.Min(v1, v2);
            aabb.UpperBound = Vector2.Max(v1, v2);
        }
Esempio n. 30
0
        public static bool collideEdgeAndPolygon(EdgeShape edge, ref FSTransform edgeTransform, PolygonShape polygon, ref FSTransform polygonTransform, out CollisionResult result)
        {
            result = new CollisionResult();
            Collision.collideEdgeAndPolygon(ref _manifold, edge, ref edgeTransform, polygon, ref polygonTransform);
            if (_manifold.pointCount > 0)
            {
                FixedArray2 <Vector2> points;
                ContactSolver.WorldManifold.initialize(ref _manifold, ref edgeTransform, edge.radius, ref polygonTransform, polygon.radius, out result.normal, out points);

                // code adapted from PositionSolverManifold.Initialize
                if (_manifold.type == ManifoldType.FaceA)
                {
                    result.normal = MathUtils.mul(edgeTransform.q, _manifold.localNormal);
                    var planePoint = MathUtils.mul(ref edgeTransform, _manifold.localPoint);

                    var clipPoint  = MathUtils.mul(ref polygonTransform, _manifold.points[0].localPoint);
                    var separation = Vector2.Dot(clipPoint - planePoint, result.normal) - edge.radius - polygon.radius;
                    result.point = clipPoint * FSConvert.simToDisplay;

                    result.minimumTranslationVector = result.normal * -separation;
                }
                else
                {
                    result.normal = MathUtils.mul(polygonTransform.q, _manifold.localNormal);
                    var planePoint = MathUtils.mul(ref polygonTransform, _manifold.localPoint);

                    var clipPoint  = MathUtils.mul(ref edgeTransform, _manifold.points[0].localPoint);
                    var separation = Vector2.Dot(clipPoint - planePoint, result.normal) - edge.radius - polygon.radius;
                    result.point = clipPoint * FSConvert.simToDisplay;

                    // Ensure normal points from A to B
                    Vector2.Negate(ref result.normal, out result.normal);

                    result.minimumTranslationVector = result.normal * -separation;
                }

                Debug.drawPixel(result.point, 5, Color.Red, 0.2f);
                Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);

                return(true);
            }

            return(false);
        }
Esempio n. 31
0
        // These support body activation/deactivation.
        internal void CreateProxies(IBroadPhase broadPhase, ref Transform xf)
        {
            Debug.Assert(ProxyCount == 0);

            // Create proxies in the broad-phase.
            ProxyCount = Shape.ChildCount;

            for (int i = 0; i < ProxyCount; ++i)
            {
                FixtureProxy proxy = new FixtureProxy();
                Shape.ComputeAABB(out proxy.AABB, ref xf, i);

                proxy.Fixture    = this;
                proxy.ChildIndex = i;
                proxy.ProxyId    = broadPhase.AddProxy(ref proxy);

                Proxies[i] = proxy;
            }
        }
Esempio n. 32
0
        /// <summary>
        /// Cast a ray against a child shape.
        /// </summary>
        /// <param name="output">The ray-cast results.</param>
        /// <param name="input">The ray-cast input parameters.</param>
        /// <param name="transform">The transform to be applied to the shape.</param>
        /// <param name="childIndex">The child shape index.</param>
        /// <returns>True if the ray-cast hits the shape</returns>
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform,
                                     int childIndex)
        {
            // Collision Detection in Interactive 3D Environments by Gino van den Bergen
            // From Section 3.1.2
            // x = s + a * r
            // norm(x) = radius

            output = new RayCastOutput();

            Vector2 position = transform.Position + MathUtils.Multiply(ref transform.R, Position);
            Vector2 s = input.Point1 - position;
            float b = Vector2.Dot(s, s) - Radius * Radius;

            // Solve quadratic equation.
            Vector2 r = input.Point2 - input.Point1;
            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 < Settings.Epsilon)
            {
                return false;
            }

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

            // Is the intersection point on the segment?
            if (0.0f <= a && a <= input.MaxFraction * rr)
            {
                a /= rr;
                output.Fraction = a;
                Vector2 norm = (s + a * r);
                norm.Normalize();
                output.Normal = norm;
                return true;
            }

            return false;
        }
Esempio n. 33
0
 /// <summary>
 /// Test a point for containment in this shape. This only works for convex shapes.
 /// </summary>
 /// <param name="transform">The shape world transform.</param>
 /// <param name="point">a point in world coordinates.</param>
 /// <returns>True if the point is inside the shape</returns>
 public override bool TestPoint(ref Transform transform, ref Vector2 point)
 {
     Vector2 center = transform.Position + MathUtils.Multiply(ref transform.R, Position);
     Vector2 d = point - center;
     return Vector2.Dot(d, d) <= Radius * Radius;
 }
Esempio n. 34
0
 /// <summary>
 /// Given a transform, compute the associated axis aligned bounding box for a child shape.
 /// </summary>
 /// <param name="aabb">The aabb results.</param>
 /// <param name="transform">The world transform of the shape.</param>
 /// <param name="childIndex">The child shape index.</param>
 public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
 {
     Vector2 p = transform.Position + MathUtils.Multiply(ref transform.R, Position);
     aabb.LowerBound = new Vector2(p.x - Radius, p.y - Radius);
     aabb.UpperBound = new Vector2(p.x + Radius, p.y + Radius);
 }
Esempio n. 35
0
        public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc)
        {
            sc = Vector2.zero;

            Vector2 p = MathUtils.Multiply(ref xf, Position);
            float l = -(Vector2.Dot(normal, p) - offset);
            if (l < -Radius + Settings.Epsilon)
            {
                //Completely dry
                return 0;
            }
            if (l > Radius)
            {
                //Completely wet
                sc = p;
                return Settings.Pi * Radius * Radius;
            }

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

            sc.x = p.x + normal.x * com;
            sc.y = p.y + normal.y * com;

            return area;
        }
Esempio n. 36
0
        /// <summary>
        /// Given a transform, compute the associated axis aligned bounding box for a child shape.
        /// </summary>
        /// <param name="aabb">The aabb results.</param>
        /// <param name="transform">The world transform of the shape.</param>
        /// <param name="childIndex">The child shape index.</param>
        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
        {
            Vector2 v1 = MathUtils.Multiply(ref transform, _vertex1);
            Vector2 v2 = MathUtils.Multiply(ref transform, _vertex2);

            Vector2 lower = Vector2.Min(v1, v2);
            Vector2 upper = Vector2.Max(v1, v2);

            Vector2 r = new Vector2(Radius, Radius);
            aabb.LowerBound = lower - r;
            aabb.UpperBound = upper + r;
        }
Esempio n. 37
0
        /// <summary>
        /// Collides and edge and a polygon, taking into account edge adjacency.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="edgeA">The edge A.</param>
        /// <param name="xfA">The xf A.</param>
        /// <param name="polygonB">The polygon B.</param>
        /// <param name="xfB">The xf B.</param>
        public static void CollideEdgeAndPolygon(ref Manifold manifold,
                                                 EdgeShape edgeA, ref Transform xfA,
                                                 PolygonShape polygonB, ref Transform xfB)
        {
            MathUtils.MultiplyT(ref xfA, ref xfB, out _xf);

            // Edge geometry
            _edgeA.V0 = edgeA.Vertex0;
            _edgeA.V1 = edgeA.Vertex1;
            _edgeA.V2 = edgeA.Vertex2;
            _edgeA.V3 = edgeA.Vertex3;
            Vector2 e = _edgeA.V2 - _edgeA.V1;

            // Normal points outwards in CCW order.
            _edgeA.Normal = new Vector2(e.y, -e.x);
            _edgeA.Normal.Normalize();
            _edgeA.HasVertex0 = edgeA.HasVertex0;
            _edgeA.HasVertex3 = edgeA.HasVertex3;

            // Proxy for edge
            _proxyA.Vertices[0] = _edgeA.V1;
            _proxyA.Vertices[1] = _edgeA.V2;
            _proxyA.Normals[0] = _edgeA.Normal;
            _proxyA.Normals[1] = -_edgeA.Normal;
            _proxyA.Centroid = 0.5f * (_edgeA.V1 + _edgeA.V2);
            _proxyA.Count = 2;

            // Proxy for polygon
            _proxyB.Count = polygonB.Vertices.Count;
            _proxyB.Centroid = MathUtils.Multiply(ref _xf, ref polygonB.MassData.Centroid);
            for (int i = 0; i < polygonB.Vertices.Count; ++i)
            {
                _proxyB.Vertices[i] = MathUtils.Multiply(ref _xf, polygonB.Vertices[i]);
                _proxyB.Normals[i] = MathUtils.Multiply(ref _xf.R, polygonB.Normals[i]);
            }

            _radius = 2.0f * Settings.PolygonRadius;

            _limit11 = Vector2.zero;
            _limit12 = Vector2.zero;
            _limit21 = Vector2.zero;
            _limit22 = Vector2.zero;

            //Collide(ref manifold); inline start
            manifold.PointCount = 0;

            //ComputeAdjacency(); inline start
            Vector2 v0 = _edgeA.V0;
            Vector2 v1 = _edgeA.V1;
            Vector2 v2 = _edgeA.V2;
            Vector2 v3 = _edgeA.V3;

            // Determine allowable the normal regions based on adjacency.
            // Note: it may be possible that no normal is admissable.
            Vector2 centerB = _proxyB.Centroid;
            if (_edgeA.HasVertex0)
            {
                Vector2 e0 = v1 - v0;
                Vector2 e1 = v2 - v1;
                Vector2 n0 = new Vector2(e0.y, -e0.x);
                Vector2 n1 = new Vector2(e1.y, -e1.x);
                n0.Normalize();
                n1.Normalize();

                bool convex = MathUtils.Cross(n0, n1) >= 0.0f;
                bool front0 = Vector2.Dot(n0, centerB - v0) >= 0.0f;
                bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;

                if (convex)
                {
                    if (front0 || front1)
                    {
                        _limit11 = n1;
                        _limit12 = n0;
                    }
                    else
                    {
                        _limit11 = -n1;
                        _limit12 = -n0;
                    }
                }
                else
                {
                    if (front0 && front1)
                    {
                        _limit11 = n0;
                        _limit12 = n1;
                    }
                    else
                    {
                        _limit11 = -n0;
                        _limit12 = -n1;
                    }
                }
            }
            else
            {
                _limit11 = Vector2.zero;
                _limit12 = Vector2.zero;
            }

            if (_edgeA.HasVertex3)
            {
                Vector2 e1 = v2 - v1;
                Vector2 e2 = v3 - v2;
                Vector2 n1 = new Vector2(e1.y, -e1.x);
                Vector2 n2 = new Vector2(e2.y, -e2.x);
                n1.Normalize();
                n2.Normalize();

                bool convex = MathUtils.Cross(n1, n2) >= 0.0f;
                bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;
                bool front2 = Vector2.Dot(n2, centerB - v2) >= 0.0f;

                if (convex)
                {
                    if (front1 || front2)
                    {
                        _limit21 = n2;
                        _limit22 = n1;
                    }
                    else
                    {
                        _limit21 = -n2;
                        _limit22 = -n1;
                    }
                }
                else
                {
                    if (front1 && front2)
                    {
                        _limit21 = n1;
                        _limit22 = n2;
                    }
                    else
                    {
                        _limit21 = -n1;
                        _limit22 = -n2;
                    }
                }
            }
            else
            {
                _limit21 = Vector2.zero;
                _limit22 = Vector2.zero;
            }

            //ComputeAdjacency(); inline end

            //EPAxis edgeAxis = ComputeEdgeSeparation(); inline start
            EPAxis edgeAxis = ComputeEdgeSeparation();

            // If no valid normal can be found than this edge should not collide.
            // This can happen on the middle edge of a 3-edge zig-zag chain.
            if (edgeAxis.Type == EPAxisType.Unknown)
            {
                return;
            }

            if (edgeAxis.Separation > _radius)
            {
                return;
            }

            EPAxis polygonAxis = ComputePolygonSeparation();
            if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > _radius)
            {
                return;
            }

            // Use hysteresis for jitter reduction.
            const float k_relativeTol = 0.98f;
            const float k_absoluteTol = 0.001f;

            EPAxis primaryAxis;
            if (polygonAxis.Type == EPAxisType.Unknown)
            {
                primaryAxis = edgeAxis;
            }
            else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol)
            {
                primaryAxis = polygonAxis;
            }
            else
            {
                primaryAxis = edgeAxis;
            }

            EPProxy proxy1;
            EPProxy proxy2;
            FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>();
            if (primaryAxis.Type == EPAxisType.EdgeA)
            {
                proxy1 = _proxyA;
                proxy2 = _proxyB;
                manifold.Type = ManifoldType.FaceA;
            }
            else
            {
                proxy1 = _proxyB;
                proxy2 = _proxyA;
                manifold.Type = ManifoldType.FaceB;
            }

            int edge1 = primaryAxis.Index;

            FindIncidentEdge(ref incidentEdge, proxy1, primaryAxis.Index, proxy2);
            int count1 = proxy1.Count;

            int iv1 = edge1;
            int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

            Vector2 v11 = proxy1.Vertices[iv1];
            Vector2 v12 = proxy1.Vertices[iv2];

            Vector2 tangent = v12 - v11;
            tangent.Normalize();

            Vector2 normal = MathUtils.Cross(tangent, 1.0f);
            Vector2 planePoint = 0.5f * (v11 + v12);

            // Face offset.
            float frontOffset = Vector2.Dot(normal, v11);

            // Side offsets, extended by polytope skin thickness.
            float sideOffset1 = -Vector2.Dot(tangent, v11) + _radius;
            float sideOffset2 = Vector2.Dot(tangent, v12) + _radius;

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

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

            if (np < Settings.MaxManifoldPoints)
            {
                return;
            }

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

            if (np < Settings.MaxManifoldPoints)
            {
                return;
            }

            // Now clipPoints2 contains the clipped points.
            if (primaryAxis.Type == EPAxisType.EdgeA)
            {
                manifold.LocalNormal = normal;
                manifold.LocalPoint = planePoint;
            }
            else
            {
                manifold.LocalNormal = MathUtils.MultiplyT(ref _xf.R, ref normal);
                manifold.LocalPoint = MathUtils.MultiplyT(ref _xf, ref planePoint);
            }

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

                if (separation <= _radius)
                {
                    ManifoldPoint cp = manifold.Points[pointCount];

                    if (primaryAxis.Type == EPAxisType.EdgeA)
                    {
                        cp.LocalPoint = MathUtils.MultiplyT(ref _xf, clipPoints2[i1].V);
                        cp.Id = clipPoints2[i1].ID;
                    }
                    else
                    {
                        cp.LocalPoint = clipPoints2[i1].V;
                        cp.Id.Features.TypeA = clipPoints2[i1].ID.Features.TypeB;
                        cp.Id.Features.TypeB = clipPoints2[i1].ID.Features.TypeA;
                        cp.Id.Features.IndexA = clipPoints2[i1].ID.Features.IndexB;
                        cp.Id.Features.IndexB = clipPoints2[i1].ID.Features.IndexA;
                    }

                    manifold.Points[pointCount] = cp;

                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;

            //Collide(ref manifold); inline end
        }
Esempio n. 38
0
 /// <summary>
 /// Get the body transform for the body's origin.
 /// </summary>
 /// <param name="transform">The transform of the body's origin.</param>
 public void GetTransform(out Transform transform)
 {
     transform = Xf;
 }
Esempio n. 39
0
        private void DrawShape(Fixture fixture, Transform xf, Color color)
        {
            switch (fixture.Shape.ShapeType)
            {
                case ShapeType.Circle:
                    {
                        var circle = (FarseerCircleShape)fixture.Shape;

                        Vector2 center = MathUtils.Mul(ref xf, circle.Position);
                        float radius = circle.Radius;
                        Vector2 axis = xf.q.GetXAxis();

                        DrawSolidCircle(center, radius, axis, color.R, color.G, color.B);
                    }
                    break;

                case ShapeType.Polygon:
                    {
                        var poly = (PolygonShape)fixture.Shape;
                        int vertexCount = poly.Vertices.Count;
                        var vertices = new Vector2[vertexCount];

                        for (int i = 0; i < vertexCount; ++i)
                        {
                            vertices[i] = MathUtils.Mul(ref xf, poly.Vertices[i]);
                        }

                        DrawSolidPolygon(vertices, vertexCount, color.R, color.G, color.B);
                    }
                    break;
                default:
                    throw new NotImplementedException();
            }
        }
Esempio n. 40
0
 /// <summary>
 /// Test a point for containment in this shape. This only works for convex shapes.
 /// </summary>
 /// <param name="transform">The shape world transform.</param>
 /// <param name="point">a point in world coordinates.</param>
 /// <returns>True if the point is inside the shape</returns>
 public override bool TestPoint(ref Transform transform, ref Vector2 point)
 {
     return false;
 }
Esempio n. 41
0
        internal void SynchronizeFixtures()
        {
            Transform xf1 = new Transform();
            float c = (float)Math.Cos(Sweep.A0), s = (float)Math.Sin(Sweep.A0);
            xf1.R.Col1.x = c;
            xf1.R.Col2.x = -s;
            xf1.R.Col1.y = s;
            xf1.R.Col2.y = c;

            xf1.Position.x = Sweep.C0.x - (xf1.R.Col1.x * Sweep.LocalCenter.x + xf1.R.Col2.x * Sweep.LocalCenter.y);
            xf1.Position.y = Sweep.C0.y - (xf1.R.Col1.y * Sweep.LocalCenter.x + xf1.R.Col2.y * Sweep.LocalCenter.y);

            IBroadPhase broadPhase = World.ContactManager.BroadPhase;
            for (int i = 0; i < FixtureList.Count; i++)
            {
                FixtureList[i].Synchronize(broadPhase, ref xf1, ref Xf);
            }
        }
Esempio n. 42
0
 public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc)
 {
     sc = Vector2.zero;
     return 0;
 }
        private void DrawShape(Fixture fixture, Transform xf, Color color)
        {
            switch (fixture.Shape.ShapeType)
            {
                case ShapeType.Circle:
                    {
                        CircleShape circle = (CircleShape)fixture.Shape;

                        Vector2 center = MathUtils.Mul(ref xf, circle.Position);
                        float radius = circle.Radius;
                        Vector2 axis = MathUtils.Mul(xf.q, new Vector2(1.0f, 0.0f));

                        DrawSolidCircle(center, radius, axis, color);
                    }
                    break;

                case ShapeType.Polygon:
                    {
                        PolygonShape poly = (PolygonShape)fixture.Shape;
                        int vertexCount = poly.Vertices.Count;
                        Debug.Assert(vertexCount <= Settings.MaxPolygonVertices);

                        for (int i = 0; i < vertexCount; ++i)
                        {
                            _tempVertices[i] = MathUtils.Mul(ref xf, poly.Vertices[i]);
                        }

                        DrawSolidPolygon(_tempVertices, vertexCount, color);
                    }
                    break;

                case ShapeType.Edge:
                    {
                        EdgeShape edge = (EdgeShape)fixture.Shape;
                        Vector2 v1 = MathUtils.Mul(ref xf, edge.Vertex1);
                        Vector2 v2 = MathUtils.Mul(ref xf, edge.Vertex2);
                        DrawSegment(v1, v2, color);
                    }
                    break;

                case ShapeType.Chain:
                    {
                        ChainShape chain = (ChainShape)fixture.Shape;

                        for (int i = 0; i < chain.Vertices.Count - 1; ++i)
                        {
                            Vector2 v1 = MathUtils.Mul(ref xf, chain.Vertices[i]);
                            Vector2 v2 = MathUtils.Mul(ref xf, chain.Vertices[i + 1]);
                            DrawSegment(v1, v2, color);
                        }
                    }
                    break;
            }
        }
Esempio n. 44
0
 public override void DrawTransform(ref Transform transform)
 {
     throw new NotImplementedException();
 }
Esempio n. 45
0
        /// <summary>
        /// Compute the collision manifold between a polygon and a circle.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="polygonA">The polygon A.</param>
        /// <param name="transformA">The transform of A.</param>
        /// <param name="circleB">The circle B.</param>
        /// <param name="transformB">The transform of B.</param>
        public static void CollidePolygonAndCircle(ref Manifold manifold,
                                                   PolygonShape polygonA, ref Transform transformA,
                                                   CircleShape circleB, ref Transform transformB)
        {
            manifold.PointCount = 0;

            // Compute circle position in the frame of the polygon.
            Vector2 c =
                new Vector2(
                    transformB.Position.x + transformB.R.Col1.x * circleB.Position.x +
                    transformB.R.Col2.x * circleB.Position.y,
                    transformB.Position.y + transformB.R.Col1.y * circleB.Position.x +
                    transformB.R.Col2.y * circleB.Position.y);
            Vector2 cLocal =
                new Vector2(
                    (c.x - transformA.Position.x) * transformA.R.Col1.x +
                    (c.y - transformA.Position.y) * transformA.R.Col1.y,
                    (c.x - transformA.Position.x) * transformA.R.Col2.x +
                    (c.y - transformA.Position.y) * transformA.R.Col2.y);

            // Find the min separating edge.
            int normalIndex = 0;
            float separation = -Settings.MaxFloat;
            float radius = polygonA.Radius + circleB.Radius;
            int vertexCount = polygonA.Vertices.Count;

            for (int i = 0; i < vertexCount; ++i)
            {
                Vector2 value1 = polygonA.Normals[i];
                Vector2 value2 = cLocal - polygonA.Vertices[i];
                float s = value1.x * value2.x + value1.y * value2.y;

                if (s > radius)
                {
                    // Early out.
                    return;
                }

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

            // Vertices that subtend the incident face.
            int vertIndex1 = normalIndex;
            int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
            Vector2 v1 = polygonA.Vertices[vertIndex1];
            Vector2 v2 = polygonA.Vertices[vertIndex2];

            // If the center is inside the polygon ...
            if (separation < Settings.Epsilon)
            {
                manifold.PointCount = 1;
                manifold.Type = ManifoldType.FaceA;
                manifold.LocalNormal = polygonA.Normals[normalIndex];
                manifold.LocalPoint = 0.5f * (v1 + v2);

                ManifoldPoint p0 = manifold.Points[0];

                p0.LocalPoint = circleB.Position;
                p0.Id.Key = 0;

                manifold.Points[0] = p0;

                return;
            }

            // Compute barycentric coordinates
            float u1 = (cLocal.x - v1.x) * (v2.x - v1.x) + (cLocal.y - v1.y) * (v2.y - v1.y);
            float u2 = (cLocal.x - v2.x) * (v1.x - v2.x) + (cLocal.y - v2.y) * (v1.y - v2.y);

            if (u1 <= 0.0f)
            {
                float r = (cLocal.x - v1.x) * (cLocal.x - v1.x) + (cLocal.y - v1.y) * (cLocal.y - v1.y);
                if (r > radius * radius)
                {
                    return;
                }

                manifold.PointCount = 1;
                manifold.Type = ManifoldType.FaceA;
                manifold.LocalNormal = cLocal - v1;
                float factor = 1f /
                               (float)
                               Mathf.Sqrt(manifold.LocalNormal.x * manifold.LocalNormal.x +
                                         manifold.LocalNormal.y * manifold.LocalNormal.y);
                manifold.LocalNormal.x = manifold.LocalNormal.x * factor;
                manifold.LocalNormal.y = manifold.LocalNormal.y * factor;
                manifold.LocalPoint = v1;

                ManifoldPoint p0b = manifold.Points[0];

                p0b.LocalPoint = circleB.Position;
                p0b.Id.Key = 0;

                manifold.Points[0] = p0b;
            }
            else if (u2 <= 0.0f)
            {
                float r = (cLocal.x - v2.x) * (cLocal.x - v2.x) + (cLocal.y - v2.y) * (cLocal.y - v2.y);
                if (r > radius * radius)
                {
                    return;
                }

                manifold.PointCount = 1;
                manifold.Type = ManifoldType.FaceA;
                manifold.LocalNormal = cLocal - v2;
                float factor = 1f /
                               (float)
                               Mathf.Sqrt(manifold.LocalNormal.x * manifold.LocalNormal.x +
                                         manifold.LocalNormal.y * manifold.LocalNormal.y);
                manifold.LocalNormal.x = manifold.LocalNormal.x * factor;
                manifold.LocalNormal.y = manifold.LocalNormal.y * factor;
                manifold.LocalPoint = v2;

                ManifoldPoint p0c = manifold.Points[0];

                p0c.LocalPoint = circleB.Position;
                p0c.Id.Key = 0;

                manifold.Points[0] = p0c;
            }
            else
            {
                Vector2 faceCenter = 0.5f * (v1 + v2);
                Vector2 value1 = cLocal - faceCenter;
                Vector2 value2 = polygonA.Normals[vertIndex1];
                float separation2 = value1.x * value2.x + value1.y * value2.y;
                if (separation2 > radius)
                {
                    return;
                }

                manifold.PointCount = 1;
                manifold.Type = ManifoldType.FaceA;
                manifold.LocalNormal = polygonA.Normals[vertIndex1];
                manifold.LocalPoint = faceCenter;

                ManifoldPoint p0d = manifold.Points[0];

                p0d.LocalPoint = circleB.Position;
                p0d.Id.Key = 0;

                manifold.Points[0] = p0d;
            }
        }
Esempio n. 46
0
        internal void Synchronize(IBroadPhase broadPhase, ref Transform transform1, ref Transform transform2)
        {
            if (ProxyCount == 0)
            {
                return;
            }

            for (int i = 0; i < ProxyCount; ++i)
            {
                FixtureProxy proxy = Proxies[i];

                // Compute an AABB that covers the swept Shape (may miss some rotation effect).
                AABB aabb1, aabb2;
                Shape.ComputeAABB(out aabb1, ref transform1, proxy.ChildIndex);
                Shape.ComputeAABB(out aabb2, ref transform2, proxy.ChildIndex);

                proxy.AABB.Combine(ref aabb1, ref aabb2);

                Vector2 displacement = transform2.Position - transform1.Position;

                broadPhase.MoveProxy(proxy.ProxyId, ref proxy.AABB, displacement);
            }
        }
Esempio n. 47
0
        public static bool TestOverlap(Shape shapeA, int indexA,
                                       Shape shapeB, int indexB,
                                       ref Transform xfA, ref Transform xfB)
        {
            _input.ProxyA.Set(shapeA, indexA);
            _input.ProxyB.Set(shapeB, indexB);
            _input.TransformA = xfA;
            _input.TransformB = xfB;
            _input.UseRadii = true;

            SimplexCache cache;
            DistanceOutput output;
            Distance.ComputeDistance(out output, out cache, _input);

            return output.Distance < 10.0f * Settings.Epsilon;
        }
Esempio n. 48
0
        /// Compute the collision manifold between two circles.
        public static void CollideCircles(ref Manifold manifold,
                                          CircleShape circleA, ref Transform xfA,
                                          CircleShape circleB, ref Transform xfB)
        {
            manifold.PointCount = 0;

            float pAx = xfA.Position.x + xfA.R.Col1.x * circleA.Position.x + xfA.R.Col2.x * circleA.Position.y;
            float pAy = xfA.Position.y + xfA.R.Col1.y * circleA.Position.x + xfA.R.Col2.y * circleA.Position.y;
            float pBx = xfB.Position.x + xfB.R.Col1.x * circleB.Position.x + xfB.R.Col2.x * circleB.Position.y;
            float pBy = xfB.Position.y + xfB.R.Col1.y * circleB.Position.x + xfB.R.Col2.y * circleB.Position.y;

            float distSqr = (pBx - pAx) * (pBx - pAx) + (pBy - pAy) * (pBy - pAy);
            float radius = circleA.Radius + circleB.Radius;
            if (distSqr > radius * radius)
            {
                return;
            }

            manifold.Type = ManifoldType.Circles;
            manifold.LocalPoint = circleA.Position;
            manifold.LocalNormal = Vector2.zero;
            manifold.PointCount = 1;

            ManifoldPoint p0 = manifold.Points[0];

            p0.LocalPoint = circleB.Position;
            p0.Id.Key = 0;

            manifold.Points[0] = p0;
        }
        public override void DrawTransform(ref Transform transform)
        {
            const float axisScale = 0.4f;
            Vector2 p1 = transform.Position;

            Vector2 p2 = p1 + axisScale * transform.R.Col1;
            DrawSegment(p1, p2, Colors.Red);

            p2 = p1 + axisScale * transform.R.Col2;
            DrawSegment(p1, p2, Colors.Green);
        }
Esempio n. 50
0
        /// <summary>
        /// Compute contact points for edge versus circle.
        /// This accounts for edge connectivity.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="edgeA">The edge A.</param>
        /// <param name="transformA">The transform A.</param>
        /// <param name="circleB">The circle B.</param>
        /// <param name="transformB">The transform B.</param>
        public static void CollideEdgeAndCircle(ref Manifold manifold,
                                                EdgeShape edgeA, ref Transform transformA,
                                                CircleShape circleB, ref Transform transformB)
        {
            manifold.PointCount = 0;

            // Compute circle in frame of edge
            Vector2 Q = MathUtils.MultiplyT(ref transformA, MathUtils.Multiply(ref transformB, ref circleB._position));

            Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2;
            Vector2 e = B - A;

            // Barycentric coordinates
            float u = Vector2.Dot(e, B - Q);
            float v = Vector2.Dot(e, Q - A);

            float radius = edgeA.Radius + circleB.Radius;

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

            Vector2 P, d;

            // Region A
            if (v <= 0.0f)
            {
                P = A;
                d = Q - P;
                float dd = Vector2.Dot(d, d);
                //Vector2.Dot(ref d, ref d, out dd);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to A?
                if (edgeA.HasVertex0)
                {
                    Vector2 A1 = edgeA.Vertex0;
                    Vector2 B1 = A;
                    Vector2 e1 = B1 - A1;
                    float u1 = Vector2.Dot(e1, B1 - Q);

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

                cf.IndexA = 0;
                cf.TypeA = (byte)ContactFeatureType.Vertex;
                manifold.PointCount = 1;
                manifold.Type = ManifoldType.Circles;
                manifold.LocalNormal = Vector2.zero;
                manifold.LocalPoint = P;
                ManifoldPoint mp = new ManifoldPoint();
                mp.Id.Key = 0;
                mp.Id.Features = cf;
                mp.LocalPoint = circleB.Position;
                manifold.Points[0] = mp;
                return;
            }

            // Region B
            if (u <= 0.0f)
            {
                P = B;
                d = Q - P;
                float dd = Vector2.Dot(d, d);
                //Vector2.Dot(ref d, ref d, out dd);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to B?
                if (edgeA.HasVertex3)
                {
                    Vector2 B2 = edgeA.Vertex3;
                    Vector2 A2 = B;
                    Vector2 e2 = B2 - A2;
                    float v2 = Vector2.Dot(e2, Q - A2);

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

                cf.IndexA = 1;
                cf.TypeA = (byte)ContactFeatureType.Vertex;
                manifold.PointCount = 1;
                manifold.Type = ManifoldType.Circles;
                manifold.LocalNormal = Vector2.zero;
                manifold.LocalPoint = P;
                ManifoldPoint mp = new ManifoldPoint();
                mp.Id.Key = 0;
                mp.Id.Features = cf;
                mp.LocalPoint = circleB.Position;
                manifold.Points[0] = mp;
                return;
            }

            // Region AB
            float den = Vector2.Dot(e, e);
            //Vector2.Dot(ref e, ref e, out den);
            //Debug.Assert(den > 0.0f);
            P = (1.0f / den) * (u * A + v * B);
            d = Q - P;
            float dd2 = Vector2.Dot(d, d);
            //Vector2.Dot(ref d, ref d, out dd2);
            if (dd2 > radius * radius)
            {
                return;
            }

            Vector2 n = new Vector2(-e.y, e.x);
            if (Vector2.Dot(n, Q - A) < 0.0f)
            {
                n = new Vector2(-n.x, -n.y);
            }
            n.Normalize();

            cf.IndexA = 0;
            cf.TypeA = (byte)ContactFeatureType.Face;
            manifold.PointCount = 1;
            manifold.Type = ManifoldType.FaceA;
            manifold.LocalNormal = n;
            manifold.LocalPoint = A;
            ManifoldPoint mp2 = new ManifoldPoint();
            mp2.Id.Key = 0;
            mp2.Id.Features = cf;
            mp2.LocalPoint = circleB.Position;
            manifold.Points[0] = mp2;
        }
Esempio n. 51
0
        /// <summary>
        /// Compute the collision manifold between two polygons.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="polyA">The poly A.</param>
        /// <param name="transformA">The transform A.</param>
        /// <param name="polyB">The poly B.</param>
        /// <param name="transformB">The transform B.</param>
        public static void CollidePolygons(ref Manifold manifold,
                                           PolygonShape polyA, ref Transform transformA,
                                           PolygonShape polyB, ref Transform transformB)
        {
            manifold.PointCount = 0;
            float totalRadius = polyA.Radius + polyB.Radius;

            int edgeA = 0;
            float separationA = FindMaxSeparation(out edgeA, polyA, ref transformA, polyB, ref transformB);
            if (separationA > totalRadius)
                return;

            int edgeB = 0;
            float separationB = FindMaxSeparation(out edgeB, polyB, ref transformB, polyA, ref transformA);
            if (separationB > totalRadius)
                return;

            PolygonShape poly1; // reference polygon
            PolygonShape poly2; // incident polygon
            Transform xf1, xf2;
            int edge1; // reference edge
            bool flip;
            const float k_relativeTol = 0.98f;
            const float k_absoluteTol = 0.001f;

            if (separationB > k_relativeTol * separationA + k_absoluteTol)
            {
                poly1 = polyB;
                poly2 = polyA;
                xf1 = transformB;
                xf2 = transformA;
                edge1 = edgeB;
                manifold.Type = ManifoldType.FaceB;
                flip = true;
            }
            else
            {
                poly1 = polyA;
                poly2 = polyB;
                xf1 = transformA;
                xf2 = transformB;
                edge1 = edgeA;
                manifold.Type = ManifoldType.FaceA;
                flip = false;
            }

            FixedArray2<ClipVertex> incidentEdge;
            FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);

            int count1 = poly1.Vertices.Count;

            int iv1 = edge1;
            int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

            Vector2 v11 = poly1.Vertices[iv1];
            Vector2 v12 = poly1.Vertices[iv2];

            float localTangentX = v12.x - v11.x;
            float localTangentY = v12.y - v11.y;

            float factor = 1f / Mathf.Sqrt(localTangentX * localTangentX + localTangentY * localTangentY);
            localTangentX = localTangentX * factor;
            localTangentY = localTangentY * factor;

            Vector2 localNormal = new Vector2(localTangentY, -localTangentX);
            Vector2 planePoint = 0.5f * (v11 + v12);

            Vector2 tangent = new Vector2(xf1.R.Col1.x * localTangentX + xf1.R.Col2.x * localTangentY,
                                          xf1.R.Col1.y * localTangentX + xf1.R.Col2.y * localTangentY);
            float normalx = tangent.y;
            float normaly = -tangent.x;

            v11 = new Vector2(xf1.Position.x + xf1.R.Col1.x * v11.x + xf1.R.Col2.x * v11.y,
                              xf1.Position.y + xf1.R.Col1.y * v11.x + xf1.R.Col2.y * v11.y);
            v12 = new Vector2(xf1.Position.x + xf1.R.Col1.x * v12.x + xf1.R.Col2.x * v12.y,
                              xf1.Position.y + xf1.R.Col1.y * v12.x + xf1.R.Col2.y * v12.y);

            // Face offset.
            float frontOffset = normalx * v11.x + normaly * v11.y;

            // Side offsets, extended by polytope skin thickness.
            float sideOffset1 = -(tangent.x * v11.x + tangent.y * v11.y) + totalRadius;
            float sideOffset2 = tangent.x * v12.x + tangent.y * v12.y + totalRadius;

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

            // Clip to box side 1
            int np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1);

            if (np < 2)
                return;

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

            if (np < 2)
            {
                return;
            }

            // Now clipPoints2 contains the clipped points.
            manifold.LocalNormal = localNormal;
            manifold.LocalPoint = planePoint;

            int pointCount = 0;
            for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
            {
                Vector2 value = clipPoints2[i].V;
                float separation = normalx * value.x + normaly * value.y - frontOffset;

                if (separation <= totalRadius)
                {
                    ManifoldPoint cp = manifold.Points[pointCount];
                    Vector2 tmp = clipPoints2[i].V;
                    float tmp1X = tmp.x - xf2.Position.x;
                    float tmp1Y = tmp.y - xf2.Position.y;
                    cp.LocalPoint.x = tmp1X * xf2.R.Col1.x + tmp1Y * xf2.R.Col1.y;
                    cp.LocalPoint.y = tmp1X * xf2.R.Col2.x + tmp1Y * xf2.R.Col2.y;
                    cp.Id = clipPoints2[i].ID;

                    if (flip)
                    {
                        // Swap features
                        ContactFeature cf = cp.Id.Features;
                        cp.Id.Features.IndexA = cf.IndexB;
                        cp.Id.Features.IndexB = cf.IndexA;
                        cp.Id.Features.TypeA = cf.TypeB;
                        cp.Id.Features.TypeB = cf.TypeA;
                    }

                    manifold.Points[pointCount] = cp;

                    ++pointCount;
                }
            }

            manifold.PointCount = pointCount;
        }
Esempio n. 52
0
        /// <summary>
        /// Cast a ray against a child shape.
        /// </summary>
        /// <param name="output">The ray-cast results.</param>
        /// <param name="input">The ray-cast input parameters.</param>
        /// <param name="transform">The transform to be applied to the shape.</param>
        /// <param name="childIndex">The child shape index.</param>
        /// <returns>True if the ray-cast hits the shape</returns>
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
                                     ref Transform transform, int childIndex)
        {
            // p = p1 + t * d
            // v = v1 + s * e
            // p1 + t * d = v1 + s * e
            // s * e - t * d = p1 - v1

            output = new RayCastOutput();

            // Put the ray into the edge's frame of reference.
            Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.Point1 - transform.Position);
            Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.Point2 - transform.Position);
            Vector2 d = p2 - p1;

            Vector2 v1 = _vertex1;
            Vector2 v2 = _vertex2;
            Vector2 e = v2 - v1;
            Vector2 normal = new Vector2(e.y, -e.x);
            normal.Normalize();

            // q = p1 + t * d
            // dot(normal, q - v1) = 0
            // dot(normal, p1 - v1) + t * dot(normal, d) = 0
            float numerator = Vector2.Dot(normal, v1 - p1);
            float denominator = Vector2.Dot(normal, d);

            if (denominator == 0.0f)
            {
                return false;
            }

            float t = numerator / denominator;
            if (t < 0.0f || 1.0f < t)
            {
                return false;
            }

            Vector2 q = p1 + t * d;

            // q = v1 + s * r
            // s = dot(q - v1, r) / dot(r, r)
            Vector2 r = v2 - v1;
            float rr = Vector2.Dot(r, r);
            if (rr == 0.0f)
            {
                return false;
            }

            float s = Vector2.Dot(q - v1, r) / rr;
            if (s < 0.0f || 1.0f < s)
            {
                return false;
            }

            output.Fraction = t;
            if (numerator > 0.0f)
            {
                output.Normal = -normal;
            }
            else
            {
                output.Normal = normal;
            }
            return true;
        }
        public override void DrawTransform(ref Transform transform)
        {
            const float axisScale = 0.4f;
            Vector2 p1 = transform.p;

            Vector2 p2 = p1 + axisScale * transform.q.GetXAxis();
            DrawSegment(p1, p2, Colors.Red);

            p2 = p1 + axisScale * transform.q.GetYAxis();
            DrawSegment(p1, p2, Colors.Green);
        }
Esempio n. 54
0
        /// <summary>
        /// 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.
        /// </summary>
        /// <param name="manifold">The manifold.</param>
        /// <param name="transformA">The transform for A.</param>
        /// <param name="radiusA">The radius for A.</param>
        /// <param name="transformB">The transform for B.</param>
        /// <param name="radiusB">The radius for B.</param>
        /// <param name="normal">World vector pointing from A to B</param>
        /// <param name="points">Torld contact point (point of intersection).</param>
        public static void GetWorldManifold(ref Manifold manifold,
                                            ref Transform transformA, float radiusA,
                                            ref Transform transformB, float radiusB, out Vector2 normal,
                                            out FixedArray2<Vector2> points)
        {
            points = new FixedArray2<Vector2>();
            normal = Vector2.zero;

            if (manifold.PointCount == 0)
            {
                normal = Vector2.up;
                return;
            }

            switch (manifold.Type)
            {
                case ManifoldType.Circles:
                    {
                        Vector2 tmp = manifold.Points[0].LocalPoint;
                        float pointAx = transformA.Position.x + transformA.R.Col1.x * manifold.LocalPoint.x +
                                        transformA.R.Col2.x * manifold.LocalPoint.y;

                        float pointAy = transformA.Position.y + transformA.R.Col1.y * manifold.LocalPoint.x +
                                        transformA.R.Col2.y * manifold.LocalPoint.y;

                        float pointBx = transformB.Position.x + transformB.R.Col1.x * tmp.x +
                                        transformB.R.Col2.x * tmp.y;

                        float pointBy = transformB.Position.y + transformB.R.Col1.y * tmp.x +
                                        transformB.R.Col2.y * tmp.y;

                        normal.x = 1;
                        normal.y = 0;

                        float result = (pointAx - pointBx) * (pointAx - pointBx) +
                                       (pointAy - pointBy) * (pointAy - pointBy);
                        if (result > Settings.Epsilon * Settings.Epsilon)
                        {
                            float tmpNormalx = pointBx - pointAx;
                            float tmpNormaly = pointBy - pointAy;
                            float factor = 1f / Mathf.Sqrt(tmpNormalx * tmpNormalx + tmpNormaly * tmpNormaly);
                            normal.x = tmpNormalx * factor;
                            normal.y = tmpNormaly * factor;
                        }

                        Vector2 c = Vector2.zero;
                        c.x = (pointAx + radiusA * normal.x) + (pointBx - radiusB * normal.x);
                        c.y = (pointAy + radiusA * normal.y) + (pointBy - radiusB * normal.y);

                        points[0] = 0.5f * c;
                    }
                    break;

                case ManifoldType.FaceA:
                    {
                        normal.x = transformA.R.Col1.x * manifold.LocalNormal.x +
                                   transformA.R.Col2.x * manifold.LocalNormal.y;
                        normal.y = transformA.R.Col1.y * manifold.LocalNormal.x +
                                   transformA.R.Col2.y * manifold.LocalNormal.y;

                        float planePointx = transformA.Position.x + transformA.R.Col1.x * manifold.LocalPoint.x +
                                            transformA.R.Col2.x * manifold.LocalPoint.y;

                        float planePointy = transformA.Position.y + transformA.R.Col1.y * manifold.LocalPoint.x +
                                            transformA.R.Col2.y * manifold.LocalPoint.y;

                        for (int i = 0; i < manifold.PointCount; ++i)
                        {
                            Vector2 tmp = manifold.Points[i].LocalPoint;

                            float clipPointx = transformB.Position.x + transformB.R.Col1.x * tmp.x +
                                               transformB.R.Col2.x * tmp.y;

                            float clipPointy = transformB.Position.y + transformB.R.Col1.y * tmp.x +
                                               transformB.R.Col2.y * tmp.y;

                            float value = (clipPointx - planePointx) * normal.x + (clipPointy - planePointy) * normal.y;

                            Vector2 c = Vector2.zero;
                            c.x = (clipPointx + (radiusA - value) * normal.x) + (clipPointx - radiusB * normal.x);
                            c.y = (clipPointy + (radiusA - value) * normal.y) + (clipPointy - radiusB * normal.y);

                            points[i] = 0.5f * c;
                        }
                    }
                    break;

                case ManifoldType.FaceB:
                    {
                        normal.x = transformB.R.Col1.x * manifold.LocalNormal.x +
                                   transformB.R.Col2.x * manifold.LocalNormal.y;
                        normal.y = transformB.R.Col1.y * manifold.LocalNormal.x +
                                   transformB.R.Col2.y * manifold.LocalNormal.y;

                        float planePointx = transformB.Position.x + transformB.R.Col1.x * manifold.LocalPoint.x +
                                            transformB.R.Col2.x * manifold.LocalPoint.y;

                        float planePointy = transformB.Position.y + transformB.R.Col1.y * manifold.LocalPoint.x +
                                            transformB.R.Col2.y * manifold.LocalPoint.y;

                        for (int i = 0; i < manifold.PointCount; ++i)
                        {
                            Vector2 tmp = manifold.Points[i].LocalPoint;

                            float clipPointx = transformA.Position.x + transformA.R.Col1.x * tmp.x +
                                               transformA.R.Col2.x * tmp.y;

                            float clipPointy = transformA.Position.y + transformA.R.Col1.y * tmp.x +
                                               transformA.R.Col2.y * tmp.y;

                            float value = (clipPointx - planePointx) * normal.x + (clipPointy - planePointy) * normal.y;

                            Vector2 c = Vector2.zero;
                            c.x = (clipPointx - radiusA * normal.x) + (clipPointx + (radiusB - value) * normal.x);
                            c.y = (clipPointy - radiusA * normal.y) + (clipPointy + (radiusB - value) * normal.y);

                            points[i] = 0.5f * c;
                        }
                        // Ensure normal points from A to B.
                        normal *= -1;
                    }
                    break;
                default:
                    normal = Vector2.up;
                    break;
            }
        }
Esempio n. 55
0
        /// <summary>
        /// Given a transform, compute the associated axis aligned bounding box for a child shape.
        /// </summary>
        /// <param name="aabb">The aabb results.</param>
        /// <param name="transform">The world transform of the shape.</param>
        /// <param name="childIndex">The child shape index.</param>
        public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
        {
            //Debug.Assert(childIndex < Vertices.Count);

            int i1 = childIndex;
            int i2 = childIndex + 1;
            if (i2 == Vertices.Count)
            {
                i2 = 0;
            }

            Vector2 v1 = MathUtils.Multiply(ref transform, Vertices[i1]);
            Vector2 v2 = MathUtils.Multiply(ref transform, Vertices[i2]);

            aabb.LowerBound = Vector2.Min(v1, v2);
            aabb.UpperBound = Vector2.Max(v1, v2);
        }
Esempio n. 56
0
        /// <summary>
        /// Find the separation between poly1 and poly2 for a give edge normal on poly1.
        /// </summary>
        /// <param name="poly1">The poly1.</param>
        /// <param name="xf1">The XF1.</param>
        /// <param name="edge1">The edge1.</param>
        /// <param name="poly2">The poly2.</param>
        /// <param name="xf2">The XF2.</param>
        /// <returns></returns>
        private static float EdgeSeparation(PolygonShape poly1, ref Transform xf1, int edge1,
                                            PolygonShape poly2, ref Transform xf2)
        {
            int count2 = poly2.Vertices.Count;

            //Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);

            // Convert normal from poly1's frame into poly2's frame.
            Vector2 p1n = poly1.Normals[edge1];

            float normalWorldx = xf1.R.Col1.x * p1n.x + xf1.R.Col2.x * p1n.y;
            float normalWorldy = xf1.R.Col1.y * p1n.x + xf1.R.Col2.y * p1n.y;

            Vector2 normal = new Vector2(normalWorldx * xf2.R.Col1.x + normalWorldy * xf2.R.Col1.y,
                                         normalWorldx * xf2.R.Col2.x + normalWorldy * xf2.R.Col2.y);

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

            for (int i = 0; i < count2; ++i)
            {
                float dot = Vector2.Dot(poly2.Vertices[i], normal);

                if (dot < minDot)
                {
                    minDot = dot;
                    index = i;
                }
            }

            Vector2 p1ve = poly1.Vertices[edge1];
            Vector2 p2vi = poly2.Vertices[index];

            return ((xf2.Position.x + xf2.R.Col1.x * p2vi.x + xf2.R.Col2.x * p2vi.y) -
                    (xf1.Position.x + xf1.R.Col1.x * p1ve.x + xf1.R.Col2.x * p1ve.y)) * normalWorldx +
                   ((xf2.Position.y + xf2.R.Col1.y * p2vi.x + xf2.R.Col2.y * p2vi.y) -
                    (xf1.Position.y + xf1.R.Col1.y * p1ve.x + xf1.R.Col2.y * p1ve.y)) * normalWorldy;
        }
Esempio n. 57
0
        /// <summary>
        /// Cast a ray against a child shape.
        /// </summary>
        /// <param name="output">The ray-cast results.</param>
        /// <param name="input">The ray-cast input parameters.</param>
        /// <param name="transform">The transform to be applied to the shape.</param>
        /// <param name="childIndex">The child shape index.</param>
        /// <returns>True if the ray-cast hits the shape</returns>
        public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
                                     ref Transform transform, int childIndex)
        {
            //Debug.Assert(childIndex < Vertices.Count);

            int i1 = childIndex;
            int i2 = childIndex + 1;
            if (i2 == Vertices.Count)
            {
                i2 = 0;
            }

            _edgeShape.Vertex1 = Vertices[i1];
            _edgeShape.Vertex2 = Vertices[i2];

            return _edgeShape.RayCast(out output, ref input, ref transform, 0);
        }
Esempio n. 58
0
        private static void FindIncidentEdge(out FixedArray2<ClipVertex> c,
                                             PolygonShape poly1, ref Transform xf1, int edge1,
                                             PolygonShape poly2, ref Transform xf2)
        {
            c = new FixedArray2<ClipVertex>();

            int count2 = poly2.Vertices.Count;

            //Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);

            // Get the normal of the reference edge in poly2's frame.
            Vector2 v = poly1.Normals[edge1];
            float tmpx = xf1.R.Col1.x * v.x + xf1.R.Col2.x * v.y;
            float tmpy = xf1.R.Col1.y * v.x + xf1.R.Col2.y * v.y;
            Vector2 normal1 = new Vector2(tmpx * xf2.R.Col1.x + tmpy * xf2.R.Col1.y,
                                          tmpx * xf2.R.Col2.x + tmpy * xf2.R.Col2.y);

            // Find the incident edge on poly2.
            int index = 0;
            float minDot = Settings.MaxFloat;
            for (int i = 0; i < count2; ++i)
            {
                float dot = Vector2.Dot(normal1, poly2.Normals[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;

            ClipVertex cv0 = c[0];

            Vector2 v1 = poly2.Vertices[i1];
            cv0.V.x = xf2.Position.x + xf2.R.Col1.x * v1.x + xf2.R.Col2.x * v1.y;
            cv0.V.y = xf2.Position.y + xf2.R.Col1.y * v1.x + xf2.R.Col2.y * v1.y;
            cv0.ID.Features.IndexA = (byte)edge1;
            cv0.ID.Features.IndexB = (byte)i1;
            cv0.ID.Features.TypeA = (byte)ContactFeatureType.Face;
            cv0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;

            c[0] = cv0;

            ClipVertex cv1 = c[1];
            Vector2 v2 = poly2.Vertices[i2];
            cv1.V.x = xf2.Position.x + xf2.R.Col1.x * v2.x + xf2.R.Col2.x * v2.y;
            cv1.V.y = xf2.Position.y + xf2.R.Col1.y * v2.x + xf2.R.Col2.y * v2.y;
            cv1.ID.Features.IndexA = (byte)edge1;
            cv1.ID.Features.IndexB = (byte)i2;
            cv1.ID.Features.TypeA = (byte)ContactFeatureType.Face;
            cv1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;

            c[1] = cv1;
        }
Esempio n. 59
0
        // These support body activation/deactivation.
        internal void CreateProxies(IBroadPhase broadPhase, ref Transform xf)
        {
            //Debug.Assert(ProxyCount == 0);

            // Create proxies in the broad-phase.
            ProxyCount = Shape.ChildCount;

            for (int i = 0; i < ProxyCount; ++i)
            {
                FixtureProxy proxy = new FixtureProxy();
                Shape.ComputeAABB(out proxy.AABB, ref xf, i);

                proxy.Fixture = this;
                proxy.ChildIndex = i;
                proxy.ProxyId = broadPhase.AddProxy(ref proxy);

                Proxies[i] = proxy;
            }
        }
Esempio n. 60
0
        /// <summary>
        /// Find the max separation between poly1 and poly2 using edge normals from poly1.
        /// </summary>
        /// <param name="edgeIndex">Index of the edge.</param>
        /// <param name="poly1">The poly1.</param>
        /// <param name="xf1">The XF1.</param>
        /// <param name="poly2">The poly2.</param>
        /// <param name="xf2">The XF2.</param>
        /// <returns></returns>
        private static float FindMaxSeparation(out int edgeIndex,
                                               PolygonShape poly1, ref Transform xf1,
                                               PolygonShape poly2, ref Transform xf2)
        {
            int count1 = poly1.Vertices.Count;

            // Vector pointing from the centroid of poly1 to the centroid of poly2.
            float dx = (xf2.Position.x + xf2.R.Col1.x * poly2.MassData.Centroid.x +
                        xf2.R.Col2.x * poly2.MassData.Centroid.y) -
                       (xf1.Position.x + xf1.R.Col1.x * poly1.MassData.Centroid.x +
                        xf1.R.Col2.x * poly1.MassData.Centroid.y);
            float dy = (xf2.Position.y + xf2.R.Col1.y * poly2.MassData.Centroid.x +
                        xf2.R.Col2.y * poly2.MassData.Centroid.y) -
                       (xf1.Position.y + xf1.R.Col1.y * poly1.MassData.Centroid.x +
                        xf1.R.Col2.y * poly1.MassData.Centroid.y);
            Vector2 dLocal1 = new Vector2(dx * xf1.R.Col1.x + dy * xf1.R.Col1.y, dx * xf1.R.Col2.x + dy * xf1.R.Col2.y);

            // Find edge normal on poly1 that has the largest projection onto d.
            int edge = 0;
            float maxDot = -Settings.MaxFloat;
            for (int i = 0; i < count1; ++i)
            {
                float dot = Vector2.Dot(poly1.Normals[i], dLocal1);
                if (dot > maxDot)
                {
                    maxDot = dot;
                    edge = i;
                }
            }

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

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

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

            // Find the best edge and the search direction.
            int bestEdge;
            float bestSeparation;
            int increment;
            if (sPrev > s && sPrev > sNext)
            {
                increment = -1;
                bestEdge = prevEdge;
                bestSeparation = sPrev;
            }
            else if (sNext > s)
            {
                increment = 1;
                bestEdge = nextEdge;
                bestSeparation = sNext;
            }
            else
            {
                edgeIndex = edge;
                return s;
            }

            // Perform a local search for the best edge normal.
            for (; ; )
            {
                if (increment == -1)
                    edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
                else
                    edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;

                s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);

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

            edgeIndex = bestEdge;
            return bestSeparation;
        }