/*
  Rotation around an arbitray 3D axis.

  Algorithm by Johannes Hubert, jhubert@algonet.se

  No Guarantees! If you use it, do so at your own risk!
  I make no claims about the correctness, completeness and/or about
  the question, if the algorithm is an optimal/fast algorithm or not!

  ------------------------------------------------------------------

  Here comes the theory:

  For rotation around an arbitrary axis in 3D, you usually have 3 vectors and
  one angle:

  Two vectors to describe the rotation axis (each vector describing one
  point, the axis is the line that runs through both points), one vector
  that is to be rotated around this axis and an angle to describe how much
  you want to rotate.

  To do the rotation, you compute a transformation matrix for the following
  steps.

  1. Translation, so that the rotation-axis now runs through the origin:
     Matrix T.
  2. Rotation around the X-axis until the rotation-axis falls into the XY-plane:
     Matrix Mx.
  3. Rotation around the Y-axis until the rotation-axis falls into the z-axis:
     Matrix My.
  4. Now you can rotate around the z-axis by the rotation angle:
     Matrix Mz
  5. Backwards-rotation of the rotation from step 3, this is achieved with
     the inverse matrix of My: My^-1
  6. Backwards-rotation of the rotation from step 2, this is achieved with
     the inverse matrix of Mx: Mx^-1
  7. Back-translation of the translation from step 1, this is achieved with
     the inverse matrix of T: T^-1

  Once you have calculated a matrix for each of the 7 steps, then you only 
  need to multiply these seven matrices with each other (in the given order).
  The resulting matrix is your rotation-matrix Mrot:

  Mrot = T * Mx * My * Mz * My^-1 * Mx^-1 * T^-1

  You can then multiply any vector with the matrix "Mrot" and the resulting
  vector will be the rotated vector.

  No here comes the drawback with my algorithm:
  My algorithm does not include the translation parts (steps 1 and 7), because
  for two reasons:
  1. In my case, I didn't need to translate, because the rotation-axis would
     always run through the origin anyway
  2. A translation-matrix has to be a 4x4 matrix, which means, that the other
     matrices also have to be 4x4 matrices, and the vectors have to be 1x4 
	 vectors. But this means, that the functions for the matrix- and vector
	 multiplications, and the function to calculate the inverse matrix would
	 be a lot more complicated.
	 Without the translation, I could stick to 3x3 matrices and 1x3 vectors
	 (because you don't need the 4th dimension for rotation-matrices).


  But hey, don't worry, the algorithm still works for any axis:

  The rotation matrix is now:

  Mrot = Mx * My * Mz * My^-1 * Mx^-1

  But before you can calculate it, you first have to translate the rotation-
  axis by hand, so that it runs through the origin.
  And before using it, you have to translate the vector that is to be rotated
  by the same amount and after rotating it, re-translate it by the negative 
  amount.
  Why isn't that so bad after all?
  Because even if we had the full 4x4 Mrot of above that includes the translation,
  and we could then reuse this matrix to rotate as many points as we want,
  the matrix/vector-multiplication required for that would have more float 
  operations than the additional translate/re-translate we do by hand now!
  See also the comments in the source-code.
*/


/////////////////////////////
// types and defines:
/////////////////////////////

#define PI			3.141592654

// a 3D vector
typedef struct _VECTOR {
	double x, y, z;
} VECTOR;

// a 3x3 matrix
typedef struct _MATRIX3X3 {
	double w11, w12, w13;
	double w21, w22, w23;
	double w31, w32, w33;
} MATRIX3X3;




////////////////////////////
// global variables:
////////////////////////////

// the identity matrix
MATRIX3X3 id_matrix = { 1.0, 0.0, 0.0, 
			  		    0.0, 1.0, 0.0, 
					    0.0, 0.0, 1.0 } ; 



////////////////////////////
// function prototypes:
////////////////////////////

// three examples for possible rotation functions:
VECTOR	  rotate_origin(VECTOR axis, double angle, VECTOR point);
VECTOR	  rotate_arbitrary(VECTOR axis1, VECTOR axis2, double angle, VECTOR point);
void	  rotate_lots(VECTOR axis1, VECTOR axis2, double angle, VECTOR *point, int num);

// needed helper-functions:
MATRIX3X3 create_rot_matrix(VECTOR axis, double angle);
MATRIX3X3 matrix_inverse(MATRIX3X3 m);
MATRIX3X3 matrix_mult(MATRIX3X3 m1, MATRIX3X3 m2);
VECTOR	  vect_add(VECTOR v1, VECTOR v2);
VECTOR    vect_sub(VECTOR v1, VECTOR v2);




////////////////////////////
// functions:
////////////////////////////

VECTOR rotate_origin(VECTOR axis, double angle, VECTOR point)
/*
  Rotates "point" around the axis that runs through the origin and the
  point "axis", counterclockwise for "angle" (in radians).
  Returns the rotated point.
*/
{
	if (angle == 0.0 || angle == 2*PI) return point;

	MATRIX3X3 m = create_rot_matrix(axis, angle);

	return vect_matrix_mult(point, m); 
}


VECTOR rotate_arbitray(VECTOR axis1, VECTOR axis2, double angle, VECTOR point)
/*
  Rotates "point" around the axis that runs through the the points "axis1"
  and "axis2", counterclockwise for "angle" (in radians).
  Returns the rotated point.

  To achieve this, first the point "axis1" is translated by the vector "axis2",
  which has the effect that it now (together with the origin) describes a
  parallel axis of rotation running through the origin. This axis can be used
  to rotate the point, but before and after, the point has to be translated
  by the same value "axis2".
*/
{
	if (angle == 0.0 || angle == 2*PI) return point;

	// translate "axis1", so that it describes the parallel axis running
	// through the origin
	axis1 = vect_sub(axis1, axis2);

	// now this translated axis can be used to construct the rotation matrix
	MATRIX3X3 m = create_rot_matrix(axis1, angle);

	// before we can rotate a vector around this translated
	// axis, we first have to translate that vector too
	point = vect_sub(point, axis2);

	// then we can rotate it:
	point = vect_matrix_mult(point, m);

	// and re-translate it after the rotation.
	point = vect_add(point, axis2);

	return point;
}


void rotate_lots(VECTOR axis1, VECTOR axis2, double angle, VECTOR *point, int num)
/*
  Rotates "num" many points (stored in the array "*point") around the axis that 
  runs through the points "axis1" and "axis2", counterclockwise for "angle" 
  (in radians). Stores the rotated vectors in the same array.

  A first idea would be to call "rotate_arbitrary" for each of the points.
  However, this would be highly inefficient, because the rotation matrix
  would be re-calculated each time.
  Instead we do so here, reusing the same rotation matrix:
*/
{
	if (angle == 0.0 || angle == 2*PI) return point;

	axis1 = vect_sub(axis1, axis2);
	MATRIX3X3 m = create_rot_matrix(axis1, angle);

	for (int i = 0; i < num; i++) {
		point[i] = vect_sub(point[i], axis2);
		point[i] = vect_matrix_mult(point[i], m);
		point[i] = vect_add(point[i], axis2);
	}
}


MATRIX3X3 create_rot_matrix(VECTOR axis, double angle)
/*
  Creates a rotation-matrix for rotation around an axis running through
  the origin.

  axis		: Defines the rotation axis: The rotation axis is the line
              that runs through the origin and through this point.
  angle		: The angle (in radians) to rotate (counterclockwise) around
              this axis.
*/
{
	if (angle == 0.0 || angle == 2*PI)
		return id_matrix;

	MATRIX3X3	rx,		// rotation-matrix Mx
				ry,		// rotation-matrix My
				ra,		// rotation-matrix Mz
				m;		// resulting rotation-matrix Mrot

	// we have 3 possible special cases, where the rotation-axis
	// already falls together with one of the main axis. In this case
	// we construct the Mrot matrix directly

	// special case: rotation-axis is the z-axis:
	if (axis.x == 0.0 && axis.y == 0.0) {
		m = id_matrix;
		m.w11 = cos(angle);
		m.w12 = sin(angle);
		m.w21 = -m.w12;
		m.w22 = m.w11;
	}

	// special case: rotation-axis is the y-axis:
	else if (axis.x == 0.0 && axis.z == 0.0) {
		m = id_matrix;
		m.w11 = cos(angle);
		m.w13 = sin(angle);
		m.w31 = -m.w13;
		m.w33 = m.w11;
	}

	// special case: rotation-axis is the x-axis:
	else if (axis.y == 0.0 && axis.z == 0.0) {
		m = id_matrix;
		m.w22 = cos(angle);
		m.w23 = sin(angle);
		m.w32 = -m.w23;
		m.w33 = m.w22;
	}

	// for any other axis, we need to create the intermediate matrices
	// and multiply them together into the Mrot matrix:
	else {
		// to make things easier, we work with an axis-vector of length 1:

		// calculate the current axis-vector's length
		double d = sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);

		// calculate a new vector (a, b, c) of the same direction, but length 1
		double a = axis.x / d;
		double b = axis.y / d;
		double c = axis.z / d;

		d = sqrt(b*b + c*c);	// d is reused here, as a helper value

		// calculate the Mx matrix
		rx = id_matrix;
		rx.w22 = c / d;
		rx.w23 = b / d;
		rx.w32 = -rx.w23;
		rx.w33 = rx.w22;

		// calculate the My matrix
		ry = id_matrix;
		ry.w11 = d;
		ry.w13 = a;
		ry.w31 = -a;
		ry.w33 = d;

		// Mrot = Mx * My
		m = matrix_mult(rx, ry);

		// calculate the Mz matrix
		ra = id_matrix;
		ra.w11 = cos(angle);
		ra.w12 = sin(angle);
		ra.w21 = -ra.w12;
		ra.w22 = ra.w11;

		// Mrot = (Mx * My) * Mz
		m = matrix_mult(m, ra);

		// calculate My^-1
		ry = matrix_inverse(ry);

		// Mrot = ((Mx * My) * Mz) * My^-1
		m = matrix_mult(m, ry);
			
		// calculate Mx^-1
		rx = matrix_inverse(rx);

		// Mrot = (((Mx * My) * Mz) * My^-1) * Mx^-1
		m = matrix_mult(m, rx);

	}

	return m;		// return the resulting Mrot
}


MATRIX3X3 matrix_inverse(MATRIX3X3 m)
/*
  Calculates the inverse matrix of the matrix "m" and returns it.
  This function assumes the the determinante of "m" is 1.
  (Which is the case for the rotation-matrices).
  If you want to use this function for matrices with det(m) != 1,
  then you have to divide each value with det(m).
  In this case, you could still not use this function for a matrix 
  with det(m) == 0 !

  det(m) would be:
  double det = m.w11*m.w22*m.w33 + m.w12*m.w23*m.w31 + m.w13*m.w21*m.w32 -
		  m.w13*m.w22*m.w31 - m.w11*m.w23*m.w32 - m.w12*m.w21*m.w33;

  and you would append a "/ det" to each of the lines below, like:
  e.w11 = (m.w22*m.w33 - m.w23*m.w32) / det;
*/
{
	MATRIX3X3	e;

	e.w11 = (m.w22*m.w33 - m.w23*m.w32);
	e.w12 = (m.w13*m.w32 - m.w12*m.w33);
	e.w13 = (m.w12*m.w23 - m.w13*m.w22);

	e.w21 = (m.w23*m.w31 - m.w21*m.w33);
	e.w22 = (m.w11*m.w33 - m.w13*m.w31);
	e.w23 = (m.w13*m.w21 - m.w11*m.w23);

	e.w31 = (m.w21*m.w32 - m.w22*m.w31);
	e.w32 = (m.w12*m.w31 - m.w11*m.w32);
	e.w33 = (m.w11*m.w22 - m.w12*m.w21);

	return e;
}


MATRIX3X3 matrix_mult(MATRIX3X3 m1, MATRIX3X3 m2)
/*
  This multiplies two matrices with each other (m1 * m2) and returns
  the resulting matrix.
*/
{
	MATRIX3X3 e;

	e.w11 = m1.w11*m2.w11 + 
			m1.w12*m2.w21 +
			m1.w13*m2.w31;
	e.w12 = m1.w11*m2.w12 + 
			m1.w12*m2.w22 +
			m1.w13*m2.w32;
	e.w13 = m1.w11*m2.w13 + 
			m1.w12*m2.w23 +
			m1.w13*m2.w33;

	e.w21 = m1.w21*m2.w11 + 
			m1.w22*m2.w21 +
			m1.w23*m2.w31;
	e.w22 = m1.w21*m2.w12 + 
			m1.w22*m2.w22 +
			m1.w23*m2.w32;
	e.w23 = m1.w21*m2.w13 + 
			m1.w22*m2.w23 +
			m1.w23*m2.w33;

	e.w31 = m1.w31*m2.w11 + 
			m1.w32*m2.w21 +
			m1.w33*m2.w31;
	e.w32 = m1.w31*m2.w12 + 
			m1.w32*m2.w22 +
			m1.w33*m2.w32;
	e.w33 = m1.w31*m2.w13 + 
			m1.w32*m2.w23 +
			m1.w33*m2.w33;

	return e;
}


VECTOR vect_matrix_mult(VECTOR v, MATRIX3X3 m)
/*
  Multiplies a vector with a matrix (v * m) and returns the resulting vector.
*/
{
	VECTOR e;

	e.x = v.x*m.w11 +
		  v.y*m.w21 +
		  v.z*m.w31;

	e.y = v.x*m.w12 +
		  v.y*m.w22 +
		  v.z*m.w32;

	e.z = v.x*m.w13 +
		  v.y*m.w23 +
		  v.z*m.w33;

	return e;
}


VECTOR vect_add(VECTOR v1, VECTOR v2)
/*
  Adds the vectors "v1" and "v2" and returns the resulting vector.
*/
{
	VECTOR e;

	e.x = v1.x + v2.x;
	e.y = v1.y + v2.y;
	e.z = v1.z + v2.z;

	return e;
}


VECTOR vect_sub(VECTOR v1, VECTOR v2)
/*
  Subtracts the vector "v2" from "v1" and returns the resulting vector.
*/
{
	VECTOR e;

	e.x = v1.x - v2.x;
	e.y = v1.y - v2.y;
	e.z = v1.z - v2.z;

	return e;
}