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