myPhysicsLab Documentation

ContactSim Math

This describes details of the math involved in the class ContactSim which is part of the 2d Rigid Body Physics Engine of myPhysicsLab.

Resources

Please see 2D Physics Engine Overview which has important information and references.

The two main references are:

Information about myPhysicsLab software in general:

Calculate the ‘A’ Matrix

This section describes the math behind the private method ContactSim.calculate_a_matrix().

ContactSim.calculate_a_matrix() calculates the A matrix which specifies how contact points react to contact forces. It returns a matrix where the (i, j)th entry is how much the relative normal acceleration at contact i will change from a unit force being applied at contact j.

For a particular contact point i, let the bodies be numbered 1 and 2, with body 2 specifying the normal vector pointing out from body 2 into body 1.

Let p1 be the point on body 1 that is in contact with the point p2 on body 2.

Let di be the distance between p1 and p2.

Because the bodies are in resting contact, it should be the case that di = 0 (within numerical tolerance).

Resting contact also implies that the velocity of separation should be di' = 0 (otherwise, the bodies are moving apart).

However, the acceleration di'' is likely to be non-zero. If di'' > 0, then the bodies are about to separate, and the reaction force should be zero. If di'' < 0, then the bodies are accelerating into each other, and a reaction force is needed to prevent them from interpenetrating.

We need to find reaction forces that will just barely ensure that di'' = 0.

This method calculates the A matrix in the equation a = A f + b. The i-j-th entry in the A matrix, Aij, specifies how the j-th contact force, fj, affects the acceleration of the i-th contact distance, ai = di''. Our goal is to find this expression for di'' (Equation D-1 of the Baraff '94 paper)

(D-1)  di'' = Ai1 f1 + Ai2 f2 + ... + Ain fn + bi

where bi is from the b vector which specifies external forces (gravity, thrust, etc.).

A contact force fj only affects di'' if that contact force operates on body 1 or body 2. If that is not the case, we have the Aij = 0. Assume now that fj affects body 1 or body 2. We can find Aij from the geometry of the situation as follows.

Let Nj be the vector normal at the j-th contact point. Then the contact force is fj Nj (note that fj is a scalar). From the geometry we can calculate the effect of the force on p1'' and p2''. (Keep in mind that the reaction force is equal and opposite for the two bodies.) The effect on di'' is given by equation D-2 of the Baraff '94 paper:

(D-2)   di'' = Ni.(p1'' - p2'') + 2 Ni'.(p1' - p2')

The term 2 Ni'.(p1' - p2') “is a velocity dependent term (i.e. you can immediately calculate it without knowing the forces involved), and is part of bi”. See method calculate_b_vector. So the fj dependent part of (D-2) is just

Ni.(p1'' - p2'').

Here is a quick derivation of equation (D-3) based on section C.3 of the Baraff '94 paper.

Let p(t) be the world space coordinates of a point on a rigid body.
Let R(t) be the vector from center of mass to p(t)
Let X(t) be the center of mass
Let V(t) be velocity of center of mass
Let w(t) be the angular velocity (about the CM) of the rigid body
Then p(t) = X(t) + R(t)

Taking derivatives:

p'(t) = X'(t) + R'(t) = V(t) + w(t) x R(t)

Here we use the knowledge that R(t) is only changing by rotation at a rate of w(t). An elementary calculus result then gives the result that R'(t) = w(t) x R(t). (See derivation of Ni' in calculate_b_vector which shows the calculus steps). Taking another derivative:

p''(t) = V'(t) + w'(t) x R(t) + w(t) x R'(t)
    = V'(t) + w'(t) x R(t) + w(t) x (w(t) x R(t))

(Baraff’s derivation in section C.3 is slightly more elaborate, and I don’t entirely understand why its necessary).

Two cases to consider here: whether the force is acting on body 1 or body 2. Suppose fj is acting on body 1. Then fj will contribute to p1''. (Otherwise if fj is acting on body 2, then fj will contribute to p2''.)

NEED: Derivation of following equation D-3.

Equation D-3 of the Baraff '94 paper gives an expression for p1''

(D-3)  p1'' = v1' + w1' x R1 + w1 x (w1 x R1)

where

v1 = linear velocity of center of mass of body 1,
w1 = angular velocity of body 1
R1 = vector from center of mass to point of contact p1

The term w1 x (w1 x R1) is velocity dependent, so it goes into the b vector. The fj dependent part of (D-3) is therefore

p1'' = v1' + w1' x R1

Because v1' is the linear acceleration of body 1, we have by Newton’s First Law

v1' = (total force on body 1)/ m1

where m1 = mass of body 1. Therefore the the force fj Njcontributes

(D-4)  fj Nj / m1

to v1'.

Next we find the term w1' x R1 in (D-3). Equation C-10 of the Baraff '94 paper gives w1' as

(C-10)  w1' = I1^-1 (t1 + L1 x w1)

where

I1 = the rotational inertia (about what axis? CM?  is it a vector?)
t1 = torque,
L1 = angular momentum.

I think that I1 is a scalar quantity, so I don’t know why he uses I1^-1 instead of dividing by I1. Additionally, the L1 vector is also perpendicular to the plane in 2D, as is w1, so L1 x w1 = 0. (In 3D, this could be non-zero, but it would go into the b vector since it is velocity dependent). So we are left with simply:

w1' = t1 / I1

To find t1, suppose that the j-th contact occurs at the point pj, and the vector Rj goes from center of mass of object 1 to pj. Then the force fj Nj contributes a torque of

Rj x fj Nj

So (C-10) becomes

w1' = (Rj x fj Nj) / I1

and we can write the fj dependent part of (D-3) as

p1'' = fj Nj / m1 + (Rj x fj Nj) x R1 / I1
   = fj (Nj / m1 + (Rj x Nj) x R1 / I1

and the fj dependent part of (D-2) is

di'' = fj Ni.(Nj / m1 + (Rj x Nj) x R1 / I1)

Therefore from (D-1) we can see that the fj dependent part, which is Aij, is:

Aij = Ni.(Nj / m1 + (Rj x Nj) x R1 / I1)

Next, we expand express the vector cross product (Rj x Nj) x R1, to help with writing the computer code. All these vectors lie in the plane initially.

Rj x Nj = [0, 0, Rjx Njy - Rjy Njx]
(Rj x Nj) x R1 = [0, 0, Rjx Njy - Rjy Njx] x [R1x, R1y, 0]
= (Rjx Njy - Rjy Njx) [-R1y, R1x, 0]

Then we can expand the dot product:

Aij = Nix (Njx / m1 + (Rjx Njy - Rjy Njx)(-R1y) / I1) +
    Niy (Njy / m1 + (Rjx Njy - Rjy Njx) R1x / I1)

Keep in mind that this is only the effect of fj on p1''. The complete picture is given in equation (D-2) above, so there can be a contribution from p2'' to Aij as well.

The above assumed that Nj pointed out of body 2 into body 1, that body 2 is the “normal object”. Actually this can vary for each contact. In the j-th contact, it could be that body 1 is the “normal object” and therefore Nj points out of body 1 into some other body. In that case we need to use -fj Nj as the force in the above analysis.

The above assumed that fj was affecting body 1. If fj is affecting body 2, then the analysis is identical except the effect is on p2''. Note that p2 has a negative sign in equation D-2, so we need to multiply the resulting Aij by -1. And of course you use m2 instead of m1, I2 instead of I1, R2 instead of R1.

So there are four cases, which we list with the overall factor needed for each case. (The factor is determined by the sign on fj and the sign of p1 or p2 in equation D-2).

fj affects body 1
  body 1 is primary object in contact j -->  fj affects p1  --> +1
  body 1 is normal object in contact j -->  -fj affects p1 --> -1

fj affects body 2
  body 2 is primary object in contact j -->  fj affects p2  --> -1
  body 2 is normal object in contact j -->  -fj affects p2 --> +1

Terminology: “primary” object just means it is the non-normal object in the contact, it is the object whose corner is colliding. See the fields of RigidBodyCollision object.

Note that in the case where fj affects both bodies 1 and 2 (which happens for the contact force at the contact point) then Aij is the sum of these two.

Contact Force on CircularEdge

For CircularEdge, there are some other differences when calculating the b-vector, but here the only difference is we use the U vector instead of the R vector.

The U vector is from center of mass to the center of the circular edge. See the Curved Edge Physics paper, where we define the gap in terms of not the two points at the contact, but instead by the center of the circle and one of the contact points. The center of the circle on body 2 is at

C2(t) = X2(t) + U2(t).

The point of contact on the other body 1 is at

P1(t) = X1(t) + R1(t).

From the Curved Edge Physics paper, the gap is N . (P1 - C2). We then take derivatives, and the result involves U2.

Another fact about using U vector vs. R vector: When calculating the effect of a force F at the contact in producing torque T, we have a calculation that is like

T = F x R

It turns out that this is the same when substituting U:

T = F x R = F x U

It has to do with both R and U being on the line of the normal thru the contact point. See docs for RigidBodyCollision for a diagram and explanation; the section is called “Equivalence of Using R or U Vector For Normal Velocity”.

Calculate the ‘b’ Vector

This section describes the math behind the method ContactSim.calculate_b_vector() which calculates the b vector which specifies how external forces (like gravity, thrust, etc) affect acceleration of contact points.

Background

In calculate_a_matrix, we found for di'' the parts that were dependent on the contact forces fi. Here we find the part of di'' that is independent of the fi’s, such as gravity, thrust, rubber band force, and damping. For the purposes of calculating the contact forces we regard these forces as “constant” (at this moment in time) in the matrix equation a = A f + b.

These “constant” or “external” forces are put into the b vector, so that we can then solve the matrix equation a = A f + b, subject to the constraints that f >= 0, a >= 0, and f.a = 0. Those constraints say that reaction forces can only push things apart, that objects cannot interpenetrate, and that if objects are separating then there is no reaction force.

Derivation

We start with the expression for di'' = the acceleration of the distance between the contact points p1, p2. This was derived above as:

(D-2)   di'' = Ni.(p1'' - p2'') + 2 Ni'.(p1' - p2')

The term Ni.(p1'' - p2'') involves accelerations and therefore forces, so we need to include the effect of any forces other than the reaction forces on the accelerations of the contact points p1 and p2. We will examine each of those forces (such as gravity) and determine through the geometry of the situation how the force affects the acceleration of each contact point.

The term 2 Ni'.(p1' - p2') is dependent only on current velocity, not acceleration, and therefore is independent of any forces being applied, and therefore belongs in the b vector.

Derivation of Ni'

Ni = (Nix, Niy, 0)

The vector Ni is rotating at a rate w. If we ignore any acceleration, we could write the vector Ni as a function of time like this:

Ni = |Ni| (cos wt, sin wt, 0)

Where

|Ni| is the magnitude of the vector Ni
w = angular velocity
t = time

For some particular value of time t, this will be equal to (Nix, Niy, 0). Now elementary calculus gives us the derivative:

Ni' = |Ni| (-w sin wt, w cos wt, 0)

And we can recognize that this is equivalent to:

Ni' = w (-Niy, Nix, 0)

Because we picked the value of t such that Nix = |Ni| cos wt, and Niy = |Ni| sin wt. Note that this can also be expressed as a cross product of two

vectors:

Ni' = W x Ni

where we treat angular velocity as a vector W with components (0, 0, w).

Derivation of p1'

This is the velocity of a particular point, p1, on the object. The object is translating and rotating. The translation is given by the velocity of the center of mass, V = (Vx, Vy, 0), and the rotation is given by the angular velocity w.

Let R be the vector from the center of mass to the point.
Let X be the vector from the origin to the center of mass of the object.
Let V be the velocity of the center of mass, so that V = X'.
Let W be the angular velocity of the object, in 2D we have W = (0, 0, w).

Then the point p1 is given by

p1 = X + R

Taking derivatives, we get

p1' = X' + R' = V + W x R

where we used the vector cross product method of finding R' (see above derivation of Ni'). This then expands to

p1' = (Vx, Vy, 0) + w (-Ry, Rx, 0)
p1' = (Vx - w Ry, Vy + w Rx, 0)

Expansion of 2 Ni'.(p1' - p2')

To bring together the entire expression 2 Ni'.(p1' - p2') we need to recognize that each body has its own angular velocity w. The w used in calculating Ni' is that of the “normal” object, which is always body 2 in our scheme, or w2. Here is the complete expansion using subscripts 1 and 2 to refer to body 1 or 2 of the contact.

2 Ni'.(p1' - p2')
= 2 w2 (-Niy, Nix, 0) . ((V1x - w1 R1y, V1y + w1 R1x, 0)
  - (V2x - w2 R2y, V2y + w2 R2x, 0))
= 2 w2 (-Niy (V1x - w1 R1y - V2x + w2 R2y) +  Nix (V1y + w1 R1x - V2y - w2 R2x)
  +  0)

External forces in Ni.(p1'' - p2'')

Next we examine the “external” forces (all forces other than the contact forces we are trying to solve for) and determine their contribution to the acceleration of the separation of the contact points. As explained above, these show up in the term Ni.(p1'' - p2'').

Equation D-3 of the Baraff '94 paper gives an expression for p1''

(D-3)  p1'' = v1' + w1' x R1 + w1 x (w1 x R1)

(this magic needs to be elucidated) where

v1' = acceleration of center of mass
w1 = angular velocity
w1' = angular acceleration (= torque? I think not!)
R1 = vector from CM to contact point p1

The corresponding expression for p2'' is

p2'' = v2' + w2' x R2 + w2 x (w2 x R2)

It turns out that the external forces have already been calculated for us by the earlier processes (see evaluate method of RigidBodySim class). In the change variable we get passed in the current values for x'', y'', th''. These accelerations are the result of all the external forces such as gravity, thrust, rubber bands, damping. They operate on each object regardless of the contact forces that are applied. So we only have to plug these in to the above equations.

Ni.(p1'' - p2'') =
Ni.( (v1' + w1' x R1 + w1 x (w1 x R1)) - (v2' + w2' x R2 + w2 x (w2 x R2)))

We can expand the above as follows. (Keep in mind that we regard w' and w as vectors perpendicular to the plane for purposes of vector cross products.)

w' x R = (-w' Ry, w' Rx, 0)
w x (w x R) = (-w^2 Rx, -w^2 Ry, 0)
V' + w' x R + w x (w x R) = (Vx -w^2 Rx - w' Ry, Vy - w^2 Ry + w' Rx, 0)

Conclusion

We now have the expansions of the two terms in equation (D-2). This gives for each contact i, the contribution to the acceleration which we add to b[i]. Keep in mind that for each contact, the RigidBodyCollision object figures out for us: which is body 1 (the “primary” object whose corner is in contact), which is body 2 (the “normal” object whose edge determines the normal vector), and the normal vector Ni.

Extra Acceleration

Extra acceleration is added to eliminate velocity at contact. See ExtraAccel enum for explanations of the various options.

See the paper Curved Edge Physics which explains the math involved in extra acceleration for curved edges.

This is a major new (as of Oct 2011) experimental change to contact handling policy. Add extra acceleration at contacts to offset the residual velocity. Turn off the “small zero elasticity impacts at every contact on every time step” policy (in CollisionSim.advance). Why? Because in the “resting state”, we continue to get lots of impacts happening resulting in a lot of undesirable wiggling jiggling of objects that should be at rest.

The underlying scenario in many “resting contact” situations is this:

  1. fire an impulse at contact A to eliminate negative velocity there; this results in some positive velocity at another contact B.

  2. the small positive velocity at contact B leads eventually to a momentary loss of contact there

  3. contact B then “falls” back into contact with negative velocity leading to the cycle starting over again.

In this way you get an oscillation of impulses at contact A, then at contact B, then at contact A, etc.

The canonical example is a long block (size 1 x 3) resting on the ground in vertical position. With no extra acceleration, you see the above scenario of a continuous cycle of a collision happening every couple seconds, alternating between the two corners on the ground. (When the block rests in horizontal position, then this doesn’t happen.)

The new idea is to not use impact impulse to correct the negative velocity at contacts. Instead we request some additional acceleration at contacts with negative velocity, by adding to the b-vector. Also, we request a little less acceleration at contacts with positive velocity so that they stay in contact.

How the amount of extra acceleration is calculated: We want an amount of acceleration a which over a single time step h will reduce the normal velocity v to zero. Assume constant acceleration. Change in velocity over timestep h is

∆v = (v - 0) = integral(a dt) = a h.

Therefore we get a = -v / h.