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); }
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); }
void killWheelOrthogonalVelocity(Body hull, Body wheel) { var p_hull = hull.GetPosition(); var p_wheel = wheel.GetPosition(); var velocity = wheel.GetLinearVelocityFromLocalPoint(Vec2.Zero); var vec1 = wheel.GetXForm().R.Col1; var projection = Vec2.Dot(velocity, vec1); vec1 *= projection; var k = vec1.Length() / 0.001f; // превышение боковой скорости 1 мм / с if (k < 0.1f) k = 0.1f; if (k > 10f) k = 10f; float force = -k * 3000; //warning - mn vec1.Normalize(); hull.ApplyForce(force * vec1, p_wheel); }