##### https://github.com/lwt831/Real-time-Locally-Injective-Volumetric-Deformation
Tip revision: 8da858a
trackball.h
``````#pragma once
#include <cmath>
#include <algorithm>

template<typename R=float>
struct Quat     // quaternion
{
Quat() {}
Quat(R x, R y, R z, R w) { q[0] = x; q[1] = y; q[2] = z; q[3] = w; }
Quat(const R* v) { memcpy(q, v, sizeof(R) * 4); }

R operator [](int i) const { return q[i]; }
R& operator [](int i) { return q[i]; }

Quat operator +(const Quat& x) const { return Quat({ q[0] + x[0], q[1] + x[1], q[2] + x[2], q[3] + x[3] }); }
Quat operator -(const Quat& x) const { return Quat({ q[0] - x[0], q[1] - x[1], q[2] - x[2], q[3] - x[3] }); }

// Quaternions always obey:  a^2 + b^2 + c^2 + d^2 = 1
// If not, dividing by their magnitude will re-normalize them
Quat normalized() const
{
double m = 1. / ::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
return Quat{ R(q[0] * m), R(q[1] * m), R(q[2] * m), R(q[3] * m) };
}

// Given two rotations, e1 and e2, expressed as quaternion rotations,
// compute the equivalent single rotation
Quat operator *(const Quat& x) const
{
R t1[3] = { q[0] * x[3], q[1] * x[3], q[2] * x[3] };
R t2[3] = { x[0] * q[3], x[1] * q[3], x[2] * q[3] };
R t3[3];
vcross(x.q, q, t3);

Quat r;
for (int i = 0; i < 3; i++)
r[i] = t1[i] + t2[i] + t3[i];

r[3] = q[3] * x[3] - q[0] * x[0] - q[1] * x[1] - q[2] * x[2];

return r.normalized();
}

// Build a rotation matrix, given a quaternion rotation.
void rotmatrix(float *m) const
{
std::fill_n(m, 16, 0.f);
m[0] = 1.f - 2.f * (q[1] * q[1] + q[2] * q[2]);
m[1] = 2.f * (q[0] * q[1] - q[2] * q[3]);
m[2] = 2.f * (q[2] * q[0] + q[1] * q[3]);

m[4 + 0] = 2.f * (q[0] * q[1] + q[2] * q[3]);
m[4 + 1] = 1.f - 2.f * (q[2] * q[2] + q[0] * q[0]);
m[4 + 2] = 2.f * (q[1] * q[2] - q[0] * q[3]);

m[8 + 0] = 2.f * (q[2] * q[0] - q[1] * q[3]);
m[8 + 1] = 2.f * (q[1] * q[2] + q[0] * q[3]);
m[8 + 2] = 1.f - 2.f * (q[1] * q[1] + q[0] * q[0]);

m[15] = 1.f;
}

// 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-- 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)
static Quat trackball(R x1, R y1, R x2, R y2)
{
// 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.
const float TRACKBALLSIZE = 0.8f;

if (x1 == x2 && y1 == y2)  // Zero rotation
return Quat( 0, 0, 0, 1 );

// First, figure out z-coordinates for projection of P1 and P2 to deformed sphere
R p1[3] = { x1, y1, project_to_sphere(TRACKBALLSIZE, x1, y1) };
R p2[3] = { x2, y2, project_to_sphere(TRACKBALLSIZE, x2, y2) };

//  Now, we want the cross product of P1 and P2
R a[3]; // Axis of rotation
vcross(p2, p1, a);

//  Figure out how much to rotate around that axis.
R d[3] = { p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2] };
double t = ::sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]) / (2.0*TRACKBALLSIZE);

// Avoid problems with out-of-control values...
t = std::max(std::min(t, 1.), -1.);
//t = std::clamp(t, -1., 1.);

// how much to rotate about axis
double phi = 2.0 * asin(t);

return axis_angle_to_quat(a, R(phi));
}

static void vcross(const R *x, const R *y, R *cross)
{
R temp[3] = { x[1] * y[2] - x[2] * y[1],
x[2] * y[0] - x[0] * y[2],
x[0] * y[1] - x[1] * y[0] };

std::copy_n(temp, 3, cross);
}

// Project point (x,y) onto a sphere of radius r OR a hyperbolic sheet
// if we are away from the center of the sphere.
static R project_to_sphere(R r, R x, R y)
{
double d2 = x*x + y*y;
double r2 = r*r;
if (d2 < r2*0.5)     /* Inside sphere */
return R( ::sqrt(r2 - d2) );

// On hyperbola
double t2 = r2*0.5;
return R(t2 / ::sqrt(d2));
}

// Given an axis and angle, compute quaternion.
static Quat axis_angle_to_quat(const R a[3], R phi)
{
double r = 1. / ::sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
double b[3] = { a[0] * r, a[1] * r, a[2] * r };

r = sin(phi / 2.);
return Quat({ R(b[0] * r), R(b[1] * r), R(b[2] * r), R(cos(phi / 2.)) });
}

R q[4];
};
``````