public DistanceTest()
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(1.0f, 1.0f);
sd.Density = 0.0f;
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 10.0f);
_body1 = _world.CreateBody(bd);
_shape1 = _body1.CreateShape(sd);
PolygonDef sd2 = new PolygonDef();
sd2.VertexCount = 3;
sd2.Vertices[0].Set(-1.0f, 0.0f);
sd2.Vertices[1].Set(1.0f, 0.0f);
sd2.Vertices[2].Set(0.0f, 15.0f);
sd2.Density = 1.0f;
BodyDef bd2 = new BodyDef();
bd2.Position.Set(0.0f, 10.0f);
_body2 = _world.CreateBody(bd2);
_shape2 = _body2.CreateShape(sd2);
_body2.SetMassFromShapes();
_world.Gravity = new Vec2(0.0f, 0.0f);
}
public PolyCollision()
{
_localPoints[0].state = ContactState.ContactRemoved;
_localPoints[1].state = ContactState.ContactRemoved;
{
PolygonDef sd = new PolygonDef();
sd.Vertices[0].Set(-9.0f, -1.1f);
sd.Vertices[1].Set(7.0f, -1.1f);
sd.Vertices[2].Set(5.0f, -0.9f);
sd.Vertices[3].Set(-11.0f, -0.9f);
sd.VertexCount = 4;
sd.Density = 0.0f;
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 10.0f);
_body1 = _world.CreateBody(bd);
_body1.CreateShape(sd);
}
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(0.5f, 0.5f);
sd.Density = 1.0f;
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 10.0f);
_body2 = _world.CreateBody(bd);
_body2.CreateShape(sd);
_body2.SetMassFromShapes();
}
_world.Gravity = Vec2.Zero;
}
public ShapeEditing()
{
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(50.0f, 10.0f);
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, -10.0f);
Body ground = _world.CreateBody(bd);
ground.CreateShape(sd);
}
BodyDef bodydef = new BodyDef();
bodydef.Position.Set(0.0f, 10.0f);
_body = _world.CreateBody(bodydef);
PolygonDef sd_ = new PolygonDef();
sd_.SetAsBox(4.0f, 4.0f, new Vec2(0.0f, 0.0f), 0.0f);
sd_.Density = 10.0f;
_shape1 = _body.CreateShape(sd_);
_body.SetMassFromShapes();
_shape2 = null;
}
public Projectile(Player creator, float x, float y, float width, float height)
: base(creator, 0, 0)
{
/* Create New Projectile Body */
BodyDef def = new BodyDef();
def.IsBullet = true;
def.Position = creator.body.GetPosition() + new Vec2(x, y);
projectile = creator.body.GetWorld().CreateBody(def);
/* Create a fixture for the projectile */
PolygonDef fixdef = new PolygonDef();
fixdef.Density = 1.0f;
fixdef.SetAsBox(width / 2, height / 2);
fixdef.Filter.GroupIndex = creator.ID;
fixture = projectile.CreateFixture(fixdef);
fixture.Filter.CategoryBits = 0x0004;
fixture.Filter.MaskBits = 0xFFFF;
/* Made a 2nd fixture, one to observe all collisions */
fixdef.IsSensor = true;
fix2 = projectile.CreateFixture(fixdef);
fix2.UserData = this;
/* Finally, give this projectile some mass */
projectile.SetMassFromShapes();
/* Also, make sure we destroy the projectile when it is time */
this.OnDestroy += Cleanup;
}
public TimeOfImpact()
{
{
PolygonDef sd = new PolygonDef();
sd.Density = 0.0f;
sd.SetAsBox(0.1f, 10.0f, new Vec2(10.0f, 0.0f), 0.0f);
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 20.0f);
bd.Angle = 0.0f;
_body1 = _world.CreateBody(bd);
_shape1 = _body1.CreateShape(sd);
}
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(0.25f, 0.25f);
sd.Density = 1.0f;
BodyDef bd = new BodyDef();
bd.Position.Set(9.6363468f, 28.050615f);
bd.Angle = 1.6408679f;
_body2 = _world.CreateBody(bd);
_shape2 = (PolygonShape)_body2.CreateShape(sd);
_body2.SetMassFromShapes();
}
}
public Breakable()
{
// Ground body
{
BodyDef bd = new BodyDef();
Body 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);
}
// Breakable dynamic body
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 40.0f);
bd.Angle = 0.25f * Box2DX.Common.Settings.PI;
_body1 = _world.CreateBody(bd);
_shape1.SetAsBox(0.5f, 0.5f, new Vec2(-0.5f, 0.0f), 0.0f);
_piece1 = _body1.CreateFixture(_shape1, 1.0f);
_shape2.SetAsBox(0.5f, 0.5f, new Vec2(0.5f, 0.0f), 0.0f);
_piece2 = _body1.CreateFixture(_shape2, 1.0f);
}
_break = false;
_broke = false;
}
public World(AABB worldAABB, Vec2 gravity, bool doSleep)
{
this._destructionListener = null;
this._boundaryListener = null;
this._contactFilter = WorldCallback.DefaultFilter;
this._contactListener = null;
this._debugDraw = null;
this._bodyList = null;
this._contactList = null;
this._jointList = null;
this._bodyCount = 0;
this._contactCount = 0;
this._jointCount = 0;
this._warmStarting = true;
this._continuousPhysics = true;
this._allowSleep = doSleep;
this._gravity = gravity;
this._lock = false;
this._inv_dt0 = 0f;
this._contactManager = new ContactManager();
this._contactManager._world = this;
this._broadPhase = new BroadPhase(worldAABB, this._contactManager);
BodyDef def = new BodyDef();
this._groundBody = this.CreateBody(def);
}
public void Initialize(Body body1, Body body2, Vec2 anchor1, Vec2 anchor2)
{
this.Body1 = body1;
this.Body2 = body2;
this.LocalAnchor1 = body1.GetLocalPoint(anchor1);
this.LocalAnchor2 = body2.GetLocalPoint(anchor2);
this.Length = (anchor2 - anchor1).Length();
}
public JointDef()
{
this.Type = JointType.UnknownJoint;
this.UserData = null;
this.Body1 = null;
this.Body2 = null;
this.CollideConnected = false;
}
public void Initialize(Body body1, Body body2, Vec2 anchor)
{
this.Body1 = body1;
this.Body2 = body2;
this.LocalAnchor1 = body1.GetLocalPoint(anchor);
this.LocalAnchor2 = body2.GetLocalPoint(anchor);
this.ReferenceAngle = body2.GetAngle() - body1.GetAngle();
}
public void Initialize(Body body1, Body body2, Vec2 anchor, Vec2 axis)
{
this.Body1 = body1;
this.Body2 = body2;
this.localAnchor1 = body1.GetLocalPoint(anchor);
this.localAnchor2 = body2.GetLocalPoint(anchor);
this.localAxis1 = body1.GetLocalVector(axis);
}
public Fixture()
{
UserData = null;
Body = null;
_next = null;
ProxyId = BroadPhase.NullProxy;
Shape = null;
}
/// <summary>
/// Initialize the bodies, anchors, axis, and reference angle using the world
/// anchor and world axis.
/// </summary>
/// <param name="body1"></param>
/// <param name="body2"></param>
/// <param name="anchor"></param>
/// <param name="axis"></param>
public void Initialize(Body body1, Body body2, Vec2 anchor, Vec2 axis)
{
Body1 = body1;
Body2 = body2;
LocalAnchor1 = body1.GetLocalPoint(anchor);
LocalAnchor2 = body2.GetLocalPoint(anchor);
LocalAxis1 = body1.GetLocalVector(axis);
ReferenceAngle = body2.GetAngle() - body1.GetAngle();
}
/// <summary>
/// Initialize the bodies, anchors, and length using the world anchors.
/// </summary>
public void Initialize(Body body1, Body body2, Vec2 anchor1, Vec2 anchor2)
{
Body1 = body1;
Body2 = body2;
LocalAnchor1 = body1.GetLocalPoint(anchor1);
LocalAnchor2 = body2.GetLocalPoint(anchor2);
Vec2 d = anchor2 - anchor1;
Length = d.Length();
}
protected Joint(JointDef def)
{
this._type = def.Type;
this._prev = null;
this._next = null;
this._body1 = def.Body1;
this._body2 = def.Body2;
this._collideConnected = def.CollideConnected;
this._islandFlag = false;
this._userData = def.UserData;
}
protected Shape(ShapeDef def)
{
this._userData = def.UserData;
this._friction = def.Friction;
this._restitution = def.Restitution;
this._density = def.Density;
this._body = null;
this._sweepRadius = 0f;
this._next = null;
this._proxyId = PairManager.NullProxy;
this._filter = def.Filter;
this._isSensor = def.IsSensor;
}
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 void Initialize(Body body1, Body body2, Vec2 groundAnchor1, Vec2 groundAnchor2, Vec2 anchor1, Vec2 anchor2, float ratio)
{
this.Body1 = body1;
this.Body2 = body2;
this.GroundAnchor1 = groundAnchor1;
this.GroundAnchor2 = groundAnchor2;
this.LocalAnchor1 = body1.GetLocalPoint(anchor1);
this.LocalAnchor2 = body2.GetLocalPoint(anchor2);
this.Length1 = (anchor1 - groundAnchor1).Length();
this.Length2 = (anchor2 - groundAnchor2).Length();
this.Ratio = ratio;
Box2DXDebug.Assert(ratio > Settings.FLT_EPSILON);
float num = this.Length1 + ratio * this.Length2;
this.MaxLength1 = num - ratio * PulleyJoint.MinPulleyLength;
this.MaxLength2 = (num - PulleyJoint.MinPulleyLength) / ratio;
}
public PulleyJoint(PulleyJointDef def)
: base(def)
{
this._ground = this._body1.GetWorld().GetGroundBody();
this._groundAnchor1 = def.GroundAnchor1 - this._ground.GetXForm().Position;
this._groundAnchor2 = def.GroundAnchor2 - this._ground.GetXForm().Position;
this._localAnchor1 = def.LocalAnchor1;
this._localAnchor2 = def.LocalAnchor2;
Box2DXDebug.Assert(def.Ratio != 0f);
this._ratio = def.Ratio;
this._constant = def.Length1 + this._ratio * def.Length2;
this._maxLength1 = Box2DX.Common.Math.Min(def.MaxLength1, this._constant - this._ratio * PulleyJoint.MinPulleyLength);
this._maxLength2 = Box2DX.Common.Math.Min(def.MaxLength2, (this._constant - PulleyJoint.MinPulleyLength) / this._ratio);
this._impulse = 0f;
this._limitImpulse1 = 0f;
this._limitImpulse2 = 0f;
}
public VerticalStack()
{
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(50.0f, 10.0f, new Vec2(0.0f, -10.0f), 0.0f);
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 0.0f);
Body ground = _world.CreateBody(bd);
ground.CreateShape(sd);
sd.SetAsBox(0.1f, 10.0f, new Vec2(20.0f, 10.0f), 0.0f);
ground.CreateShape(sd);
}
float[] xs = new float[5] { 0.0f, -10.0f, -5.0f, 5.0f, 10.0f };
for (int j = 0; j < 5; ++j)
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(0.5f, 0.5f);
sd.Density = 1.0f;
sd.Friction = 0.3f;
for (int i = 0; i < 12; ++i)
{
BodyDef bd = new BodyDef();
// For this test we are using continuous physics for all boxes.
// This is a stress test, you normally wouldn't do this for
// performance reasons.
bd.AllowSleep = true;
bd.Position.Set(xs[j], 0.752f + 1.54f * i);
Body body = _world.CreateBody(bd);
body.CreateShape(sd);
body.SetMassFromShapes();
}
}
_bullet = null;
}
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
public void Initialize(Body body1, Body body2,
Vec2 groundAnchor1, Vec2 groundAnchor2,
Vec2 anchor1, Vec2 anchor2,
float ratio)
{
Body1 = body1;
Body2 = body2;
GroundAnchor1 = groundAnchor1;
GroundAnchor2 = groundAnchor2;
LocalAnchor1 = body1.GetLocalPoint(anchor1);
LocalAnchor2 = body2.GetLocalPoint(anchor2);
Vec2 d1 = anchor1 - groundAnchor1;
Length1 = d1.Length();
Vec2 d2 = anchor2 - groundAnchor2;
Length2 = d2.Length();
Ratio = ratio;
Box2DXDebug.Assert(ratio > Settings.FLT_EPSILON);
float C = Length1 + ratio * Length2;
MaxLength1 = C - ratio * PulleyJoint.MinPulleyLength;
MaxLength2 = (C - PulleyJoint.MinPulleyLength) / ratio;
}
public RaycastTest()
{
Body ground = null;
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, -10.0f);
ground = _world.CreateBody(bd);
PolygonDef sd = new PolygonDef();
sd.SetAsBox(50.0f, 10.0f);
ground.CreateShape(sd);
}
{
BodyDef bd = new BodyDef();
bd.Position.Set(0.0f, 1.0f);
laserBody = _world.CreateBody(bd);
PolygonDef sd = new PolygonDef();
sd.SetAsBox(5.0f, 1.0f);
sd.Density = 4.0f;
laserBody.CreateShape(sd);
laserBody.SetMassFromShapes();
Body body;
//Create a few shapes
bd.Position.Set(-5.0f, 10.0f);
body = _world.CreateBody(bd);
CircleDef cd = new CircleDef();
cd.Radius = 3;
body.CreateShape(cd);
bd.Position.Set(5.0f, 10.0f);
body = _world.CreateBody(bd);
body.CreateShape(cd);
}
}
/// <summary>
/// Construct a world object.
/// </summary>
/// <param name="gravity">The world gravity vector.</param>
/// <param name="doSleep">Improve performance by not simulating inactive bodies.</param>
public World(Vec2 gravity, bool doSleep)
{
_destructionListener = null;
_debugDraw = null;
_bodyList = null;
_jointList = null;
_bodyCount = 0;
_jointCount = 0;
_warmStarting = true;
_continuousPhysics = true;
_allowSleep = doSleep;
_gravity = gravity;
_flags = 0;
_inv_dt0 = 0.0f;
_contactManager = new ContactManager();
}
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 VerticalStack()
{
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(50.0f, 10.0f, new Vec2(0.0f, -10.0f), 0.0f);
BodyDef bd = new BodyDef();
Body ground = _world.CreateBody(bd);
ground.CreateFixture(sd);
sd.SetAsBox(0.1f, 10.0f, new Vec2(20.0f, 10.0f), 0.0f);
ground.CreateFixture(sd);
}
float[] xs = new float[] { 0.0f, -10.0f, -5.0f, 5.0f, 10.0f };
for (int j = 0; j < 5; ++j)
{
PolygonDef sd = new PolygonDef();
sd.SetAsBox(0.5f, 0.5f);
sd.Density = 1.0f;
sd.Friction = 0.3f;
for (int i = 0; i < 16; ++i)
{
BodyDef bd = new BodyDef();
bd.Position.Set(xs[j], 0.752f + 1.54f * i);
Body body = _world.CreateBody(bd);
body.CreateFixture(sd);
body.SetMassFromShapes();
}
}
_bullet = null;
}
/// <summary> /// This is called for each body that leaves the world boundary. /// @warning you can't modify the world inside this callback. /// </summary> public abstract void Violation(Body body);
private Vec2 CalcPoleEndPos(Body poleBody)
{
// Determine position of top of pole relative to its center of mass.
float angle = poleBody.GetAngle();
Vec2 polePosTopRelative = new Vec2(__ArmLength * (float)-SysMath.Sin(angle), __ArmLength * (float)SysMath.Cos(angle));
return poleBody.GetPosition() + (polePosTopRelative * 0.5f);
}
/// <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);
}
public PulleyJoint(PulleyJointDef def)
: base(def)
{
_ground = _body1.GetWorld().GetGroundBody();
_groundAnchor1 = def.GroundAnchor1 - _ground.GetXForm().Position;
_groundAnchor2 = def.GroundAnchor2 - _ground.GetXForm().Position;
_localAnchor1 = def.LocalAnchor1;
_localAnchor2 = def.LocalAnchor2;
Box2DXDebug.Assert(def.Ratio != 0.0f);
_ratio = def.Ratio;
_constant = def.Length1 + _ratio * def.Length2;
_maxLength1 = Common.Math.Min(def.MaxLength1, _constant - _ratio * PulleyJoint.MinPulleyLength);
_maxLength2 = Common.Math.Min(def.MaxLength2, (_constant - PulleyJoint.MinPulleyLength) / _ratio);
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
}
// Compute the effective mass matrix.
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
_mass.Col1.x = m1 + m2 + r1.y * r1.y * i1 + r2.y * r2.y * i2;
_mass.Col2.x = -r1.y * r1.x * i1 - r2.y * r2.x * i2;
_mass.Col3.x = -r1.y * i1 - r2.y * i2;
_mass.Col1.y = _mass.Col2.x;
_mass.Col2.y = m1 + m2 + r1.x * r1.x * i1 + r2.x * r2.x * i2;
_mass.Col3.y = r1.x * i1 + r2.x * i2;
_mass.Col1.z = _mass.Col3.x;
_mass.Col2.z = _mass.Col3.y;
_mass.Col3.z = i1 + i2;
_motorMass = 1.0f / (i1 + i2);
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLowerLimit)
{
_impulse.z = 0.0f;
}
_limitState = LimitState.AtLowerLimit;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpperLimit)
{
_impulse.z = 0.0f;
}
_limitState = LimitState.AtUpperLimit;
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
}
if (step.WarmStarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.DtRatio;
_motorImpulse *= step.DtRatio;
Vector2 P = _impulse.ToVector2();
b1._linearVelocity -= m1 * P;
b1._angularVelocity -= i1 * (r1.Cross(P) + _motorImpulse + _impulse.z);
b2._linearVelocity += m2 * P;
b2._angularVelocity += i2 * (r2.Cross(P) + _motorImpulse + _impulse.z);
}
else
{
_impulse = Vector3.zero;
_motorImpulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vector2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
//Solve motor constraint.
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.Dt * _maxMotorTorque;
_motorImpulse = Mathf.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
//Solve limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vector2 Cdot1 = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1);
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.EqualLimits)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLowerLimit)
{
float newImpulse = _impulse.z + impulse.z;
if (newImpulse < 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -_impulse.z;
_impulse.x += reduced.x;
_impulse.y += reduced.y;
_impulse.z = 0.0f;
}
}
else if (_limitState == LimitState.AtUpperLimit)
{
float newImpulse = _impulse.z + impulse.z;
if (newImpulse > 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -_impulse.z;
_impulse.x += reduced.x;
_impulse.y += reduced.y;
_impulse.z = 0.0f;
}
}
Vector2 P = impulse.ToVector2();
v1 -= m1 * P;
w1 -= i1 * (r1.Cross(P) + impulse.z);
v2 += m2 * P;
w2 += i2 * (r2.Cross(P) + impulse.z);
}
else
{
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vector2 Cdot = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.x += impulse.x;
_impulse.y += impulse.y;
v1 -= m1 * impulse;
w1 -= i1 * r1.Cross(impulse);
v2 += m2 * impulse;
w2 += i2 * r2.Cross(impulse);
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
internal override bool SolvePositionConstraints(float baumgarte)
{
// TODO_ERIN block solve with limit.
Body b1 = _body1;
Body b2 = _body2;
float angularError = 0.0f;
float positionError = 0.0f;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float C = Mathf.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Box2DXMath.Abs(C);
}
else if (_limitState == LimitState.AtLowerLimit)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = Mathf.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = Mathf.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1._sweep.A -= b1._invI * limitImpulse;
b2._sweep.A += b2._invI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
Vector2 C = b2._sweep.C + r2 - b1._sweep.C - r1;
positionError = C.magnitude;
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Handle large detachment.
float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.sqrMagnitude > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vector2 u = C; u.Normalize();
float k = invMass1 + invMass2;
Box2DXDebug.Assert(k > Settings.FLT_EPSILON);
float m = 1.0f / k;
Vector2 impulse = m * (-C);
float k_beta = 0.5f;
b1._sweep.C -= k_beta * invMass1 * impulse;
b2._sweep.C += k_beta * invMass2 * impulse;
C = b2._sweep.C + r2 - b1._sweep.C - r1;
}
Mat22 K1 = new Mat22();
K1.Col1.x = invMass1 + invMass2; K1.Col2.x = 0.0f;
K1.Col1.y = 0.0f; K1.Col2.y = invMass1 + invMass2;
Mat22 K2 = new Mat22();
K2.Col1.x = invI1 * r1.y * r1.y; K2.Col2.x = -invI1 * r1.x * r1.y;
K2.Col1.y = -invI1 * r1.x * r1.y; K2.Col2.y = invI1 * r1.x * r1.x;
Mat22 K3 = new Mat22();
K3.Col1.x = invI2 * r2.y * r2.y; K3.Col2.x = -invI2 * r2.x * r2.y;
K3.Col1.y = -invI2 * r2.x * r2.y; K3.Col2.y = invI2 * r2.x * r2.x;
Mat22 K = K1 + K2 + K3;
Vector2 impulse_ = K.Solve(-C);
b1._sweep.C -= b1._invMass * impulse_;
b1._sweep.A -= b1._invI * r1.Cross(impulse_);
b2._sweep.C += b2._invMass * impulse_;
b2._sweep.A += b2._invI * r2.Cross(impulse_);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 v = body._sweep.C + vec;
Vec2 v2 = body2._sweep.C + vec2;
Vec2 v3 = this._ground.GetXForm().Position + this._groundAnchor1;
Vec2 v4 = this._ground.GetXForm().Position + this._groundAnchor2;
this._u1 = v - v3;
this._u2 = v2 - v4;
float num = this._u1.Length();
float num2 = this._u2.Length();
if (num > Settings.LinearSlop)
{
this._u1 *= 1f / num;
}
else
{
this._u1.SetZero();
}
if (num2 > Settings.LinearSlop)
{
this._u2 *= 1f / num2;
}
else
{
this._u2.SetZero();
}
float num3 = this._constant - num - this._ratio * num2;
if (num3 > 0f)
{
this._state = LimitState.InactiveLimit;
this._impulse = 0f;
}
else
{
this._state = LimitState.AtUpperLimit;
}
if (num < this._maxLength1)
{
this._limitState1 = LimitState.InactiveLimit;
this._limitImpulse1 = 0f;
}
else
{
this._limitState1 = LimitState.AtUpperLimit;
}
if (num2 < this._maxLength2)
{
this._limitState2 = LimitState.InactiveLimit;
this._limitImpulse2 = 0f;
}
else
{
this._limitState2 = LimitState.AtUpperLimit;
}
float num4 = Vec2.Cross(vec, this._u1);
float num5 = Vec2.Cross(vec2, this._u2);
this._limitMass1 = body._invMass + body._invI * num4 * num4;
this._limitMass2 = body2._invMass + body2._invI * num5 * num5;
this._pulleyMass = this._limitMass1 + this._ratio * this._ratio * this._limitMass2;
Box2DXDebug.Assert(this._limitMass1 > Settings.FLT_EPSILON);
Box2DXDebug.Assert(this._limitMass2 > Settings.FLT_EPSILON);
Box2DXDebug.Assert(this._pulleyMass > Settings.FLT_EPSILON);
this._limitMass1 = 1f / this._limitMass1;
this._limitMass2 = 1f / this._limitMass2;
this._pulleyMass = 1f / this._pulleyMass;
if (step.WarmStarting)
{
this._impulse *= step.DtRatio;
this._limitImpulse1 *= step.DtRatio;
this._limitImpulse2 *= step.DtRatio;
Vec2 vec3 = -(this._impulse + this._limitImpulse1) * this._u1;
Vec2 vec4 = (-this._ratio * this._impulse - this._limitImpulse2) * this._u2;
Body expr_37B = body;
expr_37B._linearVelocity += body._invMass * vec3;
body._angularVelocity += body._invI * Vec2.Cross(vec, vec3);
Body expr_3B5 = body2;
expr_3B5._linearVelocity += body2._invMass * vec4;
body2._angularVelocity += body2._invI * Vec2.Cross(vec2, vec4);
}
else
{
this._impulse = 0f;
this._limitImpulse1 = 0f;
this._limitImpulse2 = 0f;
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 v = this._ground.GetXForm().Position + this._groundAnchor1;
Vec2 v2 = this._ground.GetXForm().Position + this._groundAnchor2;
float num = 0f;
if (this._state == LimitState.AtUpperLimit)
{
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 v3 = body._sweep.C + vec;
Vec2 v4 = body2._sweep.C + vec2;
this._u1 = v3 - v;
this._u2 = v4 - v2;
float num2 = this._u1.Length();
float num3 = this._u2.Length();
if (num2 > Settings.LinearSlop)
{
this._u1 *= 1f / num2;
}
else
{
this._u1.SetZero();
}
if (num3 > Settings.LinearSlop)
{
this._u2 *= 1f / num3;
}
else
{
this._u2.SetZero();
}
float num4 = this._constant - num2 - this._ratio * num3;
num = Box2DX.Common.Math.Max(num, -num4);
num4 = Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
float num5 = -this._pulleyMass * num4;
Vec2 vec3 = -num5 * this._u1;
Vec2 vec4 = -this._ratio * num5 * this._u2;
Body expr_1F6_cp_0 = body;
expr_1F6_cp_0._sweep.C = expr_1F6_cp_0._sweep.C + body._invMass * vec3;
Body expr_219_cp_0 = body;
expr_219_cp_0._sweep.A = expr_219_cp_0._sweep.A + body._invI * Vec2.Cross(vec, vec3);
Body expr_23B_cp_0 = body2;
expr_23B_cp_0._sweep.C = expr_23B_cp_0._sweep.C + body2._invMass * vec4;
Body expr_25E_cp_0 = body2;
expr_25E_cp_0._sweep.A = expr_25E_cp_0._sweep.A + body2._invI * Vec2.Cross(vec2, vec4);
body.SynchronizeTransform();
body2.SynchronizeTransform();
}
if (this._limitState1 == LimitState.AtUpperLimit)
{
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 v3 = body._sweep.C + vec;
this._u1 = v3 - v;
float num2 = this._u1.Length();
if (num2 > Settings.LinearSlop)
{
this._u1 *= 1f / num2;
}
else
{
this._u1.SetZero();
}
float num4 = this._maxLength1 - num2;
num = Box2DX.Common.Math.Max(num, -num4);
num4 = Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
float num5 = -this._limitMass1 * num4;
Vec2 vec3 = -num5 * this._u1;
Body expr_381_cp_0 = body;
expr_381_cp_0._sweep.C = expr_381_cp_0._sweep.C + body._invMass * vec3;
Body expr_3A4_cp_0 = body;
expr_3A4_cp_0._sweep.A = expr_3A4_cp_0._sweep.A + body._invI * Vec2.Cross(vec, vec3);
body.SynchronizeTransform();
}
if (this._limitState2 == LimitState.AtUpperLimit)
{
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 v4 = body2._sweep.C + vec2;
this._u2 = v4 - v2;
float num3 = this._u2.Length();
if (num3 > Settings.LinearSlop)
{
this._u2 *= 1f / num3;
}
else
{
this._u2.SetZero();
}
float num4 = this._maxLength2 - num3;
num = Box2DX.Common.Math.Max(num, -num4);
num4 = Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
float num5 = -this._limitMass2 * num4;
Vec2 vec4 = -num5 * this._u2;
Body expr_4C0_cp_0 = body2;
expr_4C0_cp_0._sweep.C = expr_4C0_cp_0._sweep.C + body2._invMass * vec4;
Body expr_4E3_cp_0 = body2;
expr_4E3_cp_0._sweep.A = expr_4E3_cp_0._sweep.A + body2._invI * Vec2.Cross(vec2, vec4);
body2.SynchronizeTransform();
}
return(num < Settings.LinearSlop);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
float invMass = body._invMass;
float invMass2 = body2._invMass;
float invI = body._invI;
float invI2 = body2._invI;
this._mass.Col1.X = invMass + invMass2 + a.Y * a.Y * invI + a2.Y * a2.Y * invI2;
this._mass.Col2.X = -a.Y * a.X * invI - a2.Y * a2.X * invI2;
this._mass.Col3.X = -a.Y * invI - a2.Y * invI2;
this._mass.Col1.Y = this._mass.Col2.X;
this._mass.Col2.Y = invMass + invMass2 + a.X * a.X * invI + a2.X * a2.X * invI2;
this._mass.Col3.Y = a.X * invI + a2.X * invI2;
this._mass.Col1.Z = this._mass.Col3.X;
this._mass.Col2.Z = this._mass.Col3.Y;
this._mass.Col3.Z = invI + invI2;
this._motorMass = 1f / (invI + invI2);
if (!this._enableMotor)
{
this._motorImpulse = 0f;
}
if (this._enableLimit)
{
float num = body2._sweep.A - body._sweep.A - this._referenceAngle;
if (Box2DX.Common.Math.Abs(this._upperAngle - this._lowerAngle) < 2f * Settings.AngularSlop)
{
this._limitState = LimitState.EqualLimits;
}
else
{
if (num <= this._lowerAngle)
{
if (this._limitState != LimitState.AtLowerLimit)
{
this._impulse.Z = 0f;
}
this._limitState = LimitState.AtLowerLimit;
}
else
{
if (num >= this._upperAngle)
{
if (this._limitState != LimitState.AtUpperLimit)
{
this._impulse.Z = 0f;
}
this._limitState = LimitState.AtUpperLimit;
}
else
{
this._limitState = LimitState.InactiveLimit;
this._impulse.Z = 0f;
}
}
}
}
if (step.WarmStarting)
{
this._impulse *= step.DtRatio;
this._motorImpulse *= step.DtRatio;
Vec2 vec = new Vec2(this._impulse.X, this._impulse.Y);
Body expr_363 = body;
expr_363._linearVelocity -= invMass * vec;
body._angularVelocity -= invI * (Vec2.Cross(a, vec) + this._motorImpulse + this._impulse.Z);
Body expr_3A8 = body2;
expr_3A8._linearVelocity += invMass2 * vec;
body2._angularVelocity += invI2 * (Vec2.Cross(a2, vec) + this._motorImpulse + this._impulse.Z);
}
else
{
this._impulse.SetZero();
this._motorImpulse = 0f;
}
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 vec = body._linearVelocity;
float num = body._angularVelocity;
Vec2 vec2 = body2._linearVelocity;
float num2 = body2._angularVelocity;
float invMass = body._invMass;
float invMass2 = body2._invMass;
float invI = body._invI;
float invI2 = body2._invI;
if (this._enableMotor && this._limitState != LimitState.EqualLimits)
{
float num3 = num2 - num - this._motorSpeed;
float num4 = this._motorMass * -num3;
float motorImpulse = this._motorImpulse;
float num5 = step.Dt * this._maxMotorTorque;
this._motorImpulse = Box2DX.Common.Math.Clamp(this._motorImpulse + num4, -num5, num5);
num4 = this._motorImpulse - motorImpulse;
num -= invI * num4;
num2 += invI2 * num4;
}
if (this._enableLimit && this._limitState != LimitState.InactiveLimit)
{
Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 v = vec2 + Vec2.Cross(num2, a2) - vec - Vec2.Cross(num, a);
float z = num2 - num;
Vec3 v2 = new Vec3(v.X, v.Y, z);
Vec3 v3 = this._mass.Solve33(-v2);
if (this._limitState == LimitState.EqualLimits)
{
this._impulse += v3;
}
else
{
if (this._limitState == LimitState.AtLowerLimit)
{
float num6 = this._impulse.Z + v3.Z;
if (num6 < 0f)
{
Vec2 vec3 = this._mass.Solve22(-v);
v3.X = vec3.X;
v3.Y = vec3.Y;
v3.Z = -this._impulse.Z;
this._impulse.X = this._impulse.X + vec3.X;
this._impulse.Y = this._impulse.Y + vec3.Y;
this._impulse.Z = 0f;
}
}
else
{
if (this._limitState == LimitState.AtUpperLimit)
{
float num6 = this._impulse.Z + v3.Z;
if (num6 > 0f)
{
Vec2 vec3 = this._mass.Solve22(-v);
v3.X = vec3.X;
v3.Y = vec3.Y;
v3.Z = -this._impulse.Z;
this._impulse.X = this._impulse.X + vec3.X;
this._impulse.Y = this._impulse.Y + vec3.Y;
this._impulse.Z = 0f;
}
}
}
}
Vec2 vec4 = new Vec2(v3.X, v3.Y);
vec -= invMass * vec4;
num -= invI * (Vec2.Cross(a, vec4) + v3.Z);
vec2 += invMass2 * vec4;
num2 += invI2 * (Vec2.Cross(a2, vec4) + v3.Z);
}
else
{
Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 v4 = vec2 + Vec2.Cross(num2, a2) - vec - Vec2.Cross(num, a);
Vec2 vec5 = this._mass.Solve22(-v4);
this._impulse.X = this._impulse.X + vec5.X;
this._impulse.Y = this._impulse.Y + vec5.Y;
vec -= invMass * vec5;
num -= invI * Vec2.Cross(a, vec5);
vec2 += invMass2 * vec5;
num2 += invI2 * Vec2.Cross(a2, vec5);
}
body._linearVelocity = vec;
body._angularVelocity = num;
body2._linearVelocity = vec2;
body2._angularVelocity = num2;
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body body = this._body1;
Body body2 = this._body2;
float num = 0f;
if (this._enableLimit && this._limitState != LimitState.InactiveLimit)
{
float num2 = body2._sweep.A - body._sweep.A - this._referenceAngle;
float num3 = 0f;
if (this._limitState == LimitState.EqualLimits)
{
float num4 = Box2DX.Common.Math.Clamp(num2, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
num3 = -this._motorMass * num4;
num = Box2DX.Common.Math.Abs(num4);
}
else
{
if (this._limitState == LimitState.AtLowerLimit)
{
float num4 = num2 - this._lowerAngle;
num = -num4;
num4 = Box2DX.Common.Math.Clamp(num4 + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0f);
num3 = -this._motorMass * num4;
}
else
{
if (this._limitState == LimitState.AtUpperLimit)
{
float num4 = num2 - this._upperAngle;
num = num4;
num4 = Box2DX.Common.Math.Clamp(num4 - Settings.AngularSlop, 0f, Settings.MaxAngularCorrection);
num3 = -this._motorMass * num4;
}
}
}
Body expr_139_cp_0 = body;
expr_139_cp_0._sweep.A = expr_139_cp_0._sweep.A - body._invI * num3;
Body expr_154_cp_0 = body2;
expr_154_cp_0._sweep.A = expr_154_cp_0._sweep.A + body2._invI * num3;
body.SynchronizeTransform();
body2.SynchronizeTransform();
}
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 vec3 = body2._sweep.C + vec2 - body._sweep.C - vec;
float num5 = vec3.Length();
float invMass = body._invMass;
float invMass2 = body2._invMass;
float invI = body._invI;
float invI2 = body2._invI;
float num6 = 10f * Settings.LinearSlop;
if (vec3.LengthSquared() > num6 * num6)
{
Vec2 vec4 = vec3;
vec4.Normalize();
float num7 = invMass + invMass2;
Box2DXDebug.Assert(num7 > Settings.FLT_EPSILON);
float a = 1f / num7;
Vec2 v = a * -vec3;
float num8 = 0.5f;
Body expr_283_cp_0 = body;
expr_283_cp_0._sweep.C = expr_283_cp_0._sweep.C - num8 * invMass * v;
Body expr_2A5_cp_0 = body2;
expr_2A5_cp_0._sweep.C = expr_2A5_cp_0._sweep.C + num8 * invMass2 * v;
vec3 = body2._sweep.C + vec2 - body._sweep.C - vec;
}
Mat22 a2 = default(Mat22);
a2.Col1.X = invMass + invMass2;
a2.Col2.X = 0f;
a2.Col1.Y = 0f;
a2.Col2.Y = invMass + invMass2;
Mat22 b = default(Mat22);
b.Col1.X = invI * vec.Y * vec.Y;
b.Col2.X = -invI * vec.X * vec.Y;
b.Col1.Y = -invI * vec.X * vec.Y;
b.Col2.Y = invI * vec.X * vec.X;
Mat22 b2 = default(Mat22);
b2.Col1.X = invI2 * vec2.Y * vec2.Y;
b2.Col2.X = -invI2 * vec2.X * vec2.Y;
b2.Col1.Y = -invI2 * vec2.X * vec2.Y;
b2.Col2.Y = invI2 * vec2.X * vec2.X;
Vec2 vec5 = (a2 + b + b2).Solve(-vec3);
Body expr_465_cp_0 = body;
expr_465_cp_0._sweep.C = expr_465_cp_0._sweep.C - body._invMass * vec5;
Body expr_488_cp_0 = body;
expr_488_cp_0._sweep.A = expr_488_cp_0._sweep.A - body._invI * Vec2.Cross(vec, vec5);
Body expr_4AA_cp_0 = body2;
expr_4AA_cp_0._sweep.C = expr_4AA_cp_0._sweep.C + body2._invMass * vec5;
Body expr_4CD_cp_0 = body2;
expr_4CD_cp_0._sweep.A = expr_4CD_cp_0._sweep.A + body2._invI * Vec2.Cross(vec2, vec5);
body.SynchronizeTransform();
body2.SynchronizeTransform();
return(num5 <= Settings.LinearSlop && num <= Settings.AngularSlop);
}
