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