How to rotate a set of points on z = 0 plane in 3-D, preserving pairwise distances?
- by cagirici
I have a set of points double n[] on the plane z = 0.
And I have another set of points double[] m on the plane ax + by + cz + d = 0.
Length of n is equal to length of m.
Also, euclidean distance between n[i] and n[j] is equal to euclidean distance between m[i] and m[j].
I want to rotate n[] in 3-D, such that for all i, n[i] = m[i] would be true.
In other words, I want to turn a plane into another plane, preserving the pairwise distances.
Here's my code in java. But it does not help so much:
double[] rotate(double[] point, double[] currentEquation, double[] targetEquation)
{
double[] currentNormal = new double[]{currentEquation[0], currentEquation[1], currentEquation[2]};
double[] targetNormal = new double[]{targetEquation[0], targetEquation[1], targetEquation[2]};
targetNormal = normalize(targetNormal);
double angle = angleBetween(currentNormal, targetNormal);
double[] axis = cross(targetNormal, currentNormal);
double[][] R = getRotationMatrix(axis, angle);
return rotated;
}
double[][] getRotationMatrix(double[] axis, double angle)
{
axis = normalize(axis);
double cA = (float)Math.cos(angle);
double sA = (float)Math.sin(angle);
Matrix I = Matrix.identity(3, 3);
Matrix a = new Matrix(axis, 3);
Matrix aT = a.transpose();
Matrix a2 = a.times(aT);
double[][] B =
{
{0, axis[2], -1*axis[1]},
{-1*axis[2], 0, axis[0]},
{axis[1], -1*axis[0], 0}
};
Matrix A = new Matrix(B);
Matrix R = I.minus(a2);
R = R.times(cA);
R = R.plus(a2);
R = R.plus(A.times(sA));
return R.getArray();
}
This is what I get. The point set on the right side is actually part of a point set on the left side. But they are on another plane.
Here's a 2-D representation of what I try to do:
There are two lines. The line on the bottom is the line I have. The line on the top is the target line. The distances are preserved (a, b and c).
Edit:
I have tried both methods written in answers. They both fail (I guess).
Method of Martijn Courteaux
public static double[][] getRotationMatrix(double[] v0, double[] v1, double[] v2, double[] u0, double[] u1, double[] u2)
{
RealMatrix M1 = new Array2DRowRealMatrix(new double[][]{
{1,0,0,-1*v0[0]},
{0,1,0,-1*v0[1]},
{0,0,1,0},
{0,0,0,1}
});
RealMatrix M2 = new Array2DRowRealMatrix(new double[][]{
{1,0,0,-1*u0[0]},
{0,1,0,-1*u0[1]},
{0,0,1,-1*u0[2]},
{0,0,0,1}
});
Vector3D imX = new Vector3D((v0[1] - v1[1])*(u2[0] - u0[0]) - (v0[1] - v2[1])*(u1[0] - u0[0]),
(v0[1] - v1[1])*(u2[1] - u0[1]) - (v0[1] - v2[1])*(u1[1] - u0[1]),
(v0[1] - v1[1])*(u2[2] - u0[2]) - (v0[1] - v2[1])*(u1[2] - u0[2])
).scalarMultiply(1/((v0[0]*v1[1])-(v0[0]*v2[1])-(v1[0]*v0[1])+(v1[0]*v2[1])+(v2[0]*v0[1])-(v2[0]*v1[1])));
Vector3D imZ = new Vector3D(findEquation(u0, u1, u2));
Vector3D imY = Vector3D.crossProduct(imZ, imX);
double[] imXn = imX.normalize().toArray();
double[] imYn = imY.normalize().toArray();
double[] imZn = imZ.normalize().toArray();
RealMatrix M = new Array2DRowRealMatrix(new double[][]{
{imXn[0], imXn[1], imXn[2], 0},
{imYn[0], imYn[1], imYn[2], 0},
{imZn[0], imZn[1], imZn[2], 0},
{0, 0, 0, 1}
});
RealMatrix rotationMatrix = MatrixUtils.inverse(M2).multiply(M).multiply(M1);
return rotationMatrix.getData();
}
Method of Sam Hocevar
static double[][] makeMatrix(double[] p1, double[] p2, double[] p3)
{
double[] v1 = normalize(difference(p2,p1));
double[] v2 = normalize(cross(difference(p3,p1), difference(p2,p1)));
double[] v3 = cross(v1, v2);
double[][] M = { { v1[0], v2[0], v3[0], p1[0] },
{ v1[1], v2[1], v3[1], p1[1] },
{ v1[2], v2[2], v3[2], p1[2] },
{ 0.0, 0.0, 0.0, 1.0 } };
return M;
}
static double[][] createTransform(double[] A, double[] B, double[] C,
double[] P, double[] Q, double[] R)
{
RealMatrix c = new Array2DRowRealMatrix(makeMatrix(A,B,C));
RealMatrix t = new Array2DRowRealMatrix(makeMatrix(P,Q,R));
return MatrixUtils.inverse(c).multiply(t).getData();
}
The blue points are the calculated points. The black lines indicate the offset from the real position.
