// Use this for initialization
protected override IntPtr Init()
{
WheelJointDef jd = new WheelJointDef(other.body, body.body);
jd.Initialize(other.body, body.body, anchor, axis);
jd.enableMotor = enableMotor;
jd.maxMotorTorque = maxMotorTorque;
jd.motorSpeed = motorSpeed;
jd.frequencyHz = frequencyHz;
jd.dampingRatio = dampingRatio;
return(API.CreateWheelJoint(B2DWorld.instance.world, jd));
}
public WheelJointTestBase()
{
Body ground = null;
{
var bd = new BodyDef();
ground = World.CreateBody(bd);
var shape = new EdgeShape();
shape.SetTwoSided(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
EnableLimit = true;
EnableMotor = false;
MotorSpeed = 10.0f;
{
CircleShape shape = new CircleShape();
shape.Radius = 2.0f;
BodyDef bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
bd.Position.Set(0.0f, 10.0f);
bd.AllowSleep = false;
var body = World.CreateBody(bd);
body.CreateFixture(shape, 5.0f);
var mass = body.Mass;
var hertz = 1.0f;
var dampingRatio = 0.7f;
var omega = 2.0f * Settings.Pi * hertz;
var jd = new WheelJointDef();
// Horizontal
jd.Initialize(ground, body, bd.Position, new Vector2(0.0f, 1.0f));
jd.MotorSpeed = MotorSpeed;
jd.MaxMotorTorque = 10000.0f;
jd.EnableMotor = EnableMotor;
jd.Stiffness = mass * omega * omega;
jd.Damping = 2.0f * mass * dampingRatio * omega;
jd.LowerTranslation = -3.0f;
jd.UpperTranslation = 3.0f;
jd.EnableLimit = EnableLimit;
Joint = (WheelJoint)World.CreateJoint(jd);
}
}
private WheelJointTest()
{
Body ground;
{
BodyDef bd = new BodyDef();
ground = BodyFactory.CreateFromDef(World, bd);
EdgeShape shape = new EdgeShape();
shape.SetTwoSided(new Vector2(-40.0f, 0.0f), new Vector2(40.0f, 0.0f));
ground.AddFixture(shape);
}
_enableLimit = true;
_enableMotor = false;
_motorSpeed = 10.0f;
{
CircleShape shape = new CircleShape(5.0f);
shape.Radius = 2.0f;
BodyDef bd = new BodyDef();
bd.Type = BodyType.Dynamic;
bd.Position = new Vector2(0.0f, 10.0f);
bd.AllowSleep = false;
Body body = BodyFactory.CreateFromDef(World, bd);
body.AddFixture(shape);
WheelJointDef jd = new WheelJointDef();
// Horizontal
jd.Initialize(ground, body, bd.Position, new Vector2(0.0f, 1.0f));
jd.MotorSpeed = _motorSpeed;
jd.MaxMotorTorque = 10000.0f;
jd.EnableMotor = _enableMotor;
jd.LowerTranslation = -3.0f;
jd.UpperTranslation = 3.0f;
jd.EnableLimit = _enableLimit;
float hertz = 1.0f;
float dampingRatio = 0.7f;
JointHelper.LinearStiffness(hertz, dampingRatio, ground, body, out float stiffness, out float damping);
jd.Stiffness = stiffness;
jd.Damping = damping;
_joint = (WheelJoint)JointFactory.CreateFromDef(World, jd);
}
}
public WheelJoint(WheelJointDef def) : base(def)
{
_localAnchorA = def.LocalAnchorA;
_localAnchorB = def.LocalAnchorB;
_localXAxisA = def.LocalAxisA;
_localYAxisA = MathUtils.Cross(1.0f, _localXAxisA);
_lowerTranslation = def.LowerTranslation;
_upperTranslation = def.UpperTranslation;
_enableLimit = def.EnableLimit;
_maxMotorTorque = def.MaxMotorTorque;
_motorSpeed = def.MotorSpeed;
_enableMotor = def.EnableMotor;
_stiffness = def.Stiffness;
_damping = def.Damping;
}
/// <summary>
/// Initializes a new instance of the <see cref="WheelJoint" /> class
/// </summary>
/// <param name="def">The def</param>
public WheelJoint(WheelJointDef def) : base(def)
{
localAnchorA = def.LocalAnchorA;
localAnchorB = def.LocalAnchorB;
localXAxisA = def.LocalAxisA;
localYAxisA = MathUtils.Cross(1.0f, localXAxisA);
lowerTranslation = def.LowerTranslation;
upperTranslation = def.UpperTranslation;
enableLimit = def.EnableLimit;
maxMotorTorque = def.MaxMotorTorque;
motorSpeed = def.MotorSpeed;
enableMotor = def.EnableMotor;
stiffness = def.Stiffness;
damping = def.Damping;
}
protected override void Create()
{
_hz = 4.0f;
_zeta = 0.7f;
_speed = 50.0f;
Body ground;
{
var bd = new BodyDef();
ground = World.CreateBody(bd);
var shape = new EdgeShape();
var fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 0.0f;
fd.Friction = 0.6f;
shape.Set(new Vector2(-20.0f, 0.0f), new Vector2(20.0f, 0.0f));
ground.CreateFixture(fd);
float[] hs = { 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, dx = 5.0f;
for (var i = 0; i < 10; ++i)
{
var y2 = hs[i];
shape.Set(new Vector2(x, y1), new Vector2(x + dx, y2));
ground.CreateFixture(fd);
y1 = y2;
x += dx;
}
for (var i = 0; i < 10; ++i)
{
var y2 = hs[i];
shape.Set(new Vector2(x, y1), new Vector2(x + dx, y2));
ground.CreateFixture(fd);
y1 = y2;
x += dx;
}
shape.Set(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f));
ground.CreateFixture(fd);
x += 80.0f;
shape.Set(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f));
ground.CreateFixture(fd);
x += 40.0f;
shape.Set(new Vector2(x, 0.0f), new Vector2(x + 10.0f, 5.0f));
ground.CreateFixture(fd);
x += 20.0f;
shape.Set(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f));
ground.CreateFixture(fd);
x += 40.0f;
shape.Set(new Vector2(x, 0.0f), new Vector2(x, 20.0f));
ground.CreateFixture(fd);
}
// Teeter
{
var bd = new BodyDef();
bd.Position.Set(140.0f, 1.0f);
bd.BodyType = BodyType.DynamicBody;
var body = World.CreateBody(bd);
var box = new PolygonShape();
box.SetAsBox(10.0f, 0.25f);
body.CreateFixture(box, 1.0f);
var jd = new RevoluteJointDef();
jd.Initialize(ground, body, body.GetPosition());
jd.LowerAngle = -8.0f * Settings.Pi / 180.0f;
jd.UpperAngle = 8.0f * Settings.Pi / 180.0f;
jd.EnableLimit = true;
World.CreateJoint(jd);
body.ApplyAngularImpulse(100.0f, true);
}
// Bridge
{
var N = 20;
var shape = new PolygonShape();
shape.SetAsBox(1.0f, 0.125f);
var fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 1.0f;
fd.Friction = 0.6f;
var jd = new RevoluteJointDef();
var prevBody = ground;
for (var i = 0; i < N; ++i)
{
var bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
bd.Position.Set(161.0f + 2.0f * i, -0.125f);
var body = World.CreateBody(bd);
body.CreateFixture(fd);
var anchor = new Vector2(160.0f + 2.0f * i, -0.125f);
jd.Initialize(prevBody, body, anchor);
World.CreateJoint(jd);
prevBody = body;
}
{
var anchor = new Vector2(160.0f + 2.0f * N, -0.125f);
jd.Initialize(prevBody, ground, anchor);
World.CreateJoint(jd);
}
}
// Boxes
{
var box = new PolygonShape();
box.SetAsBox(0.5f, 0.5f);
Body body = null;
var bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
bd.Position.Set(230.0f, 0.5f);
body = World.CreateBody(bd);
body.CreateFixture(box, 0.5f);
bd.Position.Set(230.0f, 1.5f);
body = World.CreateBody(bd);
body.CreateFixture(box, 0.5f);
bd.Position.Set(230.0f, 2.5f);
body = World.CreateBody(bd);
body.CreateFixture(box, 0.5f);
bd.Position.Set(230.0f, 3.5f);
body = World.CreateBody(bd);
body.CreateFixture(box, 0.5f);
bd.Position.Set(230.0f, 4.5f);
body = World.CreateBody(bd);
body.CreateFixture(box, 0.5f);
}
// Car
{
var chassis = new PolygonShape();
var vertices = new Vector2[8];
vertices[0].Set(-1.5f, -0.5f);
vertices[1].Set(1.5f, -0.5f);
vertices[2].Set(1.5f, 0.0f);
vertices[3].Set(0.0f, 0.9f);
vertices[4].Set(-1.15f, 0.9f);
vertices[5].Set(-1.5f, 0.2f);
chassis.Set(vertices);
var circle = new CircleShape();
circle.Radius = 0.4f;
var bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
bd.Position.Set(0.0f, 1.0f);
_car = World.CreateBody(bd);
_car.CreateFixture(chassis, 1.0f);
var fd = new FixtureDef();
fd.Shape = circle;
fd.Density = 1.0f;
fd.Friction = 0.9f;
bd.Position.Set(-1.0f, 0.35f);
_wheel1 = World.CreateBody(bd);
_wheel1.CreateFixture(fd);
bd.Position.Set(1.0f, 0.4f);
_wheel2 = World.CreateBody(bd);
_wheel2.CreateFixture(fd);
var jd = new WheelJointDef();
var axis = new Vector2(0.0f, 1.0f);
jd.Initialize(_car, _wheel1, _wheel1.GetPosition(), axis);
jd.MotorSpeed = 0.0f;
jd.MaxMotorTorque = 20.0f;
jd.EnableMotor = true;
jd.FrequencyHz = _hz;
jd.DampingRatio = _zeta;
_spring1 = (WheelJoint)World.CreateJoint(jd);
jd.Initialize(_car, _wheel2, _wheel2.GetPosition(), axis);
jd.MotorSpeed = 0.0f;
jd.MaxMotorTorque = 10.0f;
jd.EnableMotor = false;
jd.FrequencyHz = _hz;
jd.DampingRatio = _zeta;
_spring2 = (WheelJoint)World.CreateJoint(jd);
}
}
public Car()
{
m_hz = 4.0f;
m_zeta = 0.7f;
m_speed = 50.0f;
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = m_world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 0.0f;
fd.friction = 0.6f;
shape.Set(new Vec2(-20.0f, 0.0f), new Vec2(20.0f, 0.0f));
ground.CreateFixture(fd);
float[] hs = new float[] { 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, dx = 5.0f;
for (int i = 0; i < 10; ++i)
{
float y2 = hs[i];
shape.Set(new Vec2(x, y1), new Vec2(x + dx, y2));
ground.CreateFixture(fd);
y1 = y2;
x += dx;
}
for (int i = 0; i < 10; ++i)
{
float y2 = hs[i];
shape.Set(new Vec2(x, y1), new Vec2(x + dx, y2));
ground.CreateFixture(fd);
y1 = y2;
x += dx;
}
shape.Set(new Vec2(x, 0.0f), new Vec2(x + 40.0f, 0.0f));
ground.CreateFixture(fd);
x += 80.0f;
shape.Set(new Vec2(x, 0.0f), new Vec2(x + 40.0f, 0.0f));
ground.CreateFixture(fd);
x += 40.0f;
shape.Set(new Vec2(x, 0.0f), new Vec2(x + 10.0f, 5.0f));
ground.CreateFixture(fd);
x += 20.0f;
shape.Set(new Vec2(x, 0.0f), new Vec2(x + 40.0f, 0.0f));
ground.CreateFixture(fd);
x += 40.0f;
shape.Set(new Vec2(x, 0.0f), new Vec2(x, 20.0f));
ground.CreateFixture(fd);
}
// Teeter
{
BodyDef bd = new BodyDef();
bd.Position.Set(140.0f, 1.0f);
bd.type = BodyType._dynamicBody;
Body body = m_world.CreateBody(bd);
PolygonShape box = new PolygonShape();
box.SetAsBox(10.0f, 0.25f);
box.Density = 1;
body.CreateFixture(box);
RevoluteJointDef jd = new RevoluteJointDef();
jd.Initialize(ground, body, body.GetPosition());
jd.lowerAngle = -8.0f * (float)Math.PI / 180.0f;
jd.upperAngle = 8.0f * (float)Math.PI / 180.0f;
jd.enableLimit = true;
m_world.CreateJoint(jd);
body.ApplyAngularImpulse(100.0f, true);
}
// Bridge
{
int N = 20;
PolygonShape shape = new PolygonShape();
shape.SetAsBox(1.0f, 0.125f);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.Density = 1.0f;
fd.friction = 0.6f;
RevoluteJointDef jd = new RevoluteJointDef();
Body prevBody = ground;
for (int i = 0; i < N; ++i)
{
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(161.0f + 2.0f * i, -0.125f);
Body body = m_world.CreateBody(bd);
body.CreateFixture(fd);
Vec2 anchor = new Vec2(160.0f + 2.0f * i, -0.125f);
jd.Initialize(prevBody, body, anchor);
m_world.CreateJoint(jd);
prevBody = body;
}
Vec2 anchor2 = new Vec2(160.0f + 2.0f * N, -0.125f);
jd.Initialize(prevBody, ground, anchor2);
m_world.CreateJoint(jd);
}
// Boxes
{
PolygonShape box = new PolygonShape();
box.SetAsBox(0.5f, 0.5f);
box.Density = 0.5f;
Body body = null;
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(230.0f, 0.5f);
body = m_world.CreateBody(bd);
body.CreateFixture(box);
bd.Position.Set(230.0f, 1.5f);
body = m_world.CreateBody(bd);
body.CreateFixture(box);
bd.Position.Set(230.0f, 2.5f);
body = m_world.CreateBody(bd);
body.CreateFixture(box);
bd.Position.Set(230.0f, 3.5f);
body = m_world.CreateBody(bd);
body.CreateFixture(box);
bd.Position.Set(230.0f, 4.5f);
body = m_world.CreateBody(bd);
body.CreateFixture(box);
}
// Car
{
PolygonShape chassis = new PolygonShape();
Vec2[] vertices = new Vec2[8];
vertices[0].Set(-1.5f, -0.5f);
vertices[1].Set(1.5f, -0.5f);
vertices[2].Set(1.5f, 0.0f);
vertices[3].Set(0.0f, 0.9f);
vertices[4].Set(-1.15f, 0.9f);
vertices[5].Set(-1.5f, 0.2f);
chassis.Set(vertices, 6);
CircleShape circle = new CircleShape();
circle.m_radius = 0.4f;
BodyDef bd = new BodyDef();
bd.type = BodyType._dynamicBody;
bd.Position.Set(0.0f, 1.0f);
m_car = m_world.CreateBody(bd);
m_car.CreateFixture(chassis);
FixtureDef fd = new FixtureDef();
fd.shape = circle;
fd.Density = 1.0f;
fd.friction = 0.9f;
bd.Position.Set(-1.0f, 0.35f);
m_wheel1 = m_world.CreateBody(bd);
m_wheel1.CreateFixture(fd);
bd.Position.Set(1.0f, 0.4f);
m_wheel2 = m_world.CreateBody(bd);
m_wheel2.CreateFixture(fd);
WheelJointDef jd = new WheelJointDef();
Vec2 axis = new Vec2(0.0f, 1.0f);
jd.Initialize(m_car, m_wheel1, m_wheel1.GetPosition(), axis);
jd.motorSpeed = 0.0f;
jd.maxMotorTorque = 20.0f;
jd.enableMotor = true;
jd.frequencyHz = m_hz;
jd.dampingRatio = m_zeta;
m_spring1 = (WheelJoint)m_world.CreateJoint(jd);
jd.Initialize(m_car, m_wheel2, m_wheel2.GetPosition(), axis);
jd.motorSpeed = 0.0f;
jd.maxMotorTorque = 10.0f;
jd.enableMotor = false;
jd.frequencyHz = m_hz;
jd.dampingRatio = m_zeta;
m_spring2 = (WheelJoint)m_world.CreateJoint(jd);
}
}
// Use this for initialization
protected override void Init()
{
m_hz = 4.0f;
m_zeta = 0.7f;
m_speed = 50.0f;
IntPtr ground = IntPtr.Zero;
{
ground = API.CreateBody(world, new Vector2(0, 0), 0, BodyType.STATIC_BODY);
ShapeDef sp = new ShapeDef(0);
sp.friction = 0.6f;
API.AddEdgeShape(ground, new Vector2(-20, 0), new Vector2(20, 0), sp);
float[] hs = new float[] { 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, dx = 5.0f;
for (int i = 0; i < 10; ++i)
{
float y2 = hs[i];
API.AddEdgeShape(ground, new Vector2(x, y1), new Vector2(x + dx, y2), sp);
y1 = y2;
x += dx;
}
for (int i = 0; i < 10; ++i)
{
float y2 = hs[i];
API.AddEdgeShape(ground, new Vector2(x, y1), new Vector2(x + dx, y2), sp);
y1 = y2;
x += dx;
}
API.AddEdgeShape(ground, new Vector2(x, 0), new Vector2(x + 40, 0), sp);
x += 80.0f;
API.AddEdgeShape(ground, new Vector2(x, 0), new Vector2(x + 40, 0), sp);
x += 40.0f;
API.AddEdgeShape(ground, new Vector2(x, 0), new Vector2(x + 10, 5), sp);
x += 20.0f;
API.AddEdgeShape(ground, new Vector2(x, 0), new Vector2(x + 40, 0), sp);
x += 40.0f;
API.AddEdgeShape(ground, new Vector2(x, 0), new Vector2(x, 20), sp);
}
// Teeter
{
IntPtr body = API.CreateBody(world, new Vector2(140, 1.0f), 0, BodyType.DYNAMIC_BODY);
API.AddBoxShape(body, 10, 0.25f, Vector2.zero, 0, new ShapeDef(1.0f));
Vector2 archor;
API.GetPosition(body, out archor);
RevoluteJointDef jd = new RevoluteJointDef();
jd.Initialize(ground, body, archor);
jd.lowerAngle = -8.0f * Mathf.Deg2Rad;
jd.upperAngle = 8.0f * Mathf.Deg2Rad;
jd.enableLimit = true;
API.CreateRevoluteJoint(world, jd);
API.ApplyAngularImpulse(body, 100.0f);
}
// Bridge
{
int N = 20;
ShapeDef sp = new ShapeDef(1.0f);
sp.friction = 0.6f;
Vector2 anchor;
RevoluteJointDef jd = new RevoluteJointDef();
IntPtr prevBody = ground;
for (int i = 0; i < N; ++i)
{
var body = API.CreateBody(world, new Vector2(161.0f + 2.0f * i, -0.125f), 0, BodyType.DYNAMIC_BODY);
API.AddBoxShape(body, 1.0f, 0.125f, Vector2.zero, 0, sp);
anchor = new Vector2(160.0f + 2.0f * i, -0.125f);
jd.Initialize(prevBody, body, anchor);
API.CreateRevoluteJoint(world, jd);
prevBody = body;
}
anchor = new Vector2(160.0f + 2.0f * N, -0.125f);
jd.Initialize(prevBody, ground, anchor);
API.CreateRevoluteJoint(world, jd);
}
// Boxes
{
var body = API.CreateBody(world, new Vector2(230.0f, 0.5f), 0, BodyType.DYNAMIC_BODY);
API.AddBoxShape(body, 0.5f, 0.5f, Vector2.zero, 0, new ShapeDef(0.5f));
body = API.CreateBody(world, new Vector2(230.0f, 1.5f), 0, BodyType.DYNAMIC_BODY);
API.AddBoxShape(body, 0.5f, 0.5f, Vector2.zero, 0, new ShapeDef(0.5f));
body = API.CreateBody(world, new Vector2(230.0f, 2.5f), 0, BodyType.DYNAMIC_BODY);
API.AddBoxShape(body, 0.5f, 0.5f, Vector2.zero, 0, new ShapeDef(0.5f));
body = API.CreateBody(world, new Vector2(230.0f, 3.5f), 0, BodyType.DYNAMIC_BODY);
API.AddBoxShape(body, 0.5f, 0.5f, Vector2.zero, 0, new ShapeDef(0.5f));
body = API.CreateBody(world, new Vector2(230.0f, 4.5f), 0, BodyType.DYNAMIC_BODY);
API.AddBoxShape(body, 0.5f, 0.5f, Vector2.zero, 0, new ShapeDef(0.5f));
}
// Car
{
ShapeDef sp = new ShapeDef(1.0f);
Vector2[] vertices = new Vector2[6];
vertices[0] = new Vector2(-1.5f, -0.5f);
vertices[1] = new Vector2(1.5f, -0.5f);
vertices[2] = new Vector2(1.5f, 0.0f);
vertices[3] = new Vector2(0.0f, 0.9f);
vertices[4] = new Vector2(-1.15f, 0.9f);
vertices[5] = new Vector2(-1.5f, 0.2f);
m_car = API.CreateBody(world, new Vector2(0, 1.0f), 0, BodyType.DYNAMIC_BODY);
API.AddPolygonShape(m_car, vertices, vertices.Length, sp);
sp.friction = 0.9f;
m_wheel1 = API.CreateBody(world, new Vector2(-1.0f, 0.35f), 0, BodyType.DYNAMIC_BODY);
API.AddCircleShape(m_wheel1, 0.4f, sp);
m_wheel2 = API.CreateBody(world, new Vector2(1.0f, 0.4f), 0, BodyType.DYNAMIC_BODY);
API.AddCircleShape(m_wheel2, 0.4f, sp);
WheelJointDef jd = new WheelJointDef();
Vector2 axis = new Vector2(0.0f, 1.0f);
Vector2 pos1, pos2;
API.GetPosition(m_wheel1, out pos1);
API.GetPosition(m_wheel2, out pos2);
jd.Initialize(m_car, m_wheel1, pos1, axis);
jd.motorSpeed = 0.0f;
jd.maxMotorTorque = 20.0f;
jd.enableMotor = true;
jd.frequencyHz = m_hz;
jd.dampingRatio = m_zeta;
m_spring1 = API.CreateWheelJoint(world, jd);
jd.Initialize(m_car, m_wheel2, pos2, axis);
jd.motorSpeed = 0.0f;
jd.maxMotorTorque = 10.0f;
jd.enableMotor = false;
jd.frequencyHz = m_hz;
jd.dampingRatio = m_zeta;
m_spring2 = API.CreateWheelJoint(world, jd);
}
}
private CarTest()
{
_speed = 50.0f;
Body ground;
{
BodyDef bd = new BodyDef();
ground = BodyFactory.CreateFromDef(World, bd);
EdgeShape shape = new EdgeShape(new Vector2(-20.0f, 0.0f), new Vector2(20.0f, 0.0f));
FixtureDef fd = new FixtureDef();
fd.Shape = shape;
fd.Friction = 0.6f;
ground.AddFixture(fd);
float[] hs = { 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, dx = 5.0f;
for (int i = 0; i < 10; ++i)
{
float y2 = hs[i];
shape.SetTwoSided(new Vector2(x, y1), new Vector2(x + dx, y2));
ground.AddFixture(fd);
y1 = y2;
x += dx;
}
for (int i = 0; i < 10; ++i)
{
float y2 = hs[i];
shape.SetTwoSided(new Vector2(x, y1), new Vector2(x + dx, y2));
ground.AddFixture(fd);
y1 = y2;
x += dx;
}
shape.SetTwoSided(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f));
ground.AddFixture(fd);
x += 80.0f;
shape.SetTwoSided(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f));
ground.AddFixture(fd);
x += 40.0f;
shape.SetTwoSided(new Vector2(x, 0.0f), new Vector2(x + 10.0f, 5.0f));
ground.AddFixture(fd);
x += 20.0f;
shape.SetTwoSided(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f));
ground.AddFixture(fd);
x += 40.0f;
shape.SetTwoSided(new Vector2(x, 0.0f), new Vector2(x, 20.0f));
ground.AddFixture(fd);
}
// Teeter
{
BodyDef bd = new BodyDef();
bd.Position = new Vector2(140.0f, 1.0f);
bd.Type = BodyType.Dynamic;
Body body = BodyFactory.CreateFromDef(World, bd);
PolygonShape box = new PolygonShape(1.0f);
box.SetAsBox(10.0f, 0.25f);
body.AddFixture(box);
RevoluteJointDef jd = new RevoluteJointDef();
jd.Initialize(ground, body, body.Position);
jd.LowerAngle = -8.0f * MathConstants.Pi / 180.0f;
jd.UpperAngle = 8.0f * MathConstants.Pi / 180.0f;
jd.EnableLimit = true;
JointFactory.CreateFromDef(World, jd);
body.ApplyAngularImpulse(100.0f);
}
//Bridge
{
int N = 20;
PolygonShape shape = new PolygonShape(1.0f);
shape.SetAsBox(1.0f, 0.125f);
FixtureDef fd = new FixtureDef();
fd.Shape = shape;
fd.Friction = 0.6f;
RevoluteJointDef jd = new RevoluteJointDef();
Body prevBody = ground;
for (int i = 0; i < N; ++i)
{
BodyDef bd = new BodyDef();
bd.Type = BodyType.Dynamic;
bd.Position = new Vector2(161.0f + 2.0f * i, -0.125f);
Body body = BodyFactory.CreateFromDef(World, bd);
body.AddFixture(fd);
Vector2 anchor = new Vector2(160.0f + 2.0f * i, -0.125f);
jd.Initialize(prevBody, body, anchor);
JointFactory.CreateFromDef(World, jd);
prevBody = body;
}
Vector2 anchor2 = new Vector2(160.0f + 2.0f * N, -0.125f);
jd.Initialize(prevBody, ground, anchor2);
JointFactory.CreateFromDef(World, jd);
}
// Boxes
{
PolygonShape box = new PolygonShape(0.5f);
box.SetAsBox(0.5f, 0.5f);
Body body;
BodyDef bd = new BodyDef();
bd.Type = BodyType.Dynamic;
bd.Position = new Vector2(230.0f, 0.5f);
body = BodyFactory.CreateFromDef(World, bd);
body.AddFixture(box);
bd.Position = new Vector2(230.0f, 1.5f);
body = BodyFactory.CreateFromDef(World, bd);
body.AddFixture(box);
bd.Position = new Vector2(230.0f, 2.5f);
body = BodyFactory.CreateFromDef(World, bd);
body.AddFixture(box);
bd.Position = new Vector2(230.0f, 3.5f);
body = BodyFactory.CreateFromDef(World, bd);
body.AddFixture(box);
bd.Position = new Vector2(230.0f, 4.5f);
body = BodyFactory.CreateFromDef(World, bd);
body.AddFixture(box);
}
// Car
{
PolygonShape chassis = new PolygonShape(1.0f);
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));
chassis.Vertices = vertices;
CircleShape circle = new CircleShape(0.4f, 1.0f);
BodyDef bd = new BodyDef();
bd.Type = BodyType.Dynamic;
bd.Position = new Vector2(0.0f, 1.0f);
_car = BodyFactory.CreateFromDef(World, bd);
_car.AddFixture(chassis);
FixtureDef fd = new FixtureDef();
fd.Shape = circle;
fd.Friction = 0.9f;
bd.Position = new Vector2(-1.0f, 0.35f);
_wheel1 = BodyFactory.CreateFromDef(World, bd);
_wheel1.AddFixture(fd);
bd.Position = new Vector2(1.0f, 0.4f);
_wheel2 = BodyFactory.CreateFromDef(World, bd);
_wheel2.AddFixture(fd);
WheelJointDef jd = new WheelJointDef();
Vector2 axis = new Vector2(0.0f, 1.0f);
float mass1 = _wheel1.Mass;
float mass2 = _wheel2.Mass;
float hertz = 4.0f;
float dampingRatio = 0.7f;
float omega = 2.0f * MathConstants.Pi * hertz;
jd.Initialize(_car, _wheel1, _wheel1.Position, axis);
jd.MotorSpeed = 0.0f;
jd.MaxMotorTorque = 20.0f;
jd.EnableMotor = true;
jd.Stiffness = mass1 * omega * omega;
jd.Damping = 2.0f * mass1 * dampingRatio * omega;
jd.LowerTranslation = -0.25f;
jd.UpperTranslation = 0.25f;
jd.EnableLimit = true;
_spring1 = (WheelJoint)JointFactory.CreateFromDef(World, jd);
jd.Initialize(_car, _wheel2, _wheel2.Position, axis);
jd.MotorSpeed = 0.0f;
jd.MaxMotorTorque = 10.0f;
jd.EnableMotor = false;
jd.Stiffness = mass2 * omega * omega;
jd.Damping = 2.0f * mass2 * dampingRatio * omega;
jd.LowerTranslation = -0.25f;
jd.UpperTranslation = 0.25f;
jd.EnableLimit = true;
_spring2 = (WheelJoint)JointFactory.CreateFromDef(World, jd);
}
}
public static World CreateWorld(out Body[] bodies, out Joint[] joints)
{
// m_speed = 50.0f;
bodies = new Body[9];
joints = new Joint[3];
var world = new World();
Body ground;
{
BodyDef bd = new BodyDef();
ground = world.CreateBody(bd);
EdgeShape shape = new EdgeShape();
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.density = 0.0f;
fd.friction = 0.6f;
shape.SetTwoSided(new b2Vec2(-20.0f, 0.0f), new b2Vec2(20.0f, 0.0f));
ground.CreateFixture(fd);
float[] hs = { 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, dx = 5.0f;
for (int32 i = 0; i < 10; ++i)
{
float y2 = hs[i];
shape.SetTwoSided(new b2Vec2(x, y1), new b2Vec2(x + dx, y2));
ground.CreateFixture(fd);
y1 = y2;
x += dx;
}
for (int32 i = 0; i < 10; ++i)
{
float y2 = hs[i];
shape.SetTwoSided(new b2Vec2(x, y1), new b2Vec2(x + dx, y2));
ground.CreateFixture(fd);
y1 = y2;
x += dx;
}
shape.SetTwoSided(new b2Vec2(x, 0.0f), new b2Vec2(x + 40.0f, 0.0f));
ground.CreateFixture(fd);
x += 80.0f;
shape.SetTwoSided(new b2Vec2(x, 0.0f), new b2Vec2(x + 40.0f, 0.0f));
ground.CreateFixture(fd);
x += 40.0f;
shape.SetTwoSided(new b2Vec2(x, 0.0f), new b2Vec2(x + 10.0f, 5.0f));
ground.CreateFixture(fd);
x += 20.0f;
shape.SetTwoSided(new b2Vec2(x, 0.0f), new b2Vec2(x + 40.0f, 0.0f));
ground.CreateFixture(fd);
x += 40.0f;
shape.SetTwoSided(new b2Vec2(x, 0.0f), new b2Vec2(x, 20.0f));
ground.CreateFixture(fd);
}
// Teeter
{
BodyDef bd = new BodyDef();
bd.position = new b2Vec2(140.0f, 1.0f);
bd.type = BodyType.Dynamic;
Body body = world.CreateBody(bd);
bodies[0] = body;
PolygonShape box = new PolygonShape();
box.SetAsBox(10.0f, 0.25f);
body.CreateFixture(box, 1.0f);
RevoluteJointDef jd = new RevoluteJointDef();
jd.Initialize(ground, body, body.GetPosition());
jd.lowerAngle = -8.0f * MathF.PI / 180.0f;
jd.upperAngle = 8.0f * MathF.PI / 180.0f;
jd.enableLimit = true;
Joint j = world.CreateJoint(jd);
joints[2] = j;
body.ApplyAngularImpulse(100.0f, true);
}
// Bridge
{
int32 N = 20;
PolygonShape shape = new PolygonShape();
shape.SetAsBox(1.0f, 0.125f);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.density = 1.0f;
fd.friction = 0.6f;
RevoluteJointDef jd = new RevoluteJointDef();
Body prevBody = ground;
for (int32 i = 0; i < N; ++i)
{
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
bd.position = new Vector2(161.0f + 2.0f * i, -0.125f);
Body body = world.CreateBody(bd);
body.CreateFixture(fd);
b2Vec2 anchor = new b2Vec2(160.0f + 2.0f * i, -0.125f);
jd.Initialize(prevBody, body, anchor);
world.CreateJoint(jd);
prevBody = body;
}
{
b2Vec2 anchor = new b2Vec2(160.0f + 2.0f * N, -0.125f);
jd.Initialize(prevBody, ground, anchor);
world.CreateJoint(jd);
}
}
// Boxes
{
PolygonShape box = new PolygonShape();
box.SetAsBox(0.5f, 0.5f);
Body body = null;
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
bd.position = new Vector2(230.0f, 0.5f);
body = world.CreateBody(bd);
bodies[1] = body;
body.CreateFixture(box, 0.5f);
bd.position = new Vector2(230.0f, 1.5f);
body = world.CreateBody(bd);
bodies[2] = body;
body.CreateFixture(box, 0.5f);
bd.position = new Vector2(230.0f, 2.5f);
body = world.CreateBody(bd);
bodies[3] = body;
body.CreateFixture(box, 0.5f);
bd.position = new Vector2(230.0f, 3.5f);
body = world.CreateBody(bd);
bodies[4] = body;
body.CreateFixture(box, 0.5f);
bd.position = new Vector2(230.0f, 4.5f);
body = world.CreateBody(bd);
bodies[5] = body;
body.CreateFixture(box, 0.5f);
}
// Car
Body m_car;
Body m_wheel1;
Body m_wheel2;
WheelJoint m_spring1;
WheelJoint m_spring2;
{
PolygonShape chassis = new PolygonShape();
b2Vec2[] vertices = new b2Vec2[6];
vertices[0] = new Vector2(-1.5f, -0.5f);
vertices[1] = new Vector2(1.5f, -0.5f);
vertices[2] = new Vector2(1.5f, 0.0f);
vertices[3] = new Vector2(0.0f, 0.9f);
vertices[4] = new Vector2(-1.15f, 0.9f);
vertices[5] = new Vector2(-1.5f, 0.2f);
chassis.Set(vertices);
CircleShape circle = new CircleShape();
circle.Radius = 0.4f;
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
bd.position = new Vector2(0.0f, 1.0f);
m_car = world.CreateBody(bd);
m_car.CreateFixture(chassis, 1.0f);
FixtureDef fd = new FixtureDef();
fd.shape = circle;
fd.density = 1.0f;
fd.friction = 0.9f;
bd.position = new Vector2(-1.0f, 0.35f);
m_wheel1 = world.CreateBody(bd);
m_wheel1.CreateFixture(fd);
bd.position = new Vector2(1.0f, 0.4f);
m_wheel2 = world.CreateBody(bd);
m_wheel2.CreateFixture(fd);
WheelJointDef jd = new WheelJointDef();
b2Vec2 axis = new Vector2(0.0f, 1.0f);
float mass1 = m_wheel1.GetMass();
float mass2 = m_wheel2.GetMass();
float hertz = 4.0f;
float dampingRatio = 0.7f;
float omega = 2.0f * MathF.PI * hertz;
jd.Initialize(m_car, m_wheel1, m_wheel1.GetPosition(), axis);
jd.motorSpeed = 0.0f;
jd.maxMotorTorque = 20.0f;
jd.enableMotor = true;
jd.stiffness = mass1 * omega * omega;
jd.damping = 2.0f * mass1 * dampingRatio * omega;
jd.lowerTranslation = -0.25f;
jd.upperTranslation = 0.25f;
jd.enableLimit = true;
m_spring1 = (WheelJoint)world.CreateJoint(jd);
jd.Initialize(m_car, m_wheel2, m_wheel2.GetPosition(), axis);
jd.motorSpeed = 0.0f;
jd.maxMotorTorque = 10.0f;
jd.enableMotor = false;
jd.stiffness = mass2 * omega * omega;
jd.damping = 2.0f * mass2 * dampingRatio * omega;
jd.lowerTranslation = -0.25f;
jd.upperTranslation = 0.25f;
jd.enableLimit = true;
m_spring2 = (WheelJoint)world.CreateJoint(jd);
}
joints[0] = m_spring1;
joints[1] = m_spring2;
bodies[6] = m_wheel1;
bodies[7] = m_wheel2;
bodies[8] = m_car;
m_spring1.SetMotorSpeed(-40);
return(world);
}
