2

I have two vectors in 3D-space, S (Start) and T (Target), and I want to find the Rotation Matrix (RM) that allows such transformation.

I know that by computing the cross product S × T I get the rotation axis. The angle between S and T is given by tan⁻¹(||S × T||, S·T), where S·T is the dot product between S and T.

This gives me the rotation vector rotvec = [S x T; angle] (the cross product is normalized). Then by using function vrrotvec2mat (in MATLAB) or transforms3d.axangles.axangle2mat (in Python) I can obtain the rotation matrix that corresponds to the transformation from S to T.

In my application T is given by the dot product RM·D, where is D is a 3x1 vector. My goal is to find RM. I know S, T and D, but I am having trouble to understand the math behind this.

In practice I want to find a RM between S and T', where T' is the target vector before D (the direction) has been applied.

A little more context: I want to obtain body joint angles from 3D points in the camera coordinate system.

jcampos
  • 67
  • 2
  • 7
  • There are countless many readily available code samples for this on the web; even the formulas themselves are easy to convert to code without having to understand how they work. – meowgoesthedog Aug 01 '18 at 21:16
  • @meowgoesthedog could you point me to an example? I've been struggling to find this on the web... Probably I am not using the right keywords. – jcampos Aug 01 '18 at 21:22
  • A formulaic example [here](https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d) - called *Rodrigues' rotation formula*. Another one [here](http://immersivemath.com/forum/question/rotation-matrix-from-one-vector-to-another/). MATLAB code sample [here](https://gist.github.com/peteristhegreat/3b76d5169d7b9fc1e333). Alternative: first obtain the [*quaternion*](https://stackoverflow.com/questions/21828801/how-to-find-correct-rotation-from-one-vector-to-another) for this rotation, then convert to a matrix. – meowgoesthedog Aug 01 '18 at 21:55
  • However it would be better for you to directly state your desired final problem, since there may be a more efficient specific solution. – meowgoesthedog Aug 01 '18 at 21:56
  • "I want to obtain body joint angles from 3D points in the camera coordinate system". Wouldn't just projecting the 3D points onto the camera plane then taking the arccos of the inner product of the two unit vectors do this? I'm not sure what the purpose of finding a rotation matrix here is. – jodag Aug 02 '18 at 00:59
  • I really don't understand your question. In the first sentence, you basically say, that `RM·S = T`, and you want to find `RM`. So applying `RM` to `S` gives `T`. If you have stopped here, I would have given an answer. But afterwards, you introduce `D`, and `T'`, what are these? Basically, you use 4 vectors (`S`, `T`, `D`, `T'`), and it is not clear how these relate to each other. Can you formulate it mathematically? Btw, how can `RM·D` be a dot product? Dot product is defined between vectors, and result in a scalar. – geza Aug 02 '18 at 07:00

1 Answers1

3

In order to make this work you also need the center of rotation (point that stays the same after rotation)... Now we need two transform matrices one representing coordinate system before and one after the transform.

To construct your 3D transform matrix you need 3 perpendicular basis vectors and origin position see:

now rotation axis will be one of the basis vectors and we can use S,T as second one so the third we can compute with cross product and the origin will be the center of rotation:

X = cross(S,T);                      // rotation axis
X /= |X|;                            // unit vector
Y = S;                               // start vector
Y /= |Y|;                            // unit vector
Z = cross(X,Y);                      // Z perpendicular to X,Y
Z /= |Z|;                            // unit vector
O = center_of_rotation;

overview

So construct 4x4 transform matrix A from those. And do the same for B but use T instead of S. Now we want to compute the difference transform so if p=(x,y,z,1) is any point to transform then:

p' = Inverse(A)*p 
p' = B*p'         

So your transform matrix M is:

M = Inverse(A)*B;

Beware this will work with standard OpenGL conventions if you use different one (multiplication order, matrix orientation, etc) the equation might change.

You can also use full pseudo inverse matrix to compute the Inverse(A) more effectively and accurately.

As you can see you do not need any goniometrics nor angle to do this (vector math is nice in this)

[Edit1] C++ example

Its using VCL (AnsiString and mm_log which you can ignore) and my vector math (used functions are in the first link).

//---------------------------------------------------------------------------
AnsiString matrix_prn(double *a)
    {
    int i; AnsiString s;
    for (s ="(",i=0;i<16;i+=4) { if (a[i]>=0.0) s+=" "; s+=AnsiString().sprintf("%2.3lf,",a[i]); } s[s.Length()]=')'; s+="\r\n";
    for (s+="(",i=1;i<16;i+=4) { if (a[i]>=0.0) s+=" "; s+=AnsiString().sprintf("%2.3lf,",a[i]); } s[s.Length()]=')'; s+="\r\n";
    for (s+="(",i=2;i<16;i+=4) { if (a[i]>=0.0) s+=" "; s+=AnsiString().sprintf("%2.3lf,",a[i]); } s[s.Length()]=')'; s+="\r\n";
    for (s+="(",i=3;i<16;i+=4) { if (a[i]>=0.0) s+=" "; s+=AnsiString().sprintf("%2.3lf,",a[i]); } s[s.Length()]=')';
    return s;
    }
//---------------------------------------------------------------------------
AnsiString vector_prn(double *a)
    {
    int i; AnsiString s;
    for (s ="(",i=0;i<3;i++) { if (a[i]>=0.0) s+=" "; s+=AnsiString().sprintf("%2.3lf,",a[i]); } s[s.Length()]=')';
    return s;
    }
//---------------------------------------------------------------------------
__fastcall TForm1::TForm1(TComponent* Owner):TForm(Owner)
    {
    int i;
    double O[3]={0.00, 0.00,0.00};  // center ofrotation
    double S[3]={4.10,-9.44,0.54};  // start vector
    double T[3]={1.40,-9.08,4.10};  // end vector
    double A[16],_A[16],B[16],M[16],X[3],Y[3],Z[3];

    // A
    vector_mul(X,S,T);  // rotation axis
    vector_one(X,X);    // unit vector
    vector_one(Y,S);    // unit start vector
    vector_mul(Z,X,Y);  // Z perpendicular to X,Y
    vector_one(Z,Z);    // unit vector
    for (i=0;i<3;i++)
        {
        A[ 0+i]=X[i];
        A[ 4+i]=Y[i];
        A[ 8+i]=Z[i];
        A[12+i]=O[i];
        A[(i<<2)+3]=0.0;
        } A[15]=1.0;
    // B
    vector_one(Y,T);    // unit end vector
    vector_mul(Z,X,Y);  // Z perpendicular to X,Y
    vector_one(Z,Z);    // unit vector
    for (i=0;i<3;i++)
        {
        B[ 0+i]=X[i];
        B[ 4+i]=Y[i];
        B[ 8+i]=Z[i];
        B[12+i]=O[i];
        B[(i<<2)+3]=0.0;
        } B[15]=1.0;
    // M = B*Inverse(A)
    matrix_inv(_A,A);
    matrix_mul(M,_A,B);

    mm_log->Lines->Add("A");
    mm_log->Lines->Add(matrix_prn(A));
    mm_log->Lines->Add("B");
    mm_log->Lines->Add(matrix_prn(B));
    mm_log->Lines->Add("M");
    mm_log->Lines->Add(matrix_prn(M));
    mm_log->Lines->Add("");

    vector_one(S,S);    // unit start vector
    vector_one(T,T);    // unit end vector
    mm_log->Lines->Add("S = "+vector_prn(S));
    matrix_mul_vector(X,M,S);
    mm_log->Lines->Add("X = "+vector_prn(X));
    mm_log->Lines->Add("T = "+vector_prn(T));
    }
//-------------------------------------------------------------------------

Here the result:

A
(-0.760, 0.398,-0.514, 0.000)
(-0.361,-0.916,-0.175, 0.000)
(-0.540, 0.052, 0.840, 0.000)
( 0.000, 0.000, 0.000, 1.000)
B
(-0.760, 0.139,-0.635, 0.000)
(-0.361,-0.903, 0.235, 0.000)
(-0.540, 0.408, 0.736, 0.000)
( 0.000, 0.000, 0.000, 1.000)
M
( 0.959, 0.258,-0.115, 0.000)
(-0.205, 0.916, 0.345, 0.000)
( 0.194,-0.307, 0.932, 0.000)
( 0.000, 0.000, 0.000, 1.000)

S = ( 0.398,-0.916, 0.052)
X = ( 0.139,-0.903, 0.408) // X = M * S
T = ( 0.139,-0.903, 0.408)

As you can see if I transform unit S by M I got the unit T vector. PS. my matrix_mul_vector and vector_mul assumes w=1.0 but as O=(0.0,0.0,0.0) the vectors and points are the same.

Spektre
  • 49,595
  • 11
  • 110
  • 380
  • thanks a lot for pointing me to 4x4 homogenous transform matrices and your clear explanation. From your explanation if p equals to my start vector (S), then p' should be my target vector (T) is that correct? – jcampos Aug 05 '18 at 21:52
  • @jcampos yes ... if `p=(x,y,z,0)` than the `p,p'` are handled as vectors and if `p=(x,y,z,1)` than they are handled as points .... – Spektre Aug 05 '18 at 22:43
  • 1
    `@Spektre` am I not being able to get there. So if I have `S=[4.10, -9.44, 0.54]` and `T=[1.40, -9.08, 4.10]` my matrix `A`, according to your explanation will be `A=[[-0.76 0.40 -0.51 0.56] [-0.36 -0.92 -0.18 -0.80] [-0.54 0.05 0.84 0.22] [ 0 0 0 1 ]]` and `B=[[-0.76 0.14 -0.64 0.56] [-0.36 -0.90 0.23 -0.80] [-0.54 0.41 0.74 0.220] [ 0 0 0 1]]`, where O = [1.38, -1.97, 0.54]. When I compute `B*Inverse(A)*S` the output is not T ([ 1.69 -9.33 3.84 1]). What am I doing wrong? I am using Python and computing the inverse with the function `np.linalg.inv()`. – jcampos Aug 06 '18 at 03:28
  • @jcampos +1 for Good catch I got a mistake (reversed order in the `M` computation) I repaired the Answer and also added C++ example with results so you can compare ... Beware I amusing unit `S,T` as you can not expect that rotation will change also vector length. Also I used `O=(0.0,0.0,0.0)` as you did not specified any. – Spektre Aug 06 '18 at 06:29