示例#1
0
 public EdgeShape(FVector2 start, FVector2 end)
     : base(0)
 {
     ShapeType = ShapeType.Edge;
     _radius = Settings.PolygonRadius;
     Set(start, end);
 }
示例#2
0
 public static FSFrictionJoint CreateFrictionJoint(FSWorld world, FSBody bodyA, FSBody bodyB, FVector2 anchorA,
                                                 FVector2 anchorB)
 {
     FSFrictionJoint frictionJoint = new FSFrictionJoint(bodyA, bodyB, anchorA, anchorB);
     world.AddJoint(frictionJoint);
     return frictionJoint;
 }
示例#3
0
        /// <summary>
        /// Attaches the bodies with revolute joints.
        /// </summary>
        /// <param name="world">The world.</param>
        /// <param name="bodies">The bodies.</param>
        /// <param name="localAnchorA">The local anchor A.</param>
        /// <param name="localAnchorB">The local anchor B.</param>
        /// <param name="connectFirstAndLast">if set to <c>true</c> [connect first and last].</param>
        /// <param name="collideConnected">if set to <c>true</c> [collide connected].</param>
        /// <param name="minLength">Minimum length of the slider joint.</param>
        /// <param name="maxLength">Maximum length of the slider joint.</param>
        /// <returns></returns>
        public static List<FSSliderJoint> AttachBodiesWithSliderJoint(FSWorld world, List<FSBody> bodies, FVector2 localAnchorA,
                                                                    FVector2 localAnchorB, bool connectFirstAndLast,
                                                                    bool collideConnected, float minLength,
                                                                    float maxLength)
        {
            List<FSSliderJoint> joints = new List<FSSliderJoint>(bodies.Count + 1);

            for (int i = 1; i < bodies.Count; i++)
            {
                FSSliderJoint joint = new FSSliderJoint(bodies[i], bodies[i - 1], localAnchorA, localAnchorB, minLength,
                                                    maxLength);
                joint.CollideConnected = collideConnected;
                world.AddJoint(joint);
                joints.Add(joint);
            }

            if (connectFirstAndLast)
            {
                FSSliderJoint lastjoint = new FSSliderJoint(bodies[0], bodies[bodies.Count - 1], localAnchorA, localAnchorB,
                                                        minLength, maxLength);
                lastjoint.CollideConnected = collideConnected;
                world.AddJoint(lastjoint);
                joints.Add(lastjoint);
            }

            return joints;
        }
示例#4
0
 public static Body CreateChainShape(World world, Vertices vertices, FVector2 position,
                                    object userData)
 {
     Body body = CreateBody(world, position);
     FixtureFactory.AttachChainShape(vertices, body, userData);
     return body;
 }
        /// <summary>
        /// Constructor
        /// </summary>
        public AbstractForceController()
            : base(ControllerType.AbstractForceController)
        {
            Enabled = true;

            Strength = 1.0f;
            Position = new FVector2(0, 0);
            MaximumSpeed = 100.0f;
            TimingMode = TimingModes.Switched;
            ImpulseTime = 0.0f;
            ImpulseLength = 1.0f;
            Triggered = false;
            StrengthCurve = new Curve();
            Variation = 0.0f;
            Randomize = new Random(1234);
            DecayMode = DecayModes.None;
            DecayCurve = new Curve();
            DecayStart = 0.0f;
            DecayEnd = 0.0f;

            StrengthCurve.Keys.Add(new CurveKey(0, 5));
            StrengthCurve.Keys.Add(new CurveKey(0.1f, 5));
            StrengthCurve.Keys.Add(new CurveKey(0.2f, -4));
            StrengthCurve.Keys.Add(new CurveKey(1f, 0));
        }
示例#6
0
 public static FSDistanceJoint CreateDistanceJoint(FSWorld world, FSBody bodyA, FSBody bodyB, FVector2 anchorA,
                                                 FVector2 anchorB)
 {
     FSDistanceJoint distanceJoint = new FSDistanceJoint(bodyA, bodyB, anchorA, anchorB);
     world.AddJoint(distanceJoint);
     return distanceJoint;
 }
示例#7
0
    public virtual Body GetBodyAtMouse(bool includeStatic)
    {
        // Make a small box
        mousePVec.X = MouseXWorldPhys;
        mousePVec.Y = MouseYWorldPhys;
        FVector2 lowerBound = new FVector2(MouseXWorldPhys - 0.001f, MouseYWorldPhys - 0.001f);
        FVector2 upperBound = new FVector2(MouseXWorldPhys + 0.001f, MouseYWorldPhys + 0.001f);
        AABB aabb = new AABB(lowerBound, upperBound);
        Body body = null;

        // Query the world for overlapping shapes
        System.Func<Fixture, bool> GetBodyCallback = delegate (Fixture fixture0)
        {
            Shape shape = fixture0.Shape;
            if(fixture0.Body.BodyType != BodyType.Static || includeStatic)
            {
                FarseerPhysics.Common.Transform transform0;
                fixture0.Body.GetTransform(out transform0);
                bool inside = shape.TestPoint(ref transform0, ref mousePVec);
                if(inside)
                {
                    body = fixture0.Body;
                    return false;
                }
            }
            return true;
        };
        FSWorldComponent.PhysicsWorld.QueryAABB(GetBodyCallback, ref aabb);
        return body;
    }
示例#8
0
 public static DistanceJoint CreateDistanceJoint(World world, Body bodyA, Body bodyB, FVector2 anchorA,
     FVector2 anchorB)
 {
     DistanceJoint distanceJoint = new DistanceJoint(bodyA, bodyB, anchorA, anchorB);
     world.AddJoint(distanceJoint);
     return distanceJoint;
 }
示例#9
0
    public virtual void MouseDrag()
    {
        // mouse press
        if(Input.GetMouseButtonDown(0) && mouseJoint == null)
        {
            Body body = GetBodyAtMouse();
            if(body != null)
            {
                FVector2 target = new FVector2(MouseXWorldPhys, MouseYWorldPhys);
                mouseJoint = JointFactory.CreateFixedMouseJoint(FSWorldComponent.PhysicsWorld, body, target);
                mouseJoint.CollideConnected = true;
                mouseJoint.MaxForce = 300f * body.Mass;
                body.Awake = true;
            }
        }
        // mouse release
        if(Input.GetMouseButtonUp(0))
        {
            if(mouseJoint != null)
            {
                FSWorldComponent.PhysicsWorld.RemoveJoint(mouseJoint);
                mouseJoint = null;
            }
        }

        // mouse move
        if(mouseJoint != null)
        {
            FVector2 p2 = new FVector2(MouseXWorldPhys, MouseYWorldPhys);
            mouseJoint.WorldAnchorB = p2;
        }
    }
示例#10
0
 public FrictionJoint(Body bodyA, Body bodyB, FVector2 localAnchorA, FVector2 localAnchorB)
     : base(bodyA, bodyB)
 {
     JointType = JointType.Friction;
     LocalAnchorA = localAnchorA;
     LocalAnchorB = localAnchorB;
 }
示例#11
0
 internal CircleShape()
     : base(0)
 {
     ShapeType = ShapeType.Circle;
     _radius = 0.0f;
     _position = FVector2.Zero;
 }
示例#12
0
 public static Fixture AttachRectangle(float width, float height, float density, FVector2 offset, Body body,
                                       object userData)
 {
     Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2);
     rectangleVertices.Translate(ref offset);
     PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density);
     return body.CreateFixture(rectangleShape, userData);
 }
示例#13
0
 public CircleShape(float radius, float density)
     : base(density)
 {
     ShapeType = ShapeType.Circle;
     _radius = radius;
     _position = FVector2.Zero;
     ComputeProperties();
 }
示例#14
0
        /// <summary>
        /// Initializes a new instance of the <see cref="Path"/> class.
        /// </summary>
        /// <param name="vertices">The vertices to created the path from.</param>
        public Path(FVector2[] vertices)
        {
            ControlPoints = new List<FVector2>(vertices.Length);

            for (int i = 0; i < vertices.Length; i++)
            {
                Add(vertices[i]);
            }
        }
示例#15
0
 public WheelJoint(Body bA, Body bB, FVector2 anchor, FVector2 axis)
     : base(bA, bB)
 {
     JointType = JointType.Wheel;
     LocalAnchorA = bA.GetLocalPoint(anchor);
     LocalAnchorB = bB.GetLocalPoint(anchor);
     m_localXAxisA = bA.GetLocalVector(axis);
     m_localYAxisA = MathUtils.Cross(1.0f, m_localXAxisA);
 }
示例#16
0
        public static Fixture AttachCircle(float radius, float density, Body body, FVector2 offset, object userData)
        {
            if (radius <= 0)
                throw new ArgumentOutOfRangeException("radius", "Radius must be more than 0 meters");

            CircleShape circleShape = new CircleShape(radius, density);
            circleShape.Position = offset;
            return body.CreateFixture(circleShape, userData);
        }
示例#17
0
	// Use this for initialization
	void Start ()
	{
		Vector3 p = transform.position;
		FVector2 aa = new FVector2(p.x + StartPoint.x, p.y + StartPoint.y);
		FVector2 bb = new FVector2(p.x + EndPoint.x, p.y + EndPoint.y);
		aabb = new AABB(aa, bb);
		buoyancyController = new BuoyancyController(aabb, Density, LinearDragCoef, RotationalDragCoef, FSWorldComponent.PhysicsWorld.Gravity);
		FSWorldComponent.PhysicsWorld.AddController(buoyancyController);
	}
示例#18
0
 /// Create a loop. This automatically adjusts connectivity.
 public void CreateLoop(Vertices vertices)
 {
     Debug.Assert(vertices.Count >= 3);
     Vertices = new Vertices(vertices);
     Vertices.Add(vertices[0]);
     _prevVertex = Vertices[Vertices.Count - 2];
     _nextVertex = Vertices[1];
     _hasPrevVertex = true;
     _hasNextVertex = true;
 }
示例#19
0
        /// <summary>
        /// Creates a breakable body. You would want to remove collinear points before using this.
        /// </summary>
        /// <param name="world">The world.</param>
        /// <param name="vertices">The vertices.</param>
        /// <param name="density">The density.</param>
        /// <param name="position">The position.</param>
        /// <returns></returns>
        public static BreakableBody CreateBreakableBody(World world, Vertices vertices, float density, FVector2 position,
            object userData)
        {
            List<Vertices> triangles = EarclipDecomposer.ConvexPartition(vertices);

            BreakableBody breakableBody = new BreakableBody(triangles, world, density, userData);
            breakableBody.MainBody.Position = position;
            world.AddBreakableBody(breakableBody);

            return breakableBody;
        }
示例#20
0
        public RopeJoint(Body bodyA, Body bodyB, FVector2 localAnchorA, FVector2 localAnchorB)
            : base(bodyA, bodyB)
        {
            JointType = JointType.Rope;
            LocalAnchorA = localAnchorA;
            LocalAnchorB = localAnchorB;

            //FPE: Setting default MaxLength
            FVector2 d = WorldAnchorB - WorldAnchorA;
            MaxLength = d.Length();
        }
示例#21
0
        /// <summary>
        /// Initializes a new instance of the <see cref="SliderJoint"/> class.
        /// Warning: Do not use a zero or short length.
        /// </summary>
        /// <param name="bodyA">The first body.</param>
        /// <param name="bodyB">The second body.</param>
        /// <param name="localAnchorA">The first body anchor.</param>
        /// <param name="localAnchorB">The second body anchor.</param>
        /// <param name="minLength">The minimum length between anchorpoints</param>
        /// <param name="maxlength">The maximum length between anchorpoints.</param>
        public SliderJoint(Body bodyA, Body bodyB, FVector2 localAnchorA, FVector2 localAnchorB, float minLength,
            float maxlength)
            : base(bodyA, bodyB)
        {
            JointType = JointType.Slider;

            LocalAnchorA = localAnchorA;
            LocalAnchorB = localAnchorB;
            MaxLength = maxlength;
            MinLength = minLength;
        }
 /// <summary>
 /// Initializes a new instance of the <see cref="BuoyancyController"/> class.
 /// </summary>
 /// <param name="container">Only bodies inside this AABB will be influenced by the controller</param>
 /// <param name="density">Density of the fluid</param>
 /// <param name="linearDragCoefficient">Linear drag coefficient of the fluid</param>
 /// <param name="rotationalDragCoefficient">Rotational drag coefficient of the fluid</param>
 /// <param name="gravity">The direction gravity acts. Buoyancy force will act in opposite direction of gravity.</param>
 public BuoyancyController(AABB container, float density, float linearDragCoefficient,
     float rotationalDragCoefficient, FVector2 gravity)
     : base(ControllerType.BuoyancyController)
 {
     Container = container;
     _normal = new FVector2(0, 1);
     Density = density;
     LinearDragCoefficient = linearDragCoefficient;
     AngularDragCoefficient = rotationalDragCoefficient;
     _gravity = gravity;
 }
示例#23
0
        /// <summary>
        /// This requires a world target point,
        /// tuning parameters, and the time step.
        /// </summary>
        /// <param name="body">The body.</param>
        /// <param name="worldAnchor">The target.</param>
        public FixedMouseJoint(Body body, FVector2 worldAnchor)
            : base(body)
        {
            JointType = JointType.FixedMouse;
            Frequency = 5.0f;
            DampingRatio = 0.7f;
            MaxForce = 1000 * body.Mass;

            Debug.Assert(worldAnchor.IsValid());

            _targetA = worldAnchor;
            LocalAnchorB = MathUtils.MulT(BodyA.Xf, worldAnchor);
        }
示例#24
0
        /// <summary>
        /// Check if the point P is inside the triangle defined by
        /// the points A, B, C
        /// </summary>
        /// <param name="a">The A point.</param>
        /// <param name="b">The B point.</param>
        /// <param name="c">The C point.</param>
        /// <param name="p">The point to be tested.</param>
        /// <returns>True if the point is inside the triangle</returns>
        private static bool InsideTriangle(ref FVector2 a, ref FVector2 b, ref FVector2 c, ref FVector2 p)
        {
            //A cross bp
            float abp = (c.X - b.X) * (p.Y - b.Y) - (c.Y - b.Y) * (p.X - b.X);

            //A cross ap
            float aap = (b.X - a.X) * (p.Y - a.Y) - (b.Y - a.Y) * (p.X - a.X);

            //b cross cp
            float bcp = (a.X - c.X) * (p.Y - c.Y) - (a.Y - c.Y) * (p.X - c.X);

            return ((abp >= 0.0f) && (bcp >= 0.0f) && (aap >= 0.0f));
        }
示例#25
0
        // From Eric Jordan's convex decomposition library
        /// <summary>
        ///Check if the lines a0->a1 and b0->b1 cross.
        ///If they do, intersectionPoint will be filled
        ///with the point of crossing.
        ///
        ///Grazing lines should not return true.
        ///
        /// </summary>
        /// <param name="a0"></param>
        /// <param name="a1"></param>
        /// <param name="b0"></param>
        /// <param name="b1"></param>
        /// <param name="intersectionPoint"></param>
        /// <returns></returns>
        public static bool LineIntersect2(FVector2 a0, FVector2 a1, FVector2 b0, FVector2 b1, out FVector2 intersectionPoint)
        {
            intersectionPoint = FVector2.Zero;

            if (a0 == b0 || a0 == b1 || a1 == b0 || a1 == b1)
            {
                return(false);
            }

            float x1 = a0.X;
            float y1 = a0.Y;
            float x2 = a1.X;
            float y2 = a1.Y;
            float x3 = b0.X;
            float y3 = b0.Y;
            float x4 = b1.X;
            float y4 = b1.Y;

            //AABB early exit
            if (Math.Max(x1, x2) < Math.Min(x3, x4) || Math.Max(x3, x4) < Math.Min(x1, x2))
            {
                return(false);
            }

            if (Math.Max(y1, y2) < Math.Min(y3, y4) || Math.Max(y3, y4) < Math.Min(y1, y2))
            {
                return(false);
            }

            float ua    = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3));
            float ub    = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3));
            float denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);

            if (Math.Abs(denom) < FSSettings.Epsilon)
            {
                //Lines are too close to parallel to call
                return(false);
            }
            ua /= denom;
            ub /= denom;

            if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1))
            {
                intersectionPoint.X = (x1 + ua * (x2 - x1));
                intersectionPoint.Y = (y1 + ua * (y2 - y1));
                return(true);
            }

            return(false);
        }
示例#26
0
        /// <summary>
        /// Initialize the bodies and local anchor.
        /// This requires defining an
        /// anchor point where the bodies are joined. The definition
        /// uses local anchor points so that the initial configuration
        /// can violate the constraint slightly. You also need to
        /// specify the initial relative angle for joint limits. This
        /// helps when saving and loading a game.
        /// The local anchor points are measured from the body's origin
        /// rather than the center of mass because:
        /// 1. you might not know where the center of mass will be.
        /// 2. if you add/remove shapes from a body and recompute the mass,
        /// the joints will be broken.
        /// </summary>
        /// <param name="bodyA">The first body.</param>
        /// <param name="bodyB">The second body.</param>
        /// <param name="localAnchorA">The first body anchor.</param>
        /// <param name="localAnchorB">The second anchor.</param>
        public RevoluteJoint(Body bodyA, Body bodyB, FVector2 localAnchorA, FVector2 localAnchorB)
            : base(bodyA, bodyB)
        {
            JointType = JointType.Revolute;

            // Changed to local coordinates.
            LocalAnchorA = localAnchorA;
            LocalAnchorB = localAnchorB;

            ReferenceAngle = BodyB.Rotation - BodyA.Rotation;

            _impulse    = FVector3.Zero;
            _limitState = LimitState.Inactive;
        }
示例#27
0
            public static void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index, out FVector2 normal, out FVector2 point, out float separation)
            {
                Debug.Assert(pc.pointCount > 0);


                switch (pc.type)
                {
                case ManifoldType.Circles:
                {
                    FVector2 pointA = MathUtils.Mul(ref xfA, pc.localPoint);
                    FVector2 pointB = MathUtils.Mul(ref xfB, pc.localPoints[0]);
                    normal = pointB - pointA;
                    normal.Normalize();
                    point      = 0.5f * (pointA + pointB);
                    separation = FVector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB;
                }
                break;

                case ManifoldType.FaceA:
                {
                    normal = MathUtils.Mul(xfA.q, pc.localNormal);
                    FVector2 planePoint = MathUtils.Mul(ref xfA, pc.localPoint);

                    FVector2 clipPoint = MathUtils.Mul(ref xfB, pc.localPoints[index]);
                    separation = FVector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
                    point      = clipPoint;
                }
                break;

                case ManifoldType.FaceB:
                {
                    normal = MathUtils.Mul(xfB.q, pc.localNormal);
                    FVector2 planePoint = MathUtils.Mul(ref xfB, pc.localPoint);

                    FVector2 clipPoint = MathUtils.Mul(ref xfA, pc.localPoints[index]);
                    separation = FVector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
                    point      = clipPoint;

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

                default:
                    normal     = FVector2.Zero;
                    point      = FVector2.Zero;
                    separation = 0;
                    break;
                }
            }
示例#28
0
        internal override bool SolvePositionConstraints(ref SolverData data)
        {
            FVector2 cA = data.positions[m_indexA].c;
            float    aA = data.positions[m_indexA].a;
            FVector2 cB = data.positions[m_indexB].c;
            float    aB = data.positions[m_indexB].a;

            Rot qA = new Rot(aA), qB = new Rot(aB);

            FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA);
            FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB);
            FVector2 d  = (cB - cA) + rB - rA;

            FVector2 ay = MathUtils.Mul(qA, m_localYAxisA);

            float sAy = MathUtils.Cross(d + rA, ay);
            float sBy = MathUtils.Cross(rB, ay);

            float C = FVector2.Dot(d, ay);

            float k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;

            float impulse;

            if (k != 0.0f)
            {
                impulse = -C / k;
            }
            else
            {
                impulse = 0.0f;
            }

            FVector2 P  = impulse * ay;
            float    LA = impulse * sAy;
            float    LB = impulse * sBy;

            cA -= m_invMassA * P;
            aA -= m_invIA * LA;
            cB += m_invMassB * P;
            aB += m_invIB * LB;

            data.positions[m_indexA].c = cA;
            data.positions[m_indexA].a = aA;
            data.positions[m_indexB].c = cB;
            data.positions[m_indexB].a = aB;

            return(Math.Abs(C) <= FSSettings.LinearSlop);
        }
示例#29
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 FVector2 point)
        {
            FVector2 pLocal = MathUtils.MulT(transform.q, point - transform.p);

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

            return(true);
        }
示例#30
0
        /// <summary>
        /// This requires defining a line of
        /// motion using an axis and an anchor point. The definition uses local
        /// anchor points and a local axis so that the initial configuration
        /// can violate the constraint slightly. The joint translation is zero
        /// when the local anchor points coincide in world space. Using local
        /// anchors and a local axis helps when saving and loading a game.
        /// </summary>
        /// <param name="bodyA">The first body.</param>
        /// <param name="bodyB">The second body.</param>
        /// <param name="localAnchorA">The first body anchor.</param>
        /// <param name="localAnchorB">The second body anchor.</param>
        /// <param name="axis">The axis.</param>
        public FSPrismaticJoint(FSBody bodyA, FSBody bodyB, FVector2 localAnchorA, FVector2 localAnchorB, FVector2 axis)
            : base(bodyA, bodyB)
        {
            JointType = JointType.Prismatic;

            LocalAnchorA = localAnchorA;
            LocalAnchorB = localAnchorB;

            _localXAxisA = BodyA.GetLocalVector(axis);
            _localXAxisA.Normalize();
            _localYAxisA     = MathUtils.Cross(1.0f, _localXAxisA);
            m_referenceAngle = BodyB.Rotation - BodyA.Rotation;

            _limitState = LimitState.Inactive;
        }
示例#31
0
        /// <summary>
        /// Needed to calculate the characteristics function of a simplex.
        /// </summary>
        /// <remarks>Used by method <c>CalculateEdgeCharacter()</c>.</remarks>
        private static float CalculateBeta(FVector2 point, Edge e, float coefficient)
        {
            float result = 0f;

            if (PointInSimplex(point, e))
            {
                result = coefficient;
            }
            if (PointOnLineSegment(FVector2.Zero, e.EdgeStart, point) ||
                PointOnLineSegment(FVector2.Zero, e.EdgeEnd, point))
            {
                result = .5f * coefficient;
            }
            return(result);
        }
示例#32
0
        /// <summary>
        /// Create a proxy in the tree as a leaf node. We return the index
        /// of the node instead of a pointer so that we can grow
        /// the node pool.        
        /// /// </summary>
        /// <param name="aabb">The aabb.</param>
        /// <param name="userData">The user data.</param>
        /// <returns>Index of the created proxy</returns>
        public int AddProxy(ref AABB aabb, T userData)
        {
            int proxyId = AllocateNode();

            // Fatten the aabb.
            FVector2 r = new FVector2(FSSettings.AABBExtension, FSSettings.AABBExtension);
            _nodes[proxyId].AABB.LowerBound = aabb.LowerBound - r;
            _nodes[proxyId].AABB.UpperBound = aabb.UpperBound + r;
            _nodes[proxyId].UserData = userData;
            _nodes[proxyId].Height = 0;

            InsertLeaf(proxyId);

            return proxyId;
        }
示例#33
0
        /// <summary>
        /// Initialize the bodies and world anchor.
        /// </summary>
        /// <param name="bodyA">The first body.</param>
        /// <param name="bodyB">The second body.</param>
        /// <param name="worldAnchor">The world coordinate anchor.</param>
        public FSRevoluteJoint(FSBody bodyA, FSBody bodyB, FVector2 worldAnchor)
            : base(bodyA, bodyB)
        {
            JointType = JointType.Revolute;

            // Changed to local coordinates.
            LocalAnchorA = bodyA.GetLocalPoint(worldAnchor);
            LocalAnchorB = bodyB.GetLocalPoint(worldAnchor);

            ReferenceAngle = BodyB.Rotation - BodyA.Rotation;

            _impulse = FVector3.Zero;

            _limitState = LimitState.Inactive;
        }
示例#34
0
        /// <summary>
        /// Apply an impulse at a point. This immediately modifies the velocity.
        /// It also modifies the angular velocity if the point of application
        /// is not at the center of mass.
        /// This wakes up the body.
        /// </summary>
        /// <param name="impulse">The world impulse vector, usually in N-seconds or kg-m/s.</param>
        /// <param name="point">The world position of the point of application.</param>
        public void ApplyLinearImpulse(ref FVector2 impulse, ref FVector2 point)
        {
            if (_bodyType != BodyType.Dynamic)
            {
                return;
            }

            if (Awake == false)
            {
                Awake = true;
            }

            LinearVelocityInternal  += InvMass * impulse;
            AngularVelocityInternal += InvI * ((point.X - Sweep.C.X) * impulse.Y - (point.Y - Sweep.C.Y) * impulse.X);
        }
示例#35
0
        public static float DistanceBetweenPointAndLineSegment(ref FVector2 point, ref FVector2 lineEndPoint1,
                                                               ref FVector2 lineEndPoint2)
        {
            FVector2 v = FVector2.Subtract(lineEndPoint2, lineEndPoint1);
            FVector2 w = FVector2.Subtract(point, lineEndPoint1);

            float c1 = FVector2.Dot(w, v);
            if (c1 <= 0) return DistanceBetweenPointAndPoint(ref point, ref lineEndPoint1);

            float c2 = FVector2.Dot(v, v);
            if (c2 <= c1) return DistanceBetweenPointAndPoint(ref point, ref lineEndPoint2);

            float b = c1 / c2;
            FVector2 pointOnLine = FVector2.Add(lineEndPoint1, FVector2.Multiply(v, b));
            return DistanceBetweenPointAndPoint(ref point, ref pointOnLine);
        }
示例#36
0
        public static Body CreateRectangle(World world, float width, float height, float density, FVector2 position,
                                           object userData)
        {
            if (width <= 0)
                throw new ArgumentOutOfRangeException("width", "Width must be more than 0 meters");

            if (height <= 0)
                throw new ArgumentOutOfRangeException("height", "Height must be more than 0 meters");

            Body newBody = CreateBody(world, position);
            Vertices rectangleVertices = PolygonTools.CreateRectangle(width / 2, height / 2);
            PolygonShape rectangleShape = new PolygonShape(rectangleVertices, density);
            newBody.CreateFixture(rectangleShape, userData);

            return newBody;
        }
示例#37
0
        public static List <FSFixture> AttachSolidArc(float density, float radians, int sides, float radius,
                                                      FVector2 position, float angle, FSBody body)
        {
            Vertices arc = PolygonTools.CreateArc(radians, sides, radius);

            arc.Rotate((MathHelper.Pi - radians) / 2 + angle);

            arc.Translate(ref position);

            //Close the arc
            arc.Add(arc[0]);

            List <Vertices> triangles = EarclipDecomposer.ConvexPartition(arc);

            return(AttachCompoundPolygon(triangles, density, body));
        }
示例#38
0
        /// <summary>
        /// Build vertices to represent an oriented box.
        /// </summary>
        /// <param name="hx">the half-width.</param>
        /// <param name="hy">the half-height.</param>
        /// <param name="center">the center of the box in local coordinates.</param>
        /// <param name="angle">the rotation of the box in local coordinates.</param>
        public static Vertices CreateRectangle(float hx, float hy, FVector2 center, float angle)
        {
            Vertices vertices = CreateRectangle(hx, hy);

            Transform xf = new Transform();
            xf.p = center;
            xf.q.Set(angle);

            // Transform vertices
            for (int i = 0; i < 4; ++i)
            {
                vertices[i] = MathUtils.Mul(ref xf, vertices[i]);
            }

            return vertices;
        }
示例#39
0
        /// <summary>
        /// Returns the coefficient of a simplex.
        /// </summary>
        /// <remarks>Used by method <c>CalculateSimplicalChain()</c>.</remarks>
        private static float CalculateSimplexCoefficient(FVector2 a, FVector2 b, FVector2 c)
        {
            float isLeft = MathUtils.Area(ref a, ref b, ref c);

            if (isLeft < 0f)
            {
                return(-1f);
            }

            if (isLeft > 0f)
            {
                return(1f);
            }

            return(0f);
        }
    private void ExcuteTestExtremaPoint()
    {
        FVector2 r = inputPoints[curRefreToPointIdx] - inputPoints[convexHull[curExtremaPointIdx]];
        FVector2 v = inputPoints[curTestPointIdx] - inputPoints[convexHull[curExtremaPointIdx]];
        float    c = Cross(r, v);

        if (c < 0.0f)
        {
            this.curRefreToPointIdx = curTestPointIdx;
        }

        // Collinearity check
        if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
        {
            this.curRefreToPointIdx = curTestPointIdx;
        }
    }
示例#41
0
        /// <summary>
        /// Get the supporting vertex in the given direction.
        /// </summary>
        /// <param name="direction">The direction.</param>
        /// <returns></returns>
        public FVector2 GetSupportVertex(FVector2 direction)
        {
            int   bestIndex = 0;
            float bestValue = FVector2.Dot(Vertices[0], direction);

            for (int i = 1; i < Vertices.Count; ++i)
            {
                float value = FVector2.Dot(Vertices[i], direction);
                if (value > bestValue)
                {
                    bestIndex = i;
                    bestValue = value;
                }
            }

            return(Vertices[bestIndex]);
        }
示例#42
0
        //=========================================================================================================================

        public void Init(int side, FVector2 pos, float rotation)
        {
            this.Side     = side;
            this.Position = pos;

            IntObj3D = new InteractiveObject3D(this);
            ForegroundGame.IntObjs3D.Add(IntObj3D);
            IntObj2D = ForegroundGame.IntObjsManager.Add(this);

            proximityToken = BackgroundGame.IntObjsLQDB.AllocateToken(this);
            proximityToken.UpdateForNewPosition(new Point3D(pos.X.ToInt(), 0, pos.Y.ToInt()));


            onInitialize();

            Initialized = true;
        }
示例#43
0
        /// <summary>
        /// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
        /// then the proxy is removed from the tree and re-inserted. Otherwise
        /// the function returns immediately.
        /// </summary>
        /// <param name="proxyId">The proxy id.</param>
        /// <param name="aabb">The aabb.</param>
        /// <param name="displacement">The displacement.</param>
        /// <returns>true if the proxy was re-inserted.</returns>
        public bool MoveProxy(int proxyId, ref AABB aabb, FVector2 displacement)
        {
            Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity);

            Debug.Assert(_nodes[proxyId].IsLeaf());

            if (_nodes[proxyId].AABB.Contains(ref aabb))
            {
                return(false);
            }

            RemoveLeaf(proxyId);

            // Extend AABB.
            AABB     b = aabb;
            FVector2 r = new FVector2(Settings.AABBExtension, Settings.AABBExtension);

            b.LowerBound = b.LowerBound - r;
            b.UpperBound = b.UpperBound + r;

            // Predict AABB displacement.
            FVector2 d = Settings.AABBMultiplier * displacement;

            if (d.X < 0.0f)
            {
                b.LowerBound.X += d.X;
            }
            else
            {
                b.UpperBound.X += d.X;
            }

            if (d.Y < 0.0f)
            {
                b.LowerBound.Y += d.Y;
            }
            else
            {
                b.UpperBound.Y += d.Y;
            }

            _nodes[proxyId].AABB = b;

            InsertLeaf(proxyId);
            return(true);
        }
示例#44
0
        public bool Test(FVector2 p)
        {
            int x = (p.X / (fint)2).ToInt();
            int y = (p.Y / (fint)2).ToInt();

            int id = x + y * Width;

            if (x < 0 || y < 0 || x >= Width || y >= Height)
            {
                return(false);
            }
            if (staticSpace[id] == 0)
            {
                return(false);
            }
            return(true);
        }
示例#45
0
        public float GetLength()
        {
            List <FVector2> verts  = GetVertices(ControlPoints.Count * 25);
            float           length = 0;

            for (int i = 1; i < verts.Count; i++)
            {
                length += FVector2.Distance(verts[i - 1], verts[i]);
            }

            if (Closed)
            {
                length += FVector2.Distance(verts[ControlPoints.Count - 1], verts[0]);
            }

            return(length);
        }
示例#46
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)
        {
            FVector2 lower = MathUtils.Mul(ref transform, Vertices[0]);
            FVector2 upper = lower;

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

            FVector2 r = new FVector2(Radius, Radius);

            aabb.LowerBound = lower - r;
            aabb.UpperBound = upper + r;
        }
示例#47
0
        /// <summary>
        /// Return the angle between two vectors on a plane
        /// The angle is from vector 1 to vector 2, positive anticlockwise
        /// The result is between -pi -> pi
        /// </summary>
        public static double VectorAngle(ref FVector2 p1, ref FVector2 p2)
        {
            double theta1 = Math.Atan2(p1.Y, p1.X);
            double theta2 = Math.Atan2(p2.Y, p2.X);
            double dtheta = theta2 - theta1;

            while (dtheta > Math.PI)
            {
                dtheta -= (2 * Math.PI);
            }
            while (dtheta < -Math.PI)
            {
                dtheta += (2 * Math.PI);
            }

            return(dtheta);
        }
示例#48
0
        /// <summary>
        /// Build vertices to represent an oriented box.
        /// </summary>
        /// <param name="hx">the half-width.</param>
        /// <param name="hy">the half-height.</param>
        /// <param name="center">the center of the box in local coordinates.</param>
        /// <param name="angle">the rotation of the box in local coordinates.</param>
        public static Vertices CreateRectangle(float hx, float hy, FVector2 center, float angle)
        {
            Vertices vertices = CreateRectangle(hx, hy);

            Transform xf = new Transform();

            xf.p = center;
            xf.q.Set(angle);

            // Transform vertices
            for (int i = 0; i < 4; ++i)
            {
                vertices[i] = MathUtils.Mul(ref xf, vertices[i]);
            }

            return(vertices);
        }
示例#49
0
        public override void ApplyForce(float dt, float strength)
        {
            foreach (FSBody body in World.BodyList)
            {
                //TODO: Consider Force Type
                float decayMultiplier = GetDecayMultiplier(body);

                if (decayMultiplier != 0)
                {
                    FVector2 forceVector;

                    if (ForceType == ForceTypes.Point)
                    {
                        forceVector = body.Position - Position;
                    }
                    else
                    {
                        Direction.Normalize();

                        forceVector = Direction;

                        if (forceVector.Length() == 0)
                        {
                            forceVector = new FVector2(0, 1);
                        }
                    }

                    //TODO: Consider Divergence:
                    //forceVector = Vector2.Transform(forceVector, Matrix.CreateRotationZ((MathHelper.Pi - MathHelper.Pi/2) * (float)Randomize.NextDouble()));

                    // Calculate random Variation
                    if (Variation != 0)
                    {
                        float strengthVariation = (float)Randomize.NextDouble() * MathHelper.Clamp(Variation, 0, 1);
                        forceVector.Normalize();
                        body.ApplyForce(forceVector * strength * decayMultiplier * strengthVariation);
                    }
                    else
                    {
                        forceVector.Normalize();
                        body.ApplyForce(forceVector * strength * decayMultiplier);
                    }
                }
            }
        }
示例#50
0
        /// <summary>
        /// Searches for the next shape.
        /// </summary>
        /// <param name="detectedPolygons">Already detected polygons.</param>
        /// <param name="start">Search start coordinate.</param>
        /// <param name="entrance">Returns the found entrance coordinate. Null if no other shapes found.</param>
        /// <returns>True if a new shape was found.</returns>
        private bool SearchNextHullEntrance(List <DetectedVertices> detectedPolygons, FVector2 start, out FVector2?entrance)
        {
            int x;

            bool foundTransparent = false;
            bool inPolygon        = false;

            for (int i = (int)start.X + (int)start.Y * _width; i <= _dataLength; i++)
            {
                if (IsSolid(ref i))
                {
                    if (foundTransparent)
                    {
                        x        = i % _width;
                        entrance = new FVector2(x, (i - x) / (float)_width);

                        inPolygon = false;
                        for (int polygonIdx = 0; polygonIdx < detectedPolygons.Count; polygonIdx++)
                        {
                            if (InPolygon(detectedPolygons[polygonIdx], entrance.Value))
                            {
                                inPolygon = true;
                                break;
                            }
                        }

                        if (inPolygon)
                        {
                            foundTransparent = false;
                        }
                        else
                        {
                            return(true);
                        }
                    }
                }
                else
                {
                    foundTransparent = true;
                }
            }

            entrance = null;
            return(false);
        }
示例#51
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;
            }

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

            aabb.LowerBound = FVector2.Min(v1, v2);
            aabb.UpperBound = FVector2.Max(v1, v2);
        }
示例#52
0
        /// <summary>
        /// For teleporting a body without considering new contacts immediately.
        /// </summary>
        /// <param name="position">The position.</param>
        /// <param name="angle">The angle.</param>
        public void SetTransformIgnoreContacts(ref FVector2 position, float angle)
        {
            Xf.q.Set(angle);
            Xf.p = position;

            Sweep.C = MathUtils.Mul(ref Xf, Sweep.LocalCenter);
            Sweep.A = angle;

            Sweep.C0 = Sweep.C;
            Sweep.A0 = angle;

            IBroadPhase broadPhase = World.ContactManager.BroadPhase;

            for (int i = 0; i < FixtureList.Count; i++)
            {
                FixtureList[i].Synchronize(broadPhase, ref Xf, ref Xf);
            }
        }
示例#53
0
        /// <summary>
        /// Creates a rounded rectangle.
        /// Note: Automatically decomposes the capsule if it contains too many vertices (controlled by Settings.MaxPolygonVertices)
        /// </summary>
        /// <param name="world">The world.</param>
        /// <param name="width">The width.</param>
        /// <param name="height">The height.</param>
        /// <param name="xRadius">The x radius.</param>
        /// <param name="yRadius">The y radius.</param>
        /// <param name="segments">The segments.</param>
        /// <param name="density">The density.</param>
        /// <param name="position">The position.</param>
        /// <returns></returns>
        public static Body CreateRoundedRectangle(World world, float width, float height, float xRadius,
                                                  float yRadius,
                                                  int segments, float density, FVector2 position,
                                                  object userData)
        {
            Vertices verts = PolygonTools.CreateRoundedRectangle(width, height, xRadius, yRadius, segments);

            //There are too many vertices in the capsule. We decompose it.
            if (verts.Count >= Settings.MaxPolygonVertices)
            {
                List <Vertices> vertList = EarclipDecomposer.ConvexPartition(verts);
                Body            body     = CreateCompoundPolygon(world, vertList, density, userData);
                body.Position = position;
                return(body);
            }

            return(CreatePolygon(world, verts, density));
        }
示例#54
0
        /// <summary>
        /// Apply a force at a world point. If the force is not
        /// applied at the center of mass, it will generate a torque and
        /// affect the angular velocity. This wakes up the body.
        /// </summary>
        /// <param name="force">The world force vector, usually in Newtons (N).</param>
        /// <param name="point">The world position of the point of application.</param>
        public void ApplyForce(ref FVector2 force, ref FVector2 point)
        {
            Debug.Assert(!float.IsNaN(force.X));
            Debug.Assert(!float.IsNaN(force.Y));
            Debug.Assert(!float.IsNaN(point.X));
            Debug.Assert(!float.IsNaN(point.Y));

            if (_bodyType == BodyType.Dynamic)
            {
                if (Awake == false)
                {
                    Awake = true;
                }

                Force  += force;
                Torque += (point.X - Sweep.C.X) * force.Y - (point.Y - Sweep.C.Y) * force.X;
            }
        }
示例#55
0
        private bool SearchNearPixels(bool searchingForSolidPixel, ref FVector2 current, out FVector2 foundPixel)
        {
            for (int i = 0; i < _CLOSEPIXELS_LENGTH; i++)
            {
                int x = (int)current.X + ClosePixels[i, 0];
                int y = (int)current.Y + ClosePixels[i, 1];

                if (!searchingForSolidPixel ^ IsSolid(ref x, ref y))
                {
                    foundPixel = new FVector2(x, y);
                    return(true);
                }
            }

            // Nothing found.
            foundPixel = FVector2.Zero;
            return(false);
        }
示例#56
0
        private bool IsNearPixel(ref FVector2 current, ref FVector2 near)
        {
            for (int i = 0; i < _CLOSEPIXELS_LENGTH; i++)
            {
                int x = (int)current.X + ClosePixels[i, 0];
                int y = (int)current.Y + ClosePixels[i, 1];

                if (x >= 0 && x <= _width && y >= 0 && y <= _height)
                {
                    if (x == (int)near.X && y == (int)near.Y)
                    {
                        return(true);
                    }
                }
            }

            return(false);
        }
示例#57
0
        // Solve a line segment using barycentric coordinates.
        //
        // p = a1 * w1 + a2 * w2
        // a1 + a2 = 1
        //
        // The vector from the origin to the closest point on the line is
        // perpendicular to the line.
        // e12 = w2 - w1
        // dot(p, e) = 0
        // a1 * dot(w1, e) + a2 * dot(w2, e) = 0
        //
        // 2-by-2 linear system
        // [1      1     ][a1] = [1]
        // [w1.e12 w2.e12][a2] = [0]
        //
        // Define
        // d12_1 =  dot(w2, e12)
        // d12_2 = -dot(w1, e12)
        // d12 = d12_1 + d12_2
        //
        // Solution
        // a1 = d12_1 / d12
        // a2 = d12_2 / d12

        internal void Solve2()
        {
            FVector2 w1  = V[0].W;
            FVector2 w2  = V[1].W;
            FVector2 e12 = w2 - w1;

            // w1 region
            float d12_2 = -FVector2.Dot(w1, e12);

            if (d12_2 <= 0.0f)
            {
                // a2 <= 0, so we clamp it to 0
                SimplexVertex v0 = V[0];
                v0.A  = 1.0f;
                V[0]  = v0;
                Count = 1;
                return;
            }

            // w2 region
            float d12_1 = FVector2.Dot(w2, e12);

            if (d12_1 <= 0.0f)
            {
                // a1 <= 0, so we clamp it to 0
                SimplexVertex v1 = V[1];
                v1.A  = 1.0f;
                V[1]  = v1;
                Count = 1;
                V[0]  = V[1];
                return;
            }

            // Must be in e12 region.
            float         inv_d12 = 1.0f / (d12_1 + d12_2);
            SimplexVertex v0_2    = V[0];
            SimplexVertex v1_2    = V[1];

            v0_2.A = d12_1 * inv_d12;
            v1_2.A = d12_2 * inv_d12;
            V[0]   = v0_2;
            V[1]   = v1_2;
            Count  = 2;
        }
示例#58
0
        public override void ApplyForce(float dt, float strength)
        {
            foreach (Body body in World.BodyList)
            {
                //TODO: Consider Force Type
                float decayMultiplier = GetDecayMultiplier(body);

                if (decayMultiplier != 0)
                {
                    FVector2 forceVector;

                    if (ForceType == ForceTypes.Point)
                    {
                        forceVector = body.Position - Position;
                    }
                    else
                    {
                        Direction.Normalize();

                        forceVector = Direction;

                        if (forceVector.Length() == 0)
                            forceVector = new FVector2(0, 1);
                    }

                    //TODO: Consider Divergence:
                    //forceVector = Vector2.Transform(forceVector, Matrix.CreateRotationZ((MathHelper.Pi - MathHelper.Pi/2) * (float)Randomize.NextDouble()));

                    // Calculate random Variation
                    if (Variation != 0)
                    {
                        float strengthVariation = (float)Randomize.NextDouble() * MathHelper.Clamp(Variation, 0, 1);
                        forceVector.Normalize();
                        body.ApplyForce(forceVector * strength * decayMultiplier * strengthVariation);
                    }
                    else
                    {
                        forceVector.Normalize();
                        body.ApplyForce(forceVector * strength * decayMultiplier);
                    }
                }
            }
        }
示例#59
0
        // From Eric Jordan's convex decomposition library
        /// <summary>
        ///Check if the lines a0->a1 and b0->b1 cross.
        ///If they do, intersectionPoint will be filled
        ///with the point of crossing.
        ///
        ///Grazing lines should not return true.
        /// 
        /// </summary>
        /// <param name="a0"></param>
        /// <param name="a1"></param>
        /// <param name="b0"></param>
        /// <param name="b1"></param>
        /// <param name="intersectionPoint"></param>
        /// <returns></returns>
        public static bool LineIntersect2(FVector2 a0, FVector2 a1, FVector2 b0, FVector2 b1, out FVector2 intersectionPoint)
        {
            intersectionPoint = FVector2.Zero;

            if (a0 == b0 || a0 == b1 || a1 == b0 || a1 == b1)
                return false;

            float x1 = a0.X;
            float y1 = a0.Y;
            float x2 = a1.X;
            float y2 = a1.Y;
            float x3 = b0.X;
            float y3 = b0.Y;
            float x4 = b1.X;
            float y4 = b1.Y;

            //AABB early exit
            if (Math.Max(x1, x2) < Math.Min(x3, x4) || Math.Max(x3, x4) < Math.Min(x1, x2))
                return false;

            if (Math.Max(y1, y2) < Math.Min(y3, y4) || Math.Max(y3, y4) < Math.Min(y1, y2))
                return false;

            float ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3));
            float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3));
            float denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
            if (Math.Abs(denom) < Settings.Epsilon)
            {
                //Lines are too close to parallel to call
                return false;
            }
            ua /= denom;
            ub /= denom;

            if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1))
            {
                intersectionPoint.X = (x1 + ua * (x2 - x1));
                intersectionPoint.Y = (y1 + ua * (y2 - y1));
                return true;
            }

            return false;
        }
示例#60
0
        //From Mark Bayazit's convex decomposition algorithm
        public static FVector2 LineIntersect(FVector2 p1, FVector2 p2, FVector2 q1, FVector2 q2)
        {
            FVector2 i = FVector2.Zero;
            float a1 = p2.Y - p1.Y;
            float b1 = p1.X - p2.X;
            float c1 = a1 * p1.X + b1 * p1.Y;
            float a2 = q2.Y - q1.Y;
            float b2 = q1.X - q2.X;
            float c2 = a2 * q1.X + b2 * q1.Y;
            float det = a1 * b2 - a2 * b1;

            if (!MathUtils.FloatEquals(det, 0))
            {
                // lines are not parallel
                i.X = (b2 * c1 - b1 * c2) / det;
                i.Y = (a1 * c2 - a2 * c1) / det;
            }
            return i;
        }