Physics simulation overview (an F# tutorial)11-Feb-11

type scalar = float
type vec    = { x:scalar; y:scalar }

type poly =
  { lverts:vec array;
    mutable verts:vec array }

type shape = poly

val poly : vec array -> shape

type body =
  { shape:shape

    mass:scalar
    inertia:scalar

    mutable pos:vec
    mutable vel:vec
    mutable ang:scalar
    mutable rot:scalar }

val body : shape -> vec -> body
val body_static : shape -> vec -> body

type contact = { a:body; b:body; p:vec; .. }

type space =
  { bodies:body list
    mutable contacts:contact ResizeArray }

val space : body list -> space
val update_space : space -> unit

(Tentative code.)
First, a look at the main public definitions that the engine will expose in simplified form.

We'll only support polygon shapes and a single shape per body.

body holds its temporal properties and a reference to shape, which describes its geometry. The body acts as a point of origin for the shape.

space represents the simulation space. An arbiter is an object handling the collision between two bodies, so when two bodies are colliding or touching in the present state, there will be an arbiter present in the list.

update_space will update the positions of bodies and the list of arbiters. The 'inside' of this procedure is called a step.

type id = int
type collision = { pts:(vec*id) list; n:vec; dist:scalar }

val collision : shape -> shape -> collision option

type contact =
  { a:body; b:body; id:int; p:vec
    mutable j:vec
    mutable snap:scalar
    perform:unit->unit }

val contact :
  body -> body ->
  (dist:scalar, n:vec, p:vec, id:id) -> j:vec ->
    contact

Let's also outline the internal definitions used to perform the step.

collision describes the direction, distance and list of points of an intersection. We attach an id to each point so we can identify matching points across steps.

Each point is matched to a contact, which represents an in-progress collision resolution instance. The perform method effects the correction of the contact. We'll call this in the cycle.

contact represents an in-progress collision resolution instance. The mutable fields are updated by perform_contact when calculating the resulting force the contact should exert on the pertaining bodies.

Note: the collision function only deals with shape data, and contact only with body. You can make a clean mental separation.

type scalar = float

let sq x = x*x
let inv x = 1. / x

type [<Struct>] vec =
  val x:scalar
  val y:scalar
  new(x,y) = {x=x;y=y}
  static member (~-) (a:vec) = vec(-a.x,-a.y)

let (+|) (a:vec) (b:vec) = vec(a.x+b.x, a.y+b.y)
let (-|) (a:vec) (b:vec) = vec(a.x-b.x, a.y-b.y)
let (.*) (a:scalar) (b:vec) = vec(a*b.x,a*b.y)

let vperp (a:vec) = vec(-a.y,a.x)

let dot (a:vec) (b:vec) = a.x*b.x+a.y*b.y

let vsq v = dot v v
let vlen v = sqrt (vsq v)
let vunit (a:vec) = vec(a.x/vlen a, a.y/vlen a)

let vrotate (a:vec) (b:vec) =
  vec(a.x*b.x - a.y*b.y, a.y*b.x + a.x*b.y)

let vpolar a = vec(cos a,sin a)

Vector definitions.

• add
• sub
• scalar multiplication
• right normal
• dot product
• square
• length
• unit vector
• rotate (adds angles)
• polar to coordinate

type [<Struct>] axis =
  val n:vec
  val d:scalar
  new(n,d) = {n=n;d=d}
  static member (~-) (a:axis) = axis(-a.n,a.d)

An axis consists of a unit vector and distance scalar. It's used to represent projections of shapes onto a particular vector.

type poly =
  { lverts:vec array
    laxes:axis array
    mutable verts:vec array
    mutable axes:axis array }

let sides (xs:_ array) =
  seq { for i in 0 .. xs.Length-1 ->
          xs.[i], xs.[(i+1)%xs.Length] }

let poly vs = 
  { lverts = vs
    laxes =
      [| for v1,v2 in sides vs ->
           let n = v2-|v1 |> vunit |> vperp
           axis(n, dot n v1) |]
    verts = null; axes = null }

Creation of poly. In anticipation of overlap checking, we precalulate the normal of each side, and also get the distance of the side projected onto the normal.

Vertices are assumed to be counter-clockwise.

type [<ReferenceEquality>] body =
  { shape:shape

    mass:scalar
    inertia:scalar

    mutable pos:vec
    mutable vel:vec
    mutable ang:scalar
    mutable rot:scalar
    mutable snap:vec
    mutable asnap:scalar }

let body d s pos =
  let a,i = 500.,500.
  { shape = s
    mass = d*a; inertia = d*i*a

    pos = pos;    ang = 0.
    vel = vec();  rot = 0.
    snap = vec(); asnap = 0. }

let body_static = body infinity

In addition to position and velocity, a body holds snap (and asnap for angle) - an 'instant' velocity used to correct the position of a body within a step. We calculate it by considering all intersections to be resolved, then end of the step we add it to the position and reset to zero.

We'll use dummy area and inertia values to skip some math.

We can have unmovable bodies by simply assigning infinite mass and inertia.

let apps b j r =
  b.snap  <- b.snap  +| inv b.mass .* j
  b.asnap <- b.asnap +  inv b.inertia * dot j (vperp r)

let appj b j r =
  b.vel <- b.vel +| inv b.mass .* j
  b.rot <- b.rot +  inv b.inertia * dot j (vperp r)

We'll use these to apply velocity and position corrections.

j is an impulse (an instantaneous force). r is the displacement vector of the point at which the impulse is applied. When j and r are perpendicular, it causes torque.

let update_shape b p =
  let dir = vpolar b.ang
  p.verts <-
    [| for v in p.lverts ->
         b.pos +| vrotate dir v |]
  p.axes <-
    [| for a in p.laxes ->
         let n = vrotate a.n dir
         axis(n, dot n b.pos + a.d) |]

let gravity = vec(0.,200.)

let update_body dt b =
  b.pos <- b.pos +| dt.*b.vel +| b.snap
  b.ang <- b.ang +  dt *b.rot +  b.asnap

  b.snap <- vec()
  b.asnap <- 0.

  if b.mass <> infinity then
    b.vel <- b.vel +| dt.*gravity

  update_shape b b.shape

Here's the entire sequence that makes bodies move.

update_shape uses the body position to transform the local polygon vertices and axes into global coordinates.

let (+>) v f = Option.bind f v
let enum = Seq.mapi (fun i x -> i,x)


type collision = { pts:(vec*id) list; n:vec; dist:scalar }

let sepaxis p1 p2 =
  let axes =
    [| for a in p1.axes ->
         let nvs = Array.map (dot a.n) p2.verts
         axis(a.n, Array.min nvs - a.d) |]

  let a = axes |> Array.maxBy (fun a -> a.d)
  if a.d > 0.
    then None
    else Some a

let containsv poly v =
  poly.axes |> Array.forall (fun a -> dot a.n v < a.d) 

let findvs p1 p2 (a:axis) =
  { pts = 
      [ for i,v in enum p1.verts do
          if containsv p2 v then yield v, i
        for i,v in enum p2.verts do
          if containsv p1 v then yield v, 100+i ]
    n = a.n; dist = a.d }

let poly_poly p1 p2 =
  sepaxis p1 p2 +> fun a1 ->
  sepaxis p2 p1 +> fun a2 ->
    findvs p1 p2 (if a1.d > a2.d then -a1 else a2)
    |> Some

let collision = poly_poly

On to collision detection.

The separating axis theorem says that polygons are intersecting if their projection on any axis is overlapping. We already calculated axes for each side of the polygon when creating poly, where the d field of the axis is the projection of the side onto the normal. It acts as a zero point on the axis, so we can get whether a vertex v is over or under the line of the side with dot a.n v - a.d.

We implement this projection in sepaxis:

We're only interested in the deepest opposing point, so we grab the smallest projected value with Array.min. The subtraction is the distance between p1's side and p2's closest corner. If it's negative, the projections are overlapping on that axis.

Now, we're only interested in the axis with the smallest intersecting distance (it would be the highest numerically, so max), because that axis is the best direction to push the bodies apart (and likely the direction the intersection came from).

If the distance is actually positive, the polygons are not intersecting and we report no axis.

Jump to poly_poly. We grab the closest axis from each body, using +> to default to None if either returns None. We select the smallest axis and pass it to findvs, which simply lists all vertices that are found to be inside the other polygon. The returned axis must be directed at the first shape.

This diagram shows projection of the other shape onto one axis, and the deepest point min is selected. This is also done for all other axes, but finally this axis is selected because it has the smallest intersection distance (the deepest point for a3 would be the top one on the shape).
Then the same projections are done using the axes of the other shape, but still the shown axis will be selected over the other result. Thus this axis will be used to push the bodies apart in a vertical direction.

let (|?>) v f = let v' = f v in (v', v' - v)
let clamp x = min x >> max -x

type contact =
  { a:body; b:body; id:int; p:vec
    mutable j:vec; perform:unit->unit }

let contact a b (dist,n,p,id) jacc =
  let r1 = p-|a.pos
  let r2 = p-|b.pos

  let kin n =
    inv <| inv a.mass + inv b.mass
      + inv a.inertia * sq (dot r1 (vperp n))
      + inv b.inertia * sq (dot r2 (vperp n))

  let mn = kin n
  let mt = kin (vperp n)
  let snapt = 0.2 * -min 0. (dist + 0.2)

  let app f j = f a -j r1; f b j r2

  let rec ct =
  { a = a; b = b; id = id; p = p
    j = jacc; perform = perform }

  and perform () =
    let rel bv br av ar =
      bv  +| br  .* vperp r2 -| av -| ar .* vperp r1

    let v = rel b.vel  b.rot   a.vel  a.rot
    let s = rel b.snap b.asnap a.snap a.asnap

    let sn = mn * (snapt - dot s n)

    if sn>0. then app apps (sn .* n)

    let jn = mn * -dot v n
    let jt = mt * -dot v (vperp n)

    let jnacc,jn = ct.j.x |?> ((+) jn >> max 0.)
    let jtacc,jt = ct.j.y |?> ((+) jt >> clamp (jnacc*0.6))

    ct.j <- vec(jnacc,jtacc)
    app appj (vrotate n (vec(jn,jt)))      

  app appj (vrotate n ct.j)
  ct

And finally, the object used for handling each intersection point. Its task is to calculate how to push apart the two associated bodies.

All creation parameters come from collision data, except one – jacc. This is the accumulated impulse, if any, that this particular contact ended up with at the end of the last step. This is the one value that persists across steps.

The actual pushing is done with the app definition. It applies the given impulse j in opposing directions, using the moment arm of each body.

We create the contact record and alias jacc to the mutable ct.j field.

The impulses are applied using the direction of the given normal n. The accumulated impulse ct.j is not stored in world coordinates. That's why before using app we rotate the impulse to n.

At the bottom is the first (imperative) thing we do, which is to apply ct.j to the velocity of the bodies for good measure.
Then we return the record. After this perform will be called.

We precalculate two things. The snap target snapt specifies the distance the bodies' positions should be corrected in the current step. It's a softened function of dist to prevent too much jumpiness. dist is negative here, so we negate it.

mn mt are the effective mass for the normal and tangent direction. They have something to do with how much the bodies are leaning on the contact point, and are used to scale up the necessary impulse.

Into perform. This is the part that is called in a cycle, so every time our bodies might have been affected by other contacts, so we must recalculate the current state, then reapply the corrections.

rel is used to calculate the current relative velocity between the bodies at the contact point. If the velocity is negative along n, the bodies are moving toward each other (so we want to make the velocity positive).

First we take the relative snap velocity dot s n. If the velocity is less than snapt, we apply snap.

Similarly for velocity, we calculate a counter-impulse, with the added step of adding it to the accumulated impulse and then constraining the result. The clamp constraint means that the friction impulse (along the tangent) can only be as strong as a quotient of the normal impulse.

The |?> operator is like |>, but also returns the delta between the old and new values.

That's it.

With respect to figuring out how to distribute velocity, the initial application of ct.j can be seen an 'initial guess' based on the state the contact ended up in in the last step. Subsequent calls to perform refine the impulse in case it over- or undershot, or the position of the bodies changed slightly since the last step.

type space =
  { dt:scalar; bodies:body list
    mutable contacts:contact ResizeArray }

let space dt bs =
  { dt = dt; bodies = bs
    contacts = ResizeArray() }


let rec pairs f =
  function
  | x::xs -> List.iter (f x) xs; pairs f xs
  | [] -> ()

let contacts s =
  let cts = ResizeArray()

  let check a b =
    let reg col = 
      for p,id in col.pts do
        let same ct = (ct.a,ct.b,ct.id) = (a,b,id)
        match Seq.tryFind same s.contacts with
          | None -> vec()
          | Some ct -> ct.j

        |> contact a b (col.dist,col.n,p,id)
        |> cts.Add

    collision a.shape b.shape |> Option.iter reg

  pairs check s.bodies
  s.contacts <- cts


let update_space s =
  for b in s.bodies do
    update_body s.dt b

  contacts s

  for _ in 1 .. 5 do
    for ct in s.contacts do
      ct.perform ()

And the rest is the bookkeeping part that does no math, here minimized to just one bit of code for matching up contacts, and then the step procedure.