X1, X2, X3, X4, X5, X6, Y1, Y2, Y3, Y4, Y5, Y6, Z1, Z2, Z3, Z4, Z5, Z6, c1, c2, c3, s1, s2, s3, 
c1^2+s1^2-1
c2^2+s2^2-1
c3^2+s3^2-1
X2*s1+X3*c2+X4*s2+X5*c3+X6*s3-X1+c1
Y2*s1+Y3*c2+Y4*s2+Y5*c3+Y6*s3-Y1+c1
Z2*s1+Z3*c2+Z4*s2+Z5*c3+Z6*s3-Z1+c1
