1 /*
2 Quaternion routines.
3
4 clib v1.1
5
6 Copyright (C) 1997-1998 Per Kraulis
7 23-Jun-1997 started writing
8 17-Jun-1998 mod's for hgen
9 */
10
11 #include "quaternion.h"
12
13 /* public ====================
14 #include <vector3.h>
15 #include <matrix3.h>
16
17 typedef struct {
18 vector3 axis;
19 double phi;
20 } quaternion;
21 ==================== public */
22
23 #include <assert.h>
24 #include <math.h>
25
26
27 /*------------------------------------------------------------*/
28 void
quat_initialize(quaternion * q,double x,double y,double z,double phi)29 quat_initialize (quaternion *q, double x, double y, double z, double phi)
30 {
31 /* pre */
32 assert (q);
33
34 q->axis.x = x;
35 q->axis.y = y;
36 q->axis.z = z;
37 v3_normalize (&(q->axis));
38 v3_scale (&(q->axis), sin (phi / 2.0));
39 q->phi = cos (phi / 2.0);
40 }
41
42
43 /*------------------------------------------------------------*/
44 void
quat_initialize_v3(quaternion * q,vector3 * v,double phi)45 quat_initialize_v3 (quaternion *q, vector3 *v, double phi)
46 {
47 /* pre */
48 assert (q);
49 assert (v);
50 assert (v3_length (v) > 0.0);
51
52 q->axis = *v;
53 v3_normalize (&(q->axis));
54 v3_scale (&(q->axis), sin (phi / 2.0));
55 q->phi = cos (phi / 2.0);
56 }
57
58
59 /*------------------------------------------------------------*/
60 void
quat_normalize(quaternion * q)61 quat_normalize (quaternion *q)
62 {
63 register double magnitude;
64
65 /* pre */
66 assert (q);
67
68 magnitude = q->axis.x * q->axis.x +
69 q->axis.y * q->axis.y +
70 q->axis.z * q->axis.z +
71 q->phi * q->phi;
72 q->axis.x /= magnitude;
73 q->axis.y /= magnitude;
74 q->axis.z /= magnitude;
75 q->phi /= magnitude;
76 }
77
78
79 /*------------------------------------------------------------*/
80 void
quat_add(quaternion * dest,quaternion * incr)81 quat_add (quaternion *dest, quaternion *incr)
82 {
83 vector3 t1, t2, t3;
84 quaternion tq;
85
86 /* pre */
87 assert (dest);
88 assert (incr);
89
90 t1 = dest->axis;
91 v3_scale (&t1, incr->phi);
92 t2 = incr->axis;
93 v3_scale (&t2, dest->phi);
94 v3_cross_product (&t3, &(dest->axis), &(incr->axis));
95 v3_sum (&(tq.axis), &t1, &t2);
96 v3_add (&(tq.axis), &t3);
97 tq.phi = incr->phi * dest->phi -
98 v3_dot_product (&(incr->axis), &(dest->axis));
99 *dest = tq;
100 }
101
102
103 /*------------------------------------------------------------*/
104 void
quat_to_matrix3(double m[4][4],quaternion * q)105 quat_to_matrix3 (double m[4][4], quaternion *q)
106 {
107 /* pre */
108 assert (m);
109 assert (q);
110
111 m[0][0] = 1.0 - 2.0 * (q->axis.y * q->axis.y + q->axis.z * q->axis.z);
112 m[0][1] = 2.0 * (q->axis.x * q->axis.y - q->axis.z * q->phi);
113 m[0][2] = 2.0 * (q->axis.z * q->axis.x + q->axis.y * q->phi);
114 m[0][3] = 0.0;
115
116 m[1][0] = 2.0 * (q->axis.x * q->axis.y + q->axis.z * q->phi);
117 m[1][1] = 1.0 - 2.0 * (q->axis.z * q->axis.z + q->axis.x * q->axis.x);
118 m[1][2] = 2.0 * (q->axis.y * q->axis.z - q->axis.x * q->phi);
119 m[1][3] = 0.0;
120
121 m[2][0] = 2.0 * (q->axis.z * q->axis.x - q->axis.y * q->phi);
122 m[2][1] = 2.0 * (q->axis.y * q->axis.z + q->axis.x * q->phi);
123 m[2][2] = 1.0 - 2.0 * (q->axis.y * q->axis.y + q->axis.x * q->axis.x);
124 m[2][3] = 0.0;
125
126 m[3][0] = 0.0;
127 m[3][1] = 0.0;
128 m[3][2] = 0.0;
129 m[3][3] = 1.0;
130 }
131
132
133 /*------------------------------------------------------------*/
134 void
quat_interpolate(quaternion * dest,quaternion * q0,quaternion * q1,double t)135 quat_interpolate (quaternion *dest, quaternion *q0, quaternion *q1, double t)
136 {
137 register double sum, theta, beta1, beta2;
138
139 /* pre */
140 assert (dest);
141 assert (q0);
142 assert (q1);
143 assert (t >= 0.0);
144 assert (t <= 1.0);
145
146 sum = q0->axis.x * q1->axis.x +
147 q0->axis.y * q1->axis.y +
148 q0->axis.z * q1->axis.z +
149 q0->phi * q1->phi;
150 theta = acos (sum);
151 if (theta < 1.0e-10) {
152 beta1 = 1.0 - t;
153 beta2 = t;
154 } else {
155 beta1 = sin ((1.0 - t) * theta) / sin (theta);
156 beta2 = sin (t * theta) / sin (theta);
157 }
158
159 dest->axis.x = beta1 * q0->axis.x + beta2 * q1->axis.x;
160 dest->axis.y = beta1 * q0->axis.y + beta2 * q1->axis.y;
161 dest->axis.z = beta1 * q0->axis.z + beta2 * q1->axis.z;
162 dest->phi = beta1 * q0->phi + beta2 * q1->phi;
163 }
164
165
166 /*------------------------------------------------------------*/
167 static double
project_to_sphere(double r,double x,double y)168 project_to_sphere (double r, double x, double y)
169 {
170 register double d, t, z;
171
172 d = sqrt (x*x + y*y);
173 if (d < r * 0.70710678118654752440) { /* Inside sphere. */
174 z = sqrt (r*r - d*d);
175 } else { /* On hyperbola. */
176 t = r / 1.41421356237309504880;
177 z = t*t / d;
178 }
179
180 return z;
181 }
182
183
184 /*------------------------------------------------------------*/
185 void
quat_trackball(quaternion * dest,double size,double p1x,double p1y,double p2x,double p2y)186 quat_trackball (quaternion *dest, double size,
187 double p1x, double p1y, double p2x, double p2y)
188 {
189 vector3 p1, p2, axis;
190 double t;
191
192 /* pre */
193 assert (dest);
194 assert (size > 0.0);
195 assert (size <= 1.0);
196 assert (p1x >= -1.0);
197 assert (p1x <= 1.0);
198 assert (p1y >= -1.0);
199 assert (p1y <= 1.0);
200 assert (p2x >= -1.0);
201 assert (p2x <= 1.0);
202 assert (p2y >= -1.0);
203 assert (p2y <= 1.0);
204
205 if ((p1x == p2x) && (p1y == p2y)) { /* Zero rotation. */
206 quat_initialize (dest, 1.0, 0.0, 0.0, 0.0);
207 return;
208 }
209
210 /* Z coord for proj onto sphere. */
211 v3_initialize (&p1, p1x, p1y, project_to_sphere (size, p1x, p1y));
212 v3_initialize (&p2, p2x, p2y, project_to_sphere (size, p2x, p2y));
213
214 v3_cross_product (&axis, &p2, &p1);
215
216 t = v3_distance (&p1, &p2) / (2.0 * size); /* Rotation amount. */
217 if (t > 1.0) {
218 t = 1.0;
219 } else if (t < -1.0) {
220 t= -1.0;
221 }
222
223 quat_initialize_v3 (dest, &axis, 2.0 * asin (t));
224 }
225