public ConstantVolumeJoint(World argWorld, ConstantVolumeJointDef def)
: base(argWorld.getPool(), def)
{
world = argWorld;
if (def.bodies.Count <= 2)
{
throw new ArgumentException(
"You cannot create a constant volume joint with less than three bodies.");
}
bodies = def.bodies.ToArray();
targetLengths = new float[bodies.Length];
for (int i = 0; i < targetLengths.Length; ++i)
{
int next = (i == targetLengths.Length - 1) ? 0 : i + 1;
float dist = bodies[i].getWorldCenter().sub(bodies[next].getWorldCenter()).length();
targetLengths[i] = dist;
}
targetVolume = getBodyArea();
if (def.joints != null && def.joints.Count != def.bodies.Count)
{
throw new ArgumentException(
"Incorrect joint definition. Joints have to correspond to the bodies");
}
if (def.joints == null)
{
DistanceJointDef djd = new DistanceJointDef();
distanceJoints = new DistanceJoint[bodies.Length];
for (int i = 0; i < targetLengths.Length; ++i)
{
int next = (i == targetLengths.Length - 1) ? 0 : i + 1;
djd.frequencyHz = def.frequencyHz; // 20.0f;
djd.dampingRatio = def.dampingRatio; // 50.0f;
djd.collideConnected = def.collideConnected;
djd.initialize(bodies[i], bodies[next], bodies[i].getWorldCenter(),
bodies[next].getWorldCenter());
distanceJoints[i] = (DistanceJoint) world.createJoint(djd);
}
}
else
{
distanceJoints = def.joints.ToArray();
}
normals = new Vec2[bodies.Length];
for (int i = 0; i < normals.Length; ++i)
{
normals[i] = new Vec2();
}
}
private void createLeg(float s, Vec2 wheelAnchor)
{
Vec2 p1 = new Vec2(5.4f*s, -6.1f);
Vec2 p2 = new Vec2(7.2f*s, -1.2f);
Vec2 p3 = new Vec2(4.3f*s, -1.9f);
Vec2 p4 = new Vec2(3.1f*s, 0.8f);
Vec2 p5 = new Vec2(6.0f*s, 1.5f);
Vec2 p6 = new Vec2(2.5f*s, 3.7f);
FixtureDef fd1 = new FixtureDef();
FixtureDef fd2 = new FixtureDef();
fd1.filter.groupIndex = -1;
fd2.filter.groupIndex = -1;
fd1.density = 1.0f;
fd2.density = 1.0f;
PolygonShape poly1 = new PolygonShape();
PolygonShape poly2 = new PolygonShape();
if (s > 0.0f)
{
Vec2[] vertices = new Vec2[3];
vertices[0] = p1;
vertices[1] = p2;
vertices[2] = p3;
poly1.set(vertices, 3);
vertices[0] = new Vec2();
vertices[1] = p5.sub(p4);
vertices[2] = p6.sub(p4);
poly2.set(vertices, 3);
}
else
{
Vec2[] vertices = new Vec2[3];
vertices[0] = p1;
vertices[1] = p3;
vertices[2] = p2;
poly1.set(vertices, 3);
vertices[0] = new Vec2();
vertices[1] = p6.sub(p4);
vertices[2] = p5.sub(p4);
poly2.set(vertices, 3);
}
fd1.shape = poly1;
fd2.shape = poly2;
BodyDef bd1 = new BodyDef(), bd2 = new BodyDef();
bd1.type = BodyType.DYNAMIC;
bd2.type = BodyType.DYNAMIC;
bd1.position = m_offset;
bd2.position = p4.add(m_offset);
bd1.angularDamping = 10.0f;
bd2.angularDamping = 10.0f;
Body body1 = getWorld().createBody(bd1);
Body body2 = getWorld().createBody(bd2);
body1.createFixture(fd1);
body2.createFixture(fd2);
DistanceJointDef djd = new DistanceJointDef();
// Using a soft distance constraint can reduce some jitter.
// It also makes the structure seem a bit more fluid by
// acting like a suspension system.
djd.dampingRatio = 0.5f;
djd.frequencyHz = 10.0f;
djd.initialize(body1, body2, p2.add(m_offset), p5.add(m_offset));
getWorld().createJoint(djd);
djd.initialize(body1, body2, p3.add(m_offset), p4.add(m_offset));
getWorld().createJoint(djd);
djd.initialize(body1, m_wheel, p3.add(m_offset), wheelAnchor.add(m_offset));
getWorld().createJoint(djd);
djd.initialize(body2, m_wheel, p6.add(m_offset), wheelAnchor.add(m_offset));
getWorld().createJoint(djd);
RevoluteJointDef rjd = new RevoluteJointDef();
rjd.initialize(body2, m_chassis, p4.add(m_offset));
getWorld().createJoint(rjd);
}
public override void initTest(bool argDeserialized)
{
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = getWorld().createBody(bd);
EdgeShape shape = new EdgeShape();
shape.set(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.createFixture(shape, 0.0f);
}
{
PolygonShape shape = new PolygonShape();
shape.setAsBox(0.5f, 0.5f);
BodyDef bd = new BodyDef();
bd.type = BodyType.DYNAMIC;
bd.position.set(-5.0f, 5.0f);
m_bodies[0] = getWorld().createBody(bd);
m_bodies[0].createFixture(shape, 5.0f);
bd.position.set(5.0f, 5.0f);
m_bodies[1] = getWorld().createBody(bd);
m_bodies[1].createFixture(shape, 5.0f);
bd.position.set(5.0f, 15.0f);
m_bodies[2] = getWorld().createBody(bd);
m_bodies[2].createFixture(shape, 5.0f);
bd.position.set(-5.0f, 15.0f);
m_bodies[3] = getWorld().createBody(bd);
m_bodies[3].createFixture(shape, 5.0f);
DistanceJointDef jd = new DistanceJointDef();
Vec2 p1 = new Vec2();
Vec2 p2 = new Vec2();
Vec2 d = new Vec2();
jd.frequencyHz = 4.0f;
jd.dampingRatio = 0.5f;
jd.bodyA = ground;
jd.bodyB = m_bodies[0];
jd.localAnchorA.set(-10.0f, 0.0f);
jd.localAnchorB.set(-0.5f, -0.5f);
p1 = jd.bodyA.getWorldPoint(jd.localAnchorA);
p2 = jd.bodyB.getWorldPoint(jd.localAnchorB);
d = p2.sub(p1);
jd.length = d.length();
m_joints[0] = getWorld().createJoint(jd);
jd.bodyA = ground;
jd.bodyB = m_bodies[1];
jd.localAnchorA.set(10.0f, 0.0f);
jd.localAnchorB.set(0.5f, -0.5f);
p1 = jd.bodyA.getWorldPoint(jd.localAnchorA);
p2 = jd.bodyB.getWorldPoint(jd.localAnchorB);
d = p2.sub(p1);
jd.length = d.length();
m_joints[1] = getWorld().createJoint(jd);
jd.bodyA = ground;
jd.bodyB = m_bodies[2];
jd.localAnchorA.set(10.0f, 20.0f);
jd.localAnchorB.set(0.5f, 0.5f);
p1 = jd.bodyA.getWorldPoint(jd.localAnchorA);
p2 = jd.bodyB.getWorldPoint(jd.localAnchorB);
d = p2.sub(p1);
jd.length = d.length();
m_joints[2] = getWorld().createJoint(jd);
jd.bodyA = ground;
jd.bodyB = m_bodies[3];
jd.localAnchorA.set(-10.0f, 20.0f);
jd.localAnchorB.set(-0.5f, 0.5f);
p1 = jd.bodyA.getWorldPoint(jd.localAnchorA);
p2 = jd.bodyB.getWorldPoint(jd.localAnchorB);
d = p2.sub(p1);
jd.length = d.length();
m_joints[3] = getWorld().createJoint(jd);
jd.bodyA = m_bodies[0];
jd.bodyB = m_bodies[1];
jd.localAnchorA.set(0.5f, 0.0f);
jd.localAnchorB.set(-0.5f, 0.0f);
;
p1 = jd.bodyA.getWorldPoint(jd.localAnchorA);
p2 = jd.bodyB.getWorldPoint(jd.localAnchorB);
d = p2.sub(p1);
jd.length = d.length();
m_joints[4] = getWorld().createJoint(jd);
jd.bodyA = m_bodies[1];
jd.bodyB = m_bodies[2];
jd.localAnchorA.set(0.0f, 0.5f);
jd.localAnchorB.set(0.0f, -0.5f);
p1 = jd.bodyA.getWorldPoint(jd.localAnchorA);
p2 = jd.bodyB.getWorldPoint(jd.localAnchorB);
d = p2.sub(p1);
jd.length = d.length();
m_joints[5] = getWorld().createJoint(jd);
jd.bodyA = m_bodies[2];
jd.bodyB = m_bodies[3];
jd.localAnchorA.set(-0.5f, 0.0f);
jd.localAnchorB.set(0.5f, 0.0f);
p1 = jd.bodyA.getWorldPoint(jd.localAnchorA);
p2 = jd.bodyB.getWorldPoint(jd.localAnchorB);
d = p2.sub(p1);
jd.length = d.length();
m_joints[6] = getWorld().createJoint(jd);
jd.bodyA = m_bodies[3];
jd.bodyB = m_bodies[0];
jd.localAnchorA.set(0.0f, -0.5f);
jd.localAnchorB.set(0.0f, 0.5f);
p1 = jd.bodyA.getWorldPoint(jd.localAnchorA);
p2 = jd.bodyB.getWorldPoint(jd.localAnchorB);
d = p2.sub(p1);
jd.length = d.length();
m_joints[7] = getWorld().createJoint(jd);
}
}
