public GearJoint(GearJointDef def)
: base(def)
{
JointType type1 = def.Joint1.GetType();
JointType type2 = def.Joint2.GetType();
Box2DXDebug.Assert(type1 == JointType.RevoluteJoint || type1 == JointType.PrismaticJoint);
Box2DXDebug.Assert(type2 == JointType.RevoluteJoint || type2 == JointType.PrismaticJoint);
Box2DXDebug.Assert(def.Joint1.GetBody1().IsStatic());
Box2DXDebug.Assert(def.Joint2.GetBody1().IsStatic());
_revolute1 = null;
_prismatic1 = null;
_revolute2 = null;
_prismatic2 = null;
float coordinate1, coordinate2;
_ground1 = def.Joint1.GetBody1();
_body1 = def.Joint1.GetBody2();
if (type1 == JointType.RevoluteJoint)
{
_revolute1 = (RevoluteJoint)def.Joint1;
_groundAnchor1 = _revolute1._localAnchor1;
_localAnchor1 = _revolute1._localAnchor2;
coordinate1 = _revolute1.JointAngle;
}
else
{
_prismatic1 = (PrismaticJoint)def.Joint1;
_groundAnchor1 = _prismatic1._localAnchor1;
_localAnchor1 = _prismatic1._localAnchor2;
coordinate1 = _prismatic1.JointTranslation;
}
_ground2 = def.Joint2.GetBody1();
_body2 = def.Joint2.GetBody2();
if (type2 == JointType.RevoluteJoint)
{
_revolute2 = (RevoluteJoint)def.Joint2;
_groundAnchor2 = _revolute2._localAnchor1;
_localAnchor2 = _revolute2._localAnchor2;
coordinate2 = _revolute2.JointAngle;
}
else
{
_prismatic2 = (PrismaticJoint)def.Joint2;
_groundAnchor2 = _prismatic2._localAnchor1;
_localAnchor2 = _prismatic2._localAnchor2;
coordinate2 = _prismatic2.JointTranslation;
}
_ratio = def.Ratio;
_constant = coordinate1 + _ratio * coordinate2;
_impulse = 0.0f;
}
internal static Joint Create(JointDef def)
{
Joint result = null;
switch (def.Type)
{
case JointType.RevoluteJoint:
{
result = new RevoluteJoint((RevoluteJointDef)def);
break;
}
case JointType.PrismaticJoint:
{
result = new PrismaticJoint((PrismaticJointDef)def);
break;
}
case JointType.DistanceJoint:
{
result = new DistanceJoint((DistanceJointDef)def);
break;
}
case JointType.PulleyJoint:
{
result = new PulleyJoint((PulleyJointDef)def);
break;
}
case JointType.MouseJoint:
{
result = new MouseJoint((MouseJointDef)def);
break;
}
case JointType.GearJoint:
{
result = new GearJoint((GearJointDef)def);
break;
}
case JointType.LineJoint:
{
result = new LineJoint((LineJointDef)def);
break;
}
default:
{
Box2DXDebug.Assert(false);
break;
}
}
return(result);
}
public GearJoint(GearJointDef def) : base(def)
{
JointType type = def.Joint1.GetType();
JointType type2 = def.Joint2.GetType();
Box2DXDebug.Assert(type == JointType.RevoluteJoint || type == JointType.PrismaticJoint);
Box2DXDebug.Assert(type2 == JointType.RevoluteJoint || type2 == JointType.PrismaticJoint);
Box2DXDebug.Assert(def.Joint1.GetBody1().IsStatic());
Box2DXDebug.Assert(def.Joint2.GetBody1().IsStatic());
this._revolute1 = null;
this._prismatic1 = null;
this._revolute2 = null;
this._prismatic2 = null;
this._ground1 = def.Joint1.GetBody1();
this._body1 = def.Joint1.GetBody2();
float num;
if (type == JointType.RevoluteJoint)
{
this._revolute1 = (RevoluteJoint)def.Joint1;
this._groundAnchor1 = this._revolute1._localAnchor1;
this._localAnchor1 = this._revolute1._localAnchor2;
num = this._revolute1.JointAngle;
}
else
{
this._prismatic1 = (PrismaticJoint)def.Joint1;
this._groundAnchor1 = this._prismatic1._localAnchor1;
this._localAnchor1 = this._prismatic1._localAnchor2;
num = this._prismatic1.JointTranslation;
}
this._ground2 = def.Joint2.GetBody1();
this._body2 = def.Joint2.GetBody2();
float num2;
if (type2 == JointType.RevoluteJoint)
{
this._revolute2 = (RevoluteJoint)def.Joint2;
this._groundAnchor2 = this._revolute2._localAnchor1;
this._localAnchor2 = this._revolute2._localAnchor2;
num2 = this._revolute2.JointAngle;
}
else
{
this._prismatic2 = (PrismaticJoint)def.Joint2;
this._groundAnchor2 = this._prismatic2._localAnchor1;
this._localAnchor2 = this._prismatic2._localAnchor2;
num2 = this._prismatic2.JointTranslation;
}
this._ratio = def.Ratio;
this._constant = num + this._ratio * num2;
this._impulse = 0f;
}
public GearJoint(GearJointDef def)
: base(def)
{
JointType type = def.Joint1.GetType();
JointType type2 = def.Joint2.GetType();
Box2DXDebug.Assert(type == JointType.RevoluteJoint || type == JointType.PrismaticJoint);
Box2DXDebug.Assert(type2 == JointType.RevoluteJoint || type2 == JointType.PrismaticJoint);
Box2DXDebug.Assert(def.Joint1.GetBody1().IsStatic());
Box2DXDebug.Assert(def.Joint2.GetBody1().IsStatic());
this._revolute1 = null;
this._prismatic1 = null;
this._revolute2 = null;
this._prismatic2 = null;
this._ground1 = def.Joint1.GetBody1();
this._body1 = def.Joint1.GetBody2();
float num;
if (type == JointType.RevoluteJoint)
{
this._revolute1 = (RevoluteJoint)def.Joint1;
this._groundAnchor1 = this._revolute1._localAnchor1;
this._localAnchor1 = this._revolute1._localAnchor2;
num = this._revolute1.JointAngle;
}
else
{
this._prismatic1 = (PrismaticJoint)def.Joint1;
this._groundAnchor1 = this._prismatic1._localAnchor1;
this._localAnchor1 = this._prismatic1._localAnchor2;
num = this._prismatic1.JointTranslation;
}
this._ground2 = def.Joint2.GetBody1();
this._body2 = def.Joint2.GetBody2();
float num2;
if (type2 == JointType.RevoluteJoint)
{
this._revolute2 = (RevoluteJoint)def.Joint2;
this._groundAnchor2 = this._revolute2._localAnchor1;
this._localAnchor2 = this._revolute2._localAnchor2;
num2 = this._revolute2.JointAngle;
}
else
{
this._prismatic2 = (PrismaticJoint)def.Joint2;
this._groundAnchor2 = this._prismatic2._localAnchor1;
this._localAnchor2 = this._prismatic2._localAnchor2;
num2 = this._prismatic2.JointTranslation;
}
this._ratio = def.Ratio;
this._constant = num + this._ratio * num2;
this._impulse = 0f;
}
internal static Joint Create(JointDef def)
{
Joint joint = null;
switch (def.Type)
{
case JointType.DistanceJoint:
{
joint = new DistanceJoint((DistanceJointDef)def);
}
break;
case JointType.MouseJoint:
{
joint = new MouseJoint((MouseJointDef)def);
}
break;
case JointType.PrismaticJoint:
{
joint = new PrismaticJoint((PrismaticJointDef)def);
}
break;
case JointType.RevoluteJoint:
{
joint = new RevoluteJoint((RevoluteJointDef)def);
}
break;
case JointType.PulleyJoint:
{
joint = new PulleyJoint((PulleyJointDef)def);
}
break;
case JointType.GearJoint:
{
joint = new GearJoint((GearJointDef)def);
}
break;
default:
Box2DXDebug.Assert(false);
break;
}
return(joint);
}
public Prismatic()
{
Body ground = null;
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(50.0f, 10.0f);
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, -10.0f);
ground = _world.CreateBody(bd);
ground.CreateShape(sd);
}
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(2.0f, 0.5f);
sd.Density = 5.0f;
sd.Friction = 0.05f;
BodyDef bd = new BodyDef();
bd.Position.Set(-10.0f, 10.0f);
bd.Angle = 0.5f * Box2DX.Common.Settings.Pi;
Body body = _world.CreateBody(bd);
body.CreateShape(sd);
body.SetMassFromShapes();
PrismaticJointDef pjd = new PrismaticJointDef();
// Bouncy limit
pjd.Initialize(ground, body, new Vec2(0.0f, 0.0f), new Vec2(1.0f, 0.0f));
// Non-bouncy limit
//pjd.Initialize(ground, body, Vec2(-10.0f, 10.0f), Vec2(1.0f, 0.0f));
pjd.MotorSpeed = 10.0f;
pjd.MaxMotorForce = 1000.0f;
pjd.EnableMotor = true;
pjd.LowerTranslation = 0.0f;
pjd.UpperTranslation = 20.0f;
pjd.EnableLimit = true;
_joint = (PrismaticJoint)_world.CreateJoint(pjd);
}
}
public Prismatic()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0);
}
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(2.0f, 0.5f);
BodyDef bd = new BodyDef();
bd.Position.Set(-10.0f, 10.0f);
bd.Angle = 0.5f * Box2DX.Common.Settings.PI;
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 5.0f);
PrismaticJointDef pjd = new PrismaticJointDef();
// Bouncy limit
pjd.Initialize(ground, body, new Vec2(0.0f, 0.0f), new Vec2(1.0f, 0.0f));
// Non-bouncy limit
//pjd.Initialize(ground, body, b2Vec2(-10.0f, 10.0f), b2Vec2(1.0f, 0.0f));
pjd.MotorSpeed = 10.0f;
pjd.MaxMotorForce = 1000.0f;
pjd.EnableMotor = true;
pjd.LowerTranslation = 0.0f;
pjd.UpperTranslation = 20.0f;
pjd.EnableLimit = true;
_joint = (PrismaticJoint)_world.CreateJoint(pjd);
}
}
/// <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);
}
public Gears()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(50.0f, 0.0f), new Vec2(-50.0f, 0.0f));
ground.CreateFixture(shape, 0);
}
{
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.Position.Set(-3.0f, 12.0f);
Body body1 = _world.CreateBody(bd1);
body1.CreateFixture(circle1, 5.0f);
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, 5.0f);
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, 5.0f);
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);
}
}
internal static Joint Create(JointDef def)
{
Joint result = null;
switch (def.Type)
{
case JointType.RevoluteJoint:
{
result = new RevoluteJoint((RevoluteJointDef)def);
break;
}
case JointType.PrismaticJoint:
{
result = new PrismaticJoint((PrismaticJointDef)def);
break;
}
case JointType.DistanceJoint:
{
result = new DistanceJoint((DistanceJointDef)def);
break;
}
case JointType.PulleyJoint:
{
result = new PulleyJoint((PulleyJointDef)def);
break;
}
case JointType.MouseJoint:
{
result = new MouseJoint((MouseJointDef)def);
break;
}
case JointType.GearJoint:
{
result = new GearJoint((GearJointDef)def);
break;
}
case JointType.LineJoint:
{
result = new LineJoint((LineJointDef)def);
break;
}
default:
{
Box2DXDebug.Assert(false);
break;
}
}
return result;
}
public GearJoint(GearJointDef def)
: base(def)
{
JointType type1 = def.Joint1.GetType();
JointType type2 = def.Joint2.GetType();
Box2DXDebug.Assert(type1 == JointType.RevoluteJoint || type1 == JointType.PrismaticJoint);
Box2DXDebug.Assert(type2 == JointType.RevoluteJoint || type2 == JointType.PrismaticJoint);
Box2DXDebug.Assert(def.Joint1.GetBody1().IsStatic());
Box2DXDebug.Assert(def.Joint2.GetBody1().IsStatic());
_revolute1 = null;
_prismatic1 = null;
_revolute2 = null;
_prismatic2 = null;
float coordinate1, coordinate2;
_ground1 = def.Joint1.GetBody1();
_body1 = def.Joint1.GetBody2();
if (type1 == JointType.RevoluteJoint)
{
_revolute1 = (RevoluteJoint)def.Joint1;
_groundAnchor1 = _revolute1._localAnchor1;
_localAnchor1 = _revolute1._localAnchor2;
coordinate1 = _revolute1.JointAngle;
}
else
{
_prismatic1 = (PrismaticJoint)def.Joint1;
_groundAnchor1 = _prismatic1._localAnchor1;
_localAnchor1 = _prismatic1._localAnchor2;
coordinate1 = _prismatic1.JointTranslation;
}
_ground2 = def.Joint2.GetBody1();
_body2 = def.Joint2.GetBody2();
if (type2 == JointType.RevoluteJoint)
{
_revolute2 = (RevoluteJoint)def.Joint2;
_groundAnchor2 = _revolute2._localAnchor1;
_localAnchor2 = _revolute2._localAnchor2;
coordinate2 = _revolute2.JointAngle;
}
else
{
_prismatic2 = (PrismaticJoint)def.Joint2;
_groundAnchor2 = _prismatic2._localAnchor1;
_localAnchor2 = _prismatic2._localAnchor2;
coordinate2 = _prismatic2.JointTranslation;
}
_ratio = def.Ratio;
_constant = coordinate1 + _ratio * coordinate2;
_impulse = 0.0f;
}
// Main...
public ElasticBody()
{
// Bottom static body
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(50.0f, 2.0f);
sd.Friction = 0.1f;
sd.Restitution = 0.1f;
BodyDef bd = new BodyDef();
bd.Position.Set(-1.0f, -7.5f);
_ground = _world.CreateBody(bd);
_ground.CreateShape(sd);
}
// Upper static body
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(20.0f, 0.50f, new Vec2(0.0f, 0.0f), 0.047f * Box2DX.Common.Settings.Pi);
sd.Friction = 0.01f;
sd.Restitution = 0.001f;
BodyDef bd = new BodyDef();
bd.Position.Set(-20.0f, 93.0f);
Body g = _world.CreateBody(bd);
g.CreateShape(sd);
sd.SetAsBox(15.0f, 0.50f, new Vec2(-15.0f, 12.5f), 0.0f);
g.CreateShape(sd);
sd.SetAsBox(20.0f, 0.5f, new Vec2(0.0f, -25.0f), -0.5f);
g.CreateShape(sd);
}
// Left channel left wall
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(0.7f, 55.0f);
sd.Friction = 0.1f;
sd.Restitution = 0.1f;
BodyDef bd = new BodyDef();
bd.Position.Set(-49.3f, 50.0f);
Body g = _world.CreateBody(bd);
g.CreateShape(sd);
}
// Right wall
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(0.7f, 55.0f);
sd.Friction = 0.1f;
sd.Restitution = 0.1f;
BodyDef bd = new BodyDef();
bd.Position.Set(45.0f, 50.0f);
Body g = _world.CreateBody(bd);
g.CreateShape(sd);
}
// Left channel right upper wall
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(0.5f, 20.0f);
sd.Friction = 0.05f;
sd.Restitution = 0.01f;
BodyDef bd = new BodyDef();
bd.Position.Set(-42.0f, 70.0f);
bd.Angle = -0.03f * Box2DX.Common.Settings.Pi;
Body g = _world.CreateBody(bd);
g.CreateShape(sd);
}
// Left channel right lower wall
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(0.50f, 23.0f);
sd.Friction = 0.05f;
sd.Restitution = 0.01f;
BodyDef bd = new BodyDef();
bd.Position.Set(-44.0f, 27.0f);
Body g = _world.CreateBody(bd);
g.CreateShape(sd);
// Bottom motors
CircleDef cd = new CircleDef();
cd.Radius = 3.0f;
cd.Density = 15.0f;
cd.Friction = 1.0f;
cd.Restitution = 0.2f;
// 1.
bd.Position.Set(-40.0f, 2.5f);
Body body = _world.CreateBody(bd);
body.CreateShape(cd);
body.SetMassFromShapes();
RevoluteJointDef jr = new RevoluteJointDef();
jr.Initialize(g, body, body.GetWorldCenter() + new Vec2(0.0f, 1.0f));
jr.MaxMotorTorque = 30000.0f;
jr.EnableMotor = true;
jr.MotorSpeed = 20.0f;
_world.CreateJoint(jr);
// 1. left down
bd.Position.Set(-46.0f, -2.5f);
cd.Radius = 1.5f; jr.MotorSpeed = -20.0f;
body = _world.CreateBody(bd);
body.CreateShape(cd);
sd.SetAsBox(2.0f, 0.50f);
body.CreateShape(sd);
body.SetMassFromShapes();
jr.Initialize(g, body, body.GetWorldCenter());
_world.CreateJoint(jr);
// 2.
cd.Radius = 3.0f; jr.MotorSpeed = 20.0f;
bd.Position.Set(-32.0f, 2.5f);
body = _world.CreateBody(bd);
body.CreateShape(cd);
body.SetMassFromShapes();
jr.Initialize(g, body, body.GetWorldCenter() + new Vec2(0.0f, 1.0f));
_world.CreateJoint(jr);
// 3.
jr.MotorSpeed = 20.0f;
bd.Position.Set(-24.0f, 1.5f);
body = _world.CreateBody(bd);
body.CreateShape(cd);
body.SetMassFromShapes();
jr.Initialize(g, body, body.GetWorldCenter() + new Vec2(0.0f, 1.0f));
_world.CreateJoint(jr);
// 4.
bd.Position.Set(-16.0f, 0.8f);
body = _world.CreateBody(bd);
body.CreateShape(cd);
body.SetMassFromShapes();
jr.Initialize(g, body, body.GetWorldCenter() + new Vec2(0.0f, 1.0f));
_world.CreateJoint(jr);
// 5.
bd.Position.Set(-8.0f, 0.5f);
body = _world.CreateBody(bd);
body.CreateShape(cd);
body.SetMassFromShapes();
jr.Initialize(g, body, body.GetWorldCenter() + new Vec2(0.0f, 1.0f));
_world.CreateJoint(jr);
// 6.
bd.Position.Set(0.0f, 0.1f);
body = _world.CreateBody(bd);
body.CreateShape(cd);
body.SetMassFromShapes();
jr.Initialize(g, body, body.GetWorldCenter() + new Vec2(0.0f, 1.0f));
_world.CreateJoint(jr);
// 7.
bd.Position.Set(8.0f, -0.5f);
body = _world.CreateBody(bd);
body.CreateShape(cd);
sd.SetAsBox(3.7f, 0.5f);
body.CreateShape(sd);
body.SetMassFromShapes();
jr.Initialize(g, body, body.GetWorldCenter() + new Vec2(0.0f, 1.0f));
_world.CreateJoint(jr);
// 8. right rotator
sd.SetAsBox(5.0f, 0.5f);
sd.Density = 2.0f;
bd.Position.Set(18.0f, 1.0f);
Body rightmotor = _world.CreateBody(bd);
rightmotor.CreateShape(sd);
sd.SetAsBox(4.5f, 0.5f, new Vec2(0.0f, 0.0f), Box2DX.Common.Settings.Pi / 3.0f);
rightmotor.CreateShape(sd);
sd.SetAsBox(4.5f, 0.5f, new Vec2(0.0f, 0.0f), Box2DX.Common.Settings.Pi * 2.0f / 3.0f);
rightmotor.CreateShape(sd);
cd.Radius = 4.2f;
rightmotor.CreateShape(cd);
rightmotor.SetMassFromShapes();
jr.Initialize(g, rightmotor, rightmotor.GetWorldCenter());
jr.MaxMotorTorque = 70000.0f;
jr.MotorSpeed = -4.0f;
_world.CreateJoint(jr);
// 9. left rotator
sd.SetAsBox(8.5f, 0.5f);
sd.Density = 2.0f;
bd.Position.Set(-34.0f, 17.0f);
body = _world.CreateBody(bd);
body.CreateShape(sd);
sd.SetAsBox(8.5f, 0.5f, new Vec2(0.0f, 0.0f), Box2DX.Common.Settings.Pi * .5f);
body.CreateShape(sd);
cd.Radius = 7.0f;
cd.Friction = 0.9f;
body.CreateShape(cd);
body.SetMassFromShapes();
jr.Initialize(g, body, body.GetWorldCenter());
jr.MaxMotorTorque = 100000.0f;
jr.MotorSpeed = -5.0f;
_world.CreateJoint(jr);
// big compressor
sd.SetAsBox(3.0f, 4.0f);
sd.Density = 10.0f;
bd.Position.Set(-16.0f, 17.0f);
Body hammerleft = _world.CreateBody(bd);
hammerleft.CreateShape(sd);
hammerleft.SetMassFromShapes();
DistanceJointDef jd = new DistanceJointDef();
jd.Initialize(body, hammerleft, body.GetWorldCenter() + new Vec2(0.0f, 6.0f), hammerleft.GetWorldCenter());
_world.CreateJoint(jd);
bd.Position.Set(4.0f, 17.0f);
Body hammerright = _world.CreateBody(bd);
hammerright.CreateShape(sd);
hammerright.SetMassFromShapes();
jd.Initialize(body, hammerright, body.GetWorldCenter() - new Vec2(0.0f, 6.0f), hammerright.GetWorldCenter());
_world.CreateJoint(jd);
// pusher
sd.SetAsBox(6.0f, 0.75f);
bd.Position.Set(-21.0f, 9.0f);
Body pusher = _world.CreateBody(bd);
pusher.CreateShape(sd);
sd.SetAsBox(2.0f, 1.5f, new Vec2(-5.0f, 0.0f), 0.0f);
pusher.SetMassFromShapes();
pusher.CreateShape(sd);
jd.Initialize(rightmotor, pusher, rightmotor.GetWorldCenter() + new Vec2(-8.0f, 0.0f),
pusher.GetWorldCenter() + new Vec2(5.0f, 0.0f));
_world.CreateJoint(jd);
}
// Static bodies above motors
{
PolygonDef sd = new PolygonDef();
CircleDef cd = new CircleDef();
sd.SetAsBox(9.0f, 0.5f);
sd.Friction = 0.05f;
sd.Restitution = 0.01f;
BodyDef bd = new BodyDef();
bd.Position.Set(-15.5f, 12.0f);
bd.Angle = 0.0f;
Body g = _world.CreateBody(bd);
g.CreateShape(sd);
sd.SetAsBox(8.0f, 0.5f, new Vec2(23.0f, 0.0f), 0.0f);
g.CreateShape(sd);
// compressor statics
sd.SetAsBox(7.0f, 0.5f, new Vec2(-2.0f, 9.0f), 0.0f);
g.CreateShape(sd);
sd.SetAsBox(9.0f, 0.5f, new Vec2(22.0f, 9.0f), 0.0f);
g.CreateShape(sd);
sd.SetAsBox(19.0f, 0.5f, new Vec2(-9.0f, 15.0f), -0.05f);
g.CreateShape(sd);
sd.SetAsBox(4.7f, 0.5f, new Vec2(15.0f, 11.5f), -0.5f);
g.CreateShape(sd);
// below compressor
sd.SetAsBox(26.0f, 0.3f, new Vec2(17.0f, -4.4f), -0.02f);
g.CreateShape(sd);
cd.Radius = 1.0f; cd.Friction = 1.0f;
cd.LocalPosition = new Vec2(29.0f, -6.0f);
g.CreateShape(cd);
cd.Radius = 0.7f;
cd.LocalPosition = new Vec2(-2.0f, -4.5f);
g.CreateShape(cd);
}
// Elevator
{
BodyDef bd = new BodyDef();
CircleDef cd = new CircleDef();
PolygonDef sd = new PolygonDef();
bd.Position.Set(40.0f, 4.0f);
_elev = _world.CreateBody(bd);
sd.SetAsBox(0.5f, 2.5f, new Vec2(3.0f, -3.0f), 0.0f);
sd.Density = 1.0f;
sd.Friction = 0.01f;
_elev.CreateShape(sd);
sd.SetAsBox(7.0f, 0.5f, new Vec2(-3.5f, -5.5f), 0.0f);
_elev.CreateShape(sd);
sd.SetAsBox(0.5f, 2.5f, new Vec2(-11.0f, -3.5f), 0.0f);
_elev.CreateShape(sd);
_elev.SetMassFromShapes();
PrismaticJointDef jp = new PrismaticJointDef();
jp.Initialize(_ground, _elev, bd.Position, new Vec2(0.0f, 1.0f));
jp.LowerTranslation = 0.0f;
jp.UpperTranslation = 100.0f;
jp.EnableLimit = true;
jp.EnableMotor = true;
jp.MaxMotorForce = 10000.0f;
jp.MotorSpeed = 0.0f;
_joint_elev = (PrismaticJoint)_world.CreateJoint(jp);
// Korb
sd.SetAsBox(2.3f, 0.5f, new Vec2(1.0f, 0.0f), 0.0f);
sd.Density = 0.5f;
bd.Position.Set(29.0f, 6.5f);
Body body = _world.CreateBody(bd);
body.CreateShape(sd);
sd.SetAsBox(2.5f, 0.5f, new Vec2(3.0f, -2.0f), Box2DX.Common.Settings.Pi / 2.0f);
body.CreateShape(sd);
sd.SetAsBox(4.6f, 0.5f, new Vec2(7.8f, -4.0f), 0.0f);
body.CreateShape(sd);
sd.SetAsBox(0.5f, 4.5f, new Vec2(12.0f, 0.0f), 0.0f);
body.CreateShape(sd);
sd.SetAsBox(0.5f, 0.5f, new Vec2(13.0f, 4.0f), 0.0f);
body.CreateShape(sd);
cd.Radius = 0.7f; cd.Density = 1.0f; cd.Friction = 0.01f;
cd.LocalPosition = new Vec2(0.0f, 0.0f);
body.CreateShape(cd);
body.SetMassFromShapes();
RevoluteJointDef jr = new RevoluteJointDef();
jr.Initialize(_elev, body, bd.Position);
jr.EnableLimit = true;
jr.LowerAngle = -0.2f;
jr.UpperAngle = Box2DX.Common.Settings.Pi * 1.1f;
jr.CollideConnected = true;
_world.CreateJoint(jr);
// upper body exit
sd.SetAsBox(14.0f, 0.5f, new Vec2(-3.5f, -10.0f), 0.0f);
bd.Position.Set(17.5f, 96.0f);
body = _world.CreateBody(bd);
body.CreateShape(sd);
}
// "Elastic body" 64 bodies - something like a lin. elastic compound
// connected via dynamic forces (springs)
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(0.55f, 0.55f);
sd.Density = 1.5f;
sd.Friction = 0.01f;
sd.Filter.GroupIndex = -1;
Vec2 startpoint = new Vec2(30.0f, 20.0f);
BodyDef bd = new BodyDef();
bd.IsBullet = false;
bd.AllowSleep = false;
for (int i = 0; i < 8; ++i)
{
for (int j = 0; j < 8; ++j)
{
bd.Position.Set(j * 1.02f, 2.51f + 1.02f * i);
bd.Position += startpoint;
Body body = _world.CreateBody(bd);
bodies[8 * i + j] = body;
body.CreateShape(sd);
body.SetMassFromShapes();
}
}
}
}
public Car(Physics ph,CarParams p,Pose pose)
{
this.p = p;
car_angle0 = Helper.GetRelAngle(new Vec2(1, 0), forwardVec);
body = ph.CreateBox(new Pose { xc = pose.xc, yc = pose.yc }, new Box2DX.Common.Vec2 { X = p.w, Y = p.h }, new BodyBehaviour { isDynamic = true, k = 0.98f * p.mass });
bodyFW1 = ph.CreateBox(new Pose { xc = pose.xc, yc = pose.yc + p.h_base / 2 }, new Box2DX.Common.Vec2 { X = p.w / 10, Y = p.h / 10 }, new BodyBehaviour { isDynamic = true, k = 0.01f * p.mass });
bodyBW1 = ph.CreateBox(new Pose { xc = pose.xc, yc = pose.yc + -p.h_base / 2 }, new Box2DX.Common.Vec2 { X = p.w / 10, Y = p.h / 10 }, new BodyBehaviour { isDynamic = true, k = 0.01f * p.mass });
var jFW1def = new RevoluteJointDef();
jFW1def.Initialize(body, bodyFW1, bodyFW1.GetWorldCenter());
jFW1def.EnableMotor = true;
jFW1def.MaxMotorTorque = 1000;
jFW1def.EnableLimit = true;
jFW1def.LowerAngle = -p.max_steer_angle * Helper.angle_to_rad;
jFW1def.UpperAngle = p.max_steer_angle * Helper.angle_to_rad;
jFW1 = (RevoluteJoint)ph.world.CreateJoint(jFW1def);
var jBW1def = new PrismaticJointDef();
jBW1def.Initialize(body, bodyBW1, bodyBW1.GetWorldCenter(), new Vec2(1, 0));
jBW1def.EnableLimit = true;
jBW1def.UpperTranslation = jBW1def.LowerTranslation = 0;
jBW1 = (PrismaticJoint)ph.world.CreateJoint(jBW1def);
//LidarParams lp = new LidarParams() { dir_deg = 0, d0 = 2.2f, dist = 20, fov_deg = 60, x0 = 0, y0 = 0, num_rays = 20 };
var p1 = new LidarParams();
p1.InitDefault();
p1.d0 = p.h / 2 + 0.2f;
InitLidar(p1);
body.SetAngle(pose.angle_rad);
//rcs = new RCS(this);
}
public MotorsAndLimits()
{
Body ground = null;
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(50.0f, 10.0f);
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, -10.0f);
ground = _world.CreateBody(bd);
ground.CreateShape(sd);
}
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(2.0f, 0.5f);
sd.Density = 5.0f;
sd.Friction = 0.05f;
BodyDef bd = new BodyDef();
RevoluteJointDef rjd = new RevoluteJointDef();
Body body = null;
Body prevBody = ground;
const float y = 8.0f;
bd.Position.Set(3.0f, y);
body = _world.CreateBody(bd);
body.CreateShape(sd);
body.SetMassFromShapes();
rjd.Initialize(prevBody, body, new Vec2(0.0f, y));
rjd.MotorSpeed = 1.0f * Box2DX.Common.Settings.Pi;
rjd.MaxMotorTorque = 10000.0f;
rjd.EnableMotor = true;
_joint1 = (RevoluteJoint)_world.CreateJoint(rjd);
prevBody = body;
bd.Position.Set(9.0f, y);
body = _world.CreateBody(bd);
body.CreateShape(sd);
body.SetMassFromShapes();
rjd.Initialize(prevBody, body, new Vec2(6.0f, y));
rjd.MotorSpeed = 0.5f * Box2DX.Common.Settings.Pi;
rjd.MaxMotorTorque = 2000.0f;
rjd.EnableMotor = true;
rjd.LowerAngle = -0.5f * Box2DX.Common.Settings.Pi;
rjd.UpperAngle = 0.5f * Box2DX.Common.Settings.Pi;
rjd.EnableLimit = true;
_joint2 = (RevoluteJoint)_world.CreateJoint(rjd);
bd.Position.Set(-10.0f, 10.0f);
bd.Angle = 0.5f * Box2DX.Common.Settings.Pi;
body = _world.CreateBody(bd);
body.CreateShape(sd);
body.SetMassFromShapes();
PrismaticJointDef pjd = new PrismaticJointDef();
pjd.Initialize(ground, body, new Vec2(-10.0f, 10.0f), new Vec2(1.0f, 0.0f));
pjd.MotorSpeed = 10.0f;
pjd.MaxMotorForce = 1000.0f;
pjd.EnableMotor = true;
pjd.LowerTranslation = 0.0f;
pjd.UpperTranslation = 20.0f;
pjd.EnableLimit = true;
_joint3 = (PrismaticJoint)_world.CreateJoint(pjd);
}
}
public SliderCrank()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = _world.CreateBody(bd);
PolygonShape shape = new PolygonShape();
shape.SetAsEdge(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0);
}
{
Body prevBody = ground;
// Define crank.
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(0.5f, 2.0f);
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 7.0f);
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 2.0f);
RevoluteJointDef rjd = new RevoluteJointDef();
rjd.Initialize(prevBody, body, new Vec2(0.0f, 5.0f));
rjd.MotorSpeed = 1.0f * Box2DX.Common.Settings.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.Position.Set(0.0f, 13.0f);
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 2.0f);
RevoluteJointDef rjd = new RevoluteJointDef();
rjd.Initialize(prevBody, body, new Vec2(0.0f, 9.0f));
rjd.EnableMotor = false;
_world.CreateJoint(rjd);
prevBody = body;
}
// Define piston
{
PolygonShape shape = new PolygonShape();
shape.SetAsBox(1.5f, 1.5f);
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 17.0f);
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 2.0f);
RevoluteJointDef rjd = new RevoluteJointDef();
rjd.Initialize(prevBody, body, new Vec2(0.0f, 17.0f));
_world.CreateJoint(rjd);
PrismaticJointDef pjd = new PrismaticJointDef();
pjd.Initialize(ground, body, new Vec2(0.0f, 17.0f), new Vec2(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.Position.Set(0.0f, 23.0f);
Body body = _world.CreateBody(bd);
body.CreateFixture(shape, 2.0f);
}
}
}
