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