1 /*Transformation.c */
2 /**********************************************************************************************************
3 Copyright (c) 2002-2013 Abdul-Rahman Allouche. All rights reserved
4
5 Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
6 documentation files (the Gabedit), to deal in the Software without restriction, including without limitation
7 the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software,
8 and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
9
10 The above copyright notice and this permission notice shall be included in all copies or substantial portions
11 of the Software.
12
13 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
14 TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
15 THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF
16 CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
17 DEALINGS IN THE SOFTWARE.
18 ************************************************************************************************************/
19
20
21 #include "../../Config.h"
22 #include <math.h>
23 #include "../Common/Global.h"
24 #include "Vector3d.h"
25 #include "Transformation.h"
26 #include "Utils.h"
27
28 #define TRACKBALLSIZE (0.8)
29
30 /* Local function prototypes */
31 static gdouble tb_project_to_sphere(gdouble, gdouble, gdouble);
32 /****************************************************/
v4d_pvect(V4d v1,V4d v2)33 gdouble *v4d_pvect(V4d v1,V4d v2)
34 {
35 gdouble* v = g_malloc(4*sizeof(gdouble));
36 v[0] = v1[1] * v2[2] - v2[1] * v1[2];
37 v[1] = v1[2] * v2[0] - v2[2] * v1[0];
38 v[2] = v1[0] * v2[1] - v2[0] * v1[1] ;
39 v[3] = (v1[3] + v2[3])/2;
40 return v;
41 }
42 /****************************************************/
v4d_pscal(V4d v1,V4d v2)43 gdouble v4d_pscal(V4d v1,V4d v2)
44 {
45 return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]+v1[3]*v2[3];
46 }
47 /****************************************************/
v4d_scal(V4d v1,gdouble scal)48 gdouble* v4d_scal(V4d v1,gdouble scal)
49 {
50 gdouble* v = g_malloc(4*sizeof(gdouble));
51 v[0] = v1[0] * scal;
52 v[1] = v1[1] * scal;
53 v[2] = v1[2] * scal;
54 v[3] = v1[3] * scal;
55 return v;
56 }
57 /****************************************************/
v4d_length(V4d v)58 gdouble v4d_length(V4d v)
59 {
60 return sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]+v[3]*v[3]);
61 }
62 /****************************************************/
v4d_normal(V4d v)63 void v4d_normal(V4d v)
64 {
65 int i;
66 gdouble len;
67
68 len = v4d_length(v);
69 for (i = 0; i < 4; i++)
70 v[i] /= len;
71 }
72 /************************************************************************/
73 /*
74 * Given an axis and angle, compute quaternion.
75 */
axis_to_quat(gdouble a[3],gdouble phi,gdouble q[4])76 void axis_to_quat(gdouble a[3], gdouble phi, gdouble q[4])
77 {
78 v3d_normal(a);
79 v3d_copy(a,q);
80 v3d_scale(q,sin(phi/2.0));
81 q[3] = cos(phi/2.0);
82 }
83 /************************************************************************/
trackball(gdouble q[4],gdouble p1x,gdouble p1y,gdouble p2x,gdouble p2y)84 void trackball(gdouble q[4],gdouble p1x,gdouble p1y,gdouble p2x,gdouble p2y)
85 {
86 gdouble a[3]; /* Axis of rotation */
87 gdouble phi; /* how much to rotate about axis */
88 gdouble p1[3];
89 gdouble p2[3];
90 gdouble d[3];
91 gdouble t;
92
93 if (p1x == p2x && p1y == p2y) {
94 /* Zero rotation */
95 v3d_zero(q);
96 q[3] = 1.0;
97 return;
98 }
99
100 /*
101 * First, figure out z-coordinates for projection of P1 and P2 to
102 * deformed sphere
103 */
104 v3d_set(p1,p1x,p1y,tb_project_to_sphere(TRACKBALLSIZE,p1x,p1y));
105 v3d_set(p2,p2x,p2y,tb_project_to_sphere(TRACKBALLSIZE,p2x,p2y));
106
107 /* Now, we want the cross product of P1 and P2 */
108 v3d_cross(p2,p1,a);
109
110 /* Figure out how much to rotate around that axis. */
111 v3d_sub(p1,p2,d);
112 t = v3d_length(d) / (2.0*TRACKBALLSIZE);
113
114 /* Avoid problems with out-of-control values...*/
115 if (t > 1.0) t = 1.0;
116 if (t < -1.0) t = -1.0;
117 phi = 2.0 * asin(t);
118
119 axis_to_quat(a,phi,q);
120 }
121
122 /************************************************************************/
123 /*
124 * Project an x,y pair onto a sphere of radius r
125 * OR a hyperbolic sheet
126 * if we are away from the center of the sphere.
127 */
tb_project_to_sphere(gdouble r,gdouble x,gdouble y)128 static gdouble tb_project_to_sphere(gdouble r, gdouble x, gdouble y)
129 {
130 gdouble d, t, z;
131
132 d = sqrt(x*x + y*y);
133 if (d < r * 0.70710678118654752440) { /* Inside sphere */
134 z = sqrt(r*r - d*d);
135 } else { /* On hyperbola */
136 t = r / 1.41421356237309504880;
137 z = t*t / d;
138 }
139 return z;
140 }
141 /************************************************************************/
142 /*
143 * Given two rotations, e1 and e2, expressed as quaternion rotations,
144 * figure out the equivalent single rotation and stuff it into dest.
145 *
146 * This routine also normalizes the result every RENORMCOUNT times it is
147 * called, to keep error from creeping in.
148 *
149 * NOTE: This routine is written so that q1 or q2 may be the same
150 * as dest (or each other).
151 */
152
153 #define RENORMCOUNT 97
154
add_quats(gdouble q1[4],gdouble q2[4],gdouble dest[4])155 void add_quats(gdouble q1[4],gdouble q2[4],gdouble dest[4])
156 {
157 static int count=0;
158 gdouble t1[4];
159 gdouble t2[4];
160 gdouble t3[4];
161 gdouble tf[4];
162
163 v3d_copy(q1,t1);
164 v3d_scale(t1,q2[3]);
165
166 v3d_copy(q2,t2);
167 v3d_scale(t2,q1[3]);
168
169 v3d_cross(q2,q1,t3);
170 v3d_add(t1,t2,tf);
171 v3d_add(t3,tf,tf);
172 tf[3] = q1[3] * q2[3] - v3d_dot(q1,q2);
173
174 dest[0] = tf[0];
175 dest[1] = tf[1];
176 dest[2] = tf[2];
177 dest[3] = tf[3];
178
179 if (++count > RENORMCOUNT) {
180 count = 0;
181 v4d_normal(dest);
182 }
183 }
184 /************************************************************************/
build_rotmatrix(gdouble m[4][4],gdouble q[4])185 void build_rotmatrix(gdouble m[4][4],gdouble q[4])
186 {
187 m[0][0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
188 m[0][1] = 2.0 * (q[0] * q[1] - q[2] * q[3]);
189 m[0][2] = 2.0 * (q[2] * q[0] + q[1] * q[3]);
190 m[0][3] = 0.0;
191
192 m[1][0] = 2.0 * (q[0] * q[1] + q[2] * q[3]);
193 m[1][1]= 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0]);
194 m[1][2] = 2.0 * (q[1] * q[2] - q[0] * q[3]);
195 m[1][3] = 0.0;
196
197 m[2][0] = 2.0 * (q[2] * q[0] - q[1] * q[3]);
198 m[2][1] = 2.0 * (q[1] * q[2] + q[0] * q[3]);
199 m[2][2] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]);
200 m[2][3] = 0.0;
201
202 m[3][0] = 0.0;
203 m[3][1] = 0.0;
204 m[3][2] = 0.0;
205 m[3][3] = 1.0;
206 }
207 /************************************************************************/
208 /**********************************************/
209 /* Name: Matrix Inversion by Gauss - Jordan*/
210 /**************************************************/
Pivoting(gdouble ** a,gint size,gint row,gint col)211 void Pivoting(gdouble **a,gint size,gint row,gint col)
212 {
213 gdouble temp;
214 gint s;
215 for(s=0;s<size*2;s++)
216 {
217 temp=a[row-1][s];
218 a[row-1][s]=a[row][s];
219 a[row][s]=temp;
220 }
221 }
222 /**************************************************/
Trunc(gdouble ** a,gint size,gdouble error)223 void Trunc(gdouble **a, gint size,gdouble error)
224 {
225 gint s,k;
226 for(s=0;s<size*2;s++)
227 for(k=0;k<size;k++)
228 {
229 if(fabs(a[k][s])<error) a[k][s]=0;
230 }
231 }
232 /**************************************************/
Factor(gdouble ** a,gint row,gint col)233 gdouble Factor(gdouble **a,gint row,gint col)
234 {
235 return (gdouble) (-1*(a[row][col]/a[col][col]));
236 }
237 /**************************************************/
Inverse(gdouble ** mat,gint size,gdouble error)238 gdouble** Inverse(gdouble **mat,gint size,gdouble error)
239 {
240 gdouble factor = 0.0;
241 gdouble **invmat = NULL;
242 gint i,j,ii;
243 gdouble **a = g_malloc(size*sizeof(gdouble*));
244 gint done=1;
245 gdouble diagonal;
246 gdouble temp;
247 gint s;
248
249 for(i=0;i<size;i++)
250 a[i] = g_malloc(2*size*sizeof(gdouble));
251
252 for(i=0;i<size;i++)
253 {
254 for(j=0;j<size;j++)
255 a[i][j] = mat[i][j];
256 for(j=size;j<2*size;j++)
257 a[i][j] = 0.0;
258 a[i][i+size] = 1.0;
259 }
260
261 Trunc(a,size,error);
262 /*
263 for(i=0;i<size;i++)
264 {
265 for(j=0;j<2*size;j++)
266 Debug("%f ",a[i][j] );
267 Debug("\n ");
268 }
269
270 Debug("erro = %f \n ",error);
271 */
272 for(j=0;j<size;j++)
273 {
274 for(i=j+1;i<size;i++)
275 {
276 /* Debug("j i %d %d %f %f\n",j,i,a[j][j],a[i][i]);*/
277 if(fabs(a[j][j]) <fabs(a[j][i]))
278 {
279 /* Debug("pivoting %d %d\n",i,j);*/
280 for(s=0;s<size*2;s++)
281 {
282 temp=a[i][s];
283 a[i][s]=a[j][s];
284 a[j][s]=temp;
285 }
286 }
287 }
288 }
289 /*
290 Debug("\n ");
291 Debug("\n ");
292 for(i=0;i<size;i++)
293 {
294 for(j=0;j<2*size;j++)
295 Debug("%f ",a[i][j] );
296 Debug("\n ");
297 }
298 */
299
300 for(j=0;j<size;j++)
301 {
302 for(i=j+1;i<size;i++)
303 {
304 if(a[j][j]!=0)
305 factor=Factor(a,i,j);
306 else
307 {
308 Pivoting(a,size,i,j+1);
309 factor=Factor(a,i,j);
310 }
311 for(ii=0;ii<size*2;ii++)
312 a[i][ii]+=factor*a[j][ii];
313
314 Trunc(a,size,error);
315 }
316 }
317 /*
318 Debug("\n ");
319 Debug("\n ");
320 for(i=0;i<size;i++)
321 {
322 for(j=0;j<2*size;j++)
323 Debug("%f ",a[i][j] );
324 Debug("\n ");
325 }
326 */
327 for(j=size-1;j>=0;j--)
328 {
329 for(i=j-1;i>=0;i--)
330 {
331 if(a[j][j]==0)
332 done=0;
333 else
334 factor=Factor(a,i,j);
335
336 /*for(ii=0;ii<=2*size;ii++)*/
337 for(ii=0;ii<2*size;ii++)
338 a[i][ii] += factor*a[j][ii];
339
340 Trunc(a,size,error);
341 }
342 }
343 if(done == 0)
344 printf("ERREUR\n");
345 if(done != 0)
346 {
347 for(i=0;i<size;i++)
348 {
349 diagonal=a[i][i];
350 for(j=0;j<size*2;j++)
351 a[i][j]/=diagonal;
352 }
353
354 invmat = g_malloc(size*sizeof(gdouble*));
355 for(i=0;i<size;i++)
356 {
357 invmat[i] = g_malloc(size*sizeof(gdouble));
358 for(j=0;j<size;j++)
359 invmat[i][j] = a[i][j+size];
360 }
361
362 /*
363 for(i=0;i<size;i++)
364 {
365 for(j=0;j<2*size;j++)
366 Debug("%f ",a[i][j] );
367 Debug("\n ");
368 }
369 */
370
371 }
372
373
374 for(i=0;i<size;i++)
375 g_free(a[i]);
376
377 g_free(a);
378
379 return invmat;
380 }
381 /**************************************************/
Inverse3(gdouble ** mat)382 gdouble** Inverse3(gdouble **mat)
383 {
384 gdouble **invmat = NULL;
385 gdouble t4,t6,t8,t10,t12,t14,t17;
386
387 t4 = mat[0][0]*mat[1][1];
388 t6 = mat[0][0]*mat[1][2];
389 t8 = mat[0][1]*mat[1][0];
390 t10 = mat[0][2]*mat[1][0];
391 t12 = mat[0][1]*mat[2][0];
392 t14 = mat[0][2]*mat[2][0];
393 t17 = 1/(t4*mat[2][2]-t6*mat[2][1]-t8*mat[2][2]+t10*mat[2][1]+t12*mat[1][2]-t14*mat
394 [1][1]);
395 invmat = g_malloc(3*sizeof(gdouble*));
396 invmat[0] = g_malloc(3*sizeof(gdouble));
397 invmat[1] = g_malloc(3*sizeof(gdouble));
398 invmat[2] = g_malloc(3*sizeof(gdouble));
399 invmat[0][0] = (mat[1][1]*mat[2][2]-mat[1][2]*mat[2][1])*t17;
400 invmat[0][1] = -(mat[0][1]*mat[2][2]-mat[0][2]*mat[2][1])*t17;
401 invmat[0][2] = -(-mat[0][1]*mat[1][2]+mat[0][2]*mat[1][1])*t17;
402 invmat[1][0] = -(mat[1][0]*mat[2][2]-mat[1][2]*mat[2][0])*t17;
403 invmat[1][1] = (mat[0][0]*mat[2][2]-t14)*t17;
404 invmat[1][2] = -(t6-t10)*t17;
405 invmat[2][0] = -(-mat[1][0]*mat[2][1]+mat[1][1]*mat[2][0])*t17;
406 invmat[2][1] = -(mat[0][0]*mat[2][1]-t12)*t17;
407 invmat[2][2] = (t4-t8)*t17;
408
409 return invmat;
410 }
411 /**************************************************/
InverseMat3D(gdouble invmat[3][3],gdouble mat[3][3])412 gboolean InverseMat3D(gdouble invmat[3][3], gdouble mat[3][3])
413 {
414 gdouble t4,t6,t8,t10,t12,t14,t17;
415
416 t4 = mat[0][0]*mat[1][1];
417 t6 = mat[0][0]*mat[1][2];
418 t8 = mat[0][1]*mat[1][0];
419 t10 = mat[0][2]*mat[1][0];
420 t12 = mat[0][1]*mat[2][0];
421 t14 = mat[0][2]*mat[2][0];
422 t17 = (t4*mat[2][2]-t6*mat[2][1]-t8*mat[2][2]+t10*mat[2][1]+t12*mat[1][2]-t14*mat[1][1]);
423 if(fabs(t17)<1e-12) return FALSE;
424
425 t17 = 1/t17;
426 invmat[0][0] = (mat[1][1]*mat[2][2]-mat[1][2]*mat[2][1])*t17;
427 invmat[0][1] = -(mat[0][1]*mat[2][2]-mat[0][2]*mat[2][1])*t17;
428 invmat[0][2] = -(-mat[0][1]*mat[1][2]+mat[0][2]*mat[1][1])*t17;
429 invmat[1][0] = -(mat[1][0]*mat[2][2]-mat[1][2]*mat[2][0])*t17;
430 invmat[1][1] = (mat[0][0]*mat[2][2]-t14)*t17;
431 invmat[1][2] = -(t6-t10)*t17;
432 invmat[2][0] = -(-mat[1][0]*mat[2][1]+mat[1][1]*mat[2][0])*t17;
433 invmat[2][1] = -(mat[0][0]*mat[2][1]-t12)*t17;
434 invmat[2][2] = (t4-t8)*t17;
435 return TRUE;
436 }
437 /**************************************************/
438