Various fixes in attempt to get this working. Currently resting contacts appear to be broken. Velocities on resting objects keep building and cause the bodies to explode.
More bug fixes to get everything working. It appears to be working now for two rigid bodies and a static body for the floor.