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 Revolute()
{
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);
}
{
CircleDef sd = new CircleDef();
sd.Radius = 0.5f;
sd.Density = 5.0f;
BodyDef bd = new BodyDef();
RevoluteJointDef rjd = new RevoluteJointDef();
bd.Position.Set(0.0f, 20.0f);
Body body = _world.CreateBody(bd);
body.CreateShape(sd);
body.SetMassFromShapes();
float w = 100.0f;
body.SetAngularVelocity(w);
body.SetLinearVelocity(new Vec2(-8.0f * w, 0.0f));
rjd.Initialize(ground, body, new Vec2(0.0f, 12.0f));
rjd.MotorSpeed = 1.0f * Box2DX.Common.Settings.Pi;
rjd.MaxMotorTorque = 10000.0f;
rjd.EnableMotor = false;
rjd.LowerAngle = -0.25f * Box2DX.Common.Settings.Pi;
rjd.UpperAngle = 0.5f * Box2DX.Common.Settings.Pi;
rjd.EnableLimit = true;
rjd.CollideConnected = true;
_joint = (RevoluteJoint)_world.CreateJoint(rjd);
}
}
/// <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 TheoJansen()
{
_offset.Set(0.0f, 8.0f);
_motorSpeed = 2.0f;
_motorOn = true;
Vec2 pivot = new Vec2(0.0f, 0.8f);
// Ground
{
BodyDef bd = new BodyDef();
Body 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);
shape.SetAsEdge(new Vec2(-50.0f, 0.0f), new Vec2(-50.0f, 10.0f));
ground.CreateFixture(shape, 0);
shape.SetAsEdge(new Vec2(50.0f, 0.0f), new Vec2(50.0f, 10.0f));
ground.CreateFixture(shape, 0);
}
// Balls
for (int i = 0; i < 40; ++i)
{
CircleShape shape = new CircleShape();
shape._radius = 0.25f;
BodyDef bd = new BodyDef();
bd.Position.Set(-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.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.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);
}
Vec2 wheelAnchor;
wheelAnchor = pivot + new Vec2(0.0f, -0.8f);
CreateLeg(-1.0f, wheelAnchor);
CreateLeg(1.0f, wheelAnchor);
_wheel.SetTransform(_wheel.GetPosition(), 120.0f * Box2DX.Common.Settings.PI / 180.0f);
CreateLeg(-1.0f, wheelAnchor);
CreateLeg(1.0f, wheelAnchor);
_wheel.SetTransform(_wheel.GetPosition(), -120.0f * Box2DX.Common.Settings.PI / 180.0f);
CreateLeg(-1.0f, wheelAnchor);
CreateLeg(1.0f, wheelAnchor);
}
protected override void CreatePhysics(World world, float positionX, float positionY)
{
var frontWheelJointDef = new RevoluteJointDef();
frontWheelJointDef.Initialize(CarBody.Body, FrontWheel.Body, FrontWheel.Body.GetWorldCenter());
frontWheelJoint = (RevoluteJoint)world.CreateJoint(frontWheelJointDef);
var backWheelJointDef = new RevoluteJointDef();
backWheelJointDef.Initialize(CarBody.Body, BackWheel.Body, BackWheel.Body.GetWorldCenter());
backWheelJoint = (RevoluteJoint)world.CreateJoint(backWheelJointDef);
var poleJointDef = new RevoluteJointDef();
var anchor = this.Pole.Body.GetWorldCenter();
anchor.Y -= Pole.Height ;
poleJointDef.CollideConnected = false;
poleJointDef.EnableLimit = true;
poleJointDef.LowerAngle = Helper.DegreesToRad(-90);
poleJointDef.UpperAngle = Helper.DegreesToRad(90);
poleJointDef.Initialize(CarBody.Body, Pole.Body, anchor);
poleJoint = (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;
}
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.CreateShape(poly1);
_vehicle.CreateShape(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.CreateShape(circ);
_rightWheel.SetMassFromShapes();
bd.Position.Set(-36.2f, 2.0f);
_leftWheel = _world.CreateBody(bd);
_leftWheel.CreateShape(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.CreateShape(box);
}
{ // more ground
PolygonDef box = new PolygonDef();
BodyDef bd = new BodyDef();
box.SetAsBox(9.5f, 0.5f, Vec2.Zero, 0.1f * Box2DX.Common.Settings.Pi);
box.Friction = 0.62f;
bd.Position.Set(27.0f - 30.0f, 3.1f);
Body ground = _world.CreateBody(bd);
ground.CreateShape(box);
}
{ // more ground
PolygonDef box = new PolygonDef();
BodyDef bd = new BodyDef();
box.SetAsBox(9.5f, 0.5f, Vec2.Zero, -0.1f * Box2DX.Common.Settings.Pi);
box.Friction = 0.62f;
bd.Position.Set(55.0f - 30.0f, 3.1f);
Body ground = _world.CreateBody(bd);
ground.CreateShape(box);
}
{ // more ground
PolygonDef box = new PolygonDef();
BodyDef bd = new BodyDef();
box.SetAsBox(9.5f, 0.5f, Vec2.Zero, 0.03f * Box2DX.Common.Settings.Pi);
box.Friction = 0.62f;
bd.Position.Set(41.0f, 2.0f);
Body ground = _world.CreateBody(bd);
ground.CreateShape(box);
}
{ // more ground
PolygonDef box = new PolygonDef();
BodyDef bd = new BodyDef();
box.SetAsBox(5.0f, 0.5f, Vec2.Zero, 0.15f * Box2DX.Common.Settings.Pi);
box.Friction = 0.62f;
bd.Position.Set(50.0f, 4.0f);
Body ground = _world.CreateBody(bd);
ground.CreateShape(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.CreateShape(box);
}
}
/// <summary>
/// Add objects to the Box2d world.
/// </summary>
protected override void PopulateWorld()
{
// ==== Define the ground body ====
BodyDef groundBodyDef = new BodyDef();
groundBodyDef.Position.Set(_trackLengthHalf, -1f);
// 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, 1f);
groundShapeDef.Friction = _simParams._defaultFriction;
groundShapeDef.Restitution = _simParams._defaultRestitution;
groundShapeDef.Filter.CategoryBits = 0x3;
// Add the ground shape to the ground body.
groundBody.CreateShape(groundShapeDef);
// ==== Define walker torso.
BodyDef torsoBodyDef = new BodyDef();
torsoBodyDef.Position.Set(0f, 1.45f);
torsoBodyDef.IsBullet = true;
// Create walker torso.
_torsoBody = _world.CreateBody(torsoBodyDef);
PolygonDef torsoShapeDef = new PolygonDef();
torsoShapeDef.SetAsBox(0.10f, 0.45f);
torsoShapeDef.Friction = _simParams._defaultFriction;
torsoShapeDef.Restitution = 0f;
torsoShapeDef.Density = 2f;
torsoShapeDef.Filter.CategoryBits = 0x2;
_torsoBody.CreateShape(torsoShapeDef);
_torsoBody.SetMassFromShapes();
// ===== Create legs.
// Leg joint definition.
RevoluteJointDef jointDef = new RevoluteJointDef();
jointDef.CollideConnected = false;
jointDef.EnableMotor = true;
jointDef.MaxMotorTorque = 0f;
// Other re-usable stuff .
const float legRadius = 0.05f; // Half the thickness of the leg
Vec2 upperLegPosBase = new Vec2(0f, 1.05f);
Vec2 lowerLegPosBase = new Vec2(0f, 0.55f);
// ===== Create left leg.
// Upper leg.
Body upperLeftLegBody = CreatePole(upperLegPosBase, 0.5f, (float)SysMath.PI, legRadius, 2f, 0x1);
// Join to torso (hip joint)
jointDef.Initialize(_torsoBody, upperLeftLegBody, upperLegPosBase);
_leftHipJoint = (RevoluteJoint)_world.CreateJoint(jointDef);
// Lower leg.
_leftLowerLegBody = CreatePole(lowerLegPosBase, __lowerLegLength, (float)SysMath.PI, legRadius, 2f, 0x1);
// Join to upper leg (knee joint)
jointDef.Initialize(upperLeftLegBody, _leftLowerLegBody, lowerLegPosBase);
_leftKneeJoint = (RevoluteJoint)_world.CreateJoint(jointDef);
// ===== Create right leg.
// Upper leg.
Body upperRightLegBody = CreatePole(upperLegPosBase, 0.5f, (float)SysMath.PI, legRadius, 2f, 0x1);
// Join to torso (hip joint)
jointDef.Initialize(_torsoBody, upperRightLegBody, upperLegPosBase);
_rightHipJoint = (RevoluteJoint)_world.CreateJoint(jointDef);
// Lower leg.
_rightLowerLegBody = CreatePole(lowerLegPosBase, __lowerLegLength, (float)SysMath.PI, legRadius, 2f, 0x1);
// Join to upper leg (knee joint)
jointDef.Initialize(upperRightLegBody, _rightLowerLegBody, lowerLegPosBase);
_rightKneeJoint = (RevoluteJoint)_world.CreateJoint(jointDef);
}
/// <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;
}
private void SetJointTorque(RevoluteJoint joint, float torque)
{
float atorque; // abs(torque)
float storque; // sign(torque)
if(torque > 0f) {
atorque = torque;
storque = 1f;
} else if (torque < 0f) {
atorque = -torque;
storque = -1f;
} else {
atorque = 0f;
storque = 0f;
}
//#
//# We have to enumerate various cases because we are emulating friction
//# with active torque and we have to take care not to add energy to
//# the system when a reversal happens inside the timestep.
//#
//# Also, the underlying sim does not let us apply torque directly, but
//# rather we have to factor it into maxMotorTorque and motorSpeed, the
//# latter of which we set to an extreme we don't normally expect to reach.
//#
float speed = joint.JointSpeed;
float fric = __jointFriction;
if(speed*storque >= 0f) //# Pushing in same direction as current motion (and against friction)
{
if(atorque > fric) { //# With enough force to overcome friction:
joint.SetMaxMotorTorque(atorque - fric); //# Remaining torque (after friction)...
joint.MotorSpeed = storque * 100f; //# serves to accelerate current motion
} else { //# Without enough force to overcome friction:
joint.SetMaxMotorTorque(fric - atorque); //# Remaining friction...
joint.MotorSpeed = 0f; //# ...serves to stop the motion (but no more)
}
}
else { //# Pushing against current motion (and with friction)
joint.SetMaxMotorTorque(fric + atorque); //# Forces sum...
joint.MotorSpeed = storque * 100f; //# ...in direction of torque
//# BUG: This case unfortunately adds energy during reversals because friction
//# continues to contribute energy in the same direction after the reversal.
//# Is there a fix for this?
}
}
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 Biped(World w, Vec2 position)
{
m_world = w;
BipedDef def = new BipedDef();
BodyDef bd;
// create body parts
bd = def.LFootDef;
bd.Position += position;
LFoot = w.CreateBody(bd);
LFoot.CreateShape(def.LFootPoly);
LFoot.SetMassFromShapes();
bd = def.RFootDef;
bd.Position += position;
RFoot = w.CreateBody(bd);
RFoot.CreateShape(def.RFootPoly);
RFoot.SetMassFromShapes();
bd = def.LCalfDef;
bd.Position += position;
LCalf = w.CreateBody(bd);
LCalf.CreateShape(def.LCalfPoly);
LCalf.SetMassFromShapes();
bd = def.RCalfDef;
bd.Position += position;
RCalf = w.CreateBody(bd);
RCalf.CreateShape(def.RCalfPoly);
RCalf.SetMassFromShapes();
bd = def.LThighDef;
bd.Position += position;
LThigh = w.CreateBody(bd);
LThigh.CreateShape(def.LThighPoly);
LThigh.SetMassFromShapes();
bd = def.RThighDef;
bd.Position += position;
RThigh = w.CreateBody(bd);
RThigh.CreateShape(def.RThighPoly);
RThigh.SetMassFromShapes();
bd = def.PelvisDef;
bd.Position += position;
Pelvis = w.CreateBody(bd);
Pelvis.CreateShape(def.PelvisPoly);
Pelvis.SetMassFromShapes();
bd = def.PelvisDef;
bd.Position += position;
Stomach = w.CreateBody(bd);
Stomach.CreateShape(def.StomachPoly);
Stomach.SetMassFromShapes();
bd = def.ChestDef;
bd.Position += position;
Chest = w.CreateBody(bd);
Chest.CreateShape(def.ChestPoly);
Chest.SetMassFromShapes();
bd = def.NeckDef;
bd.Position += position;
Neck = w.CreateBody(bd);
Neck.CreateShape(def.NeckPoly);
Neck.SetMassFromShapes();
bd = def.HeadDef;
bd.Position += position;
Head = w.CreateBody(bd);
Head.CreateShape(def.HeadCirc);
Head.SetMassFromShapes();
bd = def.LUpperArmDef;
bd.Position += position;
LUpperArm = w.CreateBody(bd);
LUpperArm.CreateShape(def.LUpperArmPoly);
LUpperArm.SetMassFromShapes();
bd = def.RUpperArmDef;
bd.Position += position;
RUpperArm = w.CreateBody(bd);
RUpperArm.CreateShape(def.RUpperArmPoly);
RUpperArm.SetMassFromShapes();
bd = def.LForearmDef;
bd.Position += position;
LForearm = w.CreateBody(bd);
LForearm.CreateShape(def.LForearmPoly);
LForearm.SetMassFromShapes();
bd = def.RForearmDef;
bd.Position += position;
RForearm = w.CreateBody(bd);
RForearm.CreateShape(def.RForearmPoly);
RForearm.SetMassFromShapes();
bd = def.LHandDef;
bd.Position += position;
LHand = w.CreateBody(bd);
LHand.CreateShape(def.LHandPoly);
LHand.SetMassFromShapes();
bd = def.RHandDef;
bd.Position += position;
RHand = w.CreateBody(bd);
RHand.CreateShape(def.RHandPoly);
RHand.SetMassFromShapes();
// link body parts
def.LAnkleDef.Body1 = LFoot;
def.LAnkleDef.Body2 = LCalf;
def.RAnkleDef.Body1 = RFoot;
def.RAnkleDef.Body2 = RCalf;
def.LKneeDef.Body1 = LCalf;
def.LKneeDef.Body2 = LThigh;
def.RKneeDef.Body1 = RCalf;
def.RKneeDef.Body2 = RThigh;
def.LHipDef.Body1 = LThigh;
def.LHipDef.Body2 = Pelvis;
def.RHipDef.Body1 = RThigh;
def.RHipDef.Body2 = Pelvis;
def.LowerAbsDef.Body1 = Pelvis;
def.LowerAbsDef.Body2 = Stomach;
def.UpperAbsDef.Body1 = Stomach;
def.UpperAbsDef.Body2 = Chest;
def.LowerNeckDef.Body1 = Chest;
def.LowerNeckDef.Body2 = Neck;
def.UpperNeckDef.Body1 = Chest;
def.UpperNeckDef.Body2 = Head;
def.LShoulderDef.Body1 = Chest;
def.LShoulderDef.Body2 = LUpperArm;
def.RShoulderDef.Body1 = Chest;
def.RShoulderDef.Body2 = RUpperArm;
def.LElbowDef.Body1 = LForearm;
def.LElbowDef.Body2 = LUpperArm;
def.RElbowDef.Body1 = RForearm;
def.RElbowDef.Body2 = RUpperArm;
def.LWristDef.Body1 = LHand;
def.LWristDef.Body2 = LForearm;
def.RWristDef.Body1 = RHand;
def.RWristDef.Body2 = RForearm;
// create joints
LAnkle = (RevoluteJoint)w.CreateJoint(def.LAnkleDef);
RAnkle = (RevoluteJoint)w.CreateJoint(def.RAnkleDef);
LKnee = (RevoluteJoint)w.CreateJoint(def.LKneeDef);
RKnee = (RevoluteJoint)w.CreateJoint(def.RKneeDef);
LHip = (RevoluteJoint)w.CreateJoint(def.LHipDef);
RHip = (RevoluteJoint)w.CreateJoint(def.RHipDef);
LowerAbs = (RevoluteJoint)w.CreateJoint(def.LowerAbsDef);
UpperAbs = (RevoluteJoint)w.CreateJoint(def.UpperAbsDef);
LowerNeck = (RevoluteJoint)w.CreateJoint(def.LowerNeckDef);
UpperNeck = (RevoluteJoint)w.CreateJoint(def.UpperNeckDef);
LShoulder = (RevoluteJoint)w.CreateJoint(def.LShoulderDef);
RShoulder = (RevoluteJoint)w.CreateJoint(def.RShoulderDef);
LElbow = (RevoluteJoint)w.CreateJoint(def.LElbowDef);
RElbow = (RevoluteJoint)w.CreateJoint(def.RElbowDef);
LWrist = (RevoluteJoint)w.CreateJoint(def.LWristDef);
RWrist = (RevoluteJoint)w.CreateJoint(def.RWristDef);
}
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);
}
}
}
