tquaternion.c - plan9port - [fork] Plan 9 from user space
HTML git clone git://src.adamsgaard.dk/plan9port
DIR Log
DIR Files
DIR Refs
DIR README
DIR LICENSE
---
tquaternion.c (5705B)
---
1 /*
2 * Quaternion arithmetic:
3 * qadd(q, r) returns q+r
4 * qsub(q, r) returns q-r
5 * qneg(q) returns -q
6 * qmul(q, r) returns q*r
7 * qdiv(q, r) returns q/r, can divide check.
8 * qinv(q) returns 1/q, can divide check.
9 * double qlen(p) returns modulus of p
10 * qunit(q) returns a unit quaternion parallel to q
11 * The following only work on unit quaternions and rotation matrices:
12 * slerp(q, r, a) returns q*(r*q^-1)^a
13 * qmid(q, r) slerp(q, r, .5)
14 * qsqrt(q) qmid(q, (Quaternion){1,0,0,0})
15 * qtom(m, q) converts a unit quaternion q into a rotation matrix m
16 * mtoq(m) returns a quaternion equivalent to a rotation matrix m
17 */
18 #include <u.h>
19 #include <libc.h>
20 #include <draw.h>
21 #include <geometry.h>
22 void qtom(Matrix m, Quaternion q){
23 #ifndef new
24 m[0][0]=1-2*(q.j*q.j+q.k*q.k);
25 m[0][1]=2*(q.i*q.j+q.r*q.k);
26 m[0][2]=2*(q.i*q.k-q.r*q.j);
27 m[0][3]=0;
28 m[1][0]=2*(q.i*q.j-q.r*q.k);
29 m[1][1]=1-2*(q.i*q.i+q.k*q.k);
30 m[1][2]=2*(q.j*q.k+q.r*q.i);
31 m[1][3]=0;
32 m[2][0]=2*(q.i*q.k+q.r*q.j);
33 m[2][1]=2*(q.j*q.k-q.r*q.i);
34 m[2][2]=1-2*(q.i*q.i+q.j*q.j);
35 m[2][3]=0;
36 m[3][0]=0;
37 m[3][1]=0;
38 m[3][2]=0;
39 m[3][3]=1;
40 #else
41 /*
42 * Transcribed from Ken Shoemake's new code -- not known to work
43 */
44 double Nq = q.r*q.r+q.i*q.i+q.j*q.j+q.k*q.k;
45 double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
46 double xs = q.i*s, ys = q.j*s, zs = q.k*s;
47 double wx = q.r*xs, wy = q.r*ys, wz = q.r*zs;
48 double xx = q.i*xs, xy = q.i*ys, xz = q.i*zs;
49 double yy = q.j*ys, yz = q.j*zs, zz = q.k*zs;
50 m[0][0] = 1.0 - (yy + zz); m[1][0] = xy + wz; m[2][0] = xz - wy;
51 m[0][1] = xy - wz; m[1][1] = 1.0 - (xx + zz); m[2][1] = yz + wx;
52 m[0][2] = xz + wy; m[1][2] = yz - wx; m[2][2] = 1.0 - (xx + yy);
53 m[0][3] = m[1][3] = m[2][3] = m[3][0] = m[3][1] = m[3][2] = 0.0;
54 m[3][3] = 1.0;
55 #endif
56 }
57 Quaternion mtoq(Matrix mat){
58 #ifndef new
59 #define EPS 1.387778780781445675529539585113525e-17 /* 2^-56 */
60 double t;
61 Quaternion q;
62 q.r=0.;
63 q.i=0.;
64 q.j=0.;
65 q.k=1.;
66 if((t=.25*(1+mat[0][0]+mat[1][1]+mat[2][2]))>EPS){
67 q.r=sqrt(t);
68 t=4*q.r;
69 q.i=(mat[1][2]-mat[2][1])/t;
70 q.j=(mat[2][0]-mat[0][2])/t;
71 q.k=(mat[0][1]-mat[1][0])/t;
72 }
73 else if((t=-.5*(mat[1][1]+mat[2][2]))>EPS){
74 q.i=sqrt(t);
75 t=2*q.i;
76 q.j=mat[0][1]/t;
77 q.k=mat[0][2]/t;
78 }
79 else if((t=.5*(1-mat[2][2]))>EPS){
80 q.j=sqrt(t);
81 q.k=mat[1][2]/(2*q.j);
82 }
83 return q;
84 #else
85 /*
86 * Transcribed from Ken Shoemake's new code -- not known to work
87 */
88 /* This algorithm avoids near-zero divides by looking for a large
89 * component -- first r, then i, j, or k. When the trace is greater than zero,
90 * |r| is greater than 1/2, which is as small as a largest component can be.
91 * Otherwise, the largest diagonal entry corresponds to the largest of |i|,
92 * |j|, or |k|, one of which must be larger than |r|, and at least 1/2.
93 */
94 Quaternion qu;
95 double tr, s;
96
97 tr = mat[0][0] + mat[1][1] + mat[2][2];
98 if (tr >= 0.0) {
99 s = sqrt(tr + mat[3][3]);
100 qu.r = s*0.5;
101 s = 0.5 / s;
102 qu.i = (mat[2][1] - mat[1][2]) * s;
103 qu.j = (mat[0][2] - mat[2][0]) * s;
104 qu.k = (mat[1][0] - mat[0][1]) * s;
105 }
106 else {
107 int i = 0;
108 if (mat[1][1] > mat[0][0]) i = 1;
109 if (mat[2][2] > mat[i][i]) i = 2;
110 switch(i){
111 case 0:
112 s = sqrt( (mat[0][0] - (mat[1][1]+mat[2][2])) + mat[3][3] );
113 qu.i = s*0.5;
114 s = 0.5 / s;
115 qu.j = (mat[0][1] + mat[1][0]) * s;
116 qu.k = (mat[2][0] + mat[0][2]) * s;
117 qu.r = (mat[2][1] - mat[1][2]) * s;
118 break;
119 case 1:
120 s = sqrt( (mat[1][1] - (mat[2][2]+mat[0][0])) + mat[3][3] );
121 qu.j = s*0.5;
122 s = 0.5 / s;
123 qu.k = (mat[1][2] + mat[2][1]) * s;
124 qu.i = (mat[0][1] + mat[1][0]) * s;
125 qu.r = (mat[0][2] - mat[2][0]) * s;
126 break;
127 case 2:
128 s = sqrt( (mat[2][2] - (mat[0][0]+mat[1][1])) + mat[3][3] );
129 qu.k = s*0.5;
130 s = 0.5 / s;
131 qu.i = (mat[2][0] + mat[0][2]) * s;
132 qu.j = (mat[1][2] + mat[2][1]) * s;
133 qu.r = (mat[1][0] - mat[0][1]) * s;
134 break;
135 }
136 }
137 if (mat[3][3] != 1.0){
138 s=1/sqrt(mat[3][3]);
139 qu.r*=s;
140 qu.i*=s;
141 qu.j*=s;
142 qu.k*=s;
143 }
144 return (qu);
145 #endif
146 }
147 Quaternion qadd(Quaternion q, Quaternion r){
148 q.r+=r.r;
149 q.i+=r.i;
150 q.j+=r.j;
151 q.k+=r.k;
152 return q;
153 }
154 Quaternion qsub(Quaternion q, Quaternion r){
155 q.r-=r.r;
156 q.i-=r.i;
157 q.j-=r.j;
158 q.k-=r.k;
159 return q;
160 }
161 Quaternion qneg(Quaternion q){
162 q.r=-q.r;
163 q.i=-q.i;
164 q.j=-q.j;
165 q.k=-q.k;
166 return q;
167 }
168 Quaternion qmul(Quaternion q, Quaternion r){
169 Quaternion s;
170 s.r=q.r*r.r-q.i*r.i-q.j*r.j-q.k*r.k;
171 s.i=q.r*r.i+r.r*q.i+q.j*r.k-q.k*r.j;
172 s.j=q.r*r.j+r.r*q.j+q.k*r.i-q.i*r.k;
173 s.k=q.r*r.k+r.r*q.k+q.i*r.j-q.j*r.i;
174 return s;
175 }
176 Quaternion qdiv(Quaternion q, Quaternion r){
177 return qmul(q, qinv(r));
178 }
179 Quaternion qunit(Quaternion q){
180 double l=qlen(q);
181 q.r/=l;
182 q.i/=l;
183 q.j/=l;
184 q.k/=l;
185 return q;
186 }
187 /*
188 * Bug?: takes no action on divide check
189 */
190 Quaternion qinv(Quaternion q){
191 double l=q.r*q.r+q.i*q.i+q.j*q.j+q.k*q.k;
192 q.r/=l;
193 q.i=-q.i/l;
194 q.j=-q.j/l;
195 q.k=-q.k/l;
196 return q;
197 }
198 double qlen(Quaternion p){
199 return sqrt(p.r*p.r+p.i*p.i+p.j*p.j+p.k*p.k);
200 }
201 Quaternion slerp(Quaternion q, Quaternion r, double a){
202 double u, v, ang, s;
203 double dot=q.r*r.r+q.i*r.i+q.j*r.j+q.k*r.k;
204 ang=dot<-1?PI:dot>1?0:acos(dot); /* acos gives NaN for dot slightly out of range */
205 s=sin(ang);
206 if(s==0) return ang<PI/2?q:r;
207 u=sin((1-a)*ang)/s;
208 v=sin(a*ang)/s;
209 q.r=u*q.r+v*r.r;
210 q.i=u*q.i+v*r.i;
211 q.j=u*q.j+v*r.j;
212 q.k=u*q.k+v*r.k;
213 return q;
214 }
215 /*
216 * Only works if qlen(q)==qlen(r)==1
217 */
218 Quaternion qmid(Quaternion q, Quaternion r){
219 double l;
220 q=qadd(q, r);
221 l=qlen(q);
222 if(l<1e-12){
223 q.r=r.i;
224 q.i=-r.r;
225 q.j=r.k;
226 q.k=-r.j;
227 }
228 else{
229 q.r/=l;
230 q.i/=l;
231 q.j/=l;
232 q.k/=l;
233 }
234 return q;
235 }
236 /*
237 * Only works if qlen(q)==1
238 */
239 static Quaternion qident={1,0,0,0};
240 Quaternion qsqrt(Quaternion q){
241 return qmid(q, qident);
242 }