public EdgeShape(FVector2 start, FVector2 end)
: base(0)
{
ShapeType = ShapeType.Edge;
_radius = Settings.PolygonRadius;
Set(start, end);
}
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;
}
/// <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;
}
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));
}
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;
}
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;
}
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;
}
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;
}
}
public FrictionJoint(Body bodyA, Body bodyB, FVector2 localAnchorA, FVector2 localAnchorB)
: base(bodyA, bodyB)
{
JointType = JointType.Friction;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
}
internal CircleShape()
: base(0)
{
ShapeType = ShapeType.Circle;
_radius = 0.0f;
_position = FVector2.Zero;
}
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);
}
public CircleShape(float radius, float density)
: base(density)
{
ShapeType = ShapeType.Circle;
_radius = radius;
_position = FVector2.Zero;
ComputeProperties();
}
/// <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]);
}
}
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);
}
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);
}
// 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);
}
/// 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;
}
/// <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;
}
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();
}
/// <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;
}
/// <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);
}
/// <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));
}
// 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);
}
/// <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;
}
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;
}
}
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);
}
/// <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);
}
/// <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;
}
/// <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);
}
/// <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;
}
/// <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;
}
/// <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);
}
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);
}
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;
}
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));
}
/// <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;
}
/// <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;
}
}
/// <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]);
}
//=========================================================================================================================
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;
}
/// <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);
}
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);
}
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);
}
/// <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;
}
/// <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);
}
/// <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);
}
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);
}
}
}
}
/// <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);
}
/// <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);
}
/// <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);
}
}
/// <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));
}
/// <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;
}
}
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);
}
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);
}
// 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;
}
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);
}
}
}
}
// 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;
}
//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;
}