/// <summary>
/// Initializes a new instance of the HumanoidAssemblerArgs class.
/// </summary>
/// <param name="spriteFamily">The sprite family.</param>
/// <param name="collisionGroup">The collision group for the body.</param>
/// <param name="bodyPosition">The position of the body.</param>
/// <param name="rightToLeftOffset">The offset from the left leg/arm to the right/leg arm.</param>
/// <param name="torsoPosition">The body-relative position of the torso.</param>
/// <param name="headPosition">The body-relative position of the head.</param>
/// <param name="armPosition">The body-relative position of the arm.</param>
/// <param name="legPosition">The body-relative position of the leg.</param>
/// <param name="beardPosition">The body-relative position of the beard.</param>
/// <param name="neckJointJoint">The neck joint info.</param>
/// <param name="shoulderJoint">The shoulder joint info.</param>
/// <param name="hipJoint">The hip joint info.</param>
public HumanoidAssemblerArgs(
string spriteFamily,
short collisionGroup,
Vector2 bodyPosition,
Vector2 rightToLeftOffset,
Vector2 torsoPosition,
Vector2 headPosition,
Vector2 armPosition,
Vector2 legPosition,
Vector2 beardPosition,
RevoluteJoint neckJointJoint,
RevoluteJoint shoulderJoint,
RevoluteJoint hipJoint)
{
this.SpriteFamily = spriteFamily;
this.CollisionGroup = collisionGroup;
this.BodyPosition = bodyPosition;
this.LeftToRightOffset = rightToLeftOffset;
this.TorsoPosition = torsoPosition;
this.HeadPosition = headPosition;
this.ArmPosition = armPosition;
this.LegPosition = legPosition;
this.BeardPosition = beardPosition;
this.NeckJointPosition = neckJointJoint;
this.ShoulderJointPosition = shoulderJoint;
this.HipJointPosition = hipJoint;
}
internal GearJoint(GearJointDef def)
: base(def)
{
JointType type1 = def.joint1.JointType;
JointType type2 = def.joint2.JointType;
Debug.Assert(type1 == JointType.Revolute || type1 == JointType.Prismatic);
Debug.Assert(type2 == JointType.Revolute || type2 == JointType.Prismatic);
Debug.Assert(def.joint1.GetBody1().IsStatic);
Debug.Assert(def.joint2.GetBody1().IsStatic);
_revolute1 = null;
_prismatic1 = null;
_revolute2 = null;
_prismatic2 = null;
float coordinate1, coordinate2;
_ground1 = def.joint1.GetBody1();
_bodyA = def.joint1.GetBody2();
if (type1 == JointType.Revolute)
{
_revolute1 = (RevoluteJoint)def.joint1;
_groundAnchor1 = _revolute1._localAnchor1;
_localAnchor1 = _revolute1._localAnchor2;
coordinate1 = _revolute1.GetJointAngle();
}
else
{
_prismatic1 = (PrismaticJoint)def.joint1;
_groundAnchor1 = _prismatic1._localAnchor1;
_localAnchor1 = _prismatic1._localAnchor2;
coordinate1 = _prismatic1.GetJointTranslation();
}
_ground2 = def.joint2.GetBody1();
_bodyB = def.joint2.GetBody2();
if (type2 == JointType.Revolute)
{
_revolute2 = (RevoluteJoint)def.joint2;
_groundAnchor2 = _revolute2._localAnchor1;
_localAnchor2 = _revolute2._localAnchor2;
coordinate2 = _revolute2.GetJointAngle();
}
else
{
_prismatic2 = (PrismaticJoint)def.joint2;
_groundAnchor2 = _prismatic2._localAnchor1;
_localAnchor2 = _prismatic2._localAnchor2;
coordinate2 = _prismatic2.GetJointTranslation();
}
_ratio = def.ratio;
_ant = coordinate1 + _ratio * coordinate2;
_impulse = 0.0f;
}
public Revolute()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
CircleShape shape = new CircleShape();
shape._radius = 0.5f;
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
RevoluteJointDef rjd = new RevoluteJointDef();
bd.position = new Vector2(0.0f, 20.0f);
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 5.0f);
float w = 100.0f;
body.SetAngularVelocity(w);
body.SetLinearVelocity(new Vector2(-8.0f * w, 0.0f));
rjd.Initialize(ground, body, new Vector2(0.0f, 12.0f));
rjd.motorSpeed = 1.0f * (float)Settings.b2_pi;
rjd.maxMotorTorque = 10000.0f;
rjd.enableMotor = false;
rjd.lowerAngle = -0.25f * (float)Settings.b2_pi;
rjd.upperAngle = 0.5f * (float)Settings.b2_pi;
rjd.enableLimit = true;
rjd.collideConnected = true;
_joint = (RevoluteJoint)_world.CreateJoint(rjd);
}
}
private void CreateLeg(float s, Vector2 wheelAnchor)
{
Vector2 p1 = new Vector2(5.4f * s, -6.1f);
Vector2 p2 = new Vector2(7.2f * s, -1.2f);
Vector2 p3 = new Vector2(4.3f * s, -1.9f);
Vector2 p4 = new Vector2(3.1f * s, 0.8f);
Vector2 p5 = new Vector2(6.0f * s, 1.5f);
Vector2 p6 = new Vector2(2.5f * s, 3.7f);
PolygonShape poly1 = new PolygonShape(1);
PolygonShape poly2 = new PolygonShape(2);
Vertices vertices = new Vertices(3);
if (s > 0.0f)
{
vertices.Add(p1);
vertices.Add(p2);
vertices.Add(p3);
poly1.Set(vertices);
vertices[0] = Vector2.Zero;
vertices[1] = p5 - p4;
vertices[2] = p6 - p4;
poly2.Set(vertices);
}
else
{
vertices.Add(p1);
vertices.Add(p3);
vertices.Add(p2);
poly1.Set(vertices);
vertices[0] = Vector2.Zero;
vertices[1] = p6 - p4;
vertices[2] = p5 - p4;
poly2.Set(vertices);
}
Body body1 = BodyFactory.CreateBody(World);
body1.BodyType = BodyType.Dynamic;
body1.Position = _offset;
body1.AngularDamping = 10.0f;
Body body2 = BodyFactory.CreateBody(World);
body2.BodyType = BodyType.Dynamic;
body2.Position = p4 + _offset;
body2.AngularDamping = 10.0f;
Fixture f1 = body1.CreateFixture(poly1);
f1.CollisionGroup = -1;
Fixture f2 = body2.CreateFixture(poly2);
f2.CollisionGroup = -1;
// Using a soft distanceraint can reduce some jitter.
// It also makes the structure seem a bit more fluid by
// acting like a suspension system.
DistanceJoint djd = new DistanceJoint(body1, body2, body1.GetLocalPoint(p2 + _offset),
body2.GetLocalPoint(p5 + _offset));
djd.DampingRatio = 0.5f;
djd.Frequency = 10.0f;
World.AddJoint(djd);
DistanceJoint djd2 = new DistanceJoint(body1, body2, body1.GetLocalPoint(p3 + _offset),
body2.GetLocalPoint(p4 + _offset));
djd2.DampingRatio = 0.5f;
djd2.Frequency = 10.0f;
World.AddJoint(djd2);
DistanceJoint djd3 = new DistanceJoint(body1, _wheel, body1.GetLocalPoint(p3 + _offset),
_wheel.GetLocalPoint(wheelAnchor + _offset));
djd3.DampingRatio = 0.5f;
djd3.Frequency = 10.0f;
World.AddJoint(djd3);
DistanceJoint djd4 = new DistanceJoint(body2, _wheel, body2.GetLocalPoint(p6 + _offset),
_wheel.GetLocalPoint(wheelAnchor + _offset));
djd4.DampingRatio = 0.5f;
djd4.Frequency = 10.0f;
World.AddJoint(djd4);
Vector2 anchor = p4 - new Vector2(0.0f, 0.8f) /*+ _offset*/;
RevoluteJoint rjd = new RevoluteJoint(body2, _chassis, body2.GetLocalPoint(_chassis.GetWorldPoint(anchor)),
anchor);
World.AddJoint(rjd);
}
Gears()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vector2(50.0f, 0.0f), new Vector2(-50.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
CircleShape circle1 = new CircleShape();
circle1._radius = 1.0f;
CircleShape circle2 = new CircleShape();
circle2._radius = 2.0f;
PolygonShape box = new PolygonShape();
box.SetAsBox(0.5f, 5.0f);
BodyDef bd1 = new BodyDef();
bd1.type = BodyType.Dynamic;
bd1.position = new Vector2(-3.0f, 12.0f);
Body body1 = _world.CreateBody(bd1);
body1.CreateFixture(circle1, 5.0f);
RevoluteJointDef jd1 = new RevoluteJointDef();
jd1.bodyA = ground;
jd1.bodyB = body1;
jd1.localAnchorA = ground.GetLocalPoint(bd1.position);
jd1.localAnchorB = body1.GetLocalPoint(bd1.position);
jd1.referenceAngle = body1.GetAngle() - ground.GetAngle();
_joint1 = (RevoluteJoint)_world.CreateJoint(jd1);
BodyDef bd2 = new BodyDef();
bd2.type = BodyType.Dynamic;
bd2.position = new Vector2(0.0f, 12.0f);
Body body2 = _world.CreateBody(bd2);
body2.CreateFixture(circle2, 5.0f);
RevoluteJointDef jd2 = new RevoluteJointDef();
jd2.Initialize(ground, body2, bd2.position);
_joint2 = (RevoluteJoint)_world.CreateJoint(jd2);
BodyDef bd3 = new BodyDef();
bd3.type = BodyType.Dynamic;
bd3.position = new Vector2(2.5f, 12.0f);
Body body3 = _world.CreateBody(bd3);
body3.CreateFixture(box, 5.0f);
PrismaticJointDef jd3 = new PrismaticJointDef();
jd3.Initialize(ground, body3, bd3.position, new Vector2(0.0f, 1.0f));
jd3.lowerTranslation = -5.0f;
jd3.upperTranslation = 5.0f;
jd3.enableLimit = true;
_joint3 = (PrismaticJoint)_world.CreateJoint(jd3);
GearJointDef jd4 = new GearJointDef();
jd4.bodyA = body1;
jd4.bodyB = body2;
jd4.joint1 = _joint1;
jd4.joint2 = _joint2;
jd4.ratio = circle2._radius / circle1._radius;
_joint4 = (GearJoint)_world.CreateJoint(jd4);
GearJointDef jd5 = new GearJointDef();
jd5.bodyA = body2;
jd5.bodyB = body3;
jd5.joint1 = _joint2;
jd5.joint2 = _joint3;
jd5.ratio = -1.0f / circle2._radius;
_joint5 = (GearJoint)_world.CreateJoint(jd5);
}
}
void Retract(RevoluteJoint joint, float dt)
{
float retractionSpeed = 2;
joint.Motor.Settings.Servo.Goal = MathHelper.Clamp(joint.Motor.Settings.Servo.Goal - retractionSpeed * dt, joint.Limit.MinimumAngle, joint.Limit.MaximumAngle);
}
private PinballTest()
{
// Ground body
Body ground;
{
ground = BodyFactory.CreateBody(World);
Vertices vertices = new Vertices(5);
vertices.Add(new Vector2(0.0f, -2.0f));
vertices.Add(new Vector2(8.0f, 6.0f));
vertices.Add(new Vector2(8.0f, 20.0f));
vertices.Add(new Vector2(-8.0f, 20.0f));
vertices.Add(new Vector2(-8.0f, 6.0f));
ChainShape chain = new ChainShape(vertices, true);
ground.CreateFixture(chain);
}
// Flippers
{
Vector2 p1 = new Vector2(-2.0f, 0f);
Vector2 p2 = new Vector2(2.0f, 0f);
Body leftFlipper = BodyFactory.CreateBody(World, p1);
leftFlipper.BodyType = BodyType.Dynamic;
Body rightFlipper = BodyFactory.CreateBody(World, p2);
rightFlipper.BodyType = BodyType.Dynamic;
PolygonShape box = new PolygonShape(1);
box.Vertices = PolygonTools.CreateRectangle(1.75f, 0.1f);
leftFlipper.CreateFixture(box);
rightFlipper.CreateFixture(box);
_leftJoint = new RevoluteJoint(ground, leftFlipper, p1, Vector2.Zero);
_leftJoint.MaxMotorTorque = 1000.0f;
_leftJoint.LimitEnabled = true;
_leftJoint.MotorEnabled = true;
_leftJoint.MotorSpeed = 0.0f;
_leftJoint.LowerLimit = -30.0f * Settings.Pi / 180.0f;
_leftJoint.UpperLimit = 5.0f * Settings.Pi / 180.0f;
World.AddJoint(_leftJoint);
_rightJoint = new RevoluteJoint(ground, rightFlipper, p2, Vector2.Zero);
_rightJoint.MaxMotorTorque = 1000.0f;
_rightJoint.LimitEnabled = true;
_rightJoint.MotorEnabled = true;
_rightJoint.MotorSpeed = 0.0f;
_rightJoint.LowerLimit = -5.0f * Settings.Pi / 180.0f;
_rightJoint.UpperLimit = 30.0f * Settings.Pi / 180.0f;
World.AddJoint(_rightJoint);
}
// Circle character
{
_ball = BodyFactory.CreateBody(World, new Vector2(1.0f, 15.0f));
_ball.BodyType = BodyType.Dynamic;
_ball.IsBullet = true;
_ball.CreateFixture(new CircleShape(0.2f, 1.0f));
}
}
public Gears()
{
Body ground;
{
var bd = new BodyDef();
ground = World.CreateBody(bd);
var shape = new EdgeShape();
shape.Set(new Vector2(50.0f, 0.0f), new Vector2(-50.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
var circle1 = new CircleShape();
circle1.Radius = 1.0f;
var box = new PolygonShape();
box.SetAsBox(0.5f, 5.0f);
var circle2 = new CircleShape();
circle2.Radius = 2.0f;
var bd1 = new BodyDef();
bd1.BodyType = BodyType.StaticBody;
bd1.Position.Set(10.0f, 9.0f);
var body1 = World.CreateBody(bd1);
body1.CreateFixture(circle1, 5.0f);
var bd2 = new BodyDef();
bd2.BodyType = BodyType.DynamicBody;
bd2.Position.Set(10.0f, 8.0f);
var body2 = World.CreateBody(bd2);
body2.CreateFixture(box, 5.0f);
var bd3 = new BodyDef();
bd3.BodyType = BodyType.DynamicBody;
bd3.Position.Set(10.0f, 6.0f);
var body3 = World.CreateBody(bd3);
body3.CreateFixture(circle2, 5.0f);
var jd1 = new RevoluteJointDef();
jd1.Initialize(body2, body1, bd1.Position);
var joint1 = World.CreateJoint(jd1);
var jd2 = new RevoluteJointDef();
jd2.Initialize(body2, body3, bd3.Position);
var joint2 = World.CreateJoint(jd2);
var jd4 = new GearJointDef();
jd4.BodyA = body1;
jd4.BodyB = body3;
jd4.Joint1 = joint1;
jd4.Joint2 = joint2;
jd4.Ratio = circle2.Radius / circle1.Radius;
World.CreateJoint(jd4);
}
{
var circle1 = new CircleShape();
circle1.Radius = 1.0f;
var circle2 = new CircleShape();
circle2.Radius = 2.0f;
var box = new PolygonShape();
box.SetAsBox(0.5f, 5.0f);
var bd1 = new BodyDef();
bd1.BodyType = BodyType.DynamicBody;
bd1.Position.Set(-3.0f, 12.0f);
var body1 = World.CreateBody(bd1);
body1.CreateFixture(circle1, 5.0f);
var jd1 = new RevoluteJointDef();
jd1.BodyA = ground;
jd1.BodyB = body1;
jd1.LocalAnchorA = ground.GetLocalPoint(bd1.Position);
jd1.LocalAnchorB = body1.GetLocalPoint(bd1.Position);
jd1.ReferenceAngle = body1.GetAngle() - ground.GetAngle();
_joint1 = (RevoluteJoint)World.CreateJoint(jd1);
var bd2 = new BodyDef();
bd2.BodyType = BodyType.DynamicBody;
bd2.Position.Set(0.0f, 12.0f);
var body2 = World.CreateBody(bd2);
body2.CreateFixture(circle2, 5.0f);
var jd2 = new RevoluteJointDef();
jd2.Initialize(ground, body2, bd2.Position);
_joint2 = (RevoluteJoint)World.CreateJoint(jd2);
var bd3 = new BodyDef();
bd3.BodyType = BodyType.DynamicBody;
bd3.Position.Set(2.5f, 12.0f);
var body3 = World.CreateBody(bd3);
body3.CreateFixture(box, 5.0f);
var jd3 = new PrismaticJointDef();
jd3.Initialize(ground, body3, bd3.Position, new Vector2(0.0f, 1.0f));
jd3.LowerTranslation = -5.0f;
jd3.UpperTranslation = 5.0f;
jd3.EnableLimit = true;
_joint3 = (PrismaticJoint)World.CreateJoint(jd3);
var jd4 = new GearJointDef();
jd4.BodyA = body1;
jd4.BodyB = body2;
jd4.Joint1 = _joint1;
jd4.Joint2 = _joint2;
jd4.Ratio = circle2.Radius / circle1.Radius;
_joint4 = (GearJoint)World.CreateJoint(jd4);
var jd5 = new GearJointDef();
jd5.BodyA = body2;
jd5.BodyB = body3;
jd5.Joint1 = _joint2;
jd5.Joint2 = _joint3;
jd5.Ratio = -1.0f / circle2.Radius;
_joint5 = (GearJoint)World.CreateJoint(jd5);
}
}
private CarTest()
{
_hz = 4.0f;
_zeta = 0.7f;
_speed = 50.0f;
Body ground = World.CreateEdge(new Vector2(-20.0f, 0.0f), new Vector2(20.0f, 0.0f));
{
float[] hs = new[] { 0.25f, 1.0f, 4.0f, 0.0f, 0.0f, -1.0f, -2.0f, -2.0f, -1.25f, 0.0f };
float x = 20.0f, y1 = 0.0f;
const float dx = 5.0f;
for (int i = 0; i < 10; ++i)
{
float y2 = hs[i];
ground.CreateEdge(new Vector2(x, y1), new Vector2(x + dx, y2));
y1 = y2;
x += dx;
}
for (int i = 0; i < 10; ++i)
{
float y2 = hs[i];
ground.CreateEdge(new Vector2(x, y1), new Vector2(x + dx, y2));
y1 = y2;
x += dx;
}
var f1 = ground.CreateEdge(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f));
f1.Friction = 0.6f;
x += 80.0f;
ground.CreateEdge(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f));
x += 40.0f;
ground.CreateEdge(new Vector2(x, 0.0f), new Vector2(x + 10.0f, 5.0f));
x += 20.0f;
ground.CreateEdge(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f));
x += 40.0f;
ground.CreateEdge(new Vector2(x, 0.0f), new Vector2(x, 20.0f));
foreach (Fixture fixture in ground.FixtureList)
{
fixture.Restitution = 0.6f;
}
}
// Teeter
{
Body body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(140.0f, 1.0f);
PolygonShape box = new PolygonShape(1);
box.Vertices = PolygonTools.CreateRectangle(10.0f, 0.25f);
body.CreateFixture(box);
RevoluteJoint jd = JointFactory.CreateRevoluteJoint(World, ground, body, Vector2.Zero);
jd.LowerLimit = -8.0f * MathHelper.Pi / 180.0f;
jd.UpperLimit = 8.0f * MathHelper.Pi / 180.0f;
jd.LimitEnabled = true;
body.ApplyAngularImpulse(100.0f);
}
//Bridge
{
const int N = 20;
PolygonShape shape = new PolygonShape(1);
shape.Vertices = PolygonTools.CreateRectangle(1.0f, 0.125f);
Body prevBody = ground;
for (int i = 0; i < N; ++i)
{
Body body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(161.0f + 2.0f * i, -0.125f);
Fixture fix = body.CreateFixture(shape);
fix.Friction = 0.6f;
Vector2 anchor = new Vector2(-1, 0);
JointFactory.CreateRevoluteJoint(World, prevBody, body, anchor);
prevBody = body;
}
Vector2 anchor2 = new Vector2(1.0f, 0);
JointFactory.CreateRevoluteJoint(World, ground, prevBody, anchor2);
}
// Boxes
{
PolygonShape box = new PolygonShape(0.5f);
box.Vertices = PolygonTools.CreateRectangle(0.5f, 0.5f);
Body body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(230.0f, 0.5f);
body.CreateFixture(box);
body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(230.0f, 1.5f);
body.CreateFixture(box);
body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(230.0f, 2.5f);
body.CreateFixture(box);
body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(230.0f, 3.5f);
body.CreateFixture(box);
body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(230.0f, 4.5f);
body.CreateFixture(box);
}
// Car
{
Vertices vertices = new Vertices(8);
vertices.Add(new Vector2(-1.5f, -0.5f));
vertices.Add(new Vector2(1.5f, -0.5f));
vertices.Add(new Vector2(1.5f, 0.0f));
vertices.Add(new Vector2(0.0f, 0.9f));
vertices.Add(new Vector2(-1.15f, 0.9f));
vertices.Add(new Vector2(-1.5f, 0.2f));
PolygonShape chassis = new PolygonShape(vertices, 1);
CircleShape circle = new CircleShape(0.4f, 1);
_car = World.CreateBody(new Vector2(0.0f, 1.0f), 0, BodyType.Dynamic);
var cfixture = _car.CreateFixture(chassis);
_wheel1 = World.CreateBody(new Vector2(-1.0f, 0.35f), 0, BodyType.Dynamic);
var w1fixture = _wheel1.CreateFixture(circle);
w1fixture.Friction = 0.9f;
_wheel2 = World.CreateBody(new Vector2(1.0f, 0.4f), 0, BodyType.Dynamic);
var w2fixture = _wheel2.CreateFixture(circle);
w2fixture.Friction = 0.9f;
Vector2 axis = new Vector2(0.0f, 1.0f);
_spring1 = new WheelJoint(_car, _wheel1, _wheel1.Position, axis, true);
_spring1.MotorSpeed = 0.0f;
_spring1.MaxMotorTorque = 20.0f;
_spring1.MotorEnabled = true;
_spring1.Frequency = _hz;
_spring1.DampingRatio = _zeta;
World.Add(_spring1);
_spring2 = new WheelJoint(_car, _wheel2, _wheel2.Position, axis, true);
_spring2.MotorSpeed = 0.0f;
_spring2.MaxMotorTorque = 10.0f;
_spring2.MotorEnabled = false;
_spring2.Frequency = _hz;
_spring2.DampingRatio = _zeta;
World.Add(_spring2);
}
}
private BodyTypesTest()
{
//Ground
BodyFactory.CreateEdge(World, new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
// Define attachment
{
_attachment = BodyFactory.CreateBody(World);
_attachment.BodyType = BodyType.Dynamic;
_attachment.Position = new Vector2(0.0f, 3.0f);
Vertices box = PolygonTools.CreateRectangle(0.5f, 2.0f);
PolygonShape shape = new PolygonShape(box, 2);
_attachment.CreateFixture(shape);
}
// Define platform
{
_platform = BodyFactory.CreateBody(World);
_platform.BodyType = BodyType.Dynamic;
_platform.Position = new Vector2(0.0f, 5.0f);
Vertices box = PolygonTools.CreateRectangle(4.0f, 0.5f);
PolygonShape shape = new PolygonShape(box, 2);
Fixture fixture = _platform.CreateFixture(shape);
fixture.Friction = 0.6f;
RevoluteJoint rjd = new RevoluteJoint(_attachment, _platform,
_attachment.GetLocalPoint(_platform.Position),
Vector2.Zero);
rjd.MaxMotorTorque = 50.0f;
rjd.MotorEnabled = true;
World.AddJoint(rjd);
FixedPrismaticJoint pjd = new FixedPrismaticJoint(_platform, new Vector2(0.0f, 5.0f),
new Vector2(1.0f, 0.0f));
pjd.MaxMotorForce = 1000.0f;
pjd.MotorEnabled = true;
pjd.LowerLimit = -10.0f;
pjd.UpperLimit = 10.0f;
pjd.LimitEnabled = true;
World.AddJoint(pjd);
_speed = 3.0f;
}
// Create a payload
{
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 8.0f);
Vertices box = PolygonTools.CreateRectangle(0.75f, 0.75f);
PolygonShape shape = new PolygonShape(box, 2);
Fixture fixture = body.CreateFixture(shape);
fixture.Friction = 0.6f;
}
}
private static void Deserialize(World world, Stream stream)
{
List <Body> bodies = new List <Body>();
List <Fixture> fixtures = new List <Fixture>();
List <Joint> joints = new List <Joint>();
List <Shape> shapes = new List <Shape>();
XMLFragmentElement root = XMLFragmentParser.LoadFromStream(stream);
if (root.Name.ToLower() != "world")
{
throw new Exception();
}
//Read gravity
foreach (XMLFragmentElement element in root.Elements)
{
if (element.Name.ToLower() == "gravity")
{
world.Gravity = ReadVector(element);
break;
}
}
//Read shapes
foreach (XMLFragmentElement shapeElement in root.Elements)
{
if (shapeElement.Name.ToLower() == "shapes")
{
foreach (XMLFragmentElement element in shapeElement.Elements)
{
if (element.Name.ToLower() != "shape")
{
throw new Exception();
}
ShapeType type = (ShapeType)Enum.Parse(typeof(ShapeType), element.Attributes[0].Value, true);
float density = float.Parse(element.Attributes[1].Value);
switch (type)
{
case ShapeType.Circle:
{
CircleShape shape = new CircleShape();
shape._density = density;
foreach (XMLFragmentElement sn in element.Elements)
{
switch (sn.Name.ToLower())
{
case "radius":
shape.Radius = float.Parse(sn.Value);
break;
case "position":
shape.Position = ReadVector(sn);
break;
default:
throw new Exception();
}
}
shapes.Add(shape);
}
break;
case ShapeType.Polygon:
{
PolygonShape shape = new PolygonShape();
shape._density = density;
foreach (XMLFragmentElement sn in element.Elements)
{
switch (sn.Name.ToLower())
{
case "vertices":
{
List <Vector2> verts = new List <Vector2>(sn.Elements.Count);
foreach (XMLFragmentElement vert in sn.Elements)
{
verts.Add(ReadVector(vert));
}
shape.Vertices = new Vertices(verts);
}
break;
case "centroid":
shape.MassData.Centroid = ReadVector(sn);
break;
}
}
shapes.Add(shape);
}
break;
case ShapeType.Edge:
{
EdgeShape shape = new EdgeShape();
shape._density = density;
foreach (XMLFragmentElement sn in element.Elements)
{
switch (sn.Name.ToLower())
{
case "hasvertex0":
shape.HasVertex0 = bool.Parse(sn.Value);
break;
case "hasvertex3":
shape.HasVertex0 = bool.Parse(sn.Value);
break;
case "vertex0":
shape.Vertex0 = ReadVector(sn);
break;
case "vertex1":
shape.Vertex1 = ReadVector(sn);
break;
case "vertex2":
shape.Vertex2 = ReadVector(sn);
break;
case "vertex3":
shape.Vertex3 = ReadVector(sn);
break;
default:
throw new Exception();
}
}
shapes.Add(shape);
}
break;
case ShapeType.Chain:
{
ChainShape shape = new ChainShape();
shape._density = density;
foreach (XMLFragmentElement sn in element.Elements)
{
switch (sn.Name.ToLower())
{
case "vertices":
{
List <Vector2> verts = new List <Vector2>(sn.Elements.Count);
foreach (XMLFragmentElement vert in sn.Elements)
{
verts.Add(ReadVector(vert));
}
shape.Vertices = new Vertices(verts);
}
break;
case "nextvertex":
shape.NextVertex = ReadVector(sn);
break;
case "prevvertex":
shape.PrevVertex = ReadVector(sn);
break;
default:
throw new Exception();
}
}
shapes.Add(shape);
}
break;
}
}
}
}
//Read fixtures
foreach (XMLFragmentElement fixtureElement in root.Elements)
{
if (fixtureElement.Name.ToLower() == "fixtures")
{
foreach (XMLFragmentElement element in fixtureElement.Elements)
{
Fixture fixture = new Fixture();
if (element.Name.ToLower() != "fixture")
{
throw new Exception();
}
fixture.FixtureId = int.Parse(element.Attributes[0].Value);
foreach (XMLFragmentElement sn in element.Elements)
{
switch (sn.Name.ToLower())
{
case "filterdata":
foreach (XMLFragmentElement ssn in sn.Elements)
{
switch (ssn.Name.ToLower())
{
case "categorybits":
fixture._collisionCategories = (Category)int.Parse(ssn.Value);
break;
case "maskbits":
fixture._collidesWith = (Category)int.Parse(ssn.Value);
break;
case "groupindex":
fixture._collisionGroup = short.Parse(ssn.Value);
break;
case "CollisionIgnores":
string[] split = ssn.Value.Split('|');
foreach (string s in split)
{
fixture._collisionIgnores.Add(int.Parse(s));
}
break;
}
}
break;
case "friction":
fixture.Friction = float.Parse(sn.Value);
break;
case "issensor":
fixture.IsSensor = bool.Parse(sn.Value);
break;
case "restitution":
fixture.Restitution = float.Parse(sn.Value);
break;
case "userdata":
fixture.UserData = ReadSimpleType(sn, null, false);
break;
}
}
fixtures.Add(fixture);
}
}
}
//Read bodies
foreach (XMLFragmentElement bodyElement in root.Elements)
{
if (bodyElement.Name.ToLower() == "bodies")
{
foreach (XMLFragmentElement element in bodyElement.Elements)
{
Body body = new Body(world);
if (element.Name.ToLower() != "body")
{
throw new Exception();
}
body.BodyType = (BodyType)Enum.Parse(typeof(BodyType), element.Attributes[0].Value, true);
foreach (XMLFragmentElement sn in element.Elements)
{
switch (sn.Name.ToLower())
{
case "active":
bool enabled = bool.Parse(sn.Value);
if (enabled)
{
body._flags |= BodyFlags.Enabled;
}
else
{
body._flags &= ~BodyFlags.Enabled;
}
break;
case "allowsleep":
body.SleepingAllowed = bool.Parse(sn.Value);
break;
case "angle":
{
Vector2 position = body.Position;
body.SetTransformIgnoreContacts(ref position, float.Parse(sn.Value));
}
break;
case "angulardamping":
body.AngularDamping = float.Parse(sn.Value);
break;
case "angularvelocity":
body.AngularVelocity = float.Parse(sn.Value);
break;
case "awake":
body.Awake = bool.Parse(sn.Value);
break;
case "bullet":
body.IsBullet = bool.Parse(sn.Value);
break;
case "fixedrotation":
body.FixedRotation = bool.Parse(sn.Value);
break;
case "lineardamping":
body.LinearDamping = float.Parse(sn.Value);
break;
case "linearvelocity":
body.LinearVelocity = ReadVector(sn);
break;
case "position":
{
float rotation = body.Rotation;
Vector2 position = ReadVector(sn);
body.SetTransformIgnoreContacts(ref position, rotation);
}
break;
case "userdata":
body.UserData = ReadSimpleType(sn, null, false);
break;
case "bindings":
{
foreach (XMLFragmentElement pair in sn.Elements)
{
Fixture fix = fixtures[int.Parse(pair.Attributes[0].Value)];
fix.Shape = shapes[int.Parse(pair.Attributes[1].Value)].Clone();
fix.CloneOnto(body);
}
break;
}
}
}
bodies.Add(body);
}
}
}
//Read joints
foreach (XMLFragmentElement jointElement in root.Elements)
{
if (jointElement.Name.ToLower() == "joints")
{
foreach (XMLFragmentElement n in jointElement.Elements)
{
Joint joint;
if (n.Name.ToLower() != "joint")
{
throw new Exception();
}
JointType type = (JointType)Enum.Parse(typeof(JointType), n.Attributes[0].Value, true);
int bodyAIndex = -1, bodyBIndex = -1;
bool collideConnected = false;
object userData = null;
foreach (XMLFragmentElement sn in n.Elements)
{
switch (sn.Name.ToLower())
{
case "bodya":
bodyAIndex = int.Parse(sn.Value);
break;
case "bodyb":
bodyBIndex = int.Parse(sn.Value);
break;
case "collideconnected":
collideConnected = bool.Parse(sn.Value);
break;
case "userdata":
userData = ReadSimpleType(sn, null, false);
break;
}
}
Body bodyA = bodies[bodyAIndex];
Body bodyB = bodies[bodyBIndex];
switch (type)
{
//case JointType.FixedMouse:
// joint = new FixedMouseJoint();
// break;
//case JointType.FixedRevolute:
// break;
//case JointType.FixedDistance:
// break;
//case JointType.FixedLine:
// break;
//case JointType.FixedPrismatic:
// break;
//case JointType.FixedAngle:
// break;
//case JointType.FixedFriction:
// break;
case JointType.Distance:
joint = new DistanceJoint();
break;
case JointType.Friction:
joint = new FrictionJoint();
break;
case JointType.Wheel:
joint = new WheelJoint();
break;
case JointType.Prismatic:
joint = new PrismaticJoint();
break;
case JointType.Pulley:
joint = new PulleyJoint();
break;
case JointType.Revolute:
joint = new RevoluteJoint();
break;
case JointType.Weld:
joint = new WeldJoint();
break;
case JointType.Rope:
joint = new RopeJoint();
break;
case JointType.Angle:
joint = new AngleJoint();
break;
case JointType.Motor:
joint = new MotorJoint();
break;
case JointType.Gear:
throw new Exception("GearJoint is not supported.");
default:
throw new Exception("Invalid or unsupported joint.");
}
joint.CollideConnected = collideConnected;
joint.UserData = userData;
joint.BodyA = bodyA;
joint.BodyB = bodyB;
joints.Add(joint);
world.AddJoint(joint);
foreach (XMLFragmentElement sn in n.Elements)
{
// check for specific nodes
switch (type)
{
case JointType.Distance:
{
switch (sn.Name.ToLower())
{
case "dampingratio":
((DistanceJoint)joint).DampingRatio = float.Parse(sn.Value);
break;
case "frequencyhz":
((DistanceJoint)joint).Frequency = float.Parse(sn.Value);
break;
case "length":
((DistanceJoint)joint).Length = float.Parse(sn.Value);
break;
case "localanchora":
((DistanceJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((DistanceJoint)joint).LocalAnchorB = ReadVector(sn);
break;
}
}
break;
case JointType.Friction:
{
switch (sn.Name.ToLower())
{
case "localanchora":
((FrictionJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((FrictionJoint)joint).LocalAnchorB = ReadVector(sn);
break;
case "maxforce":
((FrictionJoint)joint).MaxForce = float.Parse(sn.Value);
break;
case "maxtorque":
((FrictionJoint)joint).MaxTorque = float.Parse(sn.Value);
break;
}
}
break;
case JointType.Wheel:
{
switch (sn.Name.ToLower())
{
case "enablemotor":
((WheelJoint)joint).MotorEnabled = bool.Parse(sn.Value);
break;
case "localanchora":
((WheelJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((WheelJoint)joint).LocalAnchorB = ReadVector(sn);
break;
case "motorspeed":
((WheelJoint)joint).MotorSpeed = float.Parse(sn.Value);
break;
case "dampingratio":
((WheelJoint)joint).DampingRatio = float.Parse(sn.Value);
break;
case "maxmotortorque":
((WheelJoint)joint).MaxMotorTorque = float.Parse(sn.Value);
break;
case "frequencyhz":
((WheelJoint)joint).Frequency = float.Parse(sn.Value);
break;
case "axis":
((WheelJoint)joint).Axis = ReadVector(sn);
break;
}
}
break;
case JointType.Prismatic:
{
switch (sn.Name.ToLower())
{
case "enablelimit":
((PrismaticJoint)joint).LimitEnabled = bool.Parse(sn.Value);
break;
case "enablemotor":
((PrismaticJoint)joint).MotorEnabled = bool.Parse(sn.Value);
break;
case "localanchora":
((PrismaticJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((PrismaticJoint)joint).LocalAnchorB = ReadVector(sn);
break;
case "axis":
((PrismaticJoint)joint).Axis = ReadVector(sn);
break;
case "maxmotorforce":
((PrismaticJoint)joint).MaxMotorForce = float.Parse(sn.Value);
break;
case "motorspeed":
((PrismaticJoint)joint).MotorSpeed = float.Parse(sn.Value);
break;
case "lowertranslation":
((PrismaticJoint)joint).LowerLimit = float.Parse(sn.Value);
break;
case "uppertranslation":
((PrismaticJoint)joint).UpperLimit = float.Parse(sn.Value);
break;
case "referenceangle":
((PrismaticJoint)joint).ReferenceAngle = float.Parse(sn.Value);
break;
}
}
break;
case JointType.Pulley:
{
switch (sn.Name.ToLower())
{
case "worldanchora":
((PulleyJoint)joint).WorldAnchorA = ReadVector(sn);
break;
case "worldanchorb":
((PulleyJoint)joint).WorldAnchorB = ReadVector(sn);
break;
case "lengtha":
((PulleyJoint)joint).LengthA = float.Parse(sn.Value);
break;
case "lengthb":
((PulleyJoint)joint).LengthB = float.Parse(sn.Value);
break;
case "localanchora":
((PulleyJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((PulleyJoint)joint).LocalAnchorB = ReadVector(sn);
break;
case "ratio":
((PulleyJoint)joint).Ratio = float.Parse(sn.Value);
break;
case "constant":
((PulleyJoint)joint).Constant = float.Parse(sn.Value);
break;
}
}
break;
case JointType.Revolute:
{
switch (sn.Name.ToLower())
{
case "enablelimit":
((RevoluteJoint)joint).LimitEnabled = bool.Parse(sn.Value);
break;
case "enablemotor":
((RevoluteJoint)joint).MotorEnabled = bool.Parse(sn.Value);
break;
case "localanchora":
((RevoluteJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((RevoluteJoint)joint).LocalAnchorB = ReadVector(sn);
break;
case "maxmotortorque":
((RevoluteJoint)joint).MaxMotorTorque = float.Parse(sn.Value);
break;
case "motorspeed":
((RevoluteJoint)joint).MotorSpeed = float.Parse(sn.Value);
break;
case "lowerangle":
((RevoluteJoint)joint).LowerLimit = float.Parse(sn.Value);
break;
case "upperangle":
((RevoluteJoint)joint).UpperLimit = float.Parse(sn.Value);
break;
case "referenceangle":
((RevoluteJoint)joint).ReferenceAngle = float.Parse(sn.Value);
break;
}
}
break;
case JointType.Weld:
{
switch (sn.Name.ToLower())
{
case "localanchora":
((WeldJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((WeldJoint)joint).LocalAnchorB = ReadVector(sn);
break;
}
}
break;
case JointType.Rope:
{
switch (sn.Name.ToLower())
{
case "localanchora":
((RopeJoint)joint).LocalAnchorA = ReadVector(sn);
break;
case "localanchorb":
((RopeJoint)joint).LocalAnchorB = ReadVector(sn);
break;
case "maxlength":
((RopeJoint)joint).MaxLength = float.Parse(sn.Value);
break;
}
}
break;
case JointType.Gear:
throw new Exception("Gear joint is unsupported");
case JointType.Angle:
{
switch (sn.Name.ToLower())
{
case "biasfactor":
((AngleJoint)joint).BiasFactor = float.Parse(sn.Value);
break;
case "maximpulse":
((AngleJoint)joint).MaxImpulse = float.Parse(sn.Value);
break;
case "softness":
((AngleJoint)joint).Softness = float.Parse(sn.Value);
break;
case "targetangle":
((AngleJoint)joint).TargetAngle = float.Parse(sn.Value);
break;
}
}
break;
case JointType.Motor:
switch (sn.Name.ToLower())
{
case "angularoffset":
((MotorJoint)joint).AngularOffset = float.Parse(sn.Value);
break;
case "linearoffset":
((MotorJoint)joint).LinearOffset = ReadVector(sn);
break;
case "maxforce":
((MotorJoint)joint).MaxForce = float.Parse(sn.Value);
break;
case "maxtorque":
((MotorJoint)joint).MaxTorque = float.Parse(sn.Value);
break;
case "correctionfactor":
((MotorJoint)joint).CorrectionFactor = float.Parse(sn.Value);
break;
}
break;
}
}
}
}
}
world.ProcessChanges();
}
void UpdateWalkAnim(float deltaTime)
{
movement = MathUtils.SmoothStep(movement, TargetMovement * walkSpeed, 0.2f);
float mainLimbHeight, mainLimbAngle;
if (MainLimb.type == LimbType.Torso)
{
mainLimbHeight = TorsoPosition;
mainLimbAngle = torsoAngle;
}
else
{
mainLimbHeight = HeadPosition;
mainLimbAngle = headAngle;
}
MainLimb.body.SmoothRotate(mainLimbAngle * Dir, 50.0f);
Collider.LinearVelocity = new Vector2(
movement.X,
Collider.LinearVelocity.Y > 0.0f ? Collider.LinearVelocity.Y * 0.5f : Collider.LinearVelocity.Y);
MainLimb.MoveToPos(GetColliderBottom() + Vector2.UnitY * mainLimbHeight, 10.0f);
MainLimb.pullJoint.Enabled = true;
MainLimb.pullJoint.WorldAnchorB = GetColliderBottom() + Vector2.UnitY * mainLimbHeight;
walkPos -= MainLimb.LinearVelocity.X * 0.05f;
Vector2 transformedStepSize = new Vector2(
(float)Math.Cos(walkPos) * stepSize.X * 3.0f,
(float)Math.Sin(walkPos) * stepSize.Y * 2.0f);
foreach (Limb limb in Limbs)
{
switch (limb.type)
{
case LimbType.LeftFoot:
case LimbType.RightFoot:
Vector2 footPos = new Vector2(limb.SimPosition.X, MainLimb.SimPosition.Y - mainLimbHeight);
if (limb.RefJointIndex > -1)
{
RevoluteJoint refJoint = LimbJoints[limb.RefJointIndex];
footPos.X = refJoint.WorldAnchorA.X;
}
footPos.X += limb.StepOffset.X * Dir;
footPos.Y += limb.StepOffset.Y;
if (limb.type == LimbType.LeftFoot)
{
limb.MoveToPos(footPos + new Vector2(
transformedStepSize.X + movement.X * 0.1f,
(transformedStepSize.Y > 0.0f) ? transformedStepSize.Y : 0.0f),
8.0f);
}
else if (limb.type == LimbType.RightFoot)
{
limb.MoveToPos(footPos + new Vector2(
-transformedStepSize.X + movement.X * 0.1f,
(-transformedStepSize.Y > 0.0f) ? -transformedStepSize.Y : 0.0f),
8.0f);
}
if (footRotation != null)
{
limb.body.SmoothRotate((float)footRotation * Dir, 50.0f);
}
break;
case LimbType.LeftLeg:
case LimbType.RightLeg:
if (legTorque != 0.0f)
{
limb.body.ApplyTorque(limb.Mass * legTorque * Dir);
}
break;
}
}
}
protected override void Create()
{
Body ground;
{
var bd = new BodyDef();
ground = World.CreateBody(bd);
var shape = new EdgeShape();
shape.Set(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
var prevBody = ground;
// Define crank.
{
var shape = new PolygonShape();
shape.SetAsBox(0.5f, 2.0f);
var bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
bd.Position = new Vector2(0.0f, 7.0f);
var body = World.CreateBody(bd);
body.CreateFixture(shape, 2.0f);
var rjd = new RevoluteJointDef();
rjd.Initialize(prevBody, body, new Vector2(0.0f, 5.0f));
rjd.MotorSpeed = 1.0f * Settings.Pi;
rjd.MaxMotorTorque = 10000.0f;
rjd.EnableMotor = true;
_joint1 = (RevoluteJoint)World.CreateJoint(rjd);
prevBody = body;
}
// Define follower.
{
var shape = new PolygonShape();
shape.SetAsBox(0.5f, 4.0f);
var bd = new BodyDef {
BodyType = BodyType.DynamicBody, Position = new Vector2(0.0f, 13.0f)
};
var body = World.CreateBody(bd);
body.CreateFixture(shape, 2.0f);
var rjd = new RevoluteJointDef();
rjd.Initialize(prevBody, body, new Vector2(0.0f, 9.0f));
rjd.EnableMotor = false;
World.CreateJoint(rjd);
prevBody = body;
}
// Define piston
{
var shape = new PolygonShape();
shape.SetAsBox(1.5f, 1.5f);
var bd = new BodyDef
{
BodyType = BodyType.DynamicBody, FixedRotation = true,
Position = new Vector2(0.0f, 17.0f)
};
var body = World.CreateBody(bd);
body.CreateFixture(shape, 2.0f);
var rjd = new RevoluteJointDef();
rjd.Initialize(prevBody, body, new Vector2(0.0f, 17.0f));
World.CreateJoint(rjd);
var pjd = new PrismaticJointDef();
pjd.Initialize(ground, body, new Vector2(0.0f, 17.0f), new Vector2(0.0f, 1.0f));
pjd.MaxMotorForce = 1000.0f;
pjd.EnableMotor = true;
_joint2 = (PrismaticJoint)World.CreateJoint(pjd);
}
// Create a payload
{
var shape = new PolygonShape();
shape.SetAsBox(1.5f, 1.5f);
var bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
bd.Position = new Vector2(0.0f, 23.0f);
var body = World.CreateBody(bd);
body.CreateFixture(shape, 2.0f);
}
}
}
private SerializationTest()
{
Body ground = BodyFactory.CreateEdge(World, new Vector2(-20, 0), new Vector2(20, 0));
//Friction and distance joint
{
Body bodyA = BodyFactory.CreateCircle(World, 1, 1.5f, new Vector2(10, 25));
bodyA.BodyType = BodyType.Dynamic;
Body bodyB = BodyFactory.CreateRectangle(World, 1, 1, 1, new Vector2(-1, 25));
bodyB.BodyType = BodyType.Dynamic;
FrictionJoint frictionJoint = JointFactory.CreateFrictionJoint(World, bodyB, ground, Vector2.Zero);
frictionJoint.CollideConnected = true;
frictionJoint.MaxForce = 100;
JointFactory.CreateDistanceJoint(World, bodyA, bodyB);
}
//Wheel joint
{
Vertices vertices = new Vertices(6);
vertices.Add(new Vector2(-1.5f, -0.5f));
vertices.Add(new Vector2(1.5f, -0.5f));
vertices.Add(new Vector2(1.5f, 0.0f));
vertices.Add(new Vector2(0.0f, 0.9f));
vertices.Add(new Vector2(-1.15f, 0.9f));
vertices.Add(new Vector2(-1.5f, 0.2f));
Body carBody = BodyFactory.CreatePolygon(World, vertices, 1, new Vector2(0, 1));
carBody.BodyType = BodyType.Dynamic;
Body wheel1 = BodyFactory.CreateCircle(World, 0.4f, 1, new Vector2(-1.0f, 0.35f));
wheel1.BodyType = BodyType.Dynamic;
wheel1.Friction = 0.9f;
Body wheel2 = BodyFactory.CreateCircle(World, 0.4f, 1, new Vector2(1.0f, 0.4f));
wheel2.BodyType = BodyType.Dynamic;
wheel2.Friction = 0.9f;
Vector2 axis = new Vector2(0.0f, 1.0f);
WheelJoint spring1 = JointFactory.CreateWheelJoint(World, carBody, wheel1, axis);
spring1.MotorSpeed = 0.0f;
spring1.MaxMotorTorque = 20.0f;
spring1.MotorEnabled = true;
spring1.Frequency = 4;
spring1.DampingRatio = 0.7f;
WheelJoint spring2 = JointFactory.CreateWheelJoint(World, carBody, wheel2, axis);
spring2.MotorSpeed = 0.0f;
spring2.MaxMotorTorque = 10.0f;
spring2.MotorEnabled = false;
spring2.Frequency = 4;
spring2.DampingRatio = 0.7f;
}
//Prismatic joint
{
Body body = BodyFactory.CreateRectangle(World, 2, 2, 5, new Vector2(-10.0f, 10.0f));
body.BodyType = BodyType.Dynamic;
body.Rotation = 0.5f * Settings.Pi;
Vector2 axis = new Vector2(2.0f, 1.0f);
axis.Normalize();
PrismaticJoint joint = JointFactory.CreatePrismaticJoint(World, ground, body, Vector2.Zero, axis);
joint.MotorSpeed = 5.0f;
joint.MaxMotorForce = 1000.0f;
joint.MotorEnabled = true;
joint.LowerLimit = -10.0f;
joint.UpperLimit = 20.0f;
joint.LimitEnabled = true;
}
// Pulley joint
{
Body body1 = BodyFactory.CreateRectangle(World, 2, 4, 5, new Vector2(-10.0f, 16.0f));
body1.BodyType = BodyType.Dynamic;
Body body2 = BodyFactory.CreateRectangle(World, 2, 4, 5, new Vector2(10.0f, 16.0f));
body2.BodyType = BodyType.Dynamic;
Vector2 anchor1 = new Vector2(-10.0f, 16.0f + 2.0f);
Vector2 anchor2 = new Vector2(10.0f, 16.0f + 2.0f);
Vector2 worldAnchor1 = new Vector2(-10.0f, 16.0f + 2.0f + 12.0f);
Vector2 worldAnchor2 = new Vector2(10.0f, 16.0f + 2.0f + 12.0f);
JointFactory.CreatePulleyJoint(World, body1, body2, anchor1, anchor2, worldAnchor1, worldAnchor2, 1.5f, true);
}
//Revolute joint
{
Body ball = BodyFactory.CreateCircle(World, 3.0f, 5.0f, new Vector2(5.0f, 30.0f));
ball.BodyType = BodyType.Dynamic;
Body polygonBody = BodyFactory.CreateRectangle(World, 20, 0.4f, 2, new Vector2(10, 10));
polygonBody.BodyType = BodyType.Dynamic;
polygonBody.IsBullet = true;
RevoluteJoint joint = JointFactory.CreateRevoluteJoint(World, ground, polygonBody, new Vector2(10, 0));
joint.LowerLimit = -0.25f * Settings.Pi;
joint.UpperLimit = 0.0f * Settings.Pi;
joint.LimitEnabled = true;
}
//Weld joint
{
PolygonShape shape = new PolygonShape(PolygonUtils.CreateRectangle(0.5f, 0.125f), 20);
Body prevBody = ground;
for (int i = 0; i < 10; ++i)
{
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(-14.5f + 1.0f * i, 5.0f);
body.CreateFixture(shape);
Vector2 anchor = new Vector2(0.5f, 0);
if (i == 0)
{
anchor = new Vector2(-15f, 5);
}
JointFactory.CreateWeldJoint(World, prevBody, body, anchor, new Vector2(-0.5f, 0));
prevBody = body;
}
}
//Rope joint
{
LinkFactory.CreateChain(World, new Vector2(-10, 10), new Vector2(-20, 10), 0.1f, 0.5f, 10, 0.1f, true);
}
//Angle joint
{
Body fA = BodyFactory.CreateRectangle(World, 4, 4, 1, new Vector2(-5, 4));
fA.BodyType = BodyType.Dynamic;
fA.Rotation = (float)(Math.PI / 3);
Body fB = BodyFactory.CreateRectangle(World, 4, 4, 1, new Vector2(5, 4));
fB.BodyType = BodyType.Dynamic;
AngleJoint joint = new AngleJoint(fA, fB);
joint.TargetAngle = (float)Math.PI / 2;
World.AddJoint(joint);
}
//Motor joint
{
Body body = BodyFactory.CreateRectangle(World, 4, 1, 2, new Vector2(0, 35));
body.BodyType = BodyType.Dynamic;
body.Friction = 0.6f;
MotorJoint motorJoint = JointFactory.CreateMotorJoint(World, ground, body);
motorJoint.MaxForce = 1000.0f;
motorJoint.MaxTorque = 1000.0f;
motorJoint.LinearOffset = new Vector2(0, 35);
motorJoint.AngularOffset = (float)(Math.PI / 3f);
}
}
public Car()
{
{ // car body
PolygonDef poly1 = new PolygonDef(), poly2 = new PolygonDef();
// bottom half
poly1.VertexCount = 5;
poly1.Vertices[4].Set(-2.2f, -0.74f);
poly1.Vertices[3].Set(-2.2f, 0);
poly1.Vertices[2].Set(1.0f, 0);
poly1.Vertices[1].Set(2.2f, -0.2f);
poly1.Vertices[0].Set(2.2f, -0.74f);
poly1.Filter.GroupIndex = -1;
poly1.Density = 20.0f;
poly1.Friction = 0.68f;
poly1.Filter.GroupIndex = -1;
// top half
poly2.VertexCount = 4;
poly2.Vertices[3].Set(-1.7f, 0);
poly2.Vertices[2].Set(-1.3f, 0.7f);
poly2.Vertices[1].Set(0.5f, 0.74f);
poly2.Vertices[0].Set(1.0f, 0);
poly2.Filter.GroupIndex = -1;
poly2.Density = 5.0f;
poly2.Friction = 0.68f;
poly2.Filter.GroupIndex = -1;
BodyDef bd = new BodyDef();
bd.Position.Set(-35.0f, 2.8f);
_vehicle = _world.CreateBody(bd);
_vehicle.CreateFixture(poly1);
_vehicle.CreateFixture(poly2);
_vehicle.SetMassFromShapes();
}
{ // vehicle wheels
CircleDef circ = new CircleDef();
circ.Density = 40.0f;
circ.Radius = 0.38608f;
circ.Friction = 0.8f;
circ.Filter.GroupIndex = -1;
BodyDef bd = new BodyDef();
bd.AllowSleep = false;
bd.Position.Set(-33.8f, 2.0f);
_rightWheel = _world.CreateBody(bd);
_rightWheel.CreateFixture(circ);
_rightWheel.SetMassFromShapes();
bd.Position.Set(-36.2f, 2.0f);
_leftWheel = _world.CreateBody(bd);
_leftWheel.CreateFixture(circ);
_leftWheel.SetMassFromShapes();
}
{ // join wheels to chassis
Vec2 anchor = new Vec2();
RevoluteJointDef jd = new RevoluteJointDef();
jd.Initialize(_vehicle, _leftWheel, _leftWheel.GetWorldCenter());
jd.CollideConnected = false;
jd.EnableMotor = true;
jd.MaxMotorTorque = 10.0f;
jd.MotorSpeed = 0.0f;
_leftJoint = (RevoluteJoint)_world.CreateJoint(jd);
jd.Initialize(_vehicle, _rightWheel, _rightWheel.GetWorldCenter());
jd.CollideConnected = false;
_rightJoint = (RevoluteJoint)_world.CreateJoint(jd);
}
{ // ground
PolygonDef box = new PolygonDef();
box.SetAsBox(19.5f, 0.5f);
box.Friction = 0.62f;
BodyDef bd = new BodyDef();
bd.Position.Set(-25.0f, 1.0f);
Body ground = _world.CreateBody(bd);
ground.CreateFixture(box);
}
{ // more ground
PolygonDef box = new PolygonDef();
BodyDef bd = new BodyDef();
box.SetAsBox(9.5f, 0.5f, Vec2.Zero, 0.1f * Box2DNet.Common.Settings.Pi);
box.Friction = 0.62f;
bd.Position.Set(27.0f - 30.0f, 3.1f);
Body ground = _world.CreateBody(bd);
ground.CreateFixture(box);
}
{ // more ground
PolygonDef box = new PolygonDef();
BodyDef bd = new BodyDef();
box.SetAsBox(9.5f, 0.5f, Vec2.Zero, -0.1f * Box2DNet.Common.Settings.Pi);
box.Friction = 0.62f;
bd.Position.Set(55.0f - 30.0f, 3.1f);
Body ground = _world.CreateBody(bd);
ground.CreateFixture(box);
}
{ // more ground
PolygonDef box = new PolygonDef();
BodyDef bd = new BodyDef();
box.SetAsBox(9.5f, 0.5f, Vec2.Zero, 0.03f * Box2DNet.Common.Settings.Pi);
box.Friction = 0.62f;
bd.Position.Set(41.0f, 2.0f);
Body ground = _world.CreateBody(bd);
ground.CreateFixture(box);
}
{ // more ground
PolygonDef box = new PolygonDef();
BodyDef bd = new BodyDef();
box.SetAsBox(5.0f, 0.5f, Vec2.Zero, 0.15f * Box2DNet.Common.Settings.Pi);
box.Friction = 0.62f;
bd.Position.Set(50.0f, 4.0f);
Body ground = _world.CreateBody(bd);
ground.CreateFixture(box);
}
{ // more ground
PolygonDef box = new PolygonDef();
BodyDef bd = new BodyDef();
box.SetAsBox(20.0f, 0.5f);
box.Friction = 0.62f;
bd.Position.Set(85.0f, 2.0f);
Body ground = _world.CreateBody(bd);
ground.CreateFixture(box);
}
}
public RevoluteJoint CreateRevoluteJoint(Body body1, Body body2, Vector2 initialAnchorPosition)
{
RevoluteJoint revoluteJoint = new RevoluteJoint(body1, body2, initialAnchorPosition);
return(revoluteJoint);
}
public Ragdoll()
{
#region Ragdoll Entities
//Create the ragdoll's bones.
var pelvis = new Box(Vector3.Zero, .5f, .28f, .33f, 20);
var torsoBottom = new Box(pelvis.Position + new Vector3(0, .3f, 0), .42f, .48f, .3f, 15);
var torsoTop = new Box(torsoBottom.Position + new Vector3(0, .3f, 0), .5f, .38f, .32f, 20);
var neck = new Box(torsoTop.Position + new Vector3(0, .2f, .04f), .19f, .24f, .2f, 5);
var head = new Sphere(neck.Position + new Vector3(0, .22f, -.04f), .19f, 7);
var leftUpperArm = new Box(torsoTop.Position + new Vector3(-.46f, .1f, 0), .52f, .19f, .19f, 6);
var leftForearm = new Box(leftUpperArm.Position + new Vector3(-.5f, 0, 0), .52f, .18f, .18f, 5);
var leftHand = new Box(leftForearm.Position + new Vector3(-.35f, 0, 0), .28f, .13f, .22f, 4);
var rightUpperArm = new Box(torsoTop.Position + new Vector3(.46f, .1f, 0), .52f, .19f, .19f, 6);
var rightForearm = new Box(rightUpperArm.Position + new Vector3(.5f, 0, 0), .52f, .18f, .18f, 5);
var rightHand = new Box(rightForearm.Position + new Vector3(.35f, 0, 0), .28f, .13f, .22f, 4);
var leftThigh = new Box(pelvis.Position + new Vector3(-.15f, -.4f, 0), .23f, .63f, .23f, 10);
var leftShin = new Box(leftThigh.Position + new Vector3(0, -.6f, 0), .21f, .63f, .21f, 7);
var leftFoot = new Box(leftShin.Position + new Vector3(0, -.35f, -.1f), .23f, .15f, .43f, 5);
var rightThigh = new Box(pelvis.Position + new Vector3(.15f, -.4f, 0), .23f, .63f, .23f, 10);
var rightShin = new Box(rightThigh.Position + new Vector3(0, -.6f, 0), .21f, .63f, .21f, 7);
var rightFoot = new Box(rightShin.Position + new Vector3(0, -.35f, -.1f), .23f, .15f, .43f, 5);
#endregion
#region Bone List
//Make a convenient list of all of the bones.
bones.Add(pelvis);
bones.Add(torsoBottom);
bones.Add(torsoTop);
bones.Add(neck);
bones.Add(head);
bones.Add(leftUpperArm);
bones.Add(leftForearm);
bones.Add(leftHand);
bones.Add(rightUpperArm);
bones.Add(rightForearm);
bones.Add(rightHand);
bones.Add(leftThigh);
bones.Add(leftShin);
bones.Add(leftFoot);
bones.Add(rightThigh);
bones.Add(rightShin);
bones.Add(rightFoot);
#endregion
#region Collision Rules
//Prevent adjacent limbs from colliding.
CollisionRules.AddRule(pelvis, torsoBottom, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(torsoBottom, torsoTop, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(torsoTop, neck, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(neck, head, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(head, torsoTop, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(torsoTop, leftUpperArm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(leftUpperArm, leftForearm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(leftForearm, leftHand, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(torsoTop, rightUpperArm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(rightUpperArm, rightForearm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(rightForearm, rightHand, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(pelvis, leftThigh, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(leftThigh, leftShin, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(leftThigh, torsoBottom, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(leftShin, leftFoot, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(pelvis, rightThigh, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(rightThigh, rightShin, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(rightThigh, torsoBottom, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(rightShin, rightFoot, CollisionRule.NoBroadPhase);
#endregion
//Create the constraints between the bones.
#region Pelvis up to Head Constraints
var pelvisToTorsoBottomBallSocketJoint = new BallSocketJoint(pelvis, torsoBottom, pelvis.Position + new Vector3(0, .1f, 0));
var pelvisToTorsoBottomTwistLimit = new TwistLimit(pelvis, torsoBottom, Vector3.Up, Vector3.Up, -MathHelper.Pi / 6, MathHelper.Pi / 6);
var pelvisToTorsoBottomSwingLimit = new SwingLimit(pelvis, torsoBottom, Vector3.Up, Vector3.Up, MathHelper.Pi / 6);
var pelvisToTorsoBottomMotor = new AngularMotor(pelvis, torsoBottom);
pelvisToTorsoBottomMotor.Settings.VelocityMotor.Softness = .05f;
var torsoBottomToTorsoTopBallSocketJoint = new BallSocketJoint(torsoBottom, torsoTop, torsoBottom.Position + new Vector3(0, .25f, 0));
var torsoBottomToTorsoTopSwingLimit = new SwingLimit(torsoBottom, torsoTop, Vector3.Up, Vector3.Up, MathHelper.Pi / 6);
var torsoBottomToTorsoTopTwistLimit = new TwistLimit(torsoBottom, torsoTop, Vector3.Up, Vector3.Up, -MathHelper.Pi / 6, MathHelper.Pi / 6);
var torsoBottomToTorsoTopMotor = new AngularMotor(torsoBottom, torsoTop);
torsoBottomToTorsoTopMotor.Settings.VelocityMotor.Softness = .05f;
var torsoTopToNeckBallSocketJoint = new BallSocketJoint(torsoTop, neck, torsoTop.Position + new Vector3(0, .15f, .05f));
var torsoTopToNeckSwingLimit = new SwingLimit(torsoTop, neck, Vector3.Up, Vector3.Up, MathHelper.Pi / 6);
var torsoTopToNeckTwistLimit = new TwistLimit(torsoTop, neck, Vector3.Up, Vector3.Up, -MathHelper.Pi / 8, MathHelper.Pi / 8);
var torsoTopToNeckMotor = new AngularMotor(torsoTop, neck);
torsoTopToNeckMotor.Settings.VelocityMotor.Softness = .1f;
var neckToHeadBallSocketJoint = new BallSocketJoint(neck, head, neck.Position + new Vector3(0, .1f, .05f));
var neckToHeadTwistLimit = new TwistLimit(neck, head, Vector3.Up, Vector3.Up, -MathHelper.Pi / 8, MathHelper.Pi / 8);
var neckToHeadSwingLimit = new SwingLimit(neck, head, Vector3.Up, Vector3.Up, MathHelper.Pi / 6);
var neckToHeadMotor = new AngularMotor(neck, head);
neckToHeadMotor.Settings.VelocityMotor.Softness = .1f;
#endregion
#region Left Arm
var torsoTopToLeftArmBallSocketJoint = new BallSocketJoint(torsoTop, leftUpperArm, torsoTop.Position + new Vector3(-.3f, .1f, 0));
var torsoTopToLeftArmEllipseLimit = new EllipseSwingLimit(torsoTop, leftUpperArm, Vector3.Left, MathHelper.Pi * .75f, MathHelper.PiOver2);
var torsoTopToLeftArmTwistLimit = new TwistLimit(torsoTop, leftUpperArm, Vector3.Left, Vector3.Left, -MathHelper.PiOver2, MathHelper.PiOver2);
var torsoTopToLeftArmMotor = new AngularMotor(torsoTop, leftUpperArm);
torsoTopToLeftArmMotor.Settings.VelocityMotor.Softness = .2f;
var leftUpperArmToLeftForearmSwivelHingeJoint = new SwivelHingeJoint(leftUpperArm, leftForearm, leftUpperArm.Position + new Vector3(-.28f, 0, 0), Vector3.Up);
leftUpperArmToLeftForearmSwivelHingeJoint.HingeLimit.IsActive = true;
leftUpperArmToLeftForearmSwivelHingeJoint.TwistLimit.IsActive = true;
leftUpperArmToLeftForearmSwivelHingeJoint.TwistLimit.MinimumAngle = -MathHelper.Pi / 8;
leftUpperArmToLeftForearmSwivelHingeJoint.TwistLimit.MaximumAngle = MathHelper.Pi / 8;
leftUpperArmToLeftForearmSwivelHingeJoint.HingeLimit.MinimumAngle = -MathHelper.Pi * .8f;
leftUpperArmToLeftForearmSwivelHingeJoint.HingeLimit.MaximumAngle = 0;
//The SwivelHingeJoint has motors, but they are separately defined for twist/bending.
//The AngularMotor covers all degrees of freedom.
var leftUpperArmToLeftForearmMotor = new AngularMotor(leftUpperArm, leftForearm);
leftUpperArmToLeftForearmMotor.Settings.VelocityMotor.Softness = .3f;
var leftForearmToLeftHandBallSocketJoint = new BallSocketJoint(leftForearm, leftHand, leftForearm.Position + new Vector3(-.2f, 0, 0));
var leftForearmToLeftHandEllipseSwingLimit = new EllipseSwingLimit(leftForearm, leftHand, Vector3.Left, MathHelper.PiOver2, MathHelper.Pi / 6);
var leftForearmToLeftHandTwistLimit = new TwistLimit(leftForearm, leftHand, Vector3.Left, Vector3.Left, -MathHelper.Pi / 6, MathHelper.Pi / 6);
var leftForearmToLeftHandMotor = new AngularMotor(leftForearm, leftHand);
leftForearmToLeftHandMotor.Settings.VelocityMotor.Softness = .4f;
#endregion
#region Right Arm
var torsoTopToRightArmBallSocketJoint = new BallSocketJoint(torsoTop, rightUpperArm, torsoTop.Position + new Vector3(.3f, .1f, 0));
var torsoTopToRightArmEllipseLimit = new EllipseSwingLimit(torsoTop, rightUpperArm, Vector3.Right, MathHelper.Pi * .75f, MathHelper.PiOver2);
var torsoTopToRightArmTwistLimit = new TwistLimit(torsoTop, rightUpperArm, Vector3.Right, Vector3.Right, -MathHelper.PiOver2, MathHelper.PiOver2);
var torsoTopToRightArmMotor = new AngularMotor(torsoTop, rightUpperArm);
torsoTopToRightArmMotor.Settings.VelocityMotor.Softness = .2f;
var rightUpperArmToRightForearmSwivelHingeJoint = new SwivelHingeJoint(rightUpperArm, rightForearm, rightUpperArm.Position + new Vector3(.28f, 0, 0), Vector3.Up);
rightUpperArmToRightForearmSwivelHingeJoint.HingeLimit.IsActive = true;
rightUpperArmToRightForearmSwivelHingeJoint.TwistLimit.IsActive = true;
rightUpperArmToRightForearmSwivelHingeJoint.TwistLimit.MinimumAngle = -MathHelper.Pi / 8;
rightUpperArmToRightForearmSwivelHingeJoint.TwistLimit.MaximumAngle = MathHelper.Pi / 8;
rightUpperArmToRightForearmSwivelHingeJoint.HingeLimit.MinimumAngle = 0;
rightUpperArmToRightForearmSwivelHingeJoint.HingeLimit.MaximumAngle = MathHelper.Pi * .8f;
//The SwivelHingeJoint has motors, but they are separately defined for twist/bending.
//The AngularMotor covers all degrees of freedom.
var rightUpperArmToRightForearmMotor = new AngularMotor(rightUpperArm, rightForearm);
rightUpperArmToRightForearmMotor.Settings.VelocityMotor.Softness = .3f;
var rightForearmToRightHandBallSocketJoint = new BallSocketJoint(rightForearm, rightHand, rightForearm.Position + new Vector3(.2f, 0, 0));
var rightForearmToRightHandEllipseSwingLimit = new EllipseSwingLimit(rightForearm, rightHand, Vector3.Right, MathHelper.PiOver2, MathHelper.Pi / 6);
var rightForearmToRightHandTwistLimit = new TwistLimit(rightForearm, rightHand, Vector3.Right, Vector3.Right, -MathHelper.Pi / 6, MathHelper.Pi / 6);
var rightForearmToRightHandMotor = new AngularMotor(rightForearm, rightHand);
rightForearmToRightHandMotor.Settings.VelocityMotor.Softness = .4f;
#endregion
#region Left Leg
var pelvisToLeftThighBallSocketJoint = new BallSocketJoint(pelvis, leftThigh, pelvis.Position + new Vector3(-.15f, -.1f, 0));
var pelvisToLeftThighEllipseSwingLimit = new EllipseSwingLimit(pelvis, leftThigh, Vector3.Normalize(new Vector3(-.2f, -1, -.6f)), MathHelper.Pi * .7f, MathHelper.PiOver4);
pelvisToLeftThighEllipseSwingLimit.LocalTwistAxisB = Vector3.Down;
var pelvisToLeftThighTwistLimit = new TwistLimit(pelvis, leftThigh, Vector3.Down, Vector3.Down, -MathHelper.Pi / 6, MathHelper.Pi / 6);
var pelvisToLeftThighMotor = new AngularMotor(pelvis, leftThigh);
pelvisToLeftThighMotor.Settings.VelocityMotor.Softness = .1f;
var leftThighToLeftShinRevoluteJoint = new RevoluteJoint(leftThigh, leftShin, leftThigh.Position + new Vector3(0, -.3f, 0), Vector3.Right);
leftThighToLeftShinRevoluteJoint.Limit.IsActive = true;
leftThighToLeftShinRevoluteJoint.Limit.MinimumAngle = -MathHelper.Pi * .8f;
leftThighToLeftShinRevoluteJoint.Limit.MaximumAngle = 0;
leftThighToLeftShinRevoluteJoint.Motor.IsActive = true;
leftThighToLeftShinRevoluteJoint.Motor.Settings.VelocityMotor.Softness = .2f;
var leftShinToLeftFootBallSocketJoint = new BallSocketJoint(leftShin, leftFoot, leftShin.Position + new Vector3(0, -.3f, 0));
var leftShinToLeftFootSwingLimit = new SwingLimit(leftShin, leftFoot, Vector3.Forward, Vector3.Forward, MathHelper.Pi / 8);
var leftShinToLeftFootTwistLimit = new TwistLimit(leftShin, leftFoot, Vector3.Down, Vector3.Forward, -MathHelper.Pi / 8, MathHelper.Pi / 8);
var leftShinToLeftFootMotor = new AngularMotor(leftShin, leftFoot);
leftShinToLeftFootMotor.Settings.VelocityMotor.Softness = .2f;
#endregion
#region Right Leg
var pelvisToRightThighBallSocketJoint = new BallSocketJoint(pelvis, rightThigh, pelvis.Position + new Vector3(.15f, -.1f, 0));
var pelvisToRightThighEllipseSwingLimit = new EllipseSwingLimit(pelvis, rightThigh, Vector3.Normalize(new Vector3(.2f, -1, -.6f)), MathHelper.Pi * .7f, MathHelper.PiOver4);
pelvisToRightThighEllipseSwingLimit.LocalTwistAxisB = Vector3.Down;
var pelvisToRightThighTwistLimit = new TwistLimit(pelvis, rightThigh, Vector3.Down, Vector3.Down, -MathHelper.Pi / 6, MathHelper.Pi / 6);
var pelvisToRightThighMotor = new AngularMotor(pelvis, rightThigh);
pelvisToRightThighMotor.Settings.VelocityMotor.Softness = .1f;
var rightThighToRightShinRevoluteJoint = new RevoluteJoint(rightThigh, rightShin, rightThigh.Position + new Vector3(0, -.3f, 0), Vector3.Right);
rightThighToRightShinRevoluteJoint.Limit.IsActive = true;
rightThighToRightShinRevoluteJoint.Limit.MinimumAngle = -MathHelper.Pi * .8f;
rightThighToRightShinRevoluteJoint.Limit.MaximumAngle = 0;
rightThighToRightShinRevoluteJoint.Motor.IsActive = true;
rightThighToRightShinRevoluteJoint.Motor.Settings.VelocityMotor.Softness = .2f;
var rightShinToRightFootBallSocketJoint = new BallSocketJoint(rightShin, rightFoot, rightShin.Position + new Vector3(0, -.3f, 0));
var rightShinToRightFootSwingLimit = new SwingLimit(rightShin, rightFoot, Vector3.Forward, Vector3.Forward, MathHelper.Pi / 8);
var rightShinToRightFootTwistLimit = new TwistLimit(rightShin, rightFoot, Vector3.Down, Vector3.Forward, -MathHelper.Pi / 8, MathHelper.Pi / 8);
var rightShinToRightFootMotor = new AngularMotor(rightShin, rightFoot);
rightShinToRightFootMotor.Settings.VelocityMotor.Softness = .2f;
#endregion
#region Joint List
//Collect the joints.
joints.Add(pelvisToTorsoBottomBallSocketJoint);
joints.Add(pelvisToTorsoBottomTwistLimit);
joints.Add(pelvisToTorsoBottomSwingLimit);
joints.Add(pelvisToTorsoBottomMotor);
joints.Add(torsoBottomToTorsoTopBallSocketJoint);
joints.Add(torsoBottomToTorsoTopTwistLimit);
joints.Add(torsoBottomToTorsoTopSwingLimit);
joints.Add(torsoBottomToTorsoTopMotor);
joints.Add(torsoTopToNeckBallSocketJoint);
joints.Add(torsoTopToNeckTwistLimit);
joints.Add(torsoTopToNeckSwingLimit);
joints.Add(torsoTopToNeckMotor);
joints.Add(neckToHeadBallSocketJoint);
joints.Add(neckToHeadTwistLimit);
joints.Add(neckToHeadSwingLimit);
joints.Add(neckToHeadMotor);
joints.Add(torsoTopToLeftArmBallSocketJoint);
joints.Add(torsoTopToLeftArmEllipseLimit);
joints.Add(torsoTopToLeftArmTwistLimit);
joints.Add(torsoTopToLeftArmMotor);
joints.Add(leftUpperArmToLeftForearmSwivelHingeJoint);
joints.Add(leftUpperArmToLeftForearmMotor);
joints.Add(leftForearmToLeftHandBallSocketJoint);
joints.Add(leftForearmToLeftHandEllipseSwingLimit);
joints.Add(leftForearmToLeftHandTwistLimit);
joints.Add(leftForearmToLeftHandMotor);
joints.Add(torsoTopToRightArmBallSocketJoint);
joints.Add(torsoTopToRightArmEllipseLimit);
joints.Add(torsoTopToRightArmTwistLimit);
joints.Add(torsoTopToRightArmMotor);
joints.Add(rightUpperArmToRightForearmSwivelHingeJoint);
joints.Add(rightUpperArmToRightForearmMotor);
joints.Add(rightForearmToRightHandBallSocketJoint);
joints.Add(rightForearmToRightHandEllipseSwingLimit);
joints.Add(rightForearmToRightHandTwistLimit);
joints.Add(rightForearmToRightHandMotor);
joints.Add(pelvisToLeftThighBallSocketJoint);
joints.Add(pelvisToLeftThighEllipseSwingLimit);
joints.Add(pelvisToLeftThighTwistLimit);
joints.Add(pelvisToLeftThighMotor);
joints.Add(leftThighToLeftShinRevoluteJoint);
joints.Add(leftShinToLeftFootBallSocketJoint);
joints.Add(leftShinToLeftFootSwingLimit);
joints.Add(leftShinToLeftFootTwistLimit);
joints.Add(leftShinToLeftFootMotor);
joints.Add(pelvisToRightThighBallSocketJoint);
joints.Add(pelvisToRightThighEllipseSwingLimit);
joints.Add(pelvisToRightThighTwistLimit);
joints.Add(pelvisToRightThighMotor);
joints.Add(rightThighToRightShinRevoluteJoint);
joints.Add(rightShinToRightFootBallSocketJoint);
joints.Add(rightShinToRightFootSwingLimit);
joints.Add(rightShinToRightFootTwistLimit);
joints.Add(rightShinToRightFootMotor);
#endregion
}
/// <summary>
/// Add objects to the Box2d world.
/// </summary>
protected override void PopulateWorld()
{
// ==== Define the ground body ====
BodyDef groundBodyDef = new BodyDef();
groundBodyDef.Position.Set(0f, -0.25f);
// Call the body factory which creates the ground box shape.
// The body is also added to the world.
Body groundBody = _world.CreateBody(groundBodyDef);
// Define the ground box shape.
PolygonDef groundShapeDef = new PolygonDef();
// The extents are the half-widths of the box.
groundShapeDef.SetAsBox(_trackLengthHalf + 1f, 0.25f);
groundShapeDef.Friction = _simParams._defaultFriction;
groundShapeDef.Restitution = _simParams._defaultRestitution;
groundShapeDef.Filter.CategoryBits = 0x3;
// Add the ground shape to the ground body.
groundBody.CreateShape(groundShapeDef);
// ==== Define the cart body.
BodyDef cartBodyDef = new BodyDef();
cartBodyDef.Position.Set(0f, 0.15f);
// Create cart body.
_cartBody = _world.CreateBody(cartBodyDef);
PolygonDef cartShapeDef = new PolygonDef();
cartShapeDef.SetAsBox(0.5f, 0.125f);
cartShapeDef.Friction = 0f;
cartShapeDef.Restitution = 0f;
cartShapeDef.Density = 16f;
_cartBody.CreateShape(cartShapeDef);
_cartBody.SetMassFromShapes();
// Fix cart to 'track' (prismatic joint).
PrismaticJointDef cartTrackJointDef = new PrismaticJointDef();
cartTrackJointDef.EnableMotor = true;
cartTrackJointDef.LowerTranslation = -_trackLengthHalf;
cartTrackJointDef.UpperTranslation = _trackLengthHalf;
cartTrackJointDef.EnableLimit = true;
cartTrackJointDef.Initialize(groundBody, _cartBody, new Vec2(0f, 0f), new Vec2(1f, 0f));
_cartTrackJoint = (PrismaticJoint)_world.CreateJoint(cartTrackJointDef);
// ===== Create arm1.
const float poleRadius = 0.025f; // Half the thickness of the pole.
Vec2 arm1PosBase = new Vec2(0f, 0.275f);
Body arm1Body = CreatePole(arm1PosBase, _cartJointInitialAngle, poleRadius, 0f, 0f, 2f, 0x0);
// Join arm1 to cart.
RevoluteJointDef poleJointDef = new RevoluteJointDef();
poleJointDef.CollideConnected = false;
poleJointDef.EnableMotor = false;
poleJointDef.MaxMotorTorque = 0f;
poleJointDef.Initialize(_cartBody, arm1Body, arm1PosBase);
_cartJoint = (RevoluteJoint)_world.CreateJoint(poleJointDef);
// ===== Create arm2.
Vec2 arm2PosBase = CalcPoleEndPos(arm1Body);
Body arm2Body = CreatePole(arm2PosBase, _elbowJointInitialAngle, poleRadius, 0f, 0f, 2f, 0x0);
_arm2Body = arm2Body;
// Join arm2 to arm1.
poleJointDef.CollideConnected = false;
poleJointDef.EnableMotor = false;
poleJointDef.MaxMotorTorque = 0f;
poleJointDef.Initialize(arm1Body, arm2Body, arm2PosBase);
_elbowJoint = (RevoluteJoint)_world.CreateJoint(poleJointDef);
}
private void CreateLeg(World world, float direction, Vector2 wheelAnchor, out Body shoulder, out Body leg)
{
Vector2 p1 = new Vector2(5.4f * direction, -6.1f);
Vector2 p2 = new Vector2(7.2f * direction, -1.2f);
Vector2 p3 = new Vector2(4.3f * direction, -1.9f);
Vector2 p4 = new Vector2(3.1f * direction, 0.8f);
Vector2 p5 = new Vector2(6.0f * direction, 1.5f);
Vector2 p6 = new Vector2(2.5f * direction, 3.7f);
PolygonShape shoulderPolygon;
PolygonShape legPolygon;
Vertices vertices = new Vertices(3);
if (direction > 0f)
{
vertices.Add(p1);
vertices.Add(p2);
vertices.Add(p3);
shoulderPolygon = new PolygonShape(vertices, 1);
vertices[0] = Vector2.Zero;
vertices[1] = p5 - p4;
vertices[2] = p6 - p4;
legPolygon = new PolygonShape(vertices, 2);
}
else
{
vertices.Add(p1);
vertices.Add(p3);
vertices.Add(p2);
shoulderPolygon = new PolygonShape(vertices, 1);
vertices[0] = Vector2.Zero;
vertices[1] = p6 - p4;
vertices[2] = p5 - p4;
legPolygon = new PolygonShape(vertices, 2);
}
leg = world.CreateBody();
leg.BodyType = BodyType.Dynamic;
leg.Position = _position;
leg.AngularDamping = 10f;
shoulder = world.CreateBody();
shoulder.BodyType = BodyType.Dynamic;
shoulder.Position = p4 + _position;
shoulder.AngularDamping = 10f;
Fixture f1 = leg.CreateFixture(shoulderPolygon);
f1.CollisionGroup = -1;
Fixture f2 = shoulder.CreateFixture(legPolygon);
f2.CollisionGroup = -1;
// Using a soft distanceraint can reduce some jitter.
// It also makes the structure seem a bit more fluid by
// acting like a suspension system.
DistanceJoint djd = new DistanceJoint(leg, shoulder, leg.GetLocalPoint(p2 + _position), shoulder.GetLocalPoint(p5 + _position));
djd.DampingRatio = 0.5f;
djd.Frequency = 10f;
world.Add(djd);
_walkerJoints.Add(djd);
DistanceJoint djd2 = new DistanceJoint(leg, shoulder, leg.GetLocalPoint(p3 + _position), shoulder.GetLocalPoint(p4 + _position));
djd2.DampingRatio = 0.5f;
djd2.Frequency = 10f;
world.Add(djd2);
_walkerJoints.Add(djd2);
DistanceJoint djd3 = new DistanceJoint(leg, _wheel, leg.GetLocalPoint(p3 + _position), _wheel.GetLocalPoint(wheelAnchor + _position));
djd3.DampingRatio = 0.5f;
djd3.Frequency = 10f;
world.Add(djd3);
_walkerJoints.Add(djd3);
DistanceJoint djd4 = new DistanceJoint(shoulder, _wheel, shoulder.GetLocalPoint(p6 + _position), _wheel.GetLocalPoint(wheelAnchor + _position));
djd4.DampingRatio = 0.5f;
djd4.Frequency = 10f;
world.Add(djd4);
_walkerJoints.Add(djd4);
Vector2 anchor = p4 - new Vector2(0f, 0.8f);
RevoluteJoint rjd = new RevoluteJoint(shoulder, _chassis, shoulder.GetLocalPoint(_chassis.GetWorldPoint(anchor)), anchor);
world.Add(rjd);
}
protected override void Create()
{
Body ground;
{
var bd = new BodyDef();
ground = World.CreateBody(bd);
var shape = new EdgeShape();
shape.Set(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
var fd = new FixtureDef();
fd.Shape = shape;
//fd.filter.categoryBits = 2;
ground.CreateFixture(fd);
}
{
var shape = new CircleShape();
shape.Radius = 0.5f;
var bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
var rjd = new RevoluteJointDef();
bd.Position.Set(-10.0f, 20.0f);
var body = World.CreateBody(bd);
body.CreateFixture(shape, 5.0f);
var w = 100.0f;
body.SetAngularVelocity(w);
body.SetLinearVelocity(new Vector2(-8.0f * w, 0.0f));
rjd.Initialize(ground, body, new Vector2(-10.0f, 12.0f));
rjd.MotorSpeed = 1.0f * Settings.Pi;
rjd.MaxMotorTorque = 10000.0f;
rjd.EnableMotor = false;
rjd.LowerAngle = -0.25f * Settings.Pi;
rjd.UpperAngle = 0.5f * Settings.Pi;
rjd.EnableLimit = true;
rjd.CollideConnected = true;
m_joint = (RevoluteJoint)World.CreateJoint(rjd);
}
{
var circle_shape = new CircleShape();
circle_shape.Radius = 3.0f;
var circle_bd = new BodyDef();
circle_bd.BodyType = BodyType.DynamicBody;
circle_bd.Position.Set(5.0f, 30.0f);
var fd = new FixtureDef();
fd.Density = 5.0f;
var filter = fd.Filter;
filter.MaskBits = 1;
fd.Filter = filter;
fd.Shape = circle_shape;
m_ball = World.CreateBody(circle_bd);
m_ball.CreateFixture(fd);
var polygon_shape = new PolygonShape();
polygon_shape.SetAsBox(10.0f, 0.2f, new Vector2(-10.0f, 0.0f), 0.0f);
var polygon_bd = new BodyDef();
polygon_bd.Position.Set(20.0f, 10.0f);
polygon_bd.BodyType = BodyType.DynamicBody;
polygon_bd.Bullet = true;
var polygon_body = World.CreateBody(polygon_bd);
polygon_body.CreateFixture(polygon_shape, 2.0f);
var rjd = new RevoluteJointDef();
rjd.Initialize(ground, polygon_body, new Vector2(20.0f, 10.0f));
rjd.LowerAngle = -0.25f * Settings.Pi;
rjd.UpperAngle = 0.0f * Settings.Pi;
rjd.EnableLimit = true;
World.CreateJoint(rjd);
}
// Tests mass computation of a small object far from the origin
{
var bodyDef = new BodyDef();
bodyDef.BodyType = BodyType.DynamicBody;
var body = World.CreateBody(bodyDef);
var polyShape = new PolygonShape();
var verts = new Vector2[3];
verts[0].Set(17.63f, 36.31f);
verts[1].Set(17.52f, 36.69f);
verts[2].Set(17.19f, 36.36f);
polyShape.Set(verts);
var polyFixtureDef = new FixtureDef();
polyFixtureDef.Shape = polyShape;
polyFixtureDef.Density = 1;
body.CreateFixture(polyFixtureDef); //assertion hits inside here
}
}
public override void Init(int nGPU)
{
//Testing scene
/*
* Owner.Space = new Space();
* Owner.Space.ForceUpdater.Gravity = new Vector3(0, -9.81f, 0);
*
* Box ground = new Box(new Vector3(0, -0.55f, 0), 30, 1, 30);
* Owner.Space.Add(ground);
*
* Owner.Space.Add(new Box(new Vector3(0.3f, 4, 0), 1, 1, 1, 1));
* Owner.Space.Add(new Box(new Vector3(0, 8, 0.3f), 1, 1, 1, 1));
* Owner.Space.Add(new Box(new Vector3(-0.3f, 12, 0), 1, 1, 1, 1));
*
* Owner.Space.Add(new Cylinder(new Vector3(0.7f, 15, 0), 2, 1, 1));
*
* Owner.Space.Add(new Cone(new Vector3(-0.5f, 18, 0.3f), 2, 1, 1));
*
* Owner.Space.Add(new Sphere(new Vector3(2, 10, 0), 1, 1));
* */
//Robot arm scene
Owner.Space = new Space();
Owner.Space.ForceUpdater.Gravity = new Vector3(0, -9.81f, 0);
Entity ground = new Box(Vector3.Zero, 30, 1, 30);
Owner.Space.Add(ground);
var armBase = new Cylinder(new Vector3(0, 2, 0), 2.5f, 1, 40);
Owner.Space.Add(armBase);
//The arm base can rotate around the Y axis.
//Rotation is controlled by user input.
groundToBaseJoint = new RevoluteJoint(ground, armBase, Vector3.Zero, Vector3.Up);
groundToBaseJoint.Motor.IsActive = true;
groundToBaseJoint.Motor.Settings.Mode = MotorMode.Servomechanism;
groundToBaseJoint.Motor.Settings.MaximumForce = 3500;
Owner.Space.Add(groundToBaseJoint);
Entity lowerArm = new Box(armBase.Position + new Vector3(0, 2, 0), 1, 3, .5f, 10);
Owner.Space.Add(lowerArm);
shoulder = new RevoluteJoint(armBase, lowerArm, armBase.Position, Vector3.Forward);
shoulder.Motor.IsActive = true;
shoulder.Motor.Settings.Mode = MotorMode.Servomechanism;
shoulder.Motor.Settings.MaximumForce = 2500;
//Don't want it to rotate too far; this keeps the whole contraption off the ground.
shoulder.Limit.IsActive = true;
shoulder.Limit.MinimumAngle = -MathHelper.Pi / 3.0f;
shoulder.Limit.MaximumAngle = MathHelper.Pi / 3.0f;
Owner.Space.Add(shoulder);
//Make the next piece of the arm.
Entity upperArm = new Cylinder(lowerArm.Position + new Vector3(0, 3, 0), 3, .25f, 10);
Owner.Space.Add(upperArm);
//Swivel hinges allow motion around two axes. Imagine a tablet PC's monitor hinge.
elbow = new SwivelHingeJoint(lowerArm, upperArm, lowerArm.Position + new Vector3(0, 1.5f, 0), Vector3.Forward);
elbow.TwistMotor.IsActive = true;
elbow.TwistMotor.Settings.Mode = MotorMode.Servomechanism;
elbow.TwistMotor.Settings.MaximumForce = 1000;
elbow.HingeMotor.IsActive = true;
elbow.HingeMotor.Settings.Mode = MotorMode.Servomechanism;
elbow.HingeMotor.Settings.MaximumForce = 1250;
//Keep it from rotating too much.
elbow.HingeLimit.IsActive = true;
elbow.HingeLimit.MinimumAngle = -MathHelper.PiOver2;
elbow.HingeLimit.MaximumAngle = MathHelper.PiOver2;
Owner.Space.Add(elbow);
//Add a menacing claw at the end.
var lowerPosition = upperArm.Position + new Vector3(-.65f, 1.6f, 0);
CollisionRules clawPart1ARules = new CollisionRules();
var bodies = new List <CompoundChildData>()
{
new CompoundChildData()
{
Entry = new CompoundShapeEntry(new BoxShape(1, .25f, .25f), lowerPosition, 3), CollisionRules = clawPart1ARules
},
new CompoundChildData()
{
Entry = new CompoundShapeEntry(new ConeShape(1, .125f), lowerPosition + new Vector3(-.375f, .4f, 0), 3), Material = new Material(2, 2, 0)
}
};
var claw = new CompoundBody(bodies, 6);
Owner.Space.Add(claw);
clawHingeA = new RevoluteJoint(upperArm, claw, upperArm.Position + new Vector3(0, 1.5f, 0), Vector3.Forward);
clawHingeA.Motor.IsActive = true;
clawHingeA.Motor.Settings.Mode = MotorMode.Servomechanism;
clawHingeA.Motor.Settings.Servo.Goal = -MathHelper.PiOver2;
//Weaken the claw to prevent it from crushing the boxes.
clawHingeA.Motor.Settings.Servo.SpringSettings.Damping /= 100;
clawHingeA.Motor.Settings.Servo.SpringSettings.Stiffness /= 100;
clawHingeA.Limit.IsActive = true;
clawHingeA.Limit.MinimumAngle = -MathHelper.PiOver2;
clawHingeA.Limit.MaximumAngle = -MathHelper.Pi / 6;
Owner.Space.Add(clawHingeA);
//Add one more claw to complete the contraption.
lowerPosition = upperArm.Position + new Vector3(.65f, 1.6f, 0);
CollisionRules clawPart1BRules = new CollisionRules();
bodies = new List <CompoundChildData>()
{
new CompoundChildData()
{
Entry = new CompoundShapeEntry(new BoxShape(1, .25f, .25f), lowerPosition, 3), CollisionRules = clawPart1BRules
},
new CompoundChildData()
{
Entry = new CompoundShapeEntry(new ConeShape(1, .125f), lowerPosition + new Vector3(.375f, .4f, 0), 3), Material = new Material(2, 2, 0)
}
};
claw = new CompoundBody(bodies, 6);
Owner.Space.Add(claw);
clawHingeB = new RevoluteJoint(upperArm, claw, upperArm.Position + new Vector3(0, 1.5f, 0), Vector3.Forward);
clawHingeB.Motor.IsActive = true;
clawHingeB.Motor.Settings.Mode = MotorMode.Servomechanism;
clawHingeB.Motor.Settings.Servo.Goal = MathHelper.PiOver2;
//Weaken the claw to prevent it from crushing the boxes.
clawHingeB.Motor.Settings.Servo.SpringSettings.Damping /= 100;
clawHingeB.Motor.Settings.Servo.SpringSettings.Stiffness /= 100;
clawHingeB.Limit.IsActive = true;
clawHingeB.Limit.MinimumAngle = MathHelper.Pi / 6;
clawHingeB.Limit.MaximumAngle = MathHelper.PiOver2;
Owner.Space.Add(clawHingeB);
//Keep the pieces of the robot from interacting with each other.
//The CollisionRules.AddRule method is just a convenience method that adds items to the 'specific' dictionary.
//Sometimes, it's a little unwieldy to reference the collision rules,
//so the convenience method just takes the owners and hides the ugly syntax.
CollisionRules.AddRule(armBase, lowerArm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(lowerArm, upperArm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(clawPart1ARules, upperArm, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(clawPart1BRules, upperArm, CollisionRule.NoBroadPhase);
//Here's an example without a convenience method. Since they are both direct CollisionRules references, it's pretty clean.
clawPart1BRules.Specific.Add(clawPart1ARules, CollisionRule.NoBroadPhase);
//Put some boxes on the ground to try to pick up.
for (double k = 0; k < Math.PI * 2; k += Math.PI / 6)
{
Box box = new Box(new Vector3((float)Math.Cos(k) * 5.5f, 2, (float)Math.Sin(k) * 5.5f), 1, 1, 1, 10);
box.Tag = Color.LightCoral;
Owner.Space.Add(box);
}
}
public TheoJansen()
{
_offset.Set(0.0f, 8.0f);
_motorSpeed = 2.0f;
_motorOn = true;
var pivot = new Vector2(0.0f, 0.8f);
// Ground
{
var bd = new BodyDef();
var ground = World.CreateBody(bd);
var shape = new EdgeShape();
shape.SetTwoSided(new Vector2(-50.0f, 0.0f), new Vector2(50.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
shape.SetTwoSided(new Vector2(-50.0f, 0.0f), new Vector2(-50.0f, 10.0f));
ground.CreateFixture(shape, 0.0f);
shape.SetTwoSided(new Vector2(50.0f, 0.0f), new Vector2(50.0f, 10.0f));
ground.CreateFixture(shape, 0.0f);
}
// Balls
for (var i = 0; i < 40; ++i)
{
var shape = new CircleShape();
shape.Radius = 0.25f;
var bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
bd.Position.Set(-40.0f + 2.0f * i, 0.5f);
var body = World.CreateBody(bd);
body.CreateFixture(shape, 1.0f);
}
// Chassis
{
var shape = new PolygonShape();
shape.SetAsBox(2.5f, 1.0f);
var sd = new FixtureDef();
sd.Density = 1.0f;
sd.Shape = shape;
sd.Filter.GroupIndex = -1;
var bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
bd.Position = pivot + _offset;
_chassis = World.CreateBody(bd);
_chassis.CreateFixture(sd);
}
{
var shape = new CircleShape();
shape.Radius = 1.6f;
var sd = new FixtureDef();
sd.Density = 1.0f;
sd.Shape = shape;
sd.Filter.GroupIndex = -1;
var bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
bd.Position = pivot + _offset;
_wheel = World.CreateBody(bd);
_wheel.CreateFixture(sd);
}
{
var jd = new RevoluteJointDef();
jd.Initialize(_wheel, _chassis, pivot + _offset);
jd.CollideConnected = false;
jd.MotorSpeed = _motorSpeed;
jd.MaxMotorTorque = 400.0f;
jd.EnableMotor = _motorOn;
_motorJoint = (RevoluteJoint)World.CreateJoint(jd);
}
Vector2 wheelAnchor;
wheelAnchor = pivot + new Vector2(0.0f, -0.8f);
CreateLeg(-1.0f, wheelAnchor);
CreateLeg(1.0f, wheelAnchor);
_wheel.SetTransform(_wheel.GetPosition(), 120.0f * (float)Math.PI / 180.0f);
CreateLeg(-1.0f, wheelAnchor);
CreateLeg(1.0f, wheelAnchor);
_wheel.SetTransform(_wheel.GetPosition(), -120.0f * (float)Math.PI / 180.0f);
CreateLeg(-1.0f, wheelAnchor);
CreateLeg(1.0f, wheelAnchor);
}
private SliderCrankTest()
{
Body ground;
{
ground = BodyFactory.CreateBody(World);
EdgeShape shape = new EdgeShape(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.CreateFixture(shape);
}
{
Body prevBody = ground;
// Define crank.
{
PolygonShape shape = new PolygonShape(2);
shape.SetAsBox(0.5f, 2.0f);
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 7.0f);
body.CreateFixture(shape);
Vector2 anchor = new Vector2(0.0f, -2.0f);
_joint1 = new RevoluteJoint(prevBody, body, prevBody.GetLocalPoint(body.GetWorldPoint(anchor)),
anchor);
_joint1.MotorSpeed = 1.0f * Settings.Pi;
_joint1.MaxMotorTorque = 10000.0f;
_joint1.MotorEnabled = true;
World.AddJoint(_joint1);
prevBody = body;
}
// Define follower.
{
PolygonShape shape = new PolygonShape(2);
shape.SetAsBox(0.5f, 4.0f);
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 13.0f);
body.CreateFixture(shape);
Vector2 anchor = new Vector2(0.0f, -4.0f);
RevoluteJoint rjd3 = new RevoluteJoint(prevBody, body,
prevBody.GetLocalPoint(body.GetWorldPoint(anchor)), anchor);
rjd3.MotorEnabled = false;
World.AddJoint(rjd3);
prevBody = body;
}
// Define piston
{
PolygonShape shape = new PolygonShape(2);
shape.SetAsBox(1.5f, 1.5f);
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 17.0f);
body.CreateFixture(shape);
Vector2 anchor = Vector2.Zero;
RevoluteJoint rjd2 = new RevoluteJoint(prevBody, body,
prevBody.GetLocalPoint(body.GetWorldPoint(anchor)), anchor);
World.AddJoint(rjd2);
_joint2 = new FixedPrismaticJoint(body, new Vector2(0.0f, 17.0f), new Vector2(0.0f, 1.0f));
_joint2.MaxMotorForce = 1000.0f;
_joint2.MotorEnabled = true;
World.AddJoint(_joint2);
}
// Create a payload
{
PolygonShape shape = new PolygonShape(2);
shape.SetAsBox(1.5f, 1.5f);
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 23.0f);
body.CreateFixture(shape);
}
}
}
private BridgeTest()
{
Body ground;
{
ground = new Body(World);
EdgeShape shape = new EdgeShape(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.CreateFixture(shape);
}
{
Vertices box = PolygonTools.CreateRectangle(0.5f, 0.125f);
PolygonShape shape = new PolygonShape(box, 20);
Body prevBody = ground;
for (int i = 0; i < Count; ++i)
{
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(-14.5f + 1.0f * i, 5.0f);
Fixture fixture = body.CreateFixture(shape);
fixture.Friction = 0.2f;
Vector2 anchor = new Vector2(-15f + 1.0f * i, 5.0f);
RevoluteJoint jd = new RevoluteJoint(prevBody, body, anchor, true);
World.AddJoint(jd);
prevBody = body;
}
Vector2 anchor2 = new Vector2(-15.0f + 1.0f * Count, 5.0f);
RevoluteJoint jd2 = new RevoluteJoint(ground, prevBody, anchor2, true);
World.AddJoint(jd2);
}
Vertices vertices = new Vertices(3);
vertices.Add(new Vector2(-0.5f, 0.0f));
vertices.Add(new Vector2(0.5f, 0.0f));
vertices.Add(new Vector2(0.0f, 1.5f));
for (int i = 0; i < 2; ++i)
{
PolygonShape shape = new PolygonShape(vertices, 1);
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(-8.0f + 8.0f * i, 12.0f);
body.CreateFixture(shape);
}
for (int i = 0; i < 3; ++i)
{
CircleShape shape = new CircleShape(0.5f, 1);
Body body = BodyFactory.CreateBody(World);
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(-6.0f + 6.0f * i, 10.0f);
body.CreateFixture(shape);
}
}
void Extend(RevoluteJoint joint, float dt)
{
float extensionSpeed = 2;
joint.Motor.Settings.Servo.Goal = MathHelper.Clamp(joint.Motor.Settings.Servo.Goal + extensionSpeed * dt, joint.Limit.MinimumAngle, joint.Limit.MaximumAngle);
}
private GearsTest()
{
Body ground = World.CreateEdge(new Vector2(50.0f, 0.0f), new Vector2(-50.0f, 0.0f));
{
CircleShape circle1 = new CircleShape(1.0f, 5f);
PolygonShape box = new PolygonShape(5f);
box.Vertices = PolygonTools.CreateRectangle(0.5f, 5.0f);
CircleShape circle2 = new CircleShape(2.0f, 5f);
Body body1 = World.CreateBody(new Vector2(10.0f, 9.0f));
body1.CreateFixture(circle1);
Body body2 = World.CreateBody(new Vector2(10.0f, 8.0f));
body2.BodyType = BodyType.Dynamic;
body2.CreateFixture(box);
Body body3 = World.CreateBody(new Vector2(10.0f, 6.0f));
body3.BodyType = BodyType.Dynamic;
body3.CreateFixture(circle2);
RevoluteJoint joint1 = new RevoluteJoint(body2, body1, body1.Position, true);
World.Add(joint1);
RevoluteJoint joint2 = new RevoluteJoint(body2, body3, body3.Position, true);
World.Add(joint2);
GearJoint joint4 = new GearJoint(body1, body3, joint1, joint2, circle2.Radius / circle1.Radius);
World.Add(joint4);
}
{
CircleShape circle1 = new CircleShape(1.0f, 5.0f);
CircleShape circle2 = new CircleShape(2.0f, 5.0f);
PolygonShape box = new PolygonShape(5f);
box.Vertices = PolygonTools.CreateRectangle(0.5f, 5.0f);
Body body1 = World.CreateBody(new Vector2(-3.0f, 12.0f));
body1.BodyType = BodyType.Dynamic;
body1.CreateFixture(circle1);
_joint1 = new RevoluteJoint(ground, body1, body1.Position, true);
_joint1.ReferenceAngle = body1.Rotation - ground.Rotation;
World.Add(_joint1);
Body body2 = World.CreateBody(new Vector2(0.0f, 12.0f));
body2.BodyType = BodyType.Dynamic;
body2.CreateFixture(circle2);
_joint2 = new RevoluteJoint(ground, body2, body2.Position, true);
World.Add(_joint2);
Body body3 = World.CreateBody(new Vector2(2.5f, 12.0f));
body3.BodyType = BodyType.Dynamic;
body3.CreateFixture(box);
_joint3 = new PrismaticJoint(ground, body3, body3.Position, new Vector2(0.0f, 1.0f), true);
_joint3.LowerLimit = -5.0f;
_joint3.UpperLimit = 5.0f;
_joint3.LimitEnabled = true;
World.Add(_joint3);
_joint4 = new GearJoint(body1, body2, _joint1, _joint2, circle2.Radius / circle1.Radius);
World.Add(_joint4);
_joint5 = new GearJoint(body2, body3, _joint2, _joint3, -1.0f / circle2.Radius);
World.Add(_joint5);
}
}
void BuildLeg(Entity body, RigidTransform legTransform, List <RevoluteJoint> legJoints)
{
//Build the leg in local space.
var upperLeg = new Box(new Vector3(.75f, 0, 0), 1.5f, .6f, .6f, 7);
var lowerLeg = new Box(new Vector3(1.2f, -.75f, 0), .6f, 1.5f, .6f, 7);
//Increase the feetfriction to make walking easier.
lowerLeg.Material.KineticFriction = 3;
lowerLeg.Material.StaticFriction = 3;
Space.Add(upperLeg);
Space.Add(lowerLeg);
//Pull the leg entities into world space.
upperLeg.WorldTransform *= legTransform.Matrix;
lowerLeg.WorldTransform *= legTransform.Matrix;
//Don't let the leg pieces interact with each other.
CollisionRules.AddRule(upperLeg, body, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(upperLeg, lowerLeg, CollisionRule.NoBroadPhase);
//Connect the body to the upper leg.
var bodyToUpper = new RevoluteJoint(body, upperLeg, legTransform.Position, legTransform.OrientationMatrix.Forward);
//Both the motor and limit need to be activated for this leg.
bodyToUpper.Limit.IsActive = true;
//While the angular joint doesn't need any extra configuration, the limit does in order to ensure that its interpretation of angles matches ours.
bodyToUpper.Limit.LocalTestAxis = Vector3.Left;
bodyToUpper.Limit.Basis.SetWorldAxes(legTransform.OrientationMatrix.Forward, legTransform.OrientationMatrix.Left);
bodyToUpper.Limit.MinimumAngle = -MathHelper.PiOver4;
bodyToUpper.Limit.MaximumAngle = MathHelper.PiOver4;
//Similarly, the motor needs configuration.
bodyToUpper.Motor.IsActive = true;
bodyToUpper.Motor.LocalTestAxis = Vector3.Left;
bodyToUpper.Motor.Basis.SetWorldAxes(legTransform.OrientationMatrix.Forward, legTransform.OrientationMatrix.Left);
bodyToUpper.Motor.Settings.Mode = MotorMode.Servomechanism;
//Weaken the spring to prevent it from launching too much.
bodyToUpper.Motor.Settings.Servo.SpringSettings.Stiffness *= .01f;
bodyToUpper.Motor.Settings.Servo.SpringSettings.Damping *= .01f;
Space.Add(bodyToUpper);
//Connect the upper leg to the lower leg.
var upperToLower = new RevoluteJoint(upperLeg, lowerLeg, legTransform.Position - legTransform.OrientationMatrix.Left * 1.2f, legTransform.OrientationMatrix.Forward);
//Both the motor and limit need to be activated for this leg.
upperToLower.Limit.IsActive = true;
//While the angular joint doesn't need any extra configuration, the limit does in order to ensure that its interpretation of angles matches ours.
upperToLower.Limit.LocalTestAxis = Vector3.Down;
upperToLower.Limit.Basis.SetWorldAxes(legTransform.OrientationMatrix.Forward, legTransform.OrientationMatrix.Down);
upperToLower.Limit.MinimumAngle = -MathHelper.PiOver4;
upperToLower.Limit.MaximumAngle = MathHelper.PiOver4;
//Similarly, the motor needs configuration.
upperToLower.Motor.IsActive = true;
upperToLower.Motor.LocalTestAxis = Vector3.Down;
upperToLower.Motor.Basis.SetWorldAxes(legTransform.OrientationMatrix.Forward, legTransform.OrientationMatrix.Down);
upperToLower.Motor.Settings.Mode = MotorMode.Servomechanism;
//Weaken the spring to prevent it from launching too much.
upperToLower.Motor.Settings.Servo.SpringSettings.Stiffness *= .01f;
upperToLower.Motor.Settings.Servo.SpringSettings.Damping *= .01f;
Space.Add(upperToLower);
legJoints.Add(upperToLower);
legJoints.Add(bodyToUpper);
}
private SliderCrankTest()
{
Body ground;
{
ground = World.CreateBody();
EdgeShape shape = new EdgeShape(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.CreateFixture(shape);
}
{
Body prevBody = ground;
// Define crank.
{
PolygonShape shape = new PolygonShape(2);
shape.Vertices = PolygonTools.CreateRectangle(0.5f, 2.0f);
Body body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 7.0f);
body.CreateFixture(shape);
_joint1 = new RevoluteJoint(prevBody, body, new Vector2(0f, 5f), true);
_joint1.MotorSpeed = 1.0f * MathHelper.Pi;
_joint1.MaxMotorTorque = 10000.0f;
_joint1.MotorEnabled = true;
World.Add(_joint1);
prevBody = body;
}
// Define follower.
{
PolygonShape shape = new PolygonShape(2);
shape.Vertices = PolygonTools.CreateRectangle(0.5f, 4.0f);
Body body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 13.0f);
body.CreateFixture(shape);
RevoluteJoint rjd3 = new RevoluteJoint(prevBody, body, new Vector2(0, 9), true);
rjd3.MotorEnabled = false;
World.Add(rjd3);
prevBody = body;
}
// Define piston
{
PolygonShape shape = new PolygonShape(2);
shape.Vertices = PolygonTools.CreateRectangle(1.5f, 1.5f);
Body body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 17.0f);
body.CreateFixture(shape);
RevoluteJoint rjd = new RevoluteJoint(prevBody, body, new Vector2(0, 17), true);
World.Add(rjd);
_joint2 = new PrismaticJoint(ground, body, new Vector2(0.0f, 17.0f), new Vector2(0.0f, 1.0f), true);
_joint2.MaxMotorForce = 1000.0f;
_joint2.MotorEnabled = true;
World.Add(_joint2);
}
// Create a payload
{
PolygonShape shape = new PolygonShape(2);
shape.Vertices = PolygonTools.CreateRectangle(1.5f, 1.5f);
Body body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(0.0f, 23.0f);
body.CreateFixture(shape);
}
}
}
public override void Start()
{
base.Start();
Body 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;
Fixture 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;
Fixture 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);
}
public SliderCrank()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
Body prevBody = ground;
// Define crank.
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.5f, 2.0f);
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
bd.position = new Vector2(0.0f, 7.0f);
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 2.0f);
RevoluteJointDef rjd = new RevoluteJointDef();
rjd.Initialize(prevBody, body, new Vector2(0.0f, 5.0f));
rjd.motorSpeed = 1.0f * (float)Settings.b2_pi;
rjd.maxMotorTorque = 10000.0f;
rjd.enableMotor = true;
_joint1 = (RevoluteJoint)_world.CreateJoint(rjd);
prevBody = body;
}
// Define follower.
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.5f, 4.0f);
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
bd.position = new Vector2(0.0f, 13.0f);
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 2.0f);
RevoluteJointDef rjd3 = new RevoluteJointDef();
rjd3.Initialize(prevBody, body, new Vector2(0.0f, 9.0f));
rjd3.enableMotor = false;
_world.CreateJoint(rjd3);
prevBody = body;
}
// Define piston
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(1.5f, 1.5f);
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
bd.position = new Vector2(0.0f, 17.0f);
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 2.0f);
RevoluteJointDef rjd2 = new RevoluteJointDef();
rjd2.Initialize(prevBody, body, new Vector2(0.0f, 17.0f));
_world.CreateJoint(rjd2);
PrismaticJointDef pjd = new PrismaticJointDef();
pjd.Initialize(ground, body, new Vector2(0.0f, 17.0f), new Vector2(0.0f, 1.0f));
pjd.maxMotorForce = 1000.0f;
pjd.enableMotor = true;
_joint2 = (PrismaticJoint)_world.CreateJoint(pjd);
}
// Create a payload
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(1.5f, 1.5f);
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
bd.position = new Vector2(0.0f, 23.0f);
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 2.0f);
}
}
}
internal static Mechanism Create(XElement element)
{
var modelName = element.Attribute(XName.Get("model")).Value;
var manufacturer = (Manufacturers)Enum.Parse(typeof(Manufacturers), element.Attribute(XName.Get("manufacturer")).Value);
string fullName = $"{element.Name.LocalName}.{manufacturer.ToString()}.{modelName}";
bool movesRobot = false;
var movesRobotAttribute = element.Attribute(XName.Get("movesRobot"));
if (movesRobotAttribute != null) movesRobot = XmlConvert.ToBoolean(movesRobotAttribute.Value);
var meshes = GetMeshes(fullName);
double payload = Convert.ToDouble(element.Attribute(XName.Get("payload")).Value);
var baseMesh = meshes[0].DuplicateMesh();
XElement baseElement = element.Element(XName.Get("Base"));
double x = XmlConvert.ToDouble(baseElement.Attribute(XName.Get("x")).Value);
double y = XmlConvert.ToDouble(baseElement.Attribute(XName.Get("y")).Value);
double z = XmlConvert.ToDouble(baseElement.Attribute(XName.Get("z")).Value);
double q1 = XmlConvert.ToDouble(baseElement.Attribute(XName.Get("q1")).Value);
double q2 = XmlConvert.ToDouble(baseElement.Attribute(XName.Get("q2")).Value);
double q3 = XmlConvert.ToDouble(baseElement.Attribute(XName.Get("q3")).Value);
double q4 = XmlConvert.ToDouble(baseElement.Attribute(XName.Get("q4")).Value);
var basePlane = RobotCellAbb.QuaternionToPlane(x, y, z, q1, q2, q3, q4);
var jointElements = element.Element(XName.Get("Joints")).Descendants().ToArray();
Joint[] joints = new Joint[jointElements.Length];
for (int i = 0; i < jointElements.Length; i++)
{
var jointElement = jointElements[i];
double a = XmlConvert.ToDouble(jointElement.Attribute(XName.Get("a")).Value);
double d = XmlConvert.ToDouble(jointElement.Attribute(XName.Get("d")).Value);
string text = jointElement.Attribute(XName.Get("minrange")).Value;
double minRange = XmlConvert.ToDouble(text);
double maxRange = XmlConvert.ToDouble(jointElement.Attribute(XName.Get("maxrange")).Value);
Interval range = new Interval(minRange, maxRange);
double maxSpeed = XmlConvert.ToDouble(jointElement.Attribute(XName.Get("maxspeed")).Value);
Mesh mesh = meshes[i + 1].DuplicateMesh();
int number = XmlConvert.ToInt32(jointElement.Attribute(XName.Get("number")).Value) - 1;
if (jointElement.Name == "Revolute")
joints[i] = new RevoluteJoint() { Index = i, Number = number, A = a, D = d, Range = range, MaxSpeed = maxSpeed.ToRadians(), Mesh = mesh };
else if (jointElement.Name == "Prismatic")
joints[i] = new PrismaticJoint() { Index = i, Number = number, A = a, D = d, Range = range, MaxSpeed = maxSpeed, Mesh = mesh };
}
switch (element.Name.ToString())
{
case ("RobotArm"):
{
switch (manufacturer)
{
case (Manufacturers.ABB):
return new RobotAbb(modelName, payload, basePlane, baseMesh, joints);
case (Manufacturers.KUKA):
return new RobotKuka(modelName, payload, basePlane, baseMesh, joints);
case (Manufacturers.UR):
return new RobotUR(modelName, payload, basePlane, baseMesh, joints);
default:
return null;
}
}
case ("Positioner"):
return new Positioner(modelName, manufacturer, payload, basePlane, baseMesh, joints, movesRobot);
case ("Track"):
return new Track(modelName, manufacturer, payload, basePlane, baseMesh, joints, movesRobot);
default:
return null;
}
}
public Gears()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, -10.0f);
ground = _world.CreateBody(bd);
PolygonDef sd = new PolygonDef();
sd.SetAsBox(50.0f, 10.0f);
ground.CreateFixture(sd);
}
{
CircleDef circle1 = new CircleDef();
circle1.Radius = 1.0f;
circle1.Density = 5.0f;
CircleDef circle2 = new CircleDef();
circle2.Radius = 2.0f;
circle2.Density = 5.0f;
PolygonDef box = new PolygonDef();
box.SetAsBox(0.5f, 5.0f);
box.Density = 5.0f;
BodyDef bd1 = new BodyDef();
bd1.Position.Set(-3.0f, 12.0f);
Body body1 = _world.CreateBody(bd1);
body1.CreateFixture(circle1);
body1.SetMassFromShapes();
RevoluteJointDef jd1 = new RevoluteJointDef();
jd1.Body1 = ground;
jd1.Body2 = body1;
jd1.LocalAnchor1 = ground.GetLocalPoint(bd1.Position);
jd1.LocalAnchor2 = body1.GetLocalPoint(bd1.Position);
jd1.ReferenceAngle = body1.GetAngle() - ground.GetAngle();
_joint1 = (RevoluteJoint)_world.CreateJoint(jd1);
BodyDef bd2 = new BodyDef();
bd2.Position.Set(0.0f, 12.0f);
Body body2 = _world.CreateBody(bd2);
body2.CreateFixture(circle2);
body2.SetMassFromShapes();
RevoluteJointDef jd2 = new RevoluteJointDef();
jd2.Initialize(ground, body2, bd2.Position);
_joint2 = (RevoluteJoint)_world.CreateJoint(jd2);
BodyDef bd3 = new BodyDef();
bd3.Position.Set(2.5f, 12.0f);
Body body3 = _world.CreateBody(bd3);
body3.CreateFixture(box);
body3.SetMassFromShapes();
PrismaticJointDef jd3 = new PrismaticJointDef();
jd3.Initialize(ground, body3, bd3.Position, new Vec2(0.0f, 1.0f));
jd3.LowerTranslation = -5.0f;
jd3.UpperTranslation = 5.0f;
jd3.EnableLimit = true;
_joint3 = (PrismaticJoint)_world.CreateJoint(jd3);
GearJointDef jd4 = new GearJointDef();
jd4.Body1 = body1;
jd4.Body2 = body2;
jd4.Joint1 = _joint1;
jd4.Joint2 = _joint2;
jd4.Ratio = circle2.Radius / circle1.Radius;
_joint4 = (GearJoint)_world.CreateJoint(jd4);
GearJointDef jd5 = new GearJointDef();
jd5.Body1 = body2;
jd5.Body2 = body3;
jd5.Joint1 = _joint2;
jd5.Joint2 = _joint3;
jd5.Ratio = -1.0f / circle2.Radius;
_joint5 = (GearJoint)_world.CreateJoint(jd5);
}
}
private GearJointTest()
{
Body ground;
{
BodyDef bd = new BodyDef();
ground = BodyFactory.CreateFromDef(World, bd);
EdgeShape shape = new EdgeShape();
shape.SetTwoSided(new Vector2(50.0f, 0.0f), new Vector2(-50.0f, 0.0f));
ground.AddFixture(shape);
}
{
CircleShape circle1 = new CircleShape(5.0f);
circle1.Radius = 1.0f;
PolygonShape box = new PolygonShape(5.0f);
box.SetAsBox(0.5f, 5.0f);
CircleShape circle2 = new CircleShape(5.0f);
circle2.Radius = 2.0f;
BodyDef bd1 = new BodyDef();
bd1.Type = BodyType.Static;
bd1.Position = new Vector2(10.0f, 9.0f);
Body body1 = BodyFactory.CreateFromDef(World, bd1);
body1.AddFixture(circle1);
BodyDef bd2 = new BodyDef();
bd2.Type = BodyType.Dynamic;
bd2.Position = new Vector2(10.0f, 8.0f);
Body body2 = BodyFactory.CreateFromDef(World, bd2);
body2.AddFixture(box);
BodyDef bd3 = new BodyDef();
bd3.Type = BodyType.Dynamic;
bd3.Position = new Vector2(10.0f, 6.0f);
Body body3 = BodyFactory.CreateFromDef(World, bd3);
body3.AddFixture(circle2);
RevoluteJointDef jd1 = new RevoluteJointDef();
jd1.Initialize(body1, body2, bd1.Position);
Joint joint1 = JointFactory.CreateFromDef(World, jd1);
RevoluteJointDef jd2 = new RevoluteJointDef();
jd2.Initialize(body2, body3, bd3.Position);
Joint joint2 = JointFactory.CreateFromDef(World, jd2);
GearJointDef jd4 = new GearJointDef();
jd4.BodyA = body1;
jd4.BodyB = body3;
jd4.JointA = joint1;
jd4.JointB = joint2;
jd4.Ratio = circle2.Radius / circle1.Radius;
JointFactory.CreateFromDef(World, jd4);
}
{
CircleShape circle1 = new CircleShape(5.0f);
circle1.Radius = 1.0f;
CircleShape circle2 = new CircleShape(5.0f);
circle2.Radius = 2.0f;
PolygonShape box = new PolygonShape(5.0f);
box.SetAsBox(0.5f, 5.0f);
BodyDef bd1 = new BodyDef();
bd1.Type = BodyType.Dynamic;
bd1.Position = new Vector2(-3.0f, 12.0f);
Body body1 = BodyFactory.CreateFromDef(World, bd1);
body1.AddFixture(circle1);
RevoluteJointDef jd1 = new RevoluteJointDef();
jd1.BodyA = ground;
jd1.BodyB = body1;
jd1.LocalAnchorA = ground.GetLocalPoint(bd1.Position);
jd1.LocalAnchorB = body1.GetLocalPoint(bd1.Position);
jd1.ReferenceAngle = body1.Rotation - ground.Rotation;
_joint1 = (RevoluteJoint)JointFactory.CreateFromDef(World, jd1);
BodyDef bd2 = new BodyDef();
bd2.Type = BodyType.Dynamic;
bd2.Position = new Vector2(0.0f, 12.0f);
Body body2 = BodyFactory.CreateFromDef(World, bd2);
body2.AddFixture(circle2);
RevoluteJointDef jd2 = new RevoluteJointDef();
jd2.Initialize(ground, body2, bd2.Position);
_joint2 = (RevoluteJoint)JointFactory.CreateFromDef(World, jd2);
BodyDef bd3 = new BodyDef();
bd3.Type = BodyType.Dynamic;
bd3.Position = new Vector2(2.5f, 12.0f);
Body body3 = BodyFactory.CreateFromDef(World, bd3);
body3.AddFixture(box);
PrismaticJointDef jd3 = new PrismaticJointDef();
jd3.Initialize(ground, body3, bd3.Position, new Vector2(0.0f, 1.0f));
jd3.LowerTranslation = -5.0f;
jd3.UpperTranslation = 5.0f;
jd3.EnableLimit = true;
_joint3 = (PrismaticJoint)JointFactory.CreateFromDef(World, jd3);
GearJointDef jd4 = new GearJointDef();
jd4.BodyA = body1;
jd4.BodyB = body2;
jd4.JointA = _joint1;
jd4.JointB = _joint2;
jd4.Ratio = circle2.Radius / circle1.Radius;
_joint4 = (GearJoint)JointFactory.CreateFromDef(World, jd4);
GearJointDef jd5 = new GearJointDef();
jd5.BodyA = body2;
jd5.BodyB = body3;
jd5.JointA = _joint2;
jd5.JointB = _joint3;
jd5.Ratio = -1.0f / circle2.Radius;
_joint5 = (GearJoint)JointFactory.CreateFromDef(World, jd5);
}
}
protected override void Create()
{
// Ground body
Body ground;
{
var bd = new BodyDef();
ground = World.CreateBody(bd);
var vs = new Vector2[5];
vs[0].Set(0.0f, -2.0f);
vs[1].Set(8.0f, 6.0f);
vs[2].Set(8.0f, 20.0f);
vs[3].Set(-8.0f, 20.0f);
vs[4].Set(-8.0f, 6.0f);
var loop = new ChainShape();
loop.CreateLoop(vs);
var fd = new FixtureDef();
fd.Shape = loop;
fd.Density = 0.0f;
ground.CreateFixture(fd);
}
// Flippers
{
var p1 = new Vector2(-2.0f, 0.0f);
var p2 = new Vector2(2.0f, 0.0f);
var bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
bd.Position = p1;
var leftFlipper = World.CreateBody(bd);
bd.Position = p2;
var rightFlipper = World.CreateBody(bd);
var box = new PolygonShape();
box.SetAsBox(1.75f, 0.1f);
var fd = new FixtureDef();
fd.Shape = box;
fd.Density = 1.0f;
leftFlipper.CreateFixture(fd);
rightFlipper.CreateFixture(fd);
var jd = new RevoluteJointDef();
jd.BodyA = ground;
jd.LocalAnchorB.SetZero();
jd.EnableMotor = true;
jd.MaxMotorTorque = 1000.0f;
jd.EnableLimit = true;
jd.MotorSpeed = 0.0f;
jd.LocalAnchorA = p1;
jd.BodyB = leftFlipper;
jd.LowerAngle = -30.0f * Settings.Pi / 180.0f;
jd.UpperAngle = 5.0f * Settings.Pi / 180.0f;
_leftJoint = (RevoluteJoint)World.CreateJoint(jd);
jd.MotorSpeed = 0.0f;
jd.LocalAnchorA = p2;
jd.BodyB = rightFlipper;
jd.LowerAngle = -5.0f * Settings.Pi / 180.0f;
jd.UpperAngle = 30.0f * Settings.Pi / 180.0f;
_rightJoint = (RevoluteJoint)World.CreateJoint(jd);
}
// Circle character
{
var bd = new BodyDef();
bd.Position.Set(1.0f, 15.0f);
bd.BodyType = BodyType.DynamicBody;
bd.Bullet = true;
_ball = World.CreateBody(bd);
var shape = new CircleShape();
shape.Radius = 0.2f;
var fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 1.0f;
_ball.CreateFixture(fd);
}
_button = false;
}
/// <summary>
/// Construct leg interface onto the provided Box2D leg joints.
/// </summary>
public LegInterface(RevoluteJoint hipJoint, RevoluteJoint kneeJoint, Body lowerLegBody, float lowerLegLength)
{
_hipJoint = hipJoint;
_kneeJoint = kneeJoint;
_lowerLegBody = lowerLegBody;
}
public override void LoadContent()
{
base.LoadContent();
World.Gravity = new Vector2(0f, -10f);
HasCursor = false;
EnableCameraControl = true;
_hzFront = 8.5f;
_hzBack = 5.0f;
_zeta = 0.85f;
_maxSpeed = 50.0f;
// terrain
_ground = World.CreateBody();
{
Vertices terrain = new Vertices();
terrain.Add(new Vector2(-20f, 5f));
terrain.Add(new Vector2(-20f, 0f));
terrain.Add(new Vector2(20f, 0f));
terrain.Add(new Vector2(25f, 0.25f));
terrain.Add(new Vector2(30f, 1f));
terrain.Add(new Vector2(35f, 4f));
terrain.Add(new Vector2(40f, 0f));
terrain.Add(new Vector2(45f, 0f));
terrain.Add(new Vector2(50f, -1f));
terrain.Add(new Vector2(55f, -2f));
terrain.Add(new Vector2(60f, -2f));
terrain.Add(new Vector2(65f, -1.25f));
terrain.Add(new Vector2(70f, 0f));
terrain.Add(new Vector2(75f, 0.3f));
terrain.Add(new Vector2(80f, 1.5f));
terrain.Add(new Vector2(85f, 3.5f));
terrain.Add(new Vector2(90f, 0f));
terrain.Add(new Vector2(95f, -0.5f));
terrain.Add(new Vector2(100f, -1f));
terrain.Add(new Vector2(105f, -2f));
terrain.Add(new Vector2(110f, -2.5f));
terrain.Add(new Vector2(115f, -1.3f));
terrain.Add(new Vector2(120f, 0f));
terrain.Add(new Vector2(160f, 0f));
terrain.Add(new Vector2(159f, -10f));
terrain.Add(new Vector2(201f, -10f));
terrain.Add(new Vector2(200f, 0f));
terrain.Add(new Vector2(240f, 0f));
terrain.Add(new Vector2(250f, 5f));
terrain.Add(new Vector2(250f, -10f));
terrain.Add(new Vector2(270f, -10f));
terrain.Add(new Vector2(270f, 0));
terrain.Add(new Vector2(310f, 0));
terrain.Add(new Vector2(310f, 5));
for (int i = 0; i < terrain.Count - 1; ++i)
{
_ground.CreateEdge(terrain[i], terrain[i + 1]);
}
_ground.SetFriction(0.6f);
}
// teeter board
{
_board = World.CreateBody();
_board.BodyType = BodyType.Dynamic;
_board.Position = new Vector2(140.0f, 1.0f);
PolygonShape box = new PolygonShape(1f);
box.Vertices = PolygonTools.CreateRectangle(20.0f / 2f, 0.5f / 2f);
_teeter = new Sprite(ContentWrapper.TextureFromShape(box, "Stripe", ContentWrapper.Gold, ContentWrapper.Black, ContentWrapper.Black, 1f, 24f));
_board.CreateFixture(box);
RevoluteJoint teeterAxis = JointFactory.CreateRevoluteJoint(World, _ground, _board, Vector2.Zero);
teeterAxis.LowerLimit = -8.0f * MathHelper.Pi / 180.0f;
teeterAxis.UpperLimit = 8.0f * MathHelper.Pi / 180.0f;
teeterAxis.LimitEnabled = true;
_board.ApplyAngularImpulse(100.0f);
}
// bridge
{
_bridgeSegments = new List <Body>();
const int segmentCount = 20;
PolygonShape shape = new PolygonShape(1f);
shape.Vertices = PolygonTools.CreateRectangle(1.0f, 0.125f);
_bridge = new Sprite(ContentWrapper.TextureFromShape(shape, ContentWrapper.Gold, ContentWrapper.Black, 24f));
Body prevBody = _ground;
for (int i = 0; i < segmentCount; ++i)
{
Body body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(161f + 2f * i, -0.125f);
Fixture fix = body.CreateFixture(shape);
fix.Friction = 0.6f;
JointFactory.CreateRevoluteJoint(World, prevBody, body, -Vector2.UnitX);
prevBody = body;
_bridgeSegments.Add(body);
}
JointFactory.CreateRevoluteJoint(World, _ground, prevBody, Vector2.UnitX);
}
// boxes
{
_boxes = new List <Body>();
PolygonShape box = new PolygonShape(1f);
box.Vertices = PolygonTools.CreateRectangle(0.5f, 0.5f);
_box = new Sprite(ContentWrapper.TextureFromShape(box, "Square", ContentWrapper.Sky, ContentWrapper.Sunset, ContentWrapper.Black, 1f, 24f));
Body body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(220f, 0.5f);
body.CreateFixture(box);
_boxes.Add(body);
body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(220f, 1.5f);
body.CreateFixture(box);
_boxes.Add(body);
body = World.CreateBody();
body.BodyType = BodyType.Dynamic;
body.Position = new Vector2(220f, 2.5f);
body.CreateFixture(box);
_boxes.Add(body);
}
// car
{
Vertices vertices = new Vertices(8);
vertices.Add(new Vector2(-2.5f, -0.08f));
vertices.Add(new Vector2(-2.375f, 0.46f));
vertices.Add(new Vector2(-0.58f, 0.92f));
vertices.Add(new Vector2(0.46f, 0.92f));
vertices.Add(new Vector2(2.5f, 0.17f));
vertices.Add(new Vector2(2.5f, -0.205f));
vertices.Add(new Vector2(2.3f, -0.33f));
vertices.Add(new Vector2(-2.25f, -0.35f));
PolygonShape chassis = new PolygonShape(vertices, 2f);
_car = World.CreateBody();
_car.BodyType = BodyType.Dynamic;
_car.Position = new Vector2(0.0f, 1.0f);
_car.CreateFixture(chassis);
_wheelBack = World.CreateBody();
_wheelBack.BodyType = BodyType.Dynamic;
_wheelBack.Position = new Vector2(-1.709f, 0.78f);
Fixture fix = _wheelBack.CreateFixture(new CircleShape(0.5f, 0.8f));
fix.Friction = 0.9f;
_wheelFront = World.CreateBody();
_wheelFront.BodyType = BodyType.Dynamic;
_wheelFront.Position = new Vector2(1.54f, 0.8f);
_wheelFront.CreateFixture(new CircleShape(0.5f, 1f));
Vector2 axis = new Vector2(0.0f, 1.2f);
_springBack = new WheelJoint(_car, _wheelBack, _wheelBack.Position, axis, true);
_springBack.MotorSpeed = 0.0f;
_springBack.MaxMotorTorque = 20.0f;
_springBack.MotorEnabled = true;
_springBack.Frequency = _hzBack;
_springBack.DampingRatio = _zeta;
World.Add(_springBack);
_springFront = new WheelJoint(_car, _wheelFront, _wheelFront.Position, axis, true);
_springFront.MotorSpeed = 0.0f;
_springFront.MaxMotorTorque = 10.0f;
_springFront.MotorEnabled = false;
_springFront.Frequency = _hzFront;
_springFront.DampingRatio = _zeta;
World.Add(_springFront);
// GFX
_carBody = new Sprite(ContentWrapper.GetTexture("Car"), ContentWrapper.CalculateOrigin(_car, 24f));
_wheel = new Sprite(ContentWrapper.GetTexture("Wheel"));
}
Camera.MinRotation = -0.05f;
Camera.MaxRotation = 0.05f;
Camera.TrackingBody = _car;
Camera.EnableTracking = true;
}
TheoJansen()
{
_offset = new Vector2(0.0f, 8.0f);
_motorSpeed = 2.0f;
_motorOn = true;
Vector2 pivot = new Vector2(0.0f, 0.8f);
// Ground
{
BodyDef bd = new BodyDef();
Body ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vector2(-50.0f, 0.0f), new Vector2(50.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
shape.SetAsEdge(new Vector2(-50.0f, 0.0f), new Vector2(-50.0f, 10.0f));
ground.CreateFixture(shape, 0.0f);
shape.SetAsEdge(new Vector2(50.0f, 0.0f), new Vector2(50.0f, 10.0f));
ground.CreateFixture(shape, 0.0f);
}
// Balls
for (int i = 0; i < 40; ++i)
{
CircleShape shape = new CircleShape();
shape._radius = 0.25f;
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
bd.position = new Vector2(-40.0f + 2.0f * i, 0.5f);
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 1.0f);
}
// Chassis
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(2.5f, 1.0f);
FixtureDef sd = new FixtureDef();
sd.density = 1.0f;
sd.shape = shape;
sd.filter.groupIndex = -1;
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
bd.position = pivot + _offset;
_chassis = _world.CreateBody(bd);
_chassis.CreateFixture(sd);
}
{
CircleShape shape = new CircleShape();
shape._radius = 1.6f;
FixtureDef sd = new FixtureDef();
sd.density = 1.0f;
sd.shape = shape;
sd.filter.groupIndex = -1;
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
bd.position = pivot + _offset;
_wheel = _world.CreateBody(bd);
_wheel.CreateFixture(sd);
}
{
RevoluteJointDef jd = new RevoluteJointDef();
jd.Initialize(_wheel, _chassis, pivot + _offset);
jd.collideConnected = false;
jd.motorSpeed = _motorSpeed;
jd.maxMotorTorque = 400.0f;
jd.enableMotor = _motorOn;
_motorJoint = (RevoluteJoint)_world.CreateJoint(jd);
}
Vector2 wheelAnchor;
wheelAnchor = pivot + new Vector2(0.0f, -0.8f);
CreateLeg(-1.0f, wheelAnchor);
CreateLeg(1.0f, wheelAnchor);
_wheel.SetTransform(_wheel.GetPosition(), 120.0f * (float)Settings.b2_pi / 180.0f);
CreateLeg(-1.0f, wheelAnchor);
CreateLeg(1.0f, wheelAnchor);
_wheel.SetTransform(_wheel.GetPosition(), -120.0f * (float)Settings.b2_pi / 180.0f);
CreateLeg(-1.0f, wheelAnchor);
CreateLeg(1.0f, wheelAnchor);
}
internal static Joint Create(JointDef def)
{
Joint joint = null;
switch (def.type)
{
case JointType.Distance:
{
joint = new DistanceJoint((DistanceJointDef)def);
}
break;
case JointType.Mouse:
{
joint = new MouseJoint((MouseJointDef)def);
}
break;
case JointType.Prismatic:
{
joint = new PrismaticJoint((PrismaticJointDef)def);
}
break;
case JointType.Revolute:
{
joint = new RevoluteJoint((RevoluteJointDef)def);
}
break;
case JointType.Pulley:
{
joint = new PulleyJoint((PulleyJointDef)def);
}
break;
case JointType.Gear:
{
joint = new GearJoint((GearJointDef)def);
}
break;
case JointType.Line:
{
joint = new LineJoint((LineJointDef)def);
}
break;
default:
Debug.Assert(false);
break;
}
return joint;
}
