1
2 /**** BallMath.c - Essential routines for ArcBall. ****/
3 #include <math.h>
4 #include "BallMath.h"
5 #include "BallAux.h"
6
7 /* Convert window coordinates to sphere coordinates. */
MouseOnSphere(HVect mouse,HVect ballCenter,double ballRadius)8 HVect MouseOnSphere(HVect mouse, HVect ballCenter, double ballRadius)
9 {
10 HVect ballMouse;
11 register double mag;
12
13 ballMouse.x = (mouse.x - ballCenter.x) / ballRadius;
14 ballMouse.y = (mouse.y - ballCenter.y) / ballRadius;
15 mag = ballMouse.x * ballMouse.x + ballMouse.y * ballMouse.y;
16 if (mag > 1.0) {
17 register double scale = 1.0 / sqrt(mag);
18
19 ballMouse.x *= scale;
20 ballMouse.y *= scale;
21 ballMouse.z = 0.0;
22 }
23 else {
24 ballMouse.z = sqrt(1 - mag);
25 }
26 ballMouse.w = 0.0;
27 return (ballMouse);
28 }
29
30 #include <stdio.h>
31 /* Construct a unit quaternion from two points on unit sphere */
Qt_FromBallPoints(HVect from,HVect to)32 Quat Qt_FromBallPoints(HVect from, HVect to)
33 {
34 Quat qu;
35 float mag, ang, s;
36
37 qu.x = from.y * to.z - from.z * to.y;
38 qu.y = from.z * to.x - from.x * to.z;
39 qu.z = from.x * to.y - from.y * to.x;
40 qu.w = from.x * to.x + from.y * to.y + from.z * to.z;
41 mag = sqrt(qu.x * qu.x + qu.y * qu.y + qu.z * qu.z);
42 ang = atan2(mag, qu.w) / 2.;
43 s = sin(ang);
44 if (mag) {
45 qu.x *= s / mag;
46 qu.y *= s / mag;
47 qu.z *= s / mag;
48 }
49 qu.w = cos(ang);
50 return (qu);
51 }
52
53 /* Convert a unit quaternion to two points on unit sphere */
Qt_ToBallPoints(Quat q,HVect * arcFrom,HVect * arcTo)54 void Qt_ToBallPoints(Quat q, HVect * arcFrom, HVect * arcTo)
55 {
56 double s;
57
58 s = sqrt(q.x * q.x + q.y * q.y);
59 if (s == 0.0) {
60 *arcFrom = V3_(0.0, 1.0, 0.0);
61 }
62 else {
63 *arcFrom = V3_(-q.y / s, q.x / s, 0.0);
64 }
65 arcTo->x = q.w * arcFrom->x - q.z * arcFrom->y;
66 arcTo->y = q.w * arcFrom->y + q.z * arcFrom->x;
67 arcTo->z = q.x * arcFrom->y - q.y * arcFrom->x;
68 if (q.w < 0.0)
69 *arcFrom = V3_(-arcFrom->x, -arcFrom->y, 0.0);
70 }
71
72 /* Force sphere point to be perpendicular to axis. */
ConstrainToAxis(HVect loose,HVect axis)73 HVect ConstrainToAxis(HVect loose, HVect axis)
74 {
75 HVect onPlane;
76 register float norm;
77
78 onPlane = V3_Sub(loose, V3_Scale(axis, V3_Dot(axis, loose)));
79 norm = V3_Norm(onPlane);
80 if (norm > 0.0) {
81 if (onPlane.z < 0.0)
82 onPlane = V3_Negate(onPlane);
83 return (V3_Scale(onPlane, 1 / sqrt(norm)));
84 } /* else drop through */
85 if (axis.z == 1) {
86 onPlane = V3_(1.0, 0.0, 0.0);
87 }
88 else {
89 onPlane = V3_Unit(V3_(-axis.y, axis.x, 0.0));
90 }
91 return (onPlane);
92 }
93
94 /* Find the index of nearest arc of axis set. */
NearestConstraintAxis(HVect loose,HVect * axes,int nAxes)95 int NearestConstraintAxis(HVect loose, HVect * axes, int nAxes)
96 {
97 HVect onPlane;
98 register float max, dot;
99 register int i, nearest;
100
101 max = -1;
102 nearest = 0;
103 for (i = 0; i < nAxes; i++) {
104 onPlane = ConstrainToAxis(loose, axes[i]);
105 dot = V3_Dot(onPlane, loose);
106 if (dot > max) {
107 max = dot;
108 nearest = i;
109 }
110 }
111 return (nearest);
112 }
113