绕空间任意轴旋转矩阵

//绕空间任意轴旋转矩阵
        public static double[] GetRotateMatrix(XDPOINT p,XDVector axis,double angleRad)
        {
            axis = axis.Unit();
            double[] M = LCMetroKernel.IdentityMatrix4x4();
            double a = p.x; double b = p.y; double c = p.z;
            double u = axis.I;  double v = axis.J;  double w = axis.K;
            double Cos = Math.Cos(angleRad);
            double Sin = Math.Sin(angleRad);

            M[0] = u * u + (v * v + w * w) * Cos;
            M[1] = u * v * (1 - Cos) - w * Sin;
            M[2] = u * w * (1 - Cos) + v * Sin;
            M[3] = (a * (v * v + w * w) - u * (b * v + c * w)) * (1 - Cos) + (b * w - c * v) * Sin;
            M[4] = u * v * (1 - Cos) + w * Sin;
            M[5] = v * v + (u * u + w * w) * Cos;
            M[6] = v * w * (1 - Cos) - u * Sin;
            M[7] = (b * (u * u + w * w) - v * (a * u + c * w)) * (1 - Cos) + (c * u - a * w) * Sin;
            M[8] = u * w * (1 - Cos) - v * Sin;
            M[9] = v * w * (1 - Cos) + u * Sin;
            M[10] = w * w + (u * u + v * v) * Cos;
            M[11] = (c * (u * u + v * v) - w * (a * u + b * v)) * (1 - Cos) + (a * v - b * u) * Sin;

            return M;
        }