[jay]

Hi @Hugo,
I just found out that the dx for addDforce (const DataVecDeriv &dx) is not a 4d vector (quaternion), but a 3d vector.
Could you tell me how to compute the jacobian of a rigid body rotation which also depends on the theta term of the quaternion(denoted as q4 in the previous question)?

[jay]

Hi Hugo,

Thank you so much for your reply.
Now I can understand how SOFA treats the rotation of a rigid body.

Your reply clearly answered most of the questions, but can I ask just one more question?
Regarding question #6, I think the jacobian matrix (df/dx) of a rigid body should be a 6 by 7 matrix since f has 6 components and x has 7 components.

My questions are :

Q1.
In this structure, I guess the following approach is the right way of adding the addDforce in SOFA. If you don’t mind, could you check whether it is correct?

– Let’s assume that we have a jacobian matrix, M, which is 6 by 7 matrix,

  M = [df1/dx1  df1/dx2  df1/dx3  df1/dq1  df1/dq2  df1/dq3  df1/dq4
       df2/dx1  df2/dx2  df2/dx3  df2/dq1  df2/dq2  df2/dq3  df2/dq4
       df3/dx1  df3/dx2  df3/dx3  df3/dq1  df3/dq2  df3/dq3  df3/dq4
       dt1/dx1  dt1/dx2  dt1/dx3  dt1/dq1  dt1/dq2  dt1/dq3  dt1/dq4
       dt2/dx1  dt2/dx2  dt2/dx3  dt2/dq1  dt2/dq2  dt2/dq3  dt2/dq4
       dt3/dx1  dt3/dx2  dt3/dx3  dt3/dq1  dt3/dq2  dt3/dq3  dt3/dq4]

where f is force(translation), t is torque, x is position(translation), q is quaternion.

– Now split M into four parts – Mfx, Mfq, Mtx, Mtq.

  # split M into four parts
  M = [Mfx                       | Mfq
       __________________________|_____
       Mtx                       | Mtq],

  # Where
Mfx = [df1/dx1  df1/dx2  df1/dx3
       df2/dx1  df2/dx2  df2/dx3
       df3/dx1  df3/dx2  df3/dx3]
Mfq = [df1/dq1  df1/dq2  df1/dq3  df1/dq4
       df2/dq1  df2/dq2  df2/dq3  df2/dq4
       df3/dq1  df3/dq2  df3/dq3  df3/dq4]
Mtx = [dt1/dx1  dt1/dx2  dt1/dx3
       dt2/dx1  dt2/dx2  dt2/dx3
       dt3/dx1  dt3/dx2  dt3/dx3]
Mtq = [dt1/dq1  dt1/dq2  dt1/dq3  dt1/dq4
       dt2/dq1  dt2/dq2  dt2/dq3  dt2/dq4
       dt3/dq1  dt3/dq2  dt3/dq3  dt3/dq4]

– Then, the derivative of the force is calculated by the following equations

template<>
void CustomForceField<Rigid3Types>::addDForce(const core::MechanicalParams *mparams, DataVecDeriv &df, const DataVecDeriv &dx)
{
    const VecDeriv& dq = dx.getValue();
    VecDeriv& dfq = *df.beginEdit();

    ## Mfx, Mfq, Mtx, Mtq are calculated here
    ## Mfx = ~~~
    ## Mfq = ~~~
    ## Mtx = ~~~
    ## Mtq = ~~~

    for(Size n = 0 ; n < nNodes ; ++n)
    {
        PointId id = indices[n];
        dfq[id].getVCenter() += Mfx * dq[id].getCenter();
        dfq[id].getVCenter() += Mfq * dq[id].getOrientation();
        dfq[id].getVOrientation() += Mtx * dq[id].getCenter();
        dfq[id].getVOrientation() += Mtq * dq[id].getOrientation();
    }
}

Is this correct?

Q2.
A 3 by 3 Matrix can be created by

  Mat3 D;
  D(0,0) = 1 ; D(0,1) = 0 ; D(0,2) = 0;
  D(0,0) = 0 ; D(0,1) = 1 ; D(0,2) = 0;
  D(0,0) = 0 ; D(0,1) = 0 ; D(0,2) = 1;

However, we need 3 by 4 matrix for Mfq, Mtq.
How do you create 3 by 4 matrix? What is the name of the datatype?

Many thanks,
Jay

[Author]

Posts