Rotation and direction of a vector in 3D space - Inverse Order
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.
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;
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.