StepBody function

Topics: Developer Forum
Aug 4, 2014 at 4:50 PM
Edited Aug 4, 2014 at 4:51 PM
Hello, I'm currently working on a multiplayer networked game which required me to have a function to update only a single body. I was unable to find anything quite like it, so I decided to create my own, based upon code from Farseer itself of course.

Unfortunately it did not match my needs for my ongoing project, however, instead of letting it all go to waste, maybe you could use this piece of code in your projects.

This code was placed in Dynamics->World.cs. Cheers.
public void StepBody(Body b, float dt)
        {
            // We have to keep track of our own indices
            int islandIndex = 0;
            b.IslandIndex = islandIndex++;

            ContactManager.Collide();

            float h = dt;

            Vector2 c = b._sweep.C;
            float a = b._sweep.A;
            Vector2 v = b._linearVelocity;
            float w = b._angularVelocity;

            if (b.BodyType == BodyType.Dynamic)
            {
                // FPE: Only apply gravity if the body wants it.
                if (b.IgnoreGravity)
                    v += h * (b._invMass * b._force);
                else
                    v += h * (b.GravityScale * Gravity + b._invMass * b._force);

                w += h * b._invI * b._torque;

                // Apply damping.
                v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
                w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
            }

            List<Contact> bodyContacts = new List<Contact>();
            List<Position> positions = new List<Position>();
            List<Velocity> velocities = new List<Velocity>();
            // Your own position
            positions.Add(new Position { c = c, a = a });
            velocities.Add(new Velocity { v = v, w = w });

            for (ContactEdge ce = b.ContactList; ce != null; ce = ce.Next)
            {
                // Is this contact solid and touching?
                if (ce.Contact.Enabled == false || ce.Contact.IsTouching == false)
                    continue;

                // Save the contact and position / velocity data
                bodyContacts.Add(ce.Contact);
                Body other = ce.Contact.FixtureA.Body == b ? ce.Contact.FixtureB.Body : ce.Contact.FixtureA.Body;
                other.IslandIndex = islandIndex++;
                positions.Add(new Position { c = other._sweep.C, a = other._sweep.A });
                velocities.Add(new Velocity { v = other._linearVelocity, w = other._angularVelocity });
            }

            // Solver data
            TimeStep step;
            step.inv_dt = dt > 0.0f ? 1.0f / dt : 0.0f;
            step.dt = dt;
            step.dtRatio = _invDt0 * dt;
            _invDt0 = step.inv_dt;

            // Create a solver with our data
            ContactSolver solver = new ContactSolver();
            solver.Reset(step, bodyContacts.Count, bodyContacts.ToArray(), positions.ToArray(), velocities.ToArray());
            solver.InitializeVelocityConstraints();

            if (Settings.EnableWarmstarting)
            {
                solver.WarmStart();
            }

            // Solve velocity constraints.
            for (int i = 0; i < Settings.VelocityIterations; ++i)
            {
                solver.SolveVelocityConstraints();
            }

            // Retrieve updated velocities
            v = solver._velocities[0].v;
            w = solver._velocities[0].w;

            // Integrate positions
            // Check for large velocities
            Vector2 translation = h * v;
            if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
            {
                float ratio = Settings.MaxTranslation / translation.Length();
                v *= ratio;
            }

            float rotation = h * w;
            if (rotation * rotation > Settings.MaxRotationSquared)
            {
                float ratio = Settings.MaxRotation / Math.Abs(rotation);
                w *= ratio;
            }

            // Integrate
            c += h * v;
            a += h * w;
            
            // Set state back to solver
            solver._positions[0].c = c;
            solver._positions[0].a = a;
            solver._velocities[0].v = v;
            solver._velocities[0].w = w;

            for (int i = 0; i < Settings.PositionIterations; ++i)
            {
                if (solver.SolvePositionConstraints()) break;
            }

            // Copy state buffers back to the bodies
            b._sweep.C = solver._positions[0].c;
            b._sweep.A = solver._positions[0].a;
            b._linearVelocity = solver._velocities[0].v;
            b._angularVelocity = solver._velocities[0].w;
            b.SynchronizeTransform();

            // Clear forces
            if (Settings.AutoClearForces)
            {
                b._force = Vector2.Zero;
                b._torque = 0.0f;
            }

            b.SynchronizeFixtures();

            // Not entirely sure if you need this 
            ContactManager.FindNewContacts();
        }