/// <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>
public static List<FSRevoluteJoint> AttachBodiesWithRevoluteJoint(FSWorld world, List<FSBody> bodies,
FVector2 localAnchorA,
FVector2 localAnchorB, bool connectFirstAndLast,
bool collideConnected)
{
List<FSRevoluteJoint> joints = new List<FSRevoluteJoint>(bodies.Count + 1);
for (int i = 1; i < bodies.Count; i++)
{
FSRevoluteJoint joint = new FSRevoluteJoint(bodies[i], bodies[i - 1], localAnchorA, localAnchorB);
joint.CollideConnected = collideConnected;
world.AddJoint(joint);
joints.Add(joint);
}
if (connectFirstAndLast)
{
FSRevoluteJoint lastjoint = new FSRevoluteJoint(bodies[0], bodies[bodies.Count - 1], localAnchorA,
localAnchorB);
lastjoint.CollideConnected = collideConnected;
world.AddJoint(lastjoint);
joints.Add(lastjoint);
}
return joints;
}
public override void Start()
{
base.Start();
FSBody body;
tScale = physScale * 2f;
// St position in world space
m_offset = new FVector2(180f/physScale, -200f/physScale);
m_motorSpeed = 2f;
m_motorOn = true;
FVector2 pivot = new FVector2(0f, 24f/tScale);
for(int i = 0; i < 50; i++)
{
body = BodyFactory.CreateCircle(FSWorldComponent.PhysicsWorld, 3.75f / physScale, 1f, new FVector2((Random.value * 620f + 10f)/physScale, -340f/physScale));
body.BodyType = BodyType.Dynamic;
}
// chassis
{
m_chassis = BodyFactory.CreateBody(FSWorldComponent.PhysicsWorld, FVector2.Add(pivot, m_offset));
m_chassis.BodyType = BodyType.Dynamic;
FSFixture m_chassis_f = FixtureFactory.AttachRectangle(150f / tScale, 60f / tScale, 1f, FVector2.Zero, m_chassis);
//m_chassis_f.CollisionGroup = -1;
m_chassis_f.CollisionCategories = Category.Cat10;
m_chassis_f.CollidesWith = Category.Cat1;
}
// wheel
{
m_wheel = BodyFactory.CreateBody(FSWorldComponent.PhysicsWorld, FVector2.Add(pivot, m_offset));
m_wheel.BodyType = BodyType.Dynamic;
FSFixture m_wheel_f = FixtureFactory.AttachCircle(48f / tScale, 1f, m_wheel);
//m_wheel_f.CollisionGroup = -1;
m_wheel_f.CollisionCategories = Category.Cat10;
m_wheel_f.CollidesWith = Category.Cat1;
}
// glue chassis & wheel
{
m_motorJoint = JointFactory.CreateRevoluteJoint(FSWorldComponent.PhysicsWorld, m_wheel, m_chassis, FVector2.Zero);
m_motorJoint.MotorSpeed = m_motorSpeed;
m_motorJoint.MaxMotorTorque = 400f;
m_motorJoint.CollideConnected = false;
m_motorJoint.MotorEnabled = m_motorOn;
}
FVector2 wheelAnchor;
wheelAnchor = new FVector2(0f, -24f/tScale) + pivot;
CreateLeg(-1f, wheelAnchor);
CreateLeg(1f, wheelAnchor);
m_wheel.Rotation = 120f * Mathf.Deg2Rad;
CreateLeg(-1f, wheelAnchor);
CreateLeg(1f, wheelAnchor);
m_wheel.Rotation = -120f * Mathf.Deg2Rad;
CreateLeg(-1f, wheelAnchor);
CreateLeg(1f, wheelAnchor);
}
/// <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>
/// Requires two existing revolute or prismatic joints (any combination will work).
/// The provided joints must attach a dynamic body to a static body.
/// </summary>
/// <param name="jointA">The first joint.</param>
/// <param name="jointB">The second joint.</param>
/// <param name="ratio">The ratio.</param>
public FSGearJoint(FarseerJoint jointA, FarseerJoint jointB, float ratio)
: base(jointA.BodyA, jointA.BodyB)
{
JointType = JointType.Gear;
JointA = jointA;
JointB = jointB;
Ratio = ratio;
m_typeA = jointA.JointType;
m_typeB = jointB.JointType;
// Make sure its the right kind of joint
Debug.Assert(m_typeA == JointType.Revolute || m_typeA == JointType.Prismatic || m_typeA == JointType.FixedRevolute || m_typeA == JointType.FixedPrismatic);
Debug.Assert(m_typeB == JointType.Revolute || m_typeB == JointType.Prismatic || m_typeB == JointType.FixedRevolute || m_typeB == JointType.FixedPrismatic);
float coordinateA = 0.0f, coordinateB = 0.0f;
m_bodyC = JointA.BodyA;
BodyA = JointA.BodyB;
// Get geometry of joint1
Transform xfA = BodyA.Xf;
float aA = BodyA.Sweep.A;
Transform xfC = m_bodyC.Xf;
float aC = m_bodyC.Sweep.A;
if (m_typeA == JointType.Revolute)
{
FSRevoluteJoint revolute = (FSRevoluteJoint)jointA;
m_localAnchorC = revolute.LocalAnchorA;
m_localAnchorA = revolute.LocalAnchorB;
m_referenceAngleA = revolute.ReferenceAngle;
m_localAxisC = FVector2.Zero;
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
FSPrismaticJoint prismatic = (FSPrismaticJoint)jointA;
m_localAnchorC = prismatic.LocalAnchorA;
m_localAnchorA = prismatic.LocalAnchorB;
m_referenceAngleA = prismatic.ReferenceAngle;
m_localAxisC = prismatic.LocalXAxisA;
FVector2 pC = m_localAnchorC;
FVector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = FVector2.Dot(pA - pC, m_localAxisC);
}
m_bodyD = JointB.BodyA;
BodyB = JointB.BodyB;
// Get geometry of joint2
Transform xfB = BodyB.Xf;
float aB = BodyB.Sweep.A;
Transform xfD = m_bodyD.Xf;
float aD = m_bodyD.Sweep.A;
if (m_typeB == JointType.Revolute)
{
FSRevoluteJoint revolute = (FSRevoluteJoint)jointB;
m_localAnchorD = revolute.LocalAnchorA;
m_localAnchorB = revolute.LocalAnchorB;
m_referenceAngleB = revolute.ReferenceAngle;
m_localAxisD = FVector2.Zero;
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
FSPrismaticJoint prismatic = (FSPrismaticJoint)jointB;
m_localAnchorD = prismatic.LocalAnchorA;
m_localAnchorB = prismatic.LocalAnchorB;
m_referenceAngleB = prismatic.ReferenceAngle;
m_localAxisD = prismatic.LocalXAxisA;
FVector2 pD = m_localAnchorD;
FVector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = FVector2.Dot(pB - pD, m_localAxisD);
}
_ratio = ratio;
m_constant = coordinateA + _ratio * coordinateB;
}