Distance Constraint - Mass Matrix

[Steinmetz]

Ok, I know you all must be annoyed of this, but everyone seems to understand it, but I still have problems.
I want to implement a distance constraint in 2D(!), which means the distance from a point p1 on a body b1 to a point p2 on a body b2 is L
in formulas:
C = |p1-p2|- L = 0
First I need the Jacobian:
derivative with respect to time:
C' = (p1-p2)/|p1-p2| * (v2-v1)
Isolate v:
J = (p1-p2)/|p1-p2|
Ok now that thing with:
K = J * W * JT
W is the inverse mass matrix.
But what exactly is the mass matrix for a body in 2D?
And afaik JT means that J is "turned around", but is that really necessary in 2D?
And if I have finally calculated K, how do I get the impulse which is applied to b1 and b2 (negative)?
I'm sorry that I didn't understand it, but I didn't have Vector Maths until now, and all that stuff is a bit confusig
Thank you alot!

[Dirk Gregorius]

You need to be more precise. Basically what you have written is the distance constraint for a particle. Here you have:

C = |p2 - p1| - L
dC/dt = (p2 - p1) / |p2 - p1| * (v2 - v1) = n * ( v2 - v1 ), where n = (p2 - p1) / |p2 - p1|

The Jacobian is by inspection (note that you need to transpose the gradient vector when you insert it into the matrix - hence the "T"!):
J = ( -nT nT )


Now for two rigid bodies. We want to constraint two arbitrary points on the surface of each body. We write the position constraint as:
C = |p2 - p1| - L where p = x + R * r' = x + r ( x is the center of mass, R is the current orientation matrix and r' is the vector from p to some point on the body in local coordinates - hence the prime to denote the local frame and without the prime after we transformed to world space r = R * r')

dC/dt = (p2 - p1) / |p2 - p1| * (dp2/dt - dp1/dt) = n * ( dp2/dt - dp1/dt ) = n * ( v2 + cross( omega2, r2 ) - v1 - cross( omega1, r1 ) )
The trick is to realize that the inner derivative (dp2/dt - dp1/dt) is just the relative velocity at the two points.

The Jacobian now becomes:
J = ( -nT -cross( r1, n )T nT cross( r2, n )T )



HTH,
-Dirk

[Steinmetz]

Thank you alot for your response!

The Jacobian is by inspection (note that you need to transpose the gradient vector when you insert it into the matrix - hence the "T"!):
J = ( -nT nT )

Is transposing the vector(/matrix) needed in 2D? I thought, the Jacobian was just
J = JT = n
Because I thought in 2D the Jacobian is just a vector and not a matrix?

Now for two rigid bodies. We want to constraint two arbitrary points on the surface of each body. We write the position constraint as:
C = |p2 - p1| - L where p = x + R * r' = x + r ( x is the center of mass, R is the current orientation matrix and r' is the vector from p to some point on the body in local coordinates - hence the prime to denote the local frame and without the prime after we transformed to world space r = R * r')

I saved r for each body in local coordinates in the constraint, so I just have to rotate it (in 2D) at the beginning of every frame ( in the prestep method ) to get r1 and r2 in world space.

dC/dt = (p2 - p1) / |p2 - p1| * (dp2/dt - dp1/dt) = n * ( dp2/dt - dp1/dt ) = n * ( v2 + cross( omega2, r2 ) - v1 - cross( omega1, r1 ) )
The trick is to realize that the inner derivative (dp2/dt - dp1/dt) is just the relative velocity at the two points.

The Jacobian now becomes:
J = ( -nT -cross( r1, n )T nT cross( r2, n )T )

So, with "isolate v" it is not meant, that I just take out the v, but I also need to get the omega out there somehow... I think I understand

Ok now I have that formula:
J * W * JT * lambda = -J * v
Now how to calculate W ( or M )... Is it always the same for a constraint?

I think I know too less about vector and matrix maths, but how can the direction of the impulse ( the Jacobian) be a vector with 4 elements?

Thanks again

Edit:
I read the Paper "Siggraph 97" and it sais, that M is just a nxn(2x2 for 2 bodies) Matrix where the diagonal elements are the mass of the bodies, and the others are zero. It also sais, that a diagonal matrix can just be seen as a vector...

[Dirk Gregorius]

"By inspection" means that you bring an algebraic equation into vector form. We know that the time derivative of any position constraint has the form: dC/dt = del_C/del_x * dx /dt + del_C/del_t = J * v + del_C/del_t. Note that del_C/del_x is the Jacobian matrix. See e.g. http://en.wikipedia.org/wiki/Total_derivative (Differentiation with indirect dependencies). Also check this: http://en.wikipedia.org/wiki/Derivative Here we have:

dC/dt = n * ( v2 - v1 ) = -n * v1 + n * v2

If we now define the row matrix J = ( -nT nT ) and the column vector v = ( v1 v2 )T we can write this as:

dC/dt = -n * v1 + n * v2 = J * v // Write it down an multpy is out as excersise


This is not simple to get and the several different notations are also confusing. So don't give up. You will get familiar over time . You also want to familiarize yourself with block matrix notation. Basically you have large vectors of 2D/3D vectors and large matrices of 2x2/3x3 matrices. So your mass matrix looks like this:

Given the mass m of a particle we write this as a matrix M:

( m 0 )
( 0 m )

Then the mass matrix for two particles becomes:

( M1 0 )
( 0 M2 )

Note that in the first matrix m and 0 are scalars. And in the second matrix M is a 2x2 diagonal matrix and 0 is the 2x2 zero matrix.


Hope this makes sense,
-Dirk

[Steinmetz]

Thank you, I think I'm getting into it

Given the mass m of a particle we write this as a matrix M:

( m 0 )
( 0 m )

Then the mass matrix for two particles becomes:

( M1 0 )
( 0 M2 )

Note that in the first matrix m and 0 are scalars. And in the second matrix M is a 2x2 diagonal matrix and 0 is the 2x2 zero matrix.

Ah ok, I believe I understand
Then, the mass matrix for two particles would be (written out):
[(m1 0) (0 0)]
[(0 m1) (0 0)]

[(0 0) (m2 0)]
[(0 0) (0 m2)]
?
But you said "particles", what if I do the same thing for rigid bodies? Will the inertia also get into the matrix? Or is it the same as for a particle?

As you said. I will try to get a bit more familiar with matrix maths, but I think I understood the main principes now.
Thank you again, you helped me alot

[Dirk Gregorius]

Yes, but for clarity I would write it like this (it is just *one* matrix):

[(m1 0) (0 0)]
[(0 m1) (0 0)]
[(0 0) (m2 0)]
[(0 0) (0 m2)]

For a rigid body it becomes (sorry not nicely formated, but I am sure you get - see Kenny Erlebens PhD for a more nicely formated example):

[(m1 0 0) (0 0 0) (0 0 0) (0 0 0)]
[(0 m1 0) (0 0 0) (0 0 0) (0 0 0)]
[(0 0 m1) (0 0 0) (0 0 0) (0 0 0)]
[(0 0 0) (I1xx I1xy I1xz) (0 0 0) (0 0 0)]
[(0 0 0) (I1yx I1yy I1yz) (0 0 0) (0 0 0)]
[(0 0 0) (I1zx I1zy I1zz) (0 0 0) (0 0 0)]
[(0 0 0) (0 0 0) (m2 0 0) (0 0 0)]
[(0 0 0) (0 0 0) (0 m2 0) (0 0 0)]
[(0 0 0) (0 0 0) (0 0 m2) (0 0 0)]
[(0 0 0) (0 0 0) (0 0 0) (I2xx I2xy I2xz)]
[(0 0 0) (0 0 0) (0 0 0) (I2yx I2yy I2yz)]
[(0 0 0) (0 0 0) (0 0 0) (I2zx I2zy I2zz)]

In block form it would be (M is a 3x3 diagonal matrix with m on the diagonal and I is the 3x3 inertia tensor):

(M1 0 0 0)
(0 I1 0 0)
(0 0 M2 0)
(0 0 0 I2)

The velocity vector will be ( where v is the linear velocity vector and w (read omega) is the angular velocity vector):

( v1.x )
( v1.y )
( v1.z )
( w1.x )
( w1.y )
( w1.z )
( v2.x )
( v2.y )
( v2.z )
( w2.x )
( w2.y )
( w2.z )


And again in (vector) block form for completeness:

( v1 )
( w1 )
( v2 )
( w2 )

I suggest performing all calcularions in the block form since it directly maps perfect to 3D vector/matrix math operations in you library. If you multiply the mass matrix you *must* make sure that the operations between the elements are defined. Here it would be multiplications between matrices and vectors which is nicely defined. Hence e.g. why you need to transpose the Jacobian. While v * A (vector times matrix) is *not* defined vT * A is. Not that a simple trick is to write the dot product between two vector as matrix multiplication: u * v = uT * v. This is very handy since in matrix multiplication is associative.

So this is quite a lot, but give yourself some time and you will get it! Good Luck!

HTH,
-Dirk

[Steinmetz]

For a rigid body it becomes (sorry not nicely formated, but I am sure you get - see Kenny Erlebens PhD for a more nicely formated example):

[(m1 0 0) (0 0 0) (0 0 0) (0 0 0)]
[(0 m1 0) (0 0 0) (0 0 0) (0 0 0)]
[(0 0 m1) (0 0 0) (0 0 0) (0 0 0)]
[(0 0 0) (I1xx I1xy I1xz) (0 0 0) (0 0 0)]
[(0 0 0) (I1yx I1yy I1yz) (0 0 0) (0 0 0)]
[(0 0 0) (I1zx I1zy I1zz) (0 0 0) (0 0 0)]
[(0 0 0) (0 0 0) (m2 0 0) (0 0 0)]
[(0 0 0) (0 0 0) (0 m2 0) (0 0 0)]
[(0 0 0) (0 0 0) (0 0 m2) (0 0 0)]
[(0 0 0) (0 0 0) (0 0 0) (I2xx I2xy I2xz)]
[(0 0 0) (0 0 0) (0 0 0) (I2yx I2yy I2yz)]
[(0 0 0) (0 0 0) (0 0 0) (I2zx I2zy I2zz)]

In block form it would be (M is a 3x3 diagonal matrix with m on the diagonal and I is the 3x3 inertia tensor):

(M1 0 0 0)
(0 I1 0 0)
(0 0 M2 0)
(0 0 0 I2)

Ok, one last question, then I think I'm done
What is I1 or I2 for 2d-Bodies? Because there is no tensor, just scalars ...
Is it just:
(I 0)
(0 I)
So the whole Matrix for a 2 2d Bodies would become:
[(m1 0) (0 0) (0 0) (0 0)]
[(0 m1) (0 0) (0 0) (0 0)]
[(0 0) (I1 0) (0 0) (0 0)]
[(0 0) (0 I1) (0 0) (0 0)]
[(0 0) (0 0) (m2 0) (0 0)]
[(0 0) (0 0) (0 m2) (0 0)]
[(0 0) (0 0) (0 0) (I2 0)]
[(0 0) (0 0) (0 0) (0 I2)]
Or am I missing something here?

Thank you again, you helped me alot

[Dirk Gregorius]

I would use this:

[(m1 0 0) (0 0 0)]
[(0 m1 0) (0 0 0)]
[(0 0 I1 ) (0 0 0)]
[(0 0 0) (m2 0 0)]
[(0 0 0) (0 m2 0)]
[(0 0 0) (0 0 I2 )]

And the velocity vector is:

[v1.x]
[v1.y]
[omega1]
[v2.x]
[v2.y]
[omega2]


HTH,
-Dirk

[Steinmetz]

I would use this:

[(m1 0 0) (0 0 0)]
[(0 m1 0) (0 0 0)]
[(0 0 I1 ) (0 0 0)]
[(0 0 0) (m2 0 0)]
[(0 0 0) (0 m2 0)]
[(0 0 0) (0 0 I2 )]

And the velocity vector is:

[v1.x]
[v1.y]
[omega1]
[v2.x]
[v2.y]
[omega2]


HTH,
-Dirk
I would use this:

[(m1 0 0) (0 0 0)]
[(0 m1 0) (0 0 0)]
[(0 0 I1 ) (0 0 0)]
[(0 0 0) (m2 0 0)]
[(0 0 0) (0 m2 0)]
[(0 0 0) (0 0 I2 )]

And the velocity vector is:

[v1.x]
[v1.y]
[omega1]
[v2.x]
[v2.y]
[omega2]


HTH,
-Dirk

Thank you
I tried this, and it works fine if there's no angular part, but with an offset at the constraint anchors it seems buggy...
Here's how I thought, I need to do it:
Calculate K which is J * W * JT, which is
K = -(n x r1)^2 / I1 + (n x r2)^2 / I2 + n^2 / m1 + n^2 / m2
Where r1 and r2 are the "anchors" and "x" is the cross product (dotproduct with the normal)
Now I invert K and get M (For me, K and M are scalars ... should they be matrices?)
M = 1 / K
I do that in the preStep function.
Now in the actual constraint solving function I calculate delta_v
delta_v = -v1n + v2n - (n x r1) * omega1 + (n x r2) * omega2
Now the impulse is calculated by just:
P = n * (delta_v * M)
And now I apply this impulse positive on b1 and negative on b2.

But as said above, this doesn't seem right when the angular part comes into effect ... Can you tell me why?

Again, thank you, I think I am nearly understanding what I'm doing at the moment, instead of just copypasting code

[Dirk Gregorius]

I am on my way into a holiday weekend. PM me on Monday and I will check your code...

[Dirk Gregorius]

The K matrix seems correct. I would actually write it like this using the identities:

a x b = -(b x a)
n * n = 1, if |n| = 1

=> K = 1 / m1 + 1 / m2 + ( r1 x n )^2 / I1 + ( r2 x n )^2 / I2


The delta velocity is (which should be the same as your result):

v_rel = dot( v2 + cross( omega2, r2 ) - v1 - cross( omega1, r1 ), n )


Now in order to compute the impulse you do the following:

lambda = -v_rel / K

Now apply the lambda directly and don't multiply with the normal. The impulses you need to apply on each body are:

P1 = -lambda
L1 = -lambda x r1
P2 = lamba
L2 = lambda x r2

In pseudo code:
body1->applyLinearImpulse( P1 );
body1->applyAngularImpulse( L1 );
body2->applyLinearImpulse( P2 );
body2->applyAngularImpulse( L2 );

With the following implementations:

void Body::applyLinearImpulse( Vec2 P )
{
mLinearVelocity += P / mMass;
}

void Body::applyAngularImpulse( float L )
{
mAngularVelocity += L / mInertia;
}

Note that the inertia is a scalar in 2D and that in your case you use the special 2D cross product which returns a scalar.



Cheers,
-Dirk

[Steinmetz]

I think, I'm missing something here...

Now apply the lambda directly and don't multiply with the normal. The impulses you need to apply on each body are:

P1 = -lambda
L1 = -lambda x r1
P2 = lamba
L2 = lambda x r2

In pseudo code:
body1->applyLinearImpulse( P1 );
body1->applyAngularImpulse( L1 );
body2->applyLinearImpulse( P2 );
body2->applyAngularImpulse( L2 );

With the following implementations:

void Body::applyLinearImpulse( Vec2 P )
{
mLinearVelocity += P / mMass;
}

void Body::applyAngularImpulse( float L )
{
mAngularVelocity += L / mInertia;
}

If P1 or P2 is a scalar, how can I give it to the linearImpulse function, shouldn't I multiply it with the normal?
If I do that, and apply the angular impulse as you said, again it works only, if theres no angular error.
The strange thing is, that if I calculate rel_velocity after applying the impulse, it is something about 10e-15, which is correct... But if i measure the length of the constraint, it changes while the bodies rotate ... Is that due to the fact, that I have no position correction until now? I don't think so, because even without position correction there shouldn't be such a big error, as there is.
Thank you again

Ps:
Maybe my code could help:

public void preStep(double dt){
p1 = b1.pointToGlobalSpace(r1);
p2 = b2.pointToGlobalSpace(r2);
normal = p1.sub(p2).normalize();
k = b1.invmass + b2.invmass + r1.cross(normal)^2 * b1.invinertia + r2.cross(normal)^2 * b2.invinertia;
m = 1.0 / k;
}

public void satisfy(){
double dv;
dv = b2.vel.mul(normal);
dv -= b1.vel.mul(normal);
dv += normal.cross(r2) * b2.avel;
dv -= normal.cross(r1) * b1.avel;

double lambda = dv * m;
Point p = normal.mul(lambda);

b1.vel = b1.vel.add(p.mul(b1.invmass));
b2.vel = b2.vel.sub(p.mul(b2.invmass));
b1.avel -= lambda * b1.invinertia;
b2.avel += lambda * b2.invinertia;
}

[Dirk Gregorius]

Ooops, sorry. I somehow rembered that we were talking about the ball socket joint. So the impulses of the distance constraint for each body are:

P = JT * lambda

Since lambda is a scalar the impulses are (P is the linear impulse applied at the center of mass and L is the angular impulse):

Vec2 P1 = -lambda * n
float L1 = -lambda * cross( r1, n ) // cross returns a float in 2D
Vec2 P2 = lambda * n
float L2 = lambda * cross( r2, n ) // cross returns a float in 2D

So there is no need to multiply with the mass as you did. lambda is an impulse and by projecting it back onto the Jacobian you find the directions. In order to find the associated change in velocity you scale by the inverse mass and inertia of the body when you apply the impulse as shown in my earlier post.


HTH,
-Dirk

[Steinmetz]

So there is no need to multiply with the mass as you did. lambda is an impulse and by projecting it back onto the Jacobian you find the directions. In order to find the associated change in velocity you scale by the inverse mass and inertia of the body when you apply the impulse as shown in my earlier post.

Hm maybe its my silly notation, m is not the mass, its 1 / k ... (which is called effective mass?)
you said lambda = rel_v / k
I save 1 / k, and i thought k was some kind of inverse mass, so i named 1 / k "m" ...

Oh my god, I think I'm done... Had a sign error(does this word exist in english?) in one of my calculations, now it seems to work.
Thank you!!
I will now begin to despair of position correction because I still have lots of error which gets bigger with time, but it seems normal to me... And the nice thing is, I managed to implement a pulley joint, because now I understood, how to do that
Thank you again!