+
+void sball_motion(int x, int y, int z)
+{
+ torus_pos[0] += x * 0.001f;
+ torus_pos[1] += y * 0.001f;
+ torus_pos[2] -= z * 0.001f;
+ glutPostRedisplay();
+}
+
+static float rsqrt(float number)
+{
+ int i;
+ float x2, y;
+ static const float threehalfs = 1.5f;
+
+ x2 = number * 0.5f;
+ y = number;
+ i = *(int*)&y;
+ i = 0x5f3759df - (i >> 1);
+ y = *(float*)&i;
+ y *= threehalfs - (x2 * y * y);
+ y *= threehalfs - (x2 * y * y);
+ return y;
+}
+
+void sball_rotate(int rx, int ry, int rz)
+{
+ if(rx | ry | rz) {
+ float s = (float)rsqrt(rx * rx + ry * ry + rz * rz);
+ qrotate(torus_rot, 0.001f / s, rx * s, ry * s, -rz * s);
+ glutPostRedisplay();
+ }
+}
+
+void sball_button(int bn, int state)
+{
+ if(state == GLUT_DOWN) {
+ torus_pos[0] = torus_pos[1] = torus_pos[2] = 0;
+ torus_rot[0] = torus_rot[1] = torus_rot[2] = 0;
+ torus_rot[3] = 1;
+ glutPostRedisplay();
+ }
+}
+
+
+static void vcross(float *res, const float *a, const float *b)
+{
+ res[0] = a[1] * b[2] - a[2] * b[1];
+ res[1] = a[2] * b[0] - a[0] * b[2];
+ res[2] = a[0] * b[1] - a[1] * b[0];
+}
+
+static void qmul(float *a, const float *b)
+{
+ float x, y, z, dot;
+ float cross[3];
+
+ dot = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
+ vcross(cross, a, b);
+
+ x = a[3] * b[0] + b[3] * a[0] + cross[0];
+ y = a[3] * b[1] + b[3] * a[1] + cross[1];
+ z = a[3] * b[2] + b[3] * a[2] + cross[2];
+ a[3] = a[3] * b[3] - dot;
+ a[0] = x;
+ a[1] = y;
+ a[2] = z;
+}
+
+void mglut_sincos(float angle, float *sptr, float *cptr);
+float mglut_tan(float x);
+
+static void qrotation(float *q, float angle, float x, float y, float z)
+{
+ float sa, ca;
+ mglut_sincos(angle * 0.5f, &sa, &ca);
+ q[3] = ca;
+ q[0] = x * sa;
+ q[1] = y * sa;
+ q[2] = z * sa;
+}
+
+static void qrotate(float *q, float angle, float x, float y, float z)
+{
+ float qrot[4];
+ qrotation(qrot, angle, x, y, z);
+ qmul(qrot, q);
+ q[0] = qrot[0];
+ q[1] = qrot[1];
+ q[2] = qrot[2];
+ q[3] = qrot[3];
+}
+
+static void mrotation_quat(float *m, const float *q)
+{
+ float xsq2 = 2.0f * q[0] * q[0];
+ float ysq2 = 2.0f * q[1] * q[1];
+ float zsq2 = 2.0f * q[2] * q[2];
+ float sx = 1.0f - ysq2 - zsq2;
+ float sy = 1.0f - xsq2 - zsq2;
+ float sz = 1.0f - xsq2 - ysq2;
+
+ m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f;
+ m[15] = 1.0f;
+
+ m[0] = sx;
+ m[1] = 2.0f * q[0] * q[1] + 2.0f * q[3] * q[2];
+ m[2] = 2.0f * q[2] * q[0] - 2.0f * q[3] * q[1];
+ m[4] = 2.0f * q[0] * q[1] - 2.0f * q[3] * q[2];
+ m[5] = sy;
+ m[6] = 2.0f * q[1] * q[2] + 2.0f * q[3] * q[0];
+ m[8] = 2.0f * q[2] * q[0] + 2.0f * q[3] * q[1];
+ m[9] = 2.0f * q[1] * q[2] - 2.0f * q[3] * q[0];
+ m[10] = sz;
+}