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); } } }