Compare all the matrices with the ones I posted earlier and see where the difference comes from.
How to calcul 3D point in a coordinate system to another one?
-
shkyo30 -
June 20, 2016 at 11:32 AM -
Thread is Resolved
-
-
Only the matrix of the old base is okay, for all others matrix, there are some little differences...
My function:
Code
Display Moreprivate Matrix3D calculMatriceParVecteur(Vector3D myVecteur, Point4D myPoint) { double vX = myVecteur.X * Math.PI / 180.0; double vY = myVecteur.Y * Math.PI / 180.0; double vZ = myVecteur.Z * Math.PI / 180.0; double A = Math.Cos(vX); double B = Math.Sin(vX); double C = Math.Cos(vY); double D = Math.Sin(vY); double E = Math.Cos(vZ); double F = Math.Sin(vZ); Matrix3D matriceRotateTranslateXYZ = new Matrix3D((C * E), (-C * F), D, myPoint.X, (B * D * E + A * F), (-B * D * F + A * E), (-B * C), myPoint.Y, (-A * D * E + B * F), (A * D * F + B * E), (A * C), myPoint.Z, 0.0, 0.0, 0.0, 1.0); return matriceRotateTranslateXYZ; }and the code which use it:
Code
Display MorePoint4D repereBase1 = new Point4D(0.0, 1126.5, 805.0, 1.0); Vector3D vectRepBase1 = new Vector3D(0.0, 0.0, 0.0); Point4D repereBase2 = new Point4D(-49.907, 1060.275, 791.113, 1.0); Vector3D vectRepBase2 = new Vector3D(0.115, 1.9967, 3.2965); Point4D point1Base1 = new Point4D(1490.458, -1126.5, 1459.4, 1.0); Vector3D vectPt1Base1 = new Vector3D(180.0, 81.6634, 180.0); Point4D point1Base2 = new Point4D(); Matrix3D base1 = new Matrix3D(); Matrix3D base2 = new Matrix3D(); Matrix3D pt1base1 = new Matrix3D(); Matrix3D pt1base2 = new Matrix3D(); Matrix3D resultMatriceTempo = new Matrix3D(); base1 = calculMatriceParVecteur(vectRepBase1, repereBase1); base2 = calculMatriceParVecteur(vectRepBase2, repereBase2); pt1base1 = calculMatriceParVecteur(vectPt1Base1, point1Base1); base2.Invert(); resultMatriceTempo = base2 * base1; pt1base2 = resultMatriceTempo * pt1base1; point1Base2.X = pt1base2.M14; point1Base2.Y = pt1base2.M24; point1Base2.Z = pt1base2.M34; point1Base2.W = pt1base2.M44;I think that's the same of your function, but I'm wrong somewhere because my results are wrong...
-
Try with
cos(b)*cos(a) cos(a)*sin(c)*sin(b)-cos(c)*sin(a) cos(c)*cos(a)*sin(b)+sin(c)*sin(a)
cos(b)*sin(a) cos(c)*cos(a)+sin(c)*sin(b)*sin(a) -cos(a)*sin(c)+cos(c)*sin(b)*sin(a)
-sin(b) cos(b)*sin(c) cos(c)*cos(b)Your current rotation matrix seems to be transposed.
-
Try withcos(b)*cos(a) cos(a)*sin(c)*sin(b)-cos(c)*sin(a) cos(c)*cos(a)*sin(b)+sin(c)*sin(a)
cos(b)*sin(a) cos(c)*cos(a)+sin(c)*sin(b)*sin(a) -cos(a)*sin(c)+cos(c)*sin(b)*sin(a)
-sin(b) cos(b)*sin(c) cos(c)*cos(b)Your current rotation matrix seems to be transposed.
This one doesn't works (values totally false) but I tried the "NASA" transformation (look here: http://www.euclideanspace.com/maths/geometry…atrix/index.htm)
And now I've a better precision on x axis (gap of 0.05) but always around 2.0 of gap with y and z axis...
-
Here's the full code that gives the correct answer
double a, b, c;
double sx, sy, sz;
double cx, cy, cz;a = 0.0 * Math.PI / 180.0;
b = 0.0 * Math.PI / 180.0;
c = 0.0 * Math.PI / 180.0;sx = Math.Sin(c);
sy = Math.Sin(b);
sz = Math.Sin(a);
cx = Math.Cos(c);
cy = Math.Cos(b);
cz = Math.Cos(a);Matrix3D base_old = new Matrix3D(cy * cz, cz * sx * sy - cx * sz, cx * cz * sy + sx * sz, 0,
cy * sz, cx * cz + sx * sy * sz, -cz * sx + cx * sy * sz, 1126.5,
-sy, cy * sx, cx * cy, 805.0,
0, 0, 0, 1);a = 180.0 * Math.PI / 180.0;
b = 81.6634 * Math.PI / 180.0;
c = 180.0 * Math.PI / 180.0;sx = Math.Sin(c);
sy = Math.Sin(b);
sz = Math.Sin(a);
cx = Math.Cos(c);
cy = Math.Cos(b);
cz = Math.Cos(a);Matrix3D pose_old = new Matrix3D(cy * cz, cz * sx * sy - cx * sz, cx * cz * sy + sx * sz, 1490.46,
cy * sz, cx * cz + sx * sy * sz, -cz * sx + cx * sy * sz, -1126.5,
-sy, cy * sx, cx * cy, 1459.4,
0, 0, 0, 1);a = 3.2965 * Math.PI / 180.0;
b = 1.9967 * Math.PI / 180.0;
c = 0.115 * Math.PI / 180.0;sx = Math.Sin(c);
sy = Math.Sin(b);
sz = Math.Sin(a);
cx = Math.Cos(c);
cy = Math.Cos(b);
cz = Math.Cos(a);Matrix3D base_new = new Matrix3D(cy * cz, cz * sx * sy - cx * sz, cx * cz * sy + sx * sz, -49.907,
cy * sz, cx * cz + sx * sy * sz, -cz * sx + cx * sy * sz, 1060.275,
-sy, cy * sx, cx * cy, 791.113,
0, 0, 0, 1);base_new.Invert();
Matrix3D pose_new = System.Windows.Media.Media3D.Matrix3D.Multiply(base_new,base_old);
pose_new = System.Windows.Media.Media3D.Matrix3D.Multiply(pose_new, pose_old);// Convert rotation matrix to abc angles
double a_new, b_new, c_new;
if (Math.Abs(pose_new.M31 -1) < 1e-6)
{
a_new = Math.Atan2(-pose_new.M23,pose_new.M22);
b_new = -Math.PI / 2;
c_new = 0.0;
}
else if (Math.Abs(pose_new.M31 +1) < 1e-6)
{
a_new = -Math.Atan2(-pose_new.M23, pose_new.M22);
b_new = Math.PI / 2;
c_new = 0.0;
}
else
{
a_new = Math.Atan2(pose_new.M21, pose_new.M11);
b_new = Math.Asin(-pose_new.M31);
c_new = Math.Atan2(pose_new.M32, pose_new.M33);
}a_new *= 180 / Math.PI;
b_new *= 180 / Math.PI;
c_new *= 180 / Math.PI; -
YES!!
With your last post, I modified my function and know all are okay!
I saw two ways for my mistakes:
- firstable I think it's not a good idea using " * " than Matrix3D.Multiply(M1,M2) ...
- and it's not a good idea (again!) to switch a and c values before treatment, it's an error source when I wrote the 4x4 matrix ...Thank you very (VERY!) much Spl!!
Advertising from our partners
