/// Initialize the bodies and offsets using the current transforms. // Point-to-point constraint // Cdot = v2 - v1 // = v2 + cross(w2, r2) - v1 - cross(w1, r1) // J = [-I -r1_skew I r2_skew ] // Identity used: // w k % (rx i + ry j) = w * (-ry i + rx j) // Angle constraint // Cdot = w2 - w1 // J = [0 0 -1 0 0 1] // K = invI1 + invI2 public void Initialize(Body bA, Body bB) { bodyA = bA; bodyB = bB; Vec2 xB = bodyB.GetPosition(); linearOffset = bodyA.GetLocalPoint(xB); float angleA = bodyA.GetAngle(); float angleB = bodyB.GetAngle(); angularOffset = angleB - angleA; }
public Body AddNode(Body parent, Vec2 localAnchor, int depth, float offset, float a) { Vec2 h = new Vec2(0.0f, a); Vec2 p = parent.GetPosition() + localAnchor - h; BodyDef bodyDef = new BodyDef(); bodyDef.type = BodyType._dynamicBody; bodyDef.Position = p; Body body = m_world.CreateBody(bodyDef); PolygonShape shape = new PolygonShape(); shape.SetAsBox(0.25f * a, a); shape.Density = 20; body.CreateFixture(shape); if (depth == e_depth) { return body; } shape.SetAsBox(offset, 0.25f * a, new Vec2(0, -a), 0.0f); body.CreateFixture(shape); Vec2 a1 = new Vec2(offset, -a); Vec2 a2 = new Vec2(-offset, -a); Body body1 = AddNode(body, a1, depth + 1, 0.5f * offset, a); Body body2 = AddNode(body, a2, depth + 1, 0.5f * offset, a); RevoluteJointDef jointDef = new RevoluteJointDef(); jointDef.bodyA = body; jointDef.localAnchorB = h; jointDef.localAnchorA = a1; jointDef.bodyB = body1; m_world.CreateJoint(jointDef); jointDef.localAnchorA = a2; jointDef.bodyB = body2; m_world.CreateJoint(jointDef); return body; }
static void Main(string[] args) { // Define the size of the world. Simulation will still work // if bodies reach the end of the world, but it will be slower. AABB worldAABB = new AABB { UpperBound = new Vector2(100, 100), LowerBound = new Vector2(-100, -100) }; // Define the gravity vector. Vector2 gravity = new Vector2(0.0f, -10.0f); // Do we want to let bodies sleep? bool doSleep = true; // Construct a world object, which will hold and simulate the rigid bodies. World world = new World(worldAABB, gravity, doSleep); // Define the ground body. BodyDef groundBodyDef = new BodyDef(); groundBodyDef.Position = new Vector2(0.0f, -10.0f); // 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(50.0f, 10.0f); // Add the ground shape to the ground body. groundBody.CreateFixture(groundShapeDef); // Define the dynamic body. We set its position and call the body factory. BodyDef bodyDef = new BodyDef(); bodyDef.Position = new Vector2(0.0f, 4.0f); Body body = world.CreateBody(bodyDef); // Define another box shape for our dynamic body. PolygonDef shapeDef = new PolygonDef(); shapeDef.SetAsBox(1.0f, 1.0f); // Set the box density to be non-zero, so it will be dynamic. shapeDef.Density = 1.0f; // Override the default friction. shapeDef.Friction = 0.3f; // Add the shape to the body. body.CreateFixture(shapeDef); // Now tell the dynamic body to compute it's mass properties base // on its shape. body.SetMassFromShapes(); // Prepare for simulation. Typically we use a time step of 1/60 of a // second (60Hz) and 10 iterations. This provides a high quality simulation // in most game scenarios. float timeStep = 1.0f / 60.0f; int velocityIterations = 8; int positionIterations = 1; // This is our little game loop. for (int i = 0; i < 100; ++i) { // Instruct the world to perform a single step of simulation. It is // generally best to keep the time step and iterations fixed. world.Step(timeStep, velocityIterations, positionIterations); // Now print the position and angle of the body. Vector2 position = body.GetPosition(); float angle = body.GetAngle(); Console.WriteLine("Step: {3} - X: {0}, Y: {1}, Angle: {2}", new object[] { position.X.ToString(), position.Y.ToString(), angle.ToString(), i.ToString() }); } // When the world destructor is called, all bodies and joints are freed. This can // create orphaned pointers, so be careful about your world management. Console.ReadLine(); }