| 938 | } |
| 939 | |
| 940 | std::array<Matrix4D, 4> Matrix4D::decompose() const |
| 941 | { |
| 942 | // decompose the matrix to shear, scale, rotation and move |
| 943 | // so that matrix = move * rotation * scale * shear |
| 944 | // return an array of matrices |
| 945 | Matrix4D moveMatrix; |
| 946 | Matrix4D rotationMatrix; |
| 947 | Matrix4D scaleMatrix; |
| 948 | Matrix4D residualMatrix(*this); |
| 949 | // extract transform |
| 950 | moveMatrix.move(residualMatrix.getCol(3)); |
| 951 | residualMatrix.setCol(3, Vector3d()); |
| 952 | // find and extract rotation |
| 953 | int prim_dir = -1; |
| 954 | std::array<Vector3d, 3> dirs = {Vector3d(1., 0., 0.), Vector3d(0., 1., 0.), Vector3d(0., 0., 1.)}; |
| 955 | for (int i = 0; i < 3; i++) { |
| 956 | if (residualMatrix.getCol(i).IsNull()) { |
| 957 | continue; |
| 958 | } |
| 959 | if (prim_dir < 0) { |
| 960 | dirs[i] = residualMatrix.getCol(i); |
| 961 | dirs[i].Normalize(); |
| 962 | prim_dir = i; |
| 963 | continue; |
| 964 | } |
| 965 | |
| 966 | Vector3d cross = dirs[prim_dir].Cross(residualMatrix.getCol(i)); |
| 967 | if (cross.IsNull()) { |
| 968 | continue; |
| 969 | } |
| 970 | cross.Normalize(); |
| 971 | int last_dir = 3 - i - prim_dir; |
| 972 | if (i - prim_dir == 1) { |
| 973 | dirs[last_dir] = cross; |
| 974 | dirs[i] = cross.Cross(dirs[prim_dir]); |
| 975 | } |
| 976 | else { |
| 977 | dirs[last_dir] = -cross; |
| 978 | dirs[i] = dirs[prim_dir].Cross(-cross); |
| 979 | } |
| 980 | prim_dir = -2; // done |
| 981 | break; |
| 982 | } |
| 983 | if (prim_dir >= 0) { |
| 984 | // handle case with only one valid direction |
| 985 | Vector3d cross = dirs[prim_dir].Cross(Vector3d(0., 0., 1.)); |
| 986 | if (cross.IsNull()) { |
| 987 | cross = dirs[prim_dir].Cross(Vector3d(0., 1., 0.)); |
| 988 | } |
| 989 | dirs[(prim_dir + 1) % 3] = cross; |
| 990 | dirs[(prim_dir + 2) % 3] = dirs[prim_dir].Cross(cross); |
| 991 | } |
| 992 | rotationMatrix.setCol(0, dirs[0]); |
| 993 | rotationMatrix.setCol(1, dirs[1]); |
| 994 | rotationMatrix.setCol(2, dirs[2]); |
| 995 | rotationMatrix.inverseGauss(); |
| 996 | residualMatrix = rotationMatrix * residualMatrix; |
| 997 | // To keep signs of the scale factors equal |