public PointPol rotate(Vector direction, double angle, double a, double b, double c) { double phi = Math.PI / 360 * angle; PointPol p = shift(-a, -b, -c); double x1 = direction.P1.X; double y1 = direction.P1.Y; double z1 = direction.P1.Z; double x2 = direction.P2.X; double y2 = direction.P2.Y; double z2 = direction.P2.Z; double vecx = x2 - x1; double vecy = y2 - y1; double vecz = z2 - z1; double len = Math.Sqrt(vecx * vecx + vecy * vecy + vecz * vecz); double l = vecx / len; double m = vecy / len; double n = vecz / len; double[,] transfer = new double[4, 4] { { l *l + Math.Cos(phi) * (1 - l * l), l *(1 - Math.Cos(phi)) * m + n * Math.Sin(phi), l *(1 - Math.Cos(phi)) * n - m * Math.Sin(phi), 0 }, { l *(1 - Math.Cos(phi)) * m - n * Math.Sin(phi), m *m + Math.Cos(phi) * (1 - m * m), m *(1 - Math.Cos(phi)) * n + l * Math.Sin(phi), 0 }, { l *(1 - Math.Cos(phi)) * n + m * Math.Sin(phi), m *(1 - Math.Cos(phi)) * n - l * Math.Sin(phi), n *n + Math.Cos(phi) * (1 - n * n), 0 }, { 0, 0, 0, 1 } }; var t1 = matrix_multiplication(p.getP(), transfer); t1 = matrix_multiplication(t1, transfer); PointPol p2 = translatePol(t1); PointPol p3 = p2.shift(a, b, c); return(p3); }
public void shift(double a, double b, double c) { P1 = P1.shift(a, b, c); P2 = P2.shift(a, b, c); }