/* Given a unit quaternion, q, and a scale vector, k, find a unit quaternion, p,
* which permutes the axes and turns freely in the plane of duplicate scale
* factors, such that q p has the largest possible w component, i.e. the
* smallest possible angle. Permutes k's components to go with q p instead of q.
* See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
* Proceedings of Graphics Interface 1992. Details on p. 262-263.
*/
Quat snuggle(Quat q, HVect *k)
{
#define SQRTHALF (0.7071067811865475244)
#define sgn(n,v) ((n)?-(v):(v))
#define swap(a,i,j) {a[3]=a[i]; a[i]=a[j]; a[j]=a[3];}
#define cycle(a,p) if (p) {a[3]=a[0]; a[0]=a[1]; a[1]=a[2]; a[2]=a[3];}\
else {a[3]=a[2]; a[2]=a[1]; a[1]=a[0]; a[0]=a[3];}
Quat p;
double ka[4];
int i, turn = -1;
ka[X] = k->x; ka[Y] = k->y; ka[Z] = k->z;
if (ka[X]==ka[Y]) {if (ka[X]==ka[Z]) turn = W; else turn = Z;}
else {if (ka[X]==ka[Z]) turn = Y; else if (ka[Y]==ka[Z]) turn = X;}
if (turn>=0) {
Quat qtoz, qp;
unsigned neg[3], win;
double mag[3], t;
static Quat qxtoz = {0,SQRTHALF,0,SQRTHALF};
static Quat qytoz = {SQRTHALF,0,0,SQRTHALF};
static Quat qppmm = { 0.5, 0.5,-0.5,-0.5};
static Quat qpppp = { 0.5, 0.5, 0.5, 0.5};
static Quat qmpmm = {-0.5, 0.5,-0.5,-0.5};
static Quat qpppm = { 0.5, 0.5, 0.5,-0.5};
static Quat q0001 = { 0.0, 0.0, 0.0, 1.0};
static Quat q1000 = { 1.0, 0.0, 0.0, 0.0};
switch (turn) {
default: return (Qt_Conj(q));
case X: q = Qt_Mul(q, qtoz = qxtoz); swap(ka,X,Z) break;
case Y: q = Qt_Mul(q, qtoz = qytoz); swap(ka,Y,Z) break;
case Z: qtoz = q0001; break;
}
q = Qt_Conj(q);
mag[0] = (double)q.z*q.z+(double)q.w*q.w-0.5;
mag[1] = (double)q.x*q.z-(double)q.y*q.w;
mag[2] = (double)q.y*q.z+(double)q.x*q.w;
for (i=0; i<3; i++) if (neg[i] = (mag[i]<0.0)) mag[i] = -mag[i];
if (mag[0]>mag[1]) {if (mag[0]>mag[2]) win = 0; else win = 2;}
else {if (mag[1]>mag[2]) win = 1; else win = 2;}
switch (win) {
case 0: if (neg[0]) p = q1000; else p = q0001; break;
case 1: if (neg[1]) p = qppmm; else p = qpppp; cycle(ka,0) break;
case 2: if (neg[2]) p = qmpmm; else p = qpppm; cycle(ka,1) break;
}
qp = Qt_Mul(q, p);
t = sqrt(mag[win]+0.5);
p = Qt_Mul(p, Qt_(0.0,0.0,-qp.z/t,qp.w/t));
p = Qt_Mul(qtoz, Qt_Conj(p));
} else {
void
ArcBall_Update (void)
{
int setSize = setSizes[axisSet];
HVect *set = (HVect *)(sets[axisSet]);
vFrom = MouseOnSphere(vDown, center, radius);
vTo = MouseOnSphere(vNow, center, radius);
if (dragging)
{
if (axisSet!=NoAxes)
{
vFrom = ConstrainToAxis(vFrom, set[axisIndex]);
vTo = ConstrainToAxis(vTo, set[axisIndex]);
}
qDrag = Qt_FromBallPoints(vFrom, vTo);
qNow = Qt_Mul(qDrag, qDown);
}
else
{
if (axisSet!=NoAxes) axisIndex = NearestConstraintAxis(vTo, set, setSize);
}
Qt_ToBallPoints(qDown, &vrFrom, &vrTo);
Qt_ToMatrix(Qt_Conj(qNow), mNow); /* Gives transpose for GL. */
}
