@Override
	public void transform(double[] src, double[] dst) {
		// TODO Auto-generated method stub
		
		
		for (int i = 0; i < 3; i++)
			temp2[i] = src[i];

		temp2[3] = 1;

		for (int i = 0; i < 3; i++) {
			dst[i] = 0;
			for (int j = 0; j < 4; j++) {
				dst[i] += matrix[i][j] * temp2[j];
			}
		}

		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				copy[i][j] = matrix[i][j];
			}
		}
		
		Invert.invert(copy, inverseMatrix);
		transpose(inverseMatrix, inverseMatrixTranspose);
		
		for (int i = 0; i < 3; i++)
			temp2[i] = src[i+3];
		temp2[3]=0;
		
		for (int i = 0; i < 3; i++) {
			dst[i+3] = 0;
			for (int j = 0; j < 4; j++) {
				dst[i+3] += inverseMatrixTranspose[i][j] * temp2[j];
			}
		}		
	}