Phase Two - Rigid Body Simulation

Ok, now we’re getting into some good stuff here. Let's put everything we just covered on the back burner now and talk about some physics. We’re going to be using a very simple Euler integration method. Basically, each frame we accumulate a bunch of forces (in our case from each wheel of the vehicle) and calculate the resultant acceleration, which is in the form of A=F/M (the same as F=MA, Newton’s second law of motion). We use this to modify Newton’s first law of motion, “an object in motion stays in motion…” So we calculate our A, and we integrate it into our V. Without an A, V would be constant, hence staying in motion, if no forces should act on it. Newton's third law gets applied in the form that any potential force the vehicle is applying to the ground, gets applied in the opposite direction to the vehicle (I'll explain this in the vehicle section). This topic is much easier to explain with symbols. So, P is our vehicle position, V is its linear velocity, F is the net force acting on it, M is its mass, A is the resultant acceleration, and T is the time step (the value our timer gave us from the last frame).

  A = F / M
  V = V + A * T
  P = P + V * T

This is a basic linear rigid body simulator. Each frame, we total up some F, integrate it, and then zero out F to restart the accumulation the next frame. Now let’s talk about rotation. The angular case is nearly identical to the linear case (especially in 2D). Instead of P we have an Angle, instead of V we have an Angular Velocity, instead of F we have a torque, and instead of M we have inertia. So the angular model looks like this

  AngA = Torque / Inertia
  AngV = AngV + AngA * T
  Angle = Angle + AngV * T

This is what that bit of code looks like. % is the cross product operator for my vector class.

public void AddForce(Vector worldForce, Vector worldOffset)
{
  //add linar force
  m_forces += worldForce;
  //and it's associated torque
  m_torque += worldOffset % worldForce;
}

Code Dump

Here is my rigid body object. You’ll notice all the properties I mentioned above. It has a Draw function which will draw its rectangle to the provided graphics object. It has an AddForce function, a space conversion method, to and from world space (very handy), and a function that returns the velocity of a point on the body (in world space). This point velocity is a combination of the linear velocity and the angular velocity. But the angular velocity is multiplied by the distance the point is from the center of rotation and perpendicular to its offset direction. So to kill two birds with one stone, I simply find the orthogonal vector to the point offset and multiply it by the angular velocity (then add the linear velocity.)

One thing you may be curious about is how I calculate the inertia value. That is a generalized formula I found at this link.

//our simulation object
class RigidBody
{
  //linear properties
  private Vector m_position = new Vector();
  private Vector m_velocity = new Vector();
  private Vector m_forces = new Vector();
  private float m_mass;

  //angular properties
  private float m_angle;
  private float m_angularVelocity;
  private float m_torque;
  private float m_inertia;

  //graphical properties
  private Vector m_halfSize = new Vector();
  Rectangle rect = new Rectangle();
  private Color m_color;

  public RigidBody()
  { 
    //set these defaults so we dont get divide by zeros
    m_mass = 1.0f; 
    m_inertia = 1.0f; 
  }

  //intialize out parameters
  public void Setup(Vector halfSize, float mass, Color color)
  {
    //store physical parameters
    m_halfSize = halfSize;
    m_mass = mass;
    m_color = color;
    m_inertia = (1.0f / 12.0f) * (halfSize.X * halfSize.X)
       * (halfSize.Y * halfSize.Y) * mass;

    //generate our viewable rectangle
    rect.X = (int)-m_halfSize.X;
    rect.Y = (int)-m_halfSize.Y;
    rect.Width = (int)(m_halfSize.X * 2.0f);
    rect.Height = (int)(m_halfSize.Y * 2.0f);
  }

  public void SetLocation(Vector position, float angle)
  {
    m_position = position;
    m_angle = angle;
  }

  public Vector GetPosition()
  {
    return m_position;
  }

  public void Update(float timeStep)
  {
    //integrate physics
    //linear
    Vector acceleration = m_forces / m_mass;
    m_velocity += acceleration * timeStep;
    m_position += m_velocity * timeStep;
    m_forces = new Vector(0,0); //clear forces

    //angular
    float angAcc = m_torque / m_inertia;
    m_angularVelocity += angAcc * timeStep;
    m_angle += m_angularVelocity * timeStep;
    m_torque = 0; //clear torque
  }

  public void Draw(Graphics graphics, Size buffersize)
  {
    //store transform, (like opengl's glPushMatrix())
    Matrix mat1 = graphics.Transform;

    //transform into position
    graphics.TranslateTransform(m_position.X, m_position.Y);
    graphics.RotateTransform(m_angle/(float)Math.PI * 180.0f);

    try
    {
      //draw body
      graphics.DrawRectangle(new Pen(m_color), rect);

      //draw line in the "forward direction"
      graphics.DrawLine(new Pen(Color.Yellow), 1, 0, 1, 5);
    }
    catch(OverflowException exc)
    {
      //physics overflow :(
    }  

    //restore transform
    graphics.Transform = mat1;
  }

  //take a relative vector and make it a world vector
  public Vector RelativeToWorld(Vector relative)
  {
    Matrix mat = new Matrix();
    PointF[] vectors = new PointF[1];

    vectors[0].X = relative.X;
    vectors[0].Y = relative.Y;

    mat.Rotate(m_angle / (float)Math.PI * 180.0f);
    mat.TransformVectors(vectors);

    return new Vector(vectors[0].X, vectors[0].Y);
  }

  //take a world vector and make it a relative vector
  public Vector WorldToRelative(Vector world)
  {
    Matrix mat = new Matrix();
    PointF[] vectors = new PointF[1];

    vectors[0].X = world.X;
    vectors[0].Y = world.Y;

    mat.Rotate(-m_angle / (float)Math.PI * 180.0f);
    mat.TransformVectors(vectors);

    return new Vector(vectors[0].X, vectors[0].Y);
  }

  //velocity of a point on body
  public Vector PointVel(Vector worldOffset)
  {
    Vector tangent = new Vector(-worldOffset.Y, worldOffset.X);
    return tangent * m_angularVelocity + m_velocity;
  }

  public void AddForce(Vector worldForce, Vector worldOffset)
  {
    //add linar force
    m_forces += worldForce;
    //and it's associated torque
    m_torque += worldOffset % worldForce;
  }
}

Testing

To make sure your rigid body works, instantiate one in your Init() function and apply a force with some offset in the DoFrame function. If you apply a constant worldOffset, the body will continue to accelerate its angular velocity. If you take your offset and run it through the RelativeToWorld function, the body will angularly accelerate in one direction and then come back the other way, like a pendulum as the point the force is applied to changes. Play around with this for a while, this has to work and make sense in order for the next section to work.

Phase Three