/**** BallMath.c - Essential routines for ArcBall.  ****/
#include <math.h>
#include "BallMath.h"
#include "BallAux.h"

/* Convert window coordinates to sphere coordinates. */
HVect MouseOnSphere(HVect mouse, HVect ballCenter, double ballRadius)
{
    HVect ballMouse;
    register double mag;

    ballMouse.x = (mouse.x - ballCenter.x) / ballRadius;
    ballMouse.y = (mouse.y - ballCenter.y) / ballRadius;
    mag = ballMouse.x * ballMouse.x + ballMouse.y * ballMouse.y;
    if (mag > 1.0) {
	register double scale = 1.0 / sqrt(mag);

	ballMouse.x *= scale;
	ballMouse.y *= scale;
	ballMouse.z = 0.0;
    }
    else {
	ballMouse.z = sqrt(1 - mag);
    }
    ballMouse.w = 0.0;
    return (ballMouse);
}

#include <stdio.h>
/* Construct a unit quaternion from two points on unit sphere */
Quat Qt_FromBallPoints(HVect from, HVect to)
{
    Quat qu;
    float mag, ang, s;

    qu.x = from.y * to.z - from.z * to.y;
    qu.y = from.z * to.x - from.x * to.z;
    qu.z = from.x * to.y - from.y * to.x;
    qu.w = from.x * to.x + from.y * to.y + from.z * to.z;
    mag = sqrt(qu.x * qu.x + qu.y * qu.y + qu.z * qu.z);
    ang = atan2(mag, qu.w) / 2.;
    s = sin(ang);
    if (mag) {
	qu.x *= s / mag;
	qu.y *= s / mag;
	qu.z *= s / mag;
    }
    qu.w = cos(ang);
    return (qu);
}

/* Convert a unit quaternion to two points on unit sphere */
void Qt_ToBallPoints(Quat q, HVect * arcFrom, HVect * arcTo)
{
    double s;

    s = sqrt(q.x * q.x + q.y * q.y);
    if (s == 0.0) {
	*arcFrom = V3_(0.0, 1.0, 0.0);
    }
    else {
	*arcFrom = V3_(-q.y / s, q.x / s, 0.0);
    }
    arcTo->x = q.w * arcFrom->x - q.z * arcFrom->y;
    arcTo->y = q.w * arcFrom->y + q.z * arcFrom->x;
    arcTo->z = q.x * arcFrom->y - q.y * arcFrom->x;
    if (q.w < 0.0)
	*arcFrom = V3_(-arcFrom->x, -arcFrom->y, 0.0);
}

/* Force sphere point to be perpendicular to axis. */
HVect ConstrainToAxis(HVect loose, HVect axis)
{
    HVect onPlane;
    register float norm;

    onPlane = V3_Sub(loose, V3_Scale(axis, V3_Dot(axis, loose)));
    norm = V3_Norm(onPlane);
    if (norm > 0.0) {
	if (onPlane.z < 0.0)
	    onPlane = V3_Negate(onPlane);
	return (V3_Scale(onPlane, 1 / sqrt(norm)));
    }				/* else drop through */
    if (axis.z == 1) {
	onPlane = V3_(1.0, 0.0, 0.0);
    }
    else {
	onPlane = V3_Unit(V3_(-axis.y, axis.x, 0.0));
    }
    return (onPlane);
}

/* Find the index of nearest arc of axis set. */
int NearestConstraintAxis(HVect loose, HVect * axes, int nAxes)
{
    HVect onPlane;
    register float max, dot;
    register int i, nearest;

    max = -1;
    nearest = 0;
    for (i = 0; i < nAxes; i++) {
	onPlane = ConstrainToAxis(loose, axes[i]);
	dot = V3_Dot(onPlane, loose);
	if (dot > max) {
	    max = dot;
	    nearest = i;
	}
    }
    return (nearest);
}