Pb de transformation 3D avec des matrices

Version imprimable

Pour info, j'essaie d'utiliser la bibliothèque Media3D dans un projet WPF

J'ai 2 repères:
x1=0.0 y1=1126.5 z1=805 rx1=0.0 ry1=0.0 rz1=0.0
x2=-49.907 y2=1060.275 z1=791.113 rx2=0.115 ry2=1.9967 rz2=3.2965

J'ai plusieurs points 3D dont j'ai les coordonnées dans les deux repères (justement pour valider les calculs), en voici un dans le repère 1:
px1=1490.458 py1=-1126.5 pz1=1459.4 prx1=180.0 pry1=81.6634 prz1=180.0
le même point dans le repère 2
px2=1424.618 py2=-1144.036 pz2=1526.147 prx2=180.0 pry2=83.6634 prz2=176.7055

Donc si je ne me trompe pas, mon vecteur de déplacement entre les 2 repères c'est (-49.907, -66.225, -13.887)

Pour la translation je fais donc:
Code:

1 2 3 4 5 6 Vector3D vectNewRepere = new Vector3D(-49.907, -66.225, -13.887); Point4D pointDep = new Point4D(1490.458, -1126.5, 1459.4, 1); Point4D pointFin = new Point4D(); TranslateTransform3D matTransl = new TranslateTransform3D(vectNewRepere); pointFin = matTransl.Transform(pointDep);
Ensuite pour les rotation en RX, RY et RZ, je fais ça "à la main" car je n'ai pas trouvé comment faire ça avec la bibliothèque, sachant que a, b et c sont mes valeurs de rotation en radians, voici ma matrice 4D:
(cos(a)*cos(b), sin(a)*cos(c)+cos(a)*sin(b)*sin(c), sin(a)*sin(c)-cos(a)*sin(b)*cos(c), 0
-sin(a)*cos(b), cos(a)*cos(c)-sin(a)*sin(b)*sin(c), cos(a)*sin(c)+sin(a)*sin(b)*cos(c), 0
sin(b), -cos(b)*sin(c), cos(b)*cos(c), 1
0, 0, 0, 1)

Et donc dans mon code, APRES avoir fait la translation, je fais ça:
Code:

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 double aDeg = 0.115; double bDeg = 1.9967; double cDeg = 3.2965; double a = -aDeg * Math.PI / 180; double b = -bDeg * Math.PI / 180; double c = -cDeg * Math.PI / 180; double ca = Math.Cos(a); double sa = Math.Sin(a); double cb = Math.Cos(b); double sb = Math.Sin(b); double cc = Math.Cos(c); double sc = Math.Sin(c); Matrix3D matriceRotateXYZ = new Matrix3D((ca * cb), (sa * cc + ca * sb * sc), (sa * sc - ca * sb * cc), 0, (-sa * cb), (ca * cc - sa * sb * sc), (ca * sc + sa * sb * cc), 0, sb, (-cb * sc), (cb * cc), 0, 0, 0, 0, 1); pointDep = pointFin; pointFin = pointDep * matriceRotateXYZ;
Dans "pointFin" je retrouve: (1386.9165, -1107.68333, 1561.02155, 1446.513)

Ce qui n'est pas du tout ce que je devrais obtenir!!! :aie::(
J'ai du me tromper quelque part, ça c'est certain, mais je ne vois pas où pour l'instant... Si quelqu'un a une idée, je prends!

22/06/2016, 11h51
shkyo

Bon j'ai trouvé sur un autre forum la formule qui va bien pour passer le point 1 du repère 1 dans le repère 2:

(matrice4x4 du point 1 (dans le repère 2)) = (matrice4x4 inverse du repère 2) x (matrice4x4 du repère 1) x (matrice4x4 du point 1 (dans le repère 1))

A priori ça a l'air simple, mais comment passe-t-on de coordonnées x,y,z,rx,ry,rz en matrice 4x4?? :calim2:

Je suis preneur soit des calculs, soit d'une éventuelle fonction de la bibliothèque Media3D capable de faire ça... ;)
22/06/2016, 16h04
shkyo

Bon ce problème ne doit pas se poser souvent car il ne déchaine pas les foules... :calim2: Tant pis, je parle tout seul mais si ça aider quelqu'un un de ces jours... ;)

Entre temps, j'ai trouvé qu'un repère 3D (ou un point 3D avec son orientation) x,y,z,rx,ry,rz peut être "passé" sous forme de matrice 4x4 comme ceci:

cos(ry)*cos(rz) -cos(ry)*sin(rz) sin(ry) x
sin(rx)*sin(ry)*cos(rz)+cos(rx)*sin(rz) -sin(rx)*sin(ry)*sin(rz)+cos(rx)*cos(rz) -sin(rx)*cos(ry) y
-cos(rx)*sin(ry)*cos(rz)+sin(rx)*sin(rz) cos(rx)*sin(ry)*sin(rz)+sin(rx)*cos(rz) cos(rx)*cos(ry) z
0 0 0 1

Ce qui une fois calculé avec la formule ci-dessus donne bien les x,y et z du nouveau point! :mrgreen:

Je n'ai plus qu'à trouver comment extraire les valeurs de rx, ry et rz de la matrice 4x4 que j'obtiens en résultat... :aie:

Pour info avant de clore ce sujet, voici le code final qui fonctionne très bien:

Code:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
private Matrix3D calculMatrice4x4fromPoint(Point4D myPoint, Vector3D myVecteur)
{
      double a, b, c;
      double sinX, sinY, sinZ;
      double cosX, cosY, cosZ;
 
      a = myVecteur.X * Math.PI / 180.0;
      b = myVecteur.Y * Math.PI / 180.0;
      c = myVecteur.Z * Math.PI / 180.0;
 
      sinX = Math.Sin(c);
      sinY = Math.Sin(b);
      sinZ = Math.Sin(a);
      cosX = Math.Cos(c);
      cosY = Math.Cos(b);
      cosZ = Math.Cos(a);
 
      Matrix3D matriceRotateTranslateXYZ = new Matrix3D(cosY * cosZ, cosZ * sinX * sinY - cosX * sinZ, cosX * cosZ * sinY + sinX * sinZ, myPoint.X,
                                                     cosY * sinZ, cosX * cosZ + sinX * sinY * sinZ, -cosZ * sinX + cosX * sinY * sinZ, myPoint.Y,
                                                     -sinY, cosY * sinX, cosX * cosY, myPoint.Z,
                                                     0, 0, 0, 1);
 
      return matriceRotateTranslateXYZ;
}

A noter que l'inversion des valeurs a et c vient du fait que ce programme est fait pour manipuler des données venant d'un robot Kuka, où a, b et c correspondent à rz, ry, rx.

Avec un exemple d'utilisation de cette fonction:

Code:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
Point4D 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(3.2965, 1.9967, 0.115);
 
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 = calculMatrice4x4fromPoint(repereBase1, vectRepBase1);
base2 = calculMatrice4x4fromPoint(repereBase2, vectRepBase2);
base2.Invert();
 
pt1base1 = calculMatrice4x4fromPoint(point1Base1, vectPt1Base1);
 
resultMatriceTempo = Matrix3D.Multiply(base2, base1);
 
pt1base2 = Matrix3D.Multiply(resultMatriceTempo, pt1base1);
 
point1Base2.X = pt1base2.M14;
point1Base2.Y = pt1base2.M24;
point1Base2.Z = pt1base2.M34;
point1Base2.W = pt1base2.M44;
 
CustomMessageBox.Show(point1Base2.ToString());

Voilu, j'espère que ça pourra aider quelqu'un car perso, ça m'a pas mal agacé... :mrgreen: