Utils4D

package theGhastModding.nbody4.main;

public class Utils4D {

    public static final int[][] ROTATE_ZW = new int[][] {
        {3, -2, 0, 0},
        {2, 3, 0, 0},
        {0, 0, 1, 0},
        {0, 0, 0, 1},
    };

    public static final int[][] ROTATE_YW = new int[][] {
        {3, 0, -2, 0},
        {0, 1, 0, 0},
        {2, 0, 3, 0},
        {0, 0, 0, 1},
    };

    public static final int[][] ROTATE_YZ = new int[][] {
        {3, 0, 0, -2},
        {0, 1, 0, 0},
        {0, 0, 1, 0},
        {2, 0, 0, 3},
    };

    public static final int[][] ROTATE_XW = new int[][] {
        {1, 0, 0, 0},
        {0, 3, -2, 0},
        {0, 2, 3, 0},
        {0, 0, 0, 1},
    };

    public static final int[][] ROTATE_XZ = new int[][] {
        {1, 0, 0, 0},
        {0, 3, 0, -2},
        {0, 0, 1, 0},
        {0, 2, 0, 3},
    };

    public static final int[][] ROTATE_XY = new int[][] {
        {1, 0, 0, 0},
        {0, 1, 0, 0},
        {0, 0, 3, -2},
        {0, 0, 2, 3},
    };

    public static void rotate4D(double[] vec, double zw, double yw, double yz, double xw, double xz, double xy) {
        rotate(vec, ROTATE_ZW, zw);
        rotate(vec, ROTATE_YW, yw);
        rotate(vec, ROTATE_YZ, yz);
        rotate(vec, ROTATE_XW, xw);
        rotate(vec, ROTATE_XZ, xz);
        rotate(vec, ROTATE_XY, xy);
    }

    private static void rotate(double[] vec, int[][] rotationMatrix, double angle) {
        for(int i = 0; i < 4; i++) {
            double currRes = 0;
            for(int j = 0; j < 4; j++) {
                double mul = 0;
                switch(rotationMatrix[i][j]) {
                default:
                    mul = 0;
                    break;
                case 1:
                    mul = 1;
                    break;
                case 2:
                    mul = Math.sin(Math.toRadians(angle));
                    break;
                case 3:
                    mul = Math.cos(Math.toRadians(angle));
                    break;
                case -2:
                    mul = -Math.sin(Math.toRadians(angle));
                    break;
                case -3:
                    mul = -Math.cos(Math.toRadians(angle));
                    break;
                }
                currRes += vec[j] * mul;
            }
            vec[i] = currRes;
        }
    }

}