///// <summary>
///// Creates the fixed revolute joint.
///// </summary>
///// <param name="world">The world.</param>
///// <param name="body">The body.</param>
///// <param name="bodyAnchor">The body anchor.</param>
///// <param name="worldAnchor">The world anchor.</param>
///// <returns></returns>
//public static FixedRevoluteJoint CreateFixedRevoluteJoint(World world, Body body, Vector2 bodyAnchor,
// Vector2 worldAnchor)
//{
// FixedRevoluteJoint fixedRevoluteJoint = new FixedRevoluteJoint(body, bodyAnchor, worldAnchor);
// world.AddJoint(fixedRevoluteJoint);
// return fixedRevoluteJoint;
//}
#endregion
#region Weld Joint
/// <summary>
/// Creates a weld joint
/// </summary>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="worldAnchor">World space coordinates of weld joint</param>
/// <returns></returns>
public static FSWeldJoint CreateWeldJoint(FSBody bodyA, FSBody bodyB, FVector2 worldAnchor)
{
FSWeldJoint joint = new FSWeldJoint(bodyA, bodyB, bodyA.GetLocalPoint(worldAnchor),
bodyB.GetLocalPoint(worldAnchor));
return(joint);
}
/// <summary>
/// Creates a revolute joint.
/// </summary>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="localAnchorB">The anchor of bodyB in local coordinates</param>
/// <returns></returns>
public static FSRevoluteJoint CreateRevoluteJoint(FSBody bodyA, FSBody bodyB, FVector2 localAnchorB)
{
FVector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(localAnchorB));
FSRevoluteJoint joint = new FSRevoluteJoint(bodyA, bodyB, localanchorA, localAnchorB);
return(joint);
}
/// <summary>
/// Creates a prsimatic joint
/// </summary>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="localanchorB"></param>
/// <param name="axis"></param>
/// <returns></returns>
public static FSPrismaticJoint CreatePrismaticJoint(FSBody bodyA, FSBody bodyB, FVector2 localanchorB, FVector2 axis)
{
FVector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(localanchorB));
FSPrismaticJoint joint = new FSPrismaticJoint(bodyA, bodyB, localanchorA, localanchorB, axis);
return(joint);
}
public FSWheelJoint(FSBody bA, FSBody 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 FSWheelJoint(FSBody bA, FSBody 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);
}
/// <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>
/// 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;
}
public override void Start()
{
base.Start();
CircleShape circ;
PolygonShape box;
FSFixture fixture;
FSRevoluteJoint joint;
// Add 2 ragdolls along the top
for (int i = 0; i < 2; i++)
{
float startX = 70f + Random.value * 20f + 480f * (float)i;
float startY = (20f + Random.value * 50f) * -1f;
// Head
FSBody head = new FSBody(FSWorldComponent.PhysicsWorld);
circ = new CircleShape(12.5f / physScale, 1f);
fixture = new FSFixture(head, circ);
fixture.Friction = 0.4f;
fixture.Restitution = 0.3f;
head.Position = new FVector2(startX / physScale, startY / physScale);
head.ApplyLinearImpulse(new FVector2(Random.value * 100f - 50f, Random.value * 100f - 50f), head.Position);
head.BodyType = BodyType.Dynamic;
// Torso 1
FSBody torso1 = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox(15f / physScale, 10f / physScale);
fixture = new FSFixture(torso1, box);
fixture.Friction = 0.4f;
fixture.Restitution = 0.1f;
torso1.Position = new FVector2(startX / physScale, (startY - 28f) / physScale);
torso1.BodyType = BodyType.Dynamic;
// Torso 2
FSBody torso2 = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox(15f / physScale, 10f / physScale);
fixture = new FSFixture(torso2, box);
fixture.Friction = 0.4f;
fixture.Restitution = 0.1f;
torso2.Position = new FVector2(startX / physScale, (startY - 43f) / physScale);
torso2.BodyType = BodyType.Dynamic;
// Torso 3
FSBody torso3 = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox(15f / physScale, 10f / physScale);
fixture = new FSFixture(torso3, box);
fixture.Friction = 0.4f;
fixture.Restitution = 0.1f;
torso3.Position = new FVector2(startX / physScale, (startY - 58f) / physScale);
torso3.BodyType = BodyType.Dynamic;
// UpperArm
// L
FSBody upperArmL = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox(18f / physScale, 6.5f / physScale);
fixture = new FSFixture(upperArmL, box);
fixture.Friction = 0.4f;
fixture.Restitution = 0.1f;
upperArmL.Position = new FVector2((startX - 30f) / physScale, (startY - 20f) / physScale);
upperArmL.BodyType = BodyType.Dynamic;
// R
FSBody upperArmR = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox(18f / physScale, 6.5f / physScale);
fixture = new FSFixture(upperArmR, box);
fixture.Friction = 0.4f;
fixture.Restitution = 0.1f;
upperArmR.Position = new FVector2((startX + 30f) / physScale, (startY - 20f) / physScale);
upperArmR.BodyType = BodyType.Dynamic;
// LowerArm
// L
FSBody lowerArmL = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox(17f / physScale, 6f / physScale);
fixture = new FSFixture(lowerArmL, box);
fixture.Friction = 0.4f;
fixture.Restitution = 0.1f;
lowerArmL.Position = new FVector2((startX - 57f) / physScale, (startY - 20f) / physScale);
lowerArmL.BodyType = BodyType.Dynamic;
// R
FSBody lowerArmR = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox(17f / physScale, 6f / physScale);
fixture = new FSFixture(lowerArmR, box);
fixture.Friction = 0.4f;
fixture.Restitution = 0.1f;
lowerArmR.Position = new FVector2((startX + 57f) / physScale, (startY - 20f) / physScale);
lowerArmR.BodyType = BodyType.Dynamic;
// UpperLeg
// L
FSBody upperLegL = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox(7.5f / physScale, 22f / physScale);
fixture = new FSFixture(upperLegL, box);
fixture.Friction = 0.4f;
fixture.Restitution = 0.1f;
upperLegL.Position = new FVector2((startX - 8f) / physScale, (startY - 85f) / physScale);
upperLegL.BodyType = BodyType.Dynamic;
// R
FSBody upperLegR = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox(7.5f / physScale, 22f / physScale);
fixture = new FSFixture(upperLegR, box);
fixture.Friction = 0.4f;
fixture.Restitution = 0.1f;
upperLegR.Position = new FVector2((startX + 8f) / physScale, (startY - 85f) / physScale);
upperLegR.BodyType = BodyType.Dynamic;
// LowerLeg
// L
FSBody lowerLegL = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox(7.5f / physScale, 22f / physScale);
fixture = new FSFixture(lowerLegL, box);
fixture.Friction = 0.4f;
fixture.Restitution = 0.1f;
lowerLegL.Position = new FVector2((startX - 8f) / physScale, (startY - 120f) / physScale);
lowerLegL.BodyType = BodyType.Dynamic;
// R
FSBody lowerLegR = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox(7.5f / physScale, 22f / physScale);
fixture = new FSFixture(lowerLegR, box);
fixture.Friction = 0.4f;
fixture.Restitution = 0.1f;
lowerLegR.Position = new FVector2((startX + 8f) / physScale, (startY - 120f) / physScale);
lowerLegR.BodyType = BodyType.Dynamic;
// JOINTS
// Head to shoulders
joint = JointFactory.CreateRevoluteJoint(FSWorldComponent.PhysicsWorld, torso1, head, head.GetLocalPoint(new FVector2(startX / physScale, (startY - 15f) / physScale)));
joint.LowerLimit = -40f * Mathf.Deg2Rad;
joint.UpperLimit = 40f * Mathf.Deg2Rad;
joint.LimitEnabled = true;
joint.CollideConnected = false;
// Upper arm to shoulders
// L
joint = JointFactory.CreateRevoluteJoint(FSWorldComponent.PhysicsWorld, torso1, upperArmL, upperArmL.GetLocalPoint(new FVector2((startX - 18f) / physScale, (startY - 20f) / physScale)));
joint.LowerLimit = -85f * Mathf.Deg2Rad;
joint.UpperLimit = 130f * Mathf.Deg2Rad;
joint.LimitEnabled = true;
joint.CollideConnected = false;
// R
joint = JointFactory.CreateRevoluteJoint(FSWorldComponent.PhysicsWorld, torso1, upperArmR, upperArmR.GetLocalPoint(new FVector2((startX + 18f) / physScale, (startY - 20f) / physScale)));
joint.LowerLimit = -130f * Mathf.Deg2Rad;
joint.UpperLimit = 85f * Mathf.Deg2Rad;
joint.LimitEnabled = true;
joint.CollideConnected = false;
// Lower arm to upper arm
// L
joint = JointFactory.CreateRevoluteJoint(FSWorldComponent.PhysicsWorld, upperArmL, lowerArmL, lowerArmL.GetLocalPoint(new FVector2((startX - 45f) / physScale, (startY - 20f) / physScale)));
joint.LowerLimit = -130f * Mathf.Deg2Rad;
joint.UpperLimit = 10f * Mathf.Deg2Rad;
joint.LimitEnabled = true;
joint.CollideConnected = false;
// R
joint = JointFactory.CreateRevoluteJoint(FSWorldComponent.PhysicsWorld, upperArmR, lowerArmR, lowerArmR.GetLocalPoint(new FVector2((startX + 45f) / physScale, (startY - 20f) / physScale)));
joint.LowerLimit = -10f * Mathf.Deg2Rad;
joint.UpperLimit = 130f * Mathf.Deg2Rad;
joint.LimitEnabled = true;
joint.CollideConnected = false;
// Shoulders/stomach
joint = JointFactory.CreateRevoluteJoint(FSWorldComponent.PhysicsWorld, torso1, torso2, torso2.GetLocalPoint(new FVector2((startX + 0f) / physScale, (startY - 35f) / physScale)));
joint.LowerLimit = -15f * Mathf.Deg2Rad;
joint.UpperLimit = 15f * Mathf.Deg2Rad;
joint.LimitEnabled = true;
joint.CollideConnected = false;
// Stomach/hips
joint = JointFactory.CreateRevoluteJoint(FSWorldComponent.PhysicsWorld, torso2, torso3, torso3.GetLocalPoint(new FVector2((startX + 0f) / physScale, (startY - 50f) / physScale)));
joint.LowerLimit = -15f * Mathf.Deg2Rad;
joint.UpperLimit = 15f * Mathf.Deg2Rad;
joint.LimitEnabled = true;
joint.CollideConnected = false;
// Torso to upper leg
// L
joint = JointFactory.CreateRevoluteJoint(FSWorldComponent.PhysicsWorld, torso3, upperLegL, upperLegL.GetLocalPoint(new FVector2((startX - 8f) / physScale, (startY - 72f) / physScale)));
joint.LowerLimit = -25f * Mathf.Deg2Rad;
joint.UpperLimit = 45f * Mathf.Deg2Rad;
joint.LimitEnabled = true;
joint.CollideConnected = false;
// R
joint = JointFactory.CreateRevoluteJoint(FSWorldComponent.PhysicsWorld, torso3, upperLegR, upperLegR.GetLocalPoint(new FVector2((startX + 8f) / physScale, (startY - 72f) / physScale)));
joint.LowerLimit = -45f * Mathf.Deg2Rad;
joint.UpperLimit = 25f * Mathf.Deg2Rad;
joint.LimitEnabled = true;
joint.CollideConnected = false;
// Upper leg to lower leg
// L
joint = JointFactory.CreateRevoluteJoint(FSWorldComponent.PhysicsWorld, upperLegL, lowerLegL, lowerLegL.GetLocalPoint(new FVector2((startX - 8f) / physScale, (startY - 105f) / physScale)));
joint.LowerLimit = -25f * Mathf.Deg2Rad;
joint.UpperLimit = 115f * Mathf.Deg2Rad;
joint.LimitEnabled = true;
joint.CollideConnected = false;
// R
joint = JointFactory.CreateRevoluteJoint(FSWorldComponent.PhysicsWorld, upperLegR, lowerLegR, lowerLegR.GetLocalPoint(new FVector2((startX + 8f) / physScale, (startY - 105f) / physScale)));
joint.LowerLimit = -115f * Mathf.Deg2Rad;
joint.UpperLimit = 25f * Mathf.Deg2Rad;
joint.LimitEnabled = true;
joint.CollideConnected = false;
}
// Add stairs on the left, these are static bodies so set the type accordingly
for (int j = 1; j <= 10; j++)
{
FSBody body = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox((10f * (float)j) / physScale, 10f / physScale);
fixture = new FSFixture(body, box);
body.Position = new FVector2((10f * (float)j) / physScale, ((150f + 20f * (float)j) / physScale) * -1f);
}
// Add stairs on the right
for (int k = 1; k <= 10; k++)
{
FSBody body = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox((10f * (float)k) / physScale, 10f / physScale);
fixture = new FSFixture(body, box);
body.Position = new FVector2((640f - 10f * (float)k) / physScale, ((150f + 20f * (float)k) / physScale) * -1f);
}
FSBody ground = new FSBody(FSWorldComponent.PhysicsWorld);
box = new PolygonShape(1f);
box.SetAsBox(30f / physScale, 40f / physScale);
fixture = new FSFixture(ground, box);
ground.Position = new FVector2(320f / physScale, (320f / physScale) * -1f);
}
private void CreateLeg(float s, FVector2 wheelAnchor)
{
FVector2 p1, p2, p3, p4, p5, p6;
p1 = new FVector2((162f * s) / tScale, -183f / tScale);
p2 = new FVector2((216f * s) / tScale, -36f / tScale);
p3 = new FVector2((129f * s) / tScale, -57f / tScale);
p4 = new FVector2((093f * s) / tScale, 24f / tScale);
p5 = new FVector2((180f * s) / tScale, 45f / tScale);
p6 = new FVector2((075f * s) / tScale, 111f / tScale);
FSBody body1;
FSBody body2;
body1 = BodyFactory.CreateBody(FSWorldComponent.PhysicsWorld, m_offset);
body1.BodyType = BodyType.Dynamic;
body2 = BodyFactory.CreateBody(FSWorldComponent.PhysicsWorld, FVector2.Add(p4, m_offset));
body2.BodyType = BodyType.Dynamic;
FSFixture b1fix;
FSFixture b2fix;
List <Vertices> vl1 = new List <Vertices>();
List <Vertices> vl2 = new List <Vertices>();
vl1.Add(new Vertices());
vl2.Add(new Vertices());
if (s > 0f)
{
vl1[0].Add(p1);
vl1[0].Add(p2);
vl1[0].Add(p3);
vl2[0].Add(FVector2.Zero);
vl2[0].Add(FVector2.Subtract(p5, p4));
vl2[0].Add(FVector2.Subtract(p6, p4));
}
else
{
vl1[0].Add(p1);
vl1[0].Add(p3);
vl1[0].Add(p2);
vl2[0].Add(FVector2.Zero);
vl2[0].Add(FVector2.Subtract(p6, p4));
vl2[0].Add(FVector2.Subtract(p5, p4));
}
b1fix = FixtureFactory.AttachCompoundPolygon(vl1, 1f, body1)[0];
b2fix = FixtureFactory.AttachCompoundPolygon(vl2, 1f, body2)[0];
b1fix.CollisionCategories = Category.Cat10;
b1fix.CollidesWith = Category.Cat1;
b2fix.CollisionCategories = Category.Cat10;
b2fix.CollidesWith = Category.Cat1;
body1.AngularDamping = 10f;
body2.AngularDamping = 10f;
FSDistanceJoint dj;
float dampingRatio = 0.5f;
float freqHz = 10f;
// Using a soft distance constraint can reduce some jitter.
// It also makes the structure seem a bit more fluid by
// acting like a suspension system.
dj = JointFactory.CreateDistanceJoint(FSWorldComponent.PhysicsWorld, body1, body2, body1.GetLocalPoint(FVector2.Add(p2, m_offset)), body2.GetLocalPoint(FVector2.Add(p5, m_offset)));
dj.DampingRatio = dampingRatio;
dj.Frequency = freqHz;
dj = JointFactory.CreateDistanceJoint(FSWorldComponent.PhysicsWorld, body1, body2, body1.GetLocalPoint(FVector2.Add(p3, m_offset)), body2.GetLocalPoint(FVector2.Add(p4, m_offset)));
dj.DampingRatio = dampingRatio;
dj.Frequency = freqHz;
dj = JointFactory.CreateDistanceJoint(FSWorldComponent.PhysicsWorld, body1, m_wheel, body1.GetLocalPoint(FVector2.Add(p3, m_offset)), m_wheel.GetLocalPoint(FVector2.Add(wheelAnchor, m_offset)));
dj.DampingRatio = dampingRatio;
dj.Frequency = freqHz;
dj = JointFactory.CreateDistanceJoint(FSWorldComponent.PhysicsWorld, body2, m_wheel, body2.GetLocalPoint(FVector2.Add(p6, m_offset)), m_wheel.GetLocalPoint(FVector2.Add(wheelAnchor, m_offset)));
dj.DampingRatio = dampingRatio;
dj.Frequency = freqHz;
JointFactory.CreateRevoluteJoint(FSWorldComponent.PhysicsWorld, body2, m_chassis, m_chassis.GetLocalPoint(FVector2.Add(p4, m_offset)));
}
