/*
 * trackball.c
 */
#include <math.h>
#include "vectors.h"
#include "trackball.h"

/*
 * This size should really be based on the distance from the center of
 * rotation to the point on the object underneath the mouse.  That
 * point would then track the mouse as closely as possible.  This is a
 * simple example, though, so that is left as an Exercise for the
 * Programmer.
 */
#define TRACKBALLSIZE  (0.8)

/*
 * Local function prototypes (not defined in trackball.h)
 */
static float tb_project_to_sphere (float, float, float);
static void  axis_to_quat         (float a[3], float phi, float q[4]);

/*
 * Ok, simulate a track-ball.  Project the points onto the virtual
 * trackball, then figure out the axis of rotation, which is the cross
 * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
 * Note:  This is a deformed trackball-- is a trackball in the center,
 * but is deformed into a hyperbolic sheet of rotation away from the
 * center.  This particular function was chosen after trying out
 * several variations.
 *
 * It is assumed that the arguments to this routine are in the range
 * (-1.0 ... 1.0)
 */
void
trackball (float q[4], float p1x, float p1y, float p2x, float p2y)
{
  float a[3]; /* Axis of rotation */
  float phi;  /* how much to rotate about axis */
  float p1[3], p2[3], d[3];
  float t;

  if (p1x == p2x && p1y == p2y)
    {
      /* Zero rotation */
      V3DZERO (q);
      q[3] = 1.0;
      return;
    }

  /*
   * First, figure out z-coordinates for projection of P1 and P2 to
   * deformed sphere
   */
  V3DSET (p1, p1x, p1y, tb_project_to_sphere (TRACKBALLSIZE, p1x, p1y));
  V3DSET (p2, p2x, p2y, tb_project_to_sphere (TRACKBALLSIZE, p2x, p2y));
  V3DCROSS (a, p2, p1); /* a is the rotation axis */

  /* Figure out how much to rotate around that axis. */
  V3DSUB (d, p1, p2);
  t = V3DLENGTH (d) / (2.0 * TRACKBALLSIZE);
  t = CLAMP (t, -1.0, 1.0); /* just to be sure! */
  phi = 2.0 * asin (t);

  axis_to_quat (a, phi, q);
}

/*
 *  Given an axis and angle, compute quaternion.
 */
static void
axis_to_quat (float a[3], float phi, float q[4])
{
  V3DNORMALIZE (a);
  V3DCOPY (q, a);
  V3DSCALE (q, sin (phi / 2.0));
  q[3] = cos (phi / 2.0);
}

/*
 * Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
 * if we are away from the center of the sphere.
 */
static float
tb_project_to_sphere (float r, float x, float y)
{
  float d;

  d = sqrt (x * x + y * y);
  if (d < r * 0.70710678118654752440)
    return sqrt (r * r - d * d);
  else
    return r * r / d / 2.0;
}

/*
 * Given two rotations, e1 and e2, expressed as quaternion rotations,
 * figure out the equivalent single rotation and stuff it into dest.
 *
 * This routine also normalizes the result every RENORMCOUNT times it is
 * called, to keep error from creeping in.
 *
 * NOTE: This routine is written so that q1 or q2 may be the same
 * as dest (or each other).
 */

#define RENORMCOUNT 97

void
add_quats (float q1[4], float q2[4], float dest[4])
{
  static int count=0;
  float      t1[4], t2[4], t3[4];
  float      tf[4];

  V3DCOPY (t1, q1);
  V3DSCALE (t1, q2[3]);

  V3DCOPY (t2, q2);
  V3DSCALE (t2, q1[3]);

  V3DCROSS (t3, q2, q1);
  V3DADD (tf, t1, t2);
  V3DADD (tf, t3, tf);
  tf[3] = q1[3] * q2[3] - V3DDOT (q1, q2);

  V4DCOPY (dest, tf);

  if (++count > RENORMCOUNT)
    {
      count = 0;
      V4DNORMALIZE (dest);
    }
}

/*
 * Build a rotation matrix, given a quaternion rotation.
 */
void
build_rotmatrix (float m[4][4], float q[4])
{
  m[0][0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
  m[0][1] = 2.0 * (q[0] * q[1] - q[2] * q[3]);
  m[0][2] = 2.0 * (q[2] * q[0] + q[1] * q[3]);
  m[0][3] = 0.0;

  m[1][0] = 2.0 * (q[0] * q[1] + q[2] * q[3]);
  m[1][1]= 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0]);
  m[1][2] = 2.0 * (q[1] * q[2] - q[0] * q[3]);
  m[1][3] = 0.0;

  m[2][0] = 2.0 * (q[2] * q[0] - q[1] * q[3]);
  m[2][1] = 2.0 * (q[1] * q[2] + q[0] * q[3]);
  m[2][2] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]);
  m[2][3] = 0.0;

  m[3][0] = 0.0;
  m[3][1] = 0.0;
  m[3][2] = 0.0;
  m[3][3] = 1.0;
}

