FreeWRL / FreeX3D 4.3.0
LinearAlgebra.c
1/*
2
3
4???
5
6*/
7
8/****************************************************************************
9 This file is part of the FreeWRL/FreeX3D Distribution.
10
11 Copyright 2009 CRC Canada. (http://www.crc.gc.ca)
12
13 FreeWRL/FreeX3D is free software: you can redistribute it and/or modify
14 it under the terms of the GNU Lesser Public License as published by
15 the Free Software Foundation, either version 3 of the License, or
16 (at your option) any later version.
17
18 FreeWRL/FreeX3D is distributed in the hope that it will be useful,
19 but WITHOUT ANY WARRANTY; without even the implied warranty of
20 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21 GNU General Public License for more details.
22
23 You should have received a copy of the GNU General Public License
24 along with FreeWRL/FreeX3D. If not, see <http://www.gnu.org/licenses/>.
25****************************************************************************/
26
27#include <math.h>
28
29#include <config.h>
30#include <system.h>
31#include <display.h>
32#include <internal.h>
33
34#include <libFreeWRL.h>
35
36#include "../vrml_parser/Structs.h"
37#include "../main/headers.h"
38
39#include "LinearAlgebra.h"
40
41double angleNormalized(double angle){
42 //will normalize to +- 2*PI (+-180) range
43 return atan2(sin(angle),cos(angle));
44}
45
46
47#define DJ_KEEP_COMPILER_WARNING 0
48void vecprint3fb(char *name, float *p, char *eol){
49 printf("%s %f %f %f %s",name,p[0],p[1],p[2],eol);
50}
51void vecprint4fb(char *name, float *p, char *eol){
52 printf("%s %f %f %f %f %s",name,p[0],p[1],p[2],p[3],eol);
53}
54void vecprint3db(char *name, double *p, char *eol){
55 printf("%s %lf %lf %lf %s",name,p[0],p[1],p[2],eol);
56}
57void vecprint4db(char *name, double *p, char *eol){
58 printf("%s %lf %lf %lf %lf %s",name,p[0],p[1],p[2],p[3],eol);
59}
60double signd(double val){
61 return val < 0.0 ? -1.0 : val > 0.0 ? 1.0 : 0;
62}
63double * vecsignd(double *b, double *a){
64 int i;
65 for (i = 0; i<3; i++) b[i] = signd(a[i]);
66 return b;
67}
68double * vecmuld(double *c, double *a, double *b){
69 int i;
70 for(i=0;i<3;i++)
71 c[i] = a[i]*b[i];
72 return c;
73}
74double * vecsetd(double *b, double x, double y, double z){
75 b[0] = x, b[1] = y; b[2] = z;
76 return b;
77}
78double * vecset4d(double *b, double x, double y, double z, double a){
79 b[0] = x, b[1] = y; b[2] = z; b[3] = a;
80 return b;
81}
82float *double2float(float *b, const double *a, int n){
83 int i;
84 for(i=0;i<n;i++) b[i] = (float)a[i];
85 return b;
86}
87double *float2double(double *b, float *a, int n){
88 int i;
89 for(i=0;i<n;i++) b[i] = (double)a[i];
90 return b;
91}
92double * vecadd2d(double *c, double *a, double *b){
93 c[0] = a[0] + b[0];
94 c[1] = a[1] + b[1];
95 return c;
96}
97
98double *vecdif2d(double *c, double* a, double *b){
99 c[0] = a[0] - b[0];
100 c[1] = a[1] - b[1];
101 return c;
102}
103double veclength2d( double *p ){
104 return sqrt(p[0]*p[0] + p[1]*p[1]);
105}
106double vecdot2d(double *a, double *b){
107 return a[0]*b[0] + a[1]*b[1];
108}
109
110double* vecscale2d(double* r, double* v, double s){
111 r[0] = v[0] * s;
112 r[1] = v[1] * s;
113 return r;
114}
115
116double vecnormal2d(double *r, double *v){
117 double ret = sqrt(vecdot2d(v,v));
118 /* if(ret == 0.) return 0.; */
119 if (APPROX(ret, 0)) return 0.;
120 vecscale2d(r,v,1./ret);
121 return ret;
122}
123
124float * vecadd2f(float *c, float *a, float *b){
125 c[0] = a[0] + b[0];
126 c[1] = a[1] + b[1];
127 return c;
128}
129
130float *vecdif2f(float *c, float* a, float *b){
131 c[0] = a[0] - b[0];
132 c[1] = a[1] - b[1];
133 return c;
134}
135float veclength2f( float *p ){
136 return (float) sqrt(p[0]*p[0] + p[1]*p[1]);
137}
138float vecdot2f(float *a, float *b){
139 return a[0]*b[0] + a[1]*b[1];
140}
141
142float* vecscale2f(float* r, float* v, float s){
143 r[0] = v[0] * s;
144 r[1] = v[1] * s;
145 return r;
146}
147
148float vecnormal2f(float *r, float *v){
149 float ret = (float)sqrt(vecdot2f(v,v));
150 /* if(ret == 0.) return 0.; */
151 if (APPROX(ret, 0)) return 0.0f;
152 vecscale2f(r,v,1.0f/ret);
153 return ret;
154}
155
156
157
158
159/* Altenate implemetations available, should merge them eventually */
160double *vecaddd(double *c, double* a, double *b)
161{
162 c[0] = a[0] + b[0];
163 c[1] = a[1] + b[1];
164 c[2] = a[2] + b[2];
165 return c;
166}
167float *vecadd3f(float *c, float *a, float *b)
168{
169 c[0] = a[0] + b[0];
170 c[1] = a[1] + b[1];
171 c[2] = a[2] + b[2];
172 return c;
173}
174double *vecdifd(double *c, double* a, double *b)
175{
176 c[0] = a[0] - b[0];
177 c[1] = a[1] - b[1];
178 c[2] = a[2] - b[2];
179 return c;
180}
181double *vecdif4d(double *c, double* a, double *b)
182{
183 c[0] = a[0] - b[0];
184 c[1] = a[1] - b[1];
185 c[2] = a[2] - b[2];
186 c[3] = a[3] - b[3];
187 return c;
188}
189float *vecdif3f(float *c, float *a, float *b)
190{
191 c[0] = a[0] - b[0];
192 c[1] = a[1] - b[1];
193 c[2] = a[2] - b[2];
194 return c;
195}
196double *veccopyd(double *c, double *a)
197{
198 c[0] = a[0];
199 c[1] = a[1];
200 c[2] = a[2];
201 return c;
202}
203double *vecnegated(double *b, double *a)
204{
205 b[0] = -a[0];
206 b[1] = -a[1];
207 b[2] = -a[2];
208 return b;
209}
210double *vecswizzle2d(double *inout ){
211 double tmp = inout[0];
212 inout[0] = inout[1];
213 inout[1] = tmp;
214 return inout;
215}
216
217double *veccopy4d(double *c, double *a)
218{
219 c[0] = a[0];
220 c[1] = a[1];
221 c[2] = a[2];
222 c[3] = a[3];
223 return c;
224}
225
226
227float *veccopy3f(float *b, float *a)
228{
229 b[0] = a[0];
230 b[1] = a[1];
231 b[2] = a[2];
232 return b;
233}
234float *vecnegate3f(float *b, float *a)
235{
236 b[0] = -a[0];
237 b[1] = -a[1];
238 b[2] = -a[2];
239 return b;
240}
241int vecsame2f(float *b, float *a){
242 return a[0] == b[0] && a[1] == b[1] ? TRUE : FALSE;
243}
244float *veccopy2f(float *b, float *a)
245{
246 b[0] = a[0];
247 b[1] = a[1];
248 return b;
249}
250float *vecset2f(float *b, float x, float y)
251{
252 b[0] = x; b[1] = y;
253 return b;
254}
255double * veccrossd(double *c, double *a, double *b)
256{
257 double aa[3], bb[3];
258 veccopyd(aa,a);
259 veccopyd(bb,b);
260 c[0] = aa[1] * bb[2] - aa[2] * bb[1];
261 c[1] = aa[2] * bb[0] - aa[0] * bb[2];
262 c[2] = aa[0] * bb[1] - aa[1] * bb[0];
263 return c;
264}
265double veclengthd( double *p )
266{
267 return sqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2]);
268}
269double veclength4d( double *p )
270{
271 return sqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2] + p[3]*p[3]);
272}
273double vecdotd(double *a, double *b)
274{
275 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
276}
277double* vecscaled(double* r, double* v, double s)
278{
279 r[0] = v[0] * s;
280 r[1] = v[1] * s;
281 r[2] = v[2] * s;
282 return r;
283}
284/* returns vector length, too */
285double vecnormald(double *r, double *v)
286{
287 double ret = sqrt(vecdotd(v,v));
288 /* if(ret == 0.) return 0.; */
289 if (APPROX(ret, 0)) return 0.;
290 vecscaled(r,v,1./ret);
291 return ret;
292}
293
294void veccross(struct point_XYZ *c, struct point_XYZ a, struct point_XYZ b)
295{
296 c->x = a.y * b.z - a.z * b.y;
297 c->y = a.z * b.x - a.x * b.z;
298 c->z = a.x * b.y - a.y * b.x;
299}
300float *veccross3f(float *c, float *a, float *b)
301{
302 /*FLOPs 6 float*/
303 c[0] = a[1]*b[2] - a[2]*b[1];
304 c[1] = a[2]*b[0] - a[0]*b[2];
305 c[2] = a[0]*b[1] - a[1]*b[0];
306 return c;
307}
308float veclength( struct point_XYZ p )
309{
310 return (float) sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
311}
312float vecdot3f( float *a, float *b )
313{
314 /*FLOPs 3 float */
315 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
316}
317float vecdot4f( float *a, float *b )
318{
319 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + + a[3]*b[3];
320}
321float *vecset3f(float *b, float x, float y, float z)
322{
323 b[0] = x; b[1] = y; b[2] = z;
324 return b;
325}
326float *vecset4f(float *b, float x, float y, float z, float a)
327{
328 b[0] = x; b[1] = y; b[2] = z; b[3] = a;
329 return b;
330}
331float veclength3f(float *a){
332 return (float)sqrt(vecdot3f(a, a));
333}
334
335double vecangle(struct point_XYZ* V1, struct point_XYZ* V2) {
336 return acos((V1->x*V2->x + V1->y*V2->y +V1->z*V2->z) /
337 sqrt( (V1->x*V1->x + V1->y*V1->y + V1->z*V1->z)*(V2->x*V2->x + V2->y*V2->y + V2->z*V2->z) ) );
338};
339
340float *veccopy4f(float *b, float *a)
341{
342 b[0] = a[0];
343 b[1] = a[1];
344 b[2] = a[2];
345 b[3] = a[3];
346 return b;
347}
348float calc_angle_between_two_vectors3f(float * a, float * b){
349 //scalar angle between 2 vectors (doesn't say which way on a great circle)
350 float an[3], bn[3], dotf, anglef, flen;
351 flen = veclength3f(a);
352 if(flen <= 0.0f) return 0.0f;
353 flen = veclength3f(b);
354 if(flen <= 0.0f) return 0.0f;
355 vecnormalize3f(an,a);
356 vecnormalize3f(bn,b);
357 dotf = vecdot3f(an,bn);
358 anglef = acos(dotf);
359 return anglef;
360}
361float calc_angle_between_two_vectors(struct point_XYZ a, struct point_XYZ b)
362{
363 float length_a, length_b, scalar, temp;
364 scalar = (float) calc_vector_scalar_product(a,b);
365 length_a = calc_vector_length(a);
366 length_b = calc_vector_length(b);
367
368 /* printf("scalar: %f length_a: %f length_b: %f \n", scalar, length_a, length_b);*/
369
370 /* if (scalar == 0) */
371 if (APPROX(scalar, 0)) {
372 return (float) (M_PI/2.0);
373 }
374
375 if ( (length_a <= 0) || (length_b <= 0)){
376 printf("Divide by 0 in calc_angle_between_two_vectors(): No can do! \n");
377 return 0;
378 }
379
380 temp = scalar /(length_a * length_b);
381 /* printf("temp: %f", temp);*/
382
383 /*acos() appears to be unable to handle 1 and -1 */
384 /* fixed to handle border case where temp <=-1.0 for 0.39 JAS */
385 if ((temp >= 1) || (temp <= -1)){
386 if (temp < 0.0f) return 3.141526f;
387 return 0.0f;
388 }
389 return (float) acos(temp);
390}
391
392int vecsame3f(float *a, float *b){
393 int i,isame = TRUE;
394 for(i=0;i<3;i++)
395 if(a[i] != b[i]) isame = FALSE;
396 return isame;
397}
398int vecsame4f(float *a, float *b){
399 int i,isame = TRUE;
400 for(i=0;i<4;i++)
401 if(a[i] != b[i]) isame = FALSE;
402 return isame;
403}
404int vecsamed(double *a, double *b){
405 int i,isame = TRUE;
406 for(i=0;i<3;i++)
407 if(a[i] != b[i]) isame = FALSE;
408 return isame;
409}
410/* returns vector length, too */
411GLDOUBLE vecnormal(struct point_XYZ*r, struct point_XYZ* v)
412{
413 GLDOUBLE ret = sqrt(vecdot(v,v));
414 /* if(ret == 0.) return 0.; */
415 if (APPROX(ret, 0)) return 0.;
416 vecscale(r,v,1./ret);
417 return ret;
418}
419float vecnormsquared3f(float *a){
420 return a[0] * a[0] + a[1] * a[1] + a[2] * a[2];
421}
422float *vecscale3f(float *b, float *a, float scale){
423 b[0] = a[0] * scale;
424 b[1] = a[1] * scale;
425 b[2] = a[2] * scale;
426 return b;
427}
428float *vecscale4f(float *b, float *a, float scale){
429 b[0] = a[0] * scale;
430 b[1] = a[1] * scale;
431 b[2] = a[2] * scale;
432 b[3] = a[3] * scale;
433 return b;
434}
435float *vecmult3f(float *c, float *a, float *b){
436 /* c[i] = a[i]*b[i] */
437 int i=0;
438 for(;i<3;i++) c[i] = a[i]*b[i];
439 return c;
440}
441float *vecmult2f(float *c, float *a, float *b){
442 /* c[i] = a[i]*b[i] */
443 int i=0;
444 for(;i<2;i++) c[i] = a[i]*b[i];
445 return c;
446}
447
448float *vecnormalize3f(float *b, float *a)
449{
450 float norm = veclength3f(a);
451 if (APPROX(norm, 0.0f)){
452 b[0] = 1.0f; b[1] = 0.0f; b[2] = 0.0f;
453 }else{
454 vecscale3f(b, a, 1.0f / norm);
455 }
456 return b;
457}
458/*will add functions here as needed. */
459
460GLDOUBLE det3x3(GLDOUBLE* data)
461{
462 return -data[1]*data[10]*data[4] +data[0]*data[10]*data[5] -data[2]*data[5]*data[8] +data[1]*data[6]*data[8] +data[2]*data[4]*data[9] -data[0]*data[6]*data[9];
463}
464float det3f(float *a, float *b, float *c)
465{
466 /*FLOPs 9 float: dot 3, cross 6 */
467 float temp[3];
468 return vecdot3f(a,veccross3f(temp,b, c));
469}
470
471struct point_XYZ* transform(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b)
472{
473 //FLOPs 9 double
474 // r = a x b
475 struct point_XYZ tmp; /* JAS*/
476
477 if(r != a) { /*protect from self-assignments */
478 r->x = b[0]*a->x +b[4]*a->y +b[8]*a->z +b[12];
479 r->y = b[1]*a->x +b[5]*a->y +b[9]*a->z +b[13];
480 r->z = b[2]*a->x +b[6]*a->y +b[10]*a->z +b[14];
481 } else {
482 /* JAS was - struct point_XYZ tmp = {a->x,a->y,a->z};*/
483 tmp.x = a->x; tmp.y = a->y; tmp.z = a->z;
484 r->x = b[0]*tmp.x +b[4]*tmp.y +b[8]*tmp.z +b[12];
485 r->y = b[1]*tmp.x +b[5]*tmp.y +b[9]*tmp.z +b[13];
486 r->z = b[2]*tmp.x +b[6]*tmp.y +b[10]*tmp.z +b[14];
487 }
488 return r;
489}
490GLDOUBLE* transformUPPER3X3d(double *r,double *a, const GLDOUBLE* b){
491 double tmp[3];
492 veccopyd(tmp,a);
493 r[0] = b[0]*tmp[0] +b[4]*tmp[1] +b[8]*tmp[2];
494 r[1] = b[1]*tmp[0] +b[5]*tmp[1] +b[9]*tmp[2];
495 r[2] = b[2]*tmp[0] +b[6]*tmp[1] +b[10]*tmp[2];
496 return r;
497}
498struct point_XYZ* transformAFFINE(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b){
499 //FLOPs 9 double
500 // r = a x b
501 return transform(r,a,b);
502}
503GLDOUBLE* pointxyz2double(double* r, struct point_XYZ *p){
504 r[0] = p->x; r[1] = p->y; r[2] = p->z;
505 return r;
506}
507struct point_XYZ* double2pointxyz(struct point_XYZ* r, double* p){
508 r->x = p[0]; r->y = p[1]; r->z = p[2];
509 return r;
510}
511double * matrixAFFINE2RotationMatrix(double* rotmat, double *fullmat){
512 //takes a full affine matrix
513 // and returns a pure rotation matrix
514 //could be used in background and/or billboard
515 //in freewrl terminology, an affine matrix has no perspectives (typical for modelview matrix):
516 //- perspectives are zero
517 //- has translations rotations, scale and shear
518 //- shear is not seen in web3d or very rare (an assymmetric offdiagonal); here we assume no shear
519 double sx,sy,sz, pp[3], q[3],p[3],d[3], matinv[16], matt[16], matr[16], mats[16];
520
521 //cancel / undo translation part
522 matinverseAFFINE(matinv,fullmat);
523 vecsetd(pp,0.0,0.0,0.0);
524 transformAFFINEd(pp,pp,matinv);
525 mattranslate(matt,pp[0],pp[1],pp[2]);
526 matmultiplyAFFINE(matr,matt,fullmat);
527
528 //cancel/undo scale part, so that our background mesh stays at radius 1.0
529 /* Get scale */
530 vecsetd(p,0.0,0.0,0.0);
531 transformAFFINEd(p,p,matr);
532 vecsetd(q,1.0,0.0,0.0);
533 transformAFFINEd(q,q,matr);
534 sx = 1.0/veclengthd(vecdifd(d,q,p));
535 vecsetd(q,0.0,1.0,0.0);
536 transformAFFINEd(q,q,matr);
537 vecdifd(d,q,p);
538 sy = 1.0/veclengthd(vecdifd(d,q,p));
539 vecsetd(q,0.0,0.0,1.0);
540 transformAFFINEd(q,q,matr);
541 sz = 1.0/veclengthd(vecdifd(d,q,p));
542 /* Undo the scale effects */
543 matscale(mats,sx,sy,sz);
544 matmultiplyAFFINE(rotmat,mats,matr);
545
546 //result should be pure rotation matrix (with possible rare shear)
547 return rotmat; //we return it too, in case you want to do fancy chain multiplication
548}
549
550double *transformAFFINEd(double *r, double *a, const GLDOUBLE* mat){
551 // r = a x mat
552 struct point_XYZ pa, pr;
553 double2pointxyz(&pa,a);
554 transformAFFINE(&pr,&pa,mat);
555 pointxyz2double(r,&pr);
556 return r;
557}
558double *transformFULL4d(double *r, double *a, double *mat){
559 //same as __gluMultMatrixVecd elsewhere
560 int i;
561 for (i=0; i<4; i++) {
562 r[i] =
563 a[0] * mat[0*4+i] +
564 a[1] * mat[1*4+i] +
565 a[2] * mat[2*4+i] +
566 a[3] * mat[3*4+i];
567 }
568 return r;
569}
570float* transformf(float* r, const float* a, const GLDOUBLE* b)
571{
572 //r = a x b
573 float tmp[3]; /* JAS*/
574
575 if(r != a) { /*protect from self-assignments */
576 r[0] = (float) (b[0]*a[0] +b[4]*a[1] +b[8]*a[2] +b[12]);
577 r[1] = (float) (b[1]*a[0] +b[5]*a[1] +b[9]*a[2] +b[13]);
578 r[2] = (float) (b[2]*a[0] +b[6]*a[1] +b[10]*a[2] +b[14]);
579 } else {
580 tmp[0] =a[0]; tmp[1] = a[1]; tmp[2] = a[2]; /* JAS*/
581 r[0] = (float) (b[0]*tmp[0] +b[4]*tmp[1] +b[8]*tmp[2] +b[12]);
582 r[1] = (float) (b[1]*tmp[0] +b[5]*tmp[1] +b[9]*tmp[2] +b[13]);
583 r[2] = (float) (b[2]*tmp[0] +b[6]*tmp[1] +b[10]*tmp[2] +b[14]);
584 }
585 return r;
586}
587float* matmultvec4f(float* r4, float *mat4, float* a4 )
588{
589 int i,j;
590 float t4[4], *b[4];
591 memcpy(t4,a4,4*sizeof(float));
592 for(i=0;i<4;i++){
593 r4[i] = 0.0f;
594 b[i] = &mat4[i*4];
595 for(j=0;j<4;j++)
596 r4[i] += b[i][j]*t4[j];
597 }
598 return r4;
599}
600float* vecmultmat4f_broken(float* r4, float* a4, float *mat4 )
601{
602 int i,j;
603 float t4[4], *b[4];
604 memcpy(t4,a4,4*sizeof(float));
605 for(i=0;i<4;i++){
606 r4[i] = 0.0f;
607 b[i] = &mat4[i*4];
608 for(j=0;j<4;j++)
609 r4[i] += t4[j]*b[j][i];
610 }
611 return r4;
612}
613float* vecmultmat4f(float* r4, float* a4, float *mat4 )
614{
615 int i,j;
616 float t4[4], *b;
617 memcpy(t4,a4,4*sizeof(float));
618 for(i=0;i<4;i++){
619 r4[i] = 0.0f;
620 b = &mat4[i*4];
621 for(j=0;j<4;j++)
622 r4[i] += t4[j]*b[j];
623 }
624 return r4;
625}
626float* matmultvec3f(float* r3, float *mat3, float* a3 )
627{
628 int i,j;
629 float t3[3], *b[3];
630 memcpy(t3,a3,3*sizeof(float));
631 for(i=0;i<3;i++){
632 r3[i] = 0.0f;
633 b[i] = &mat3[i*3];
634 for(j=0;j<3;j++)
635 r3[i] += b[i][j]*t3[j];
636 }
637 return r3;
638}
639float* vecmultmat3f(float* r3, float* a3, float *mat3 )
640{
641 int i,j;
642 float t3[3], *b[3];
643 memcpy(t3,a3,3*sizeof(float));
644 for(i=0;i<3;i++){
645 r3[i] = 0.0f;
646 b[i] = &mat3[i*4];
647 for(j=0;j<3;j++)
648 r3[i] += t3[j]*b[j][i];
649 }
650
651 return r3;
652}
653
654/*transform point, but ignores translation.*/
655struct point_XYZ* transform3x3(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b)
656{
657 struct point_XYZ tmp;
658
659 if(r != a) { /*protect from self-assignments */
660 r->x = b[0]*a->x +b[4]*a->y +b[8]*a->z;
661 r->y = b[1]*a->x +b[5]*a->y +b[9]*a->z;
662 r->z = b[2]*a->x +b[6]*a->y +b[10]*a->z;
663 } else {
664 /* JAS struct point_XYZ tmp = {a->x,a->y,a->z};*/
665 tmp.x = a->x; tmp.y = a->y; tmp.z = a->z;
666 r->x = b[0]*tmp.x +b[4]*tmp.y +b[8]*tmp.z;
667 r->y = b[1]*tmp.x +b[5]*tmp.y +b[9]*tmp.z;
668 r->z = b[2]*tmp.x +b[6]*tmp.y +b[10]*tmp.z;
669 }
670 return r;
671}
672
673struct point_XYZ* vecscale(struct point_XYZ* r, struct point_XYZ* v, GLDOUBLE s)
674{
675 r->x = v->x * s;
676 r->y = v->y * s;
677 r->z = v->z * s;
678 return r;
679}
680double vecdot(struct point_XYZ* a, struct point_XYZ* b)
681{
682 return (a->x*b->x) + (a->y*b->y) + (a->z*b->z);
683}
684
685
686/* returns 0 if p1 is closest, 1 if p2 is closest, and a fraction if the closest point is in between */
687/* To get the closest point, use pclose = retval*p1 + (1-retval)p2; */
688/* y1 must be smaller than y2 */
689/*double closest_point_of_segment_to_y_axis_segment(double y1, double y2, struct point_XYZ p1, struct point_XYZ p2) {
690 double imin = (y1- p1.y) / (p2.y - p1.y);
691 double imax = (y2- p1.y) / (p2.y - p1.y);
692
693 double x21 = (p2.x - p1.x);
694 double z21 = (p2.z - p1.z);
695 double i = (p2.x * x21 + p2.z * z21) /
696 ( x21*x21 + z21*z21 );
697 return max(min(i,imax),imin);
698
699 }*/
700
701double closest_point_of_segment_to_y_axis_segment(double y1, double y2, struct point_XYZ p1, struct point_XYZ p2) {
702 /*cylinder constraints (to be between y1 and y2) */
703 double imin = (y1- p1.y) / (p2.y - p1.y);
704 double imax = (y2- p1.y) / (p2.y - p1.y);
705
706 /*the equation */
707 double x12 = (p1.x - p2.x);
708 double z12 = (p1.z - p2.z);
709 double q = ( x12*x12 + z12*z12 );
710
711 /* double i = ((q == 0.) ? 0 : (p1.x * x12 + p1.z * z12) / q); */
712 double i = ((APPROX(q, 0)) ? 0 : (p1.x * x12 + p1.z * z12) / q);
713
714 printf("imin=%f, imax=%f => ",imin,imax);
715
716 if(imin > imax) {
717 double tmp = imax;
718 imax = imin;
719 imin = tmp;
720 }
721
722 /*clamp constraints to segment*/
723 if(imin < 0) imin = 0;
724 if(imax > 1) imax = 1;
725
726 printf("imin=%f, imax=%f\n",imin,imax);
727
728 /*clamp result to constraints */
729 if(i < imin) i = imin;
730 if(i > imax) i = imax;
731 return i;
732
733}
734BOOL line_intersect_line_3f(float *p1, float *v1, float *p2, float *v2, float *t, float *s, float *x1, float *x2)
735{
736 //from Graphics Gems I, p.304 http://inis.jinr.ru/sl/vol1/CMC/Graphics_Gems_1,ed_A.Glassner.pdf
737 //L1: P1 + V1*t
738 //L2: P2 + V2*s
739 //t and s are at the footpoint/point of closest passing
740 //t = det(P2-P1,V2,V1xV2) / |V1 x V2|**2
741 //s = det(P2-P1,V1,V1xV2) / |V1 x V2|**2
742 float t1[3], cross[3]; //temp intermediate variables
743 float crosslength2, ss, tt;
744 veccross3f(cross, v1, v2);
745 crosslength2 = vecnormsquared3f(cross);
746 if (APPROX(crosslength2, 0.0f)) return FALSE; //lines are parallel, no intersection
747 crosslength2 = 1.0f / crosslength2;
748 tt = det3f(vecdif3f(t1, p2, p1), v2, cross) * crosslength2;
749 ss = det3f(vecdif3f(t1, p2, p1), v1, cross) * crosslength2;
750 if (x1)
751 vecadd3f(x1, p1, vecscale3f(t1, v1, tt));
752 if (x2)
753 vecadd3f(x2, p2, vecscale3f(t1, v2, ss));
754 if (t)
755 *t = tt;
756 if (s)
757 *s = ss;
758 return TRUE; //success we have 2 footpoints.
759}
760
761BOOL line_intersect_planed_3f(float *p, float *v, float *N, float d, float *pi, float *t)
762{
763 //from graphics gems I, p.391 http://inis.jinr.ru/sl/vol1/CMC/Graphics_Gems_1,ed_A.Glassner.pdf
764 // V dot N = d = const for points on a plane, or N dot P + d = 0
765 // line/ray P1 + v1*t = P2 (intersection point)
766 // combining t = -(d + N dot P1)/(N dot v1)
767 float t1[3], t2[3], nd, tt;
768 nd = vecdot3f(N, v);
769 if (APPROX(nd, 0.0f)) return FALSE;
770 tt = -(d + vecdot3f(N, p)) / nd;
771 vecadd3f(t2, p, vecscale3f(t1, v, tt));
772 if (t) *t = tt;
773 if (pi) veccopy3f(pi, t2);
774 return TRUE;
775}
776BOOL line_intersect_plane_3f(float *p, float *v, float *N, float *pp, float *pi, float *t)
777{
778 float d;
779 d = vecdot3f(N, pp);
780 return line_intersect_planed_3f(p, v, N, d, pi, t);
781}
782
783BOOL line_intersect_cylinder_3f(float *p, float *v, float radius, float *pi)
784{
785 //from rendray_Cylinder
786 //intersects arbitrary ray (p,v) with cylinder of radius, origiin 0,0,0 and axis 0,1,0
787 //april 2014: NOT TESTED, NOT USED - just hacked in and compiled
788 // if((!XEQ) && (!ZEQ)) {
789 float pp[3];
790 float dx = v[0];
791 float dz = v[2];
792 float a = dx*dx + dz*dz;
793 float b = 2.0f*(dx * p[0] + dz * p[2]);
794 float c = p[0] * p[0] + p[2] * p[2] - radius*radius;
795 float und;
796 if (APPROX(a, 0.0f))return FALSE;
797 b /= a; c /= a;
798 und = b*b - 4*c;
799 if(und > 0) { /* HITS the infinite cylinder */
800 float t2[3];
801 //float sol1 = (-b+(float) sqrt(und))/2;
802 float sol2 = (-b-(float) sqrt(und))/2;
803 float sol = sol2;// sol1 < sol2 ? sol1 : sol2; //take the one closest to p (but should these be abs? what about direction
804 vecadd3f(pp, p, vecscale3f(t2, v, sol));
805 if (pi) veccopy3f(pi, pp);
806 return TRUE;
807 }
808 // }
809 return FALSE;
810}
811
812struct point_XYZ* vecadd(struct point_XYZ* r, struct point_XYZ* v, struct point_XYZ* v2)
813{
814 r->x = v->x + v2->x;
815 r->y = v->y + v2->y;
816 r->z = v->z + v2->z;
817 return r;
818}
819
820struct point_XYZ* vecdiff(struct point_XYZ* r, struct point_XYZ* v, struct point_XYZ* v2)
821{
822 r->x = v->x - v2->x;
823 r->y = v->y - v2->y;
824 r->z = v->z - v2->z;
825 return r;
826}
827
828/*i,j,n will form an orthogonal vector space */
829void make_orthogonal_vector_space(struct point_XYZ* i, struct point_XYZ* j, struct point_XYZ n) {
830 /* optimal axis finding algorithm. the solution isn't unique.*/
831 /* each of these three calculations doesn't work (or works poorly)*/
832 /* in certain distinct cases. (gives zero vectors when two axes are 0)*/
833 /* selecting the calculations according to smallest axis avoids this problem.*/
834 /* (the two remaining axis are thus far from zero, if n is normal)*/
835 if(fabs(n.x) <= fabs(n.y) && fabs(n.x) <= fabs(n.z)) { /* x smallest*/
836 i->x = 0;
837 i->y = n.z;
838 i->z = -n.y;
839 vecnormal(i,i);
840 j->x = n.y*n.y + n.z*n.z;
841 j->y = (-n.x)*n.y;
842 j->z = (-n.x)*n.z;
843 } else if(fabs(n.y) <= fabs(n.x) && fabs(n.y) <= fabs(n.z)) { /* y smallest*/
844 i->x = -n.z;
845 i->y = 0;
846 i->z = n.x;
847 vecnormal(i,i);
848 j->x = (-n.x)*n.y;
849 j->y = n.x*n.x + n.z*n.z;
850 j->z = (-n.y)*n.z;
851 } else { /* z smallest*/
852 i->x = n.y;
853 i->y = -n.x;
854 i->z = 0;
855 vecnormal(i,i);
856 j->x = (-n.x)*n.z;
857 j->y = (-n.y)*n.z;
858 j->z = n.x*n.x + n.y*n.y;
859 }
860}
861
862
863GLDOUBLE* mattranspose(GLDOUBLE* res, GLDOUBLE* mm)
864{
865 GLDOUBLE mcpy[16];
866 int i, j;
867 GLDOUBLE *m;
868
869 m = mm;
870 if(res == m) {
871 memcpy(mcpy,m,sizeof(GLDOUBLE)*16);
872 m = mcpy;
873 }
874
875 for (i = 0; i < 4; i++) {
876 //for (j = i + 1; j < 4; j++) {
877 // res[i*4+j] = m[j*4+i];
878 //}
879 for (j = 0; j < 4; j++) {
880 res[i*4+j] = m[j*4+i];
881 }
882 }
883 return res;
884}
885
886float* mattranspose4f(float* res, float* mm)
887{
888 float mcpy[16];
889 int i, j;
890 float *m;
891
892 m = mm;
893 if(res == m) {
894 memcpy(mcpy,m,sizeof(float)*16);
895 m = mcpy;
896 }
897
898 for (i = 0; i < 4; i++) {
899 for (j = 0; j < 4; j++) {
900 res[i*4+j] = m[j*4+i];
901 }
902 }
903 return res;
904}
905float* mattranspose3f(float* res, float* mm)
906{
907 float mcpy[9];
908 int i, j;
909 float *m;
910
911 m = mm;
912 if(res == m) {
913 memcpy(mcpy,m,sizeof(float)*9);
914 m = mcpy;
915 }
916
917 for (i = 0; i < 3; i++) {
918 for (j = 0; j < 3; j++) {
919 res[i*3+j] = m[j*3+i];
920 }
921 }
922 return res;
923}
924
925GLDOUBLE* matinverse98(GLDOUBLE* res, GLDOUBLE* mm)
926{
927 /*FLOPs 98 double: det3 9, 1/det 1, adj3x3 9x4=36, inv*T 13x4=52 */
928 //July 2016 THIS IS WRONG DON'T USE
929 //see the glu equivalent elsewhere
930 //you can check with A*A-1 = I and this function doesn't give I
931 double Deta;
932 GLDOUBLE mcpy[16];
933 GLDOUBLE *m;
934
935 m = mm;
936 if(res == m) {
937 memcpy(mcpy,m,sizeof(GLDOUBLE)*16);
938 m = mcpy;
939 }
940
941 Deta = det3x3(m);
942 if(APPROX(Deta,0.0))
943 printf("deta 0\n");
944 Deta = 1.0 / Deta;
945
946 res[0] = (-m[9]*m[6] +m[5]*m[10])*Deta;
947 res[4] = (+m[8]*m[6] -m[4]*m[10])*Deta;
948 res[8] = (-m[8]*m[5] +m[4]*m[9])*Deta;
949 res[12] = ( m[12]*m[9]*m[6] -m[8]*m[13]*m[6] -m[12]*m[5]*m[10] +m[4]*m[13]*m[10] +m[8]*m[5]*m[14] -m[4]*m[9]*m[14])*Deta;
950
951 res[1] = (+m[9]*m[2] -m[1]*m[10])*Deta;
952 res[5] = (-m[8]*m[2] +m[0]*m[10])*Deta;
953 res[9] = (+m[8]*m[1] -m[0]*m[9])*Deta;
954 res[13] = (-m[12]*m[9]*m[2] +m[8]*m[13]*m[2] +m[12]*m[1]*m[10] -m[0]*m[13]*m[10] -m[8]*m[1]*m[14] +m[0]*m[9]*m[14])*Deta;
955
956 res[2] = (-m[5]*m[2] +m[1]*m[6])*Deta;
957 res[6] = (+m[4]*m[2] -m[0]*m[6])*Deta;
958 res[10] = (-m[4]*m[1] +m[0]*m[5])*Deta;
959 res[14] = ( m[12]*m[5]*m[2] -m[4]*m[13]*m[2] -m[12]*m[1]*m[6] +m[0]*m[13]*m[6] +m[4]*m[1]*m[14] -m[0]*m[5]*m[14])*Deta;
960
961 res[3] = (+m[5]*m[2]*m[11] -m[1]*m[6]*m[11])*Deta;
962 res[7] = (-m[4]*m[2]*m[11] +m[0]*m[6]*m[11])*Deta;
963 res[11] = (+m[4]*m[1]*m[11] -m[0]*m[5]*m[11])*Deta;
964 res[15] = (-m[8]*m[5]*m[2] +m[4]*m[9]*m[2] +m[8]*m[1]*m[6] -m[0]*m[9]*m[6] -m[4]*m[1]*m[10] +m[0]*m[5]*m[10])*Deta;
965
966 return res;
967}
968float* matinverse4f(float* res, float* mm)
969{
970 /*FLOPs 98 float: det3 9, 1/det 1, adj3x3 9x4=36, inv*T 13x4=52 */
971
972 float Deta;
973 float mcpy[16];
974 float *m;
975
976 m = mm;
977 if(res == m) {
978 memcpy(mcpy,m,sizeof(float)*16);
979 m = mcpy;
980 }
981
982 Deta = det3f(&m[0],&m[4],&m[8]); //det3x3(m);
983 Deta = 1.0f / Deta;
984
985 res[0] = (-m[9]*m[6] +m[5]*m[10])*Deta;
986 res[4] = (+m[8]*m[6] -m[4]*m[10])*Deta;
987 res[8] = (-m[8]*m[5] +m[4]*m[9])*Deta;
988 res[12] = ( m[12]*m[9]*m[6] -m[8]*m[13]*m[6] -m[12]*m[5]*m[10] +m[4]*m[13]*m[10] +m[8]*m[5]*m[14] -m[4]*m[9]*m[14])*Deta;
989
990 res[1] = (+m[9]*m[2] -m[1]*m[10])*Deta;
991 res[5] = (-m[8]*m[2] +m[0]*m[10])*Deta;
992 res[9] = (+m[8]*m[1] -m[0]*m[9])*Deta;
993 res[13] = (-m[12]*m[9]*m[2] +m[8]*m[13]*m[2] +m[12]*m[1]*m[10] -m[0]*m[13]*m[10] -m[8]*m[1]*m[14] +m[0]*m[9]*m[14])*Deta;
994
995 res[2] = (-m[5]*m[2] +m[1]*m[6])*Deta;
996 res[6] = (+m[4]*m[2] -m[0]*m[6])*Deta;
997 res[10] = (-m[4]*m[1] +m[0]*m[5])*Deta;
998 res[14] = ( m[12]*m[5]*m[2] -m[4]*m[13]*m[2] -m[12]*m[1]*m[6] +m[0]*m[13]*m[6] +m[4]*m[1]*m[14] -m[0]*m[5]*m[14])*Deta;
999
1000 res[3] = (+m[5]*m[2]*m[11] -m[1]*m[6]*m[11])*Deta;
1001 res[7] = (-m[4]*m[2]*m[11] +m[0]*m[6]*m[11])*Deta;
1002 res[11] = (+m[4]*m[1]*m[11] -m[0]*m[5]*m[11])*Deta;
1003 res[15] = (-m[8]*m[5]*m[2] +m[4]*m[9]*m[2] +m[8]*m[1]*m[6] -m[0]*m[9]*m[6] -m[4]*m[1]*m[10] +m[0]*m[5]*m[10])*Deta;
1004
1005 return res;
1006}
1007
1008struct point_XYZ* polynormal(struct point_XYZ* r, struct point_XYZ* p1, struct point_XYZ* p2, struct point_XYZ* p3) {
1009 struct point_XYZ v1;
1010 struct point_XYZ v2;
1011 VECDIFF(*p2,*p1,v1);
1012 VECDIFF(*p3,*p1,v2);
1013 veccross(r,v1,v2);
1014 vecnormal(r,r);
1015 return r;
1016}
1017
1018/*simple wrapper for now. optimize later */
1019struct point_XYZ* polynormalf(struct point_XYZ* r, float* p1, float* p2, float* p3) {
1020 struct point_XYZ pp[3];
1021 pp[0].x = p1[0];
1022 pp[0].y = p1[1];
1023 pp[0].z = p1[2];
1024 pp[1].x = p2[0];
1025 pp[1].y = p2[1];
1026 pp[1].z = p2[2];
1027 pp[2].x = p3[0];
1028 pp[2].y = p3[1];
1029 pp[2].z = p3[2];
1030 return polynormal(r,pp+0,pp+1,pp+2);
1031}
1032
1033
1034GLDOUBLE* matrotate(GLDOUBLE* Result, double Theta, double x, double y, double z)
1035{
1036 GLDOUBLE CosTheta = cos(Theta);
1037 GLDOUBLE SinTheta = sin(Theta);
1038
1039 Result[0] = x*x + (y*y+z*z)*CosTheta;
1040 Result[1] = x*y - x*y*CosTheta + z*SinTheta;
1041 Result[2] = x*z - x*z*CosTheta - y*SinTheta;
1042 Result[4] = x*y - x*y*CosTheta - z*SinTheta;
1043 Result[5] = y*y + (x*x+z*z)*CosTheta;
1044 Result[6] = z*y - z*y*CosTheta + x*SinTheta;
1045 Result[8] = z*x - z*x*CosTheta + y*SinTheta;
1046 Result[9] = z*y - z*y*CosTheta - x*SinTheta;
1047 Result[10]= z*z + (x*x+y*y)*CosTheta;
1048 Result[3] = 0;
1049 Result[7] = 0;
1050 Result[11] = 0;
1051 Result[12] = 0;
1052 Result[13] = 0;
1053 Result[14] = 0;
1054 Result[15] = 1;
1055
1056 return Result;
1057}
1058
1059GLDOUBLE* mattranslate(GLDOUBLE* r, double dx, double dy, double dz)
1060{
1061 r[0] = r[5] = r[10] = r[15] = 1;
1062 r[1] = r[2] = r[3] = r[4] =
1063 r[6] = r[7] = r[8] = r[9] =
1064 r[11] = 0;
1065 r[12] = dx;
1066 r[13] = dy;
1067 r[14] = dz;
1068 return r;
1069}
1070GLDOUBLE* matscale(GLDOUBLE* r, double sx, double sy, double sz)
1071{
1072
1073 r[0] = r[5] = r[10] = r[15] =
1074 r[1] = r[2] = r[3] = r[4] =
1075 r[6] = r[7] = r[8] = r[9] =
1076 r[11] = r[12] = r[13] = r[14] = 0.0;
1077 r[0] = sx;
1078 r[5] = sy;
1079 r[10] = sz;
1080 return r;
1081}
1082
1083GLDOUBLE* matmultiplyFULL(GLDOUBLE* r, GLDOUBLE* mm , GLDOUBLE* nn)
1084{
1085 /* full 4x4, will do perspectives
1086 FLOPs 64 double: 4x4x4
1087 r = mm x nn
1088 */
1089 GLDOUBLE tm[16],tn[16];
1090 GLDOUBLE *m, *n;
1091 int i,j,k;
1092 /* prevent self-multiplication problems.*/
1093 m = mm;
1094 n = nn;
1095 if(r == m) {
1096 memcpy(tm,m,sizeof(GLDOUBLE)*16);
1097 m = tm;
1098 }
1099 if(r == n) {
1100 memcpy(tn,n,sizeof(GLDOUBLE)*16);
1101 n = tn;
1102 }
1103 if(0){
1104 if(1) for(i=0;i<3;i++){
1105 if(mm[i +12] != 0.0){
1106 double p = mm[i +12];
1107 printf("Ft[%d]%f ",i,p);
1108 }
1109 if(nn[i + 12] != 0.0){
1110 double p = nn[i +12];
1111 printf("FT[%d]%f ",i,p);
1112 }
1113 }
1114 if(1) for(i=0;i<3;i++){
1115 if(mm[i*4 + 3] != 0.0){
1116 double p = mm[i*4 + 3];
1117 printf("Fp[%d]%f ",i,p);
1118 }
1119 if(nn[i*4 + 3] != 0.0){
1120 double p = nn[i*4 + 3];
1121 printf("FP[%d]%f ",i,p);
1122 }
1123 }
1124 }
1125 /* assume 4x4 homgenous transform */
1126 for(i=0;i<4;i++)
1127 for(j=0;j<4;j++)
1128 {
1129 r[i*4+j] = 0.0;
1130 for(k=0;k<4;k++)
1131 r[i*4+j] += m[i*4+k]*n[k*4+j];
1132 }
1133 return r;
1134}
1135
1136GLDOUBLE* matmultiplyAFFINE(GLDOUBLE* r, GLDOUBLE* nn , GLDOUBLE* mm)
1137{
1138 /* AFFINE subset - ignores perspectives, use for MODELVIEW
1139 FLOPs 36 double: 3x3x4
1140 r = nn x mm
1141 */
1142 GLDOUBLE tm[16],tn[16];
1143 GLDOUBLE *m, *n;
1144 int i; //,j,k;
1145 /* prevent self-multiplication problems.*/
1146 m = mm;
1147 n = nn;
1148 if(r == m) {
1149 memcpy(tm,m,sizeof(GLDOUBLE)*16);
1150 m = tm;
1151 }
1152 if(r == n) {
1153 memcpy(tn,n,sizeof(GLDOUBLE)*16);
1154 n = tn;
1155 }
1156 if(0){
1157 if(0) for(i=0;i<3;i++){
1158 if(mm[i +12] != 0.0){
1159 double p = mm[i +12];
1160 printf("At[%d]%lf ",i,p);
1161 }
1162 if(nn[i + 12] != 0.0){
1163 double p = nn[i +12];
1164 printf("AT[%d]%lf ",i,p);
1165 }
1166 }
1167 if(1) for(i=0;i<3;i++){
1168 if(mm[i*4 + 3] != 0.0){
1169 double p = mm[i*4 + 3];
1170 printf("Ap[%d]%lf ",i,p);
1171 }
1172 if(nn[i*4 + 3] != 0.0){
1173 double p = nn[i*4 + 3];
1174 printf("AP[%d]%lf ",i,p);
1175 }
1176 }
1177 }
1178
1179
1180 /* this method ignors the perspectives */
1181 r[0] = m[0]*n[0] +m[4]*n[1] +m[8]*n[2];
1182 r[4] = m[0]*n[4] +m[4]*n[5] +m[8]*n[6];
1183 r[8] = m[0]*n[8] +m[4]*n[9] +m[8]*n[10];
1184 r[12]= m[0]*n[12]+m[4]*n[13]+m[8]*n[14] +m[12];
1185
1186 r[1] = m[1]*n[0] +m[5]*n[1] +m[9]*n[2];
1187 r[5] = m[1]*n[4] +m[5]*n[5] +m[9]*n[6];
1188 r[9] = m[1]*n[8] +m[5]*n[9] +m[9]*n[10];
1189 r[13]= m[1]*n[12]+m[5]*n[13]+m[9]*n[14] +m[13];
1190
1191 r[2] = m[2]*n[0]+ m[6]*n[1] +m[10]*n[2];
1192 r[6] = m[2]*n[4]+ m[6]*n[5] +m[10]*n[6];
1193 r[10]= m[2]*n[8]+ m[6]*n[9] +m[10]*n[10];
1194 r[14]= m[2]*n[12]+m[6]*n[13]+m[10]*n[14] +m[14];
1195
1196 r[3] = r[7] = r[11] = 0.0;
1197 r[15] = 1.0;
1198
1199 return r;
1200}
1201GLDOUBLE* matmultiply(GLDOUBLE* r, GLDOUBLE* mm , GLDOUBLE* nn){
1202 return matmultiplyFULL(r,mm,nn);
1203}
1204
1205float* matmultiply4f(float* r, float* mm , float* nn)
1206{
1207 /* FLOPs 64 float: N^3 = 4x4x4
1208 r = mm x nn
1209 */
1210 float tm[16],tn[16];
1211 float *m, *n;
1212 int i,j,k;
1213 /* prevent self-multiplication problems.*/
1214 m = mm;
1215 n = nn;
1216 if(r == m) {
1217 memcpy(tm,m,sizeof(float)*16);
1218 m = tm;
1219 }
1220 if(r == n) {
1221 memcpy(tn,n,sizeof(float)*16);
1222 n = tn;
1223 }
1224 /* assume 4x4 homgenous transform */
1225 for(i=0;i<4;i++)
1226 for(j=0;j<4;j++)
1227 {
1228 r[i*4+j] = 0.0;
1229 for(k=0;k<4;k++)
1230 r[i*4+j] += m[i*4+k]*n[k*4+j];
1231 }
1232 return r;
1233}
1234float* matmultiply3f(float* r, float* mm , float* nn)
1235{
1236 /* FLOPs 27 float: N^3 = 3x3x3
1237 r = mm x nn
1238 */
1239 float tm[9],tn[9];
1240 float *m, *n;
1241 int i,j,k;
1242 /* prevent self-multiplication problems.*/
1243 m = mm;
1244 n = nn;
1245 if(r == m) {
1246 memcpy(tm,m,sizeof(float)*9);
1247 m = tm;
1248 }
1249 if(r == n) {
1250 memcpy(tn,n,sizeof(float)*9);
1251 n = tn;
1252 }
1253 /* assume 4x4 homgenous transform */
1254 for(i=0;i<3;i++)
1255 for(j=0;j<3;j++)
1256 {
1257 r[i*3+j] = 0.0;
1258 for(k=0;k<3;k++)
1259 r[i*3+j] += m[i*3+k]*n[k*3+j];
1260 }
1261 return r;
1262}
1263float *axisangle_rotate3f(float* b, float *a, float *axisangle)
1264{
1265 /* http://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
1266 uses Rodrigues formula axisangle (axis,angle)
1267 somewhat expensive, so if tranforming many points with the same rotation,
1268 it might be more efficient to use another method (like axisangle -> matrix, then matrix transforms)
1269 b = a*cos(angle) + (axis cross a)*sin(theta) + axis*(axis dot a)*(1 - cos(theta))
1270 */
1271 float cosine, sine, cross[3], dot, theta, *axis, t1[3], t2[3],t3[3],t4[3];
1272 theta = axisangle[3];
1273 axis = axisangle;
1274 cosine = (float)cos(theta);
1275 sine = (float)sin(theta);
1276 veccross3f(cross,axis, a);
1277 dot = vecdot3f(axis, a);
1278 vecadd3f(b,vecscale3f(t1, a, cosine), vecadd3f(t2, vecscale3f(t3, cross, sine), vecscale3f(t4, axis, dot*(1.0f - cosine))));
1279 return b;
1280}
1281struct SFRotation *sfrotation_multiply(struct SFRotation* T, struct SFRotation *A, struct SFRotation *B);
1282float *axisangle_rotate4f(float* axisAngleC, float *axisAngleA, float *axisAngleB)
1283{
1284 if(1){
1285 //this is a float[4] version of / wrapper for sfrotation_multiply
1286 struct SFRotation A,B,C;
1287 veccopy4f(A.c,axisAngleA);
1288 veccopy4f(B.c,axisAngleB);
1289 sfrotation_multiply(&C,&A,&B);
1290 veccopy4f(axisAngleC,C.c);
1291 }else{
1292 // Dec 2017, dug9: I think the rodrigues chain is too hard for us:
1293 // https://math.stackexchange.com/questions/382760/composition-of-two-axis-angle-rotations
1294 // and its math looks a lot like quaternion math anyway, I've read its equivalent
1295 // http://www.mathoman.com/en/index.php/1537-axis-and-angle-of-the-composition-of-two-rotations
1296 // one way I can visualize: rotating a point by 2 rotations, then using the start and end points
1297 // to get an angle between them and axis perpendicular to the new angle
1298 // dug9's (untested) formula
1299 // (uses full angles, not the half-angles seen in rodrigues or quaternions, so likely wrong)
1300 // for axis angles C = A * B
1301 // 1. find an arbitrary point p1 (xyz) on the perpendicular plane to B's axis, on unit sphere
1302 // i) using a cross product trick:
1303 // a) find the largest compoent of B.axis {x,y,z}
1304 // b) exhange that compoent with any other component ie a2 = {y,x,z)
1305 // c) p1 = normalize( a2 cross B.axis )
1306 // problem: what if A.axis is right on p1? then A.angle will have no effect / be 'missed' in result
1307 // ii) perpendicular to both axes
1308 // perpaxis = B.axis cross A.axis
1309 // if(length(perpaxis) == 0)
1310 // axes are parallel, just add angle
1311 // C = (B.axis, B.angle + A.angle)
1312 // else
1313 // p1 = normalize(perpaxis)
1314 // 2. rotate point p1 by B -> p2. (It will still be on B's perpendicular plane)
1315 // 3. rotate point p2 by A -> p3. (now we have 2 points on a sphere, p1, p3)
1316 // 4. find the perpendicular axis shared by origin O (0,0,0), p1 and p3.
1317 // axis_sinealpha = (p1-O) cross (p3 - O) = p1 cross p3 (cross product is scaled by sin(alpha))
1318 // sine_alpha = length(axis_sinealpha)
1319 // axis = axis_sinealpha / sine_alpha
1320 // 5. find the cosine_alpha = p3 dot p1
1321 // 6. find the full circle angle
1322 // angle = atan2(sine_alpha,cosine_alpha)
1323 // 7. C = (axis,angle)
1324 // Hypothesis: substituting half-angles in A,B before beginning, and doubling the final angle in C
1325 // will give the quaternion/rodrigues equivalent full angle
1326 // not implemented.
1327 veccopy4f(axisAngleC,axisAngleA);
1328 }
1329 return axisAngleC; //so can chain
1330}
1331double *matidentity4d(double *b){
1332 // zeros a 4x4 and puts 1's down the diagonal to make a 4x4 identity matrix
1333 int i,j;
1334 double *mat[4];
1335 for(i=0;i<4;i++){
1336 mat[i] = &b[i*4];
1337 for(j=0;j<4;j++)
1338 mat[i][j] = 0.0;
1339 mat[i][i] = 1.0;
1340 }
1341 return b;
1342}
1343double *mattranslate4d(double *mat, double* xyz){
1344 // untested, want it to work like fw_glTranslated
1345 double mtemp[16];
1346 matidentity4d(mtemp);
1347 mtemp[12] = xyz[0];
1348 mtemp[13] = xyz[1];
1349 mtemp[14] = xyz[2];
1350 matmultiplyFULL(mat,mtemp,mat);
1351 return mat;
1352}
1353float *matidentity4f(float *b){
1354 // zeros a 4x4 and puts 1's down the diagonal to make a 4x4 identity matrix
1355 int i,j;
1356 float *mat[4];
1357 for(i=0;i<4;i++){
1358 mat[i] = &b[i*4];
1359 for(j=0;j<4;j++)
1360 mat[i][j] = 0.0f;
1361 mat[i][i] = 1.0f;
1362 }
1363 return b;
1364}
1365float *matidentity3f(float *b){
1366 // zeros a 4x4 and puts 1's down the diagonal to make a 4x4 identity matrix
1367 int i;
1368 for(i=0;i<9;i++) b[i] = 0.0f;
1369 for(i=0;i<3;i++) b[i*3 +i] = 1.0f;
1370 return b;
1371}
1372float *axisangle2matrix4f(float *b, float *axisangle){
1373 //untested as of july 2014
1374 int i; //,j;
1375 float *mat[4];
1376 for(i=0;i<4;i++)
1377 mat[i] = &b[i*4];
1378 matidentity4f(b);
1379 //rotate identity using axisangle
1380 for(i=0;i<3;i++){ //could be 4 here if there was anything on line 4, but with identity its 0 0 0 1
1381 axisangle_rotate3f(mat[i], mat[i], axisangle);
1382 }
1383 return b;
1384}
1385
1386void matrixFromAxisAngle4d(double *mat, double rangle, double x, double y, double z)
1387{
1388
1389 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/
1390 int i;
1391 double c, s, t;
1392 double *m[4];
1393 double tmp1;
1394 double tmp2;
1395
1396 c = cos(rangle);
1397 s = sin(rangle);
1398 t = 1.0 - c;
1399 //row indexes
1400 m[0] = &mat[0];
1401 m[1] = &mat[4];
1402 m[2] = &mat[8];
1403 m[3] = &mat[12];
1404 //identity
1405 for(i=0;i<16;i++) mat[i] = 0.0;
1406 for(i=0;i<4;i++) m[i][i] = 1.0;
1407 // if axis is not already normalised then uncomment this
1408 // double magnitude = Math.sqrt(a1.x*a1.x + a1.y*a1.y + a1.z*a1.z);
1409 // if (magnitude==0) throw error;
1410 // a1.x /= magnitude;
1411 // a1.y /= magnitude;
1412 // a1.z /= magnitude;
1413
1414 m[0][0] = c + x*x*t;
1415 m[1][1] = c + y*y*t;
1416 m[2][2] = c + z*z*t;
1417
1418
1419 tmp1 = x*y*t;
1420 tmp2 = z*s;
1421 m[1][0] = tmp1 + tmp2;
1422 m[0][1] = tmp1 - tmp2;
1423 tmp1 = x*z*t;
1424 tmp2 = y*s;
1425 m[2][0] = tmp1 - tmp2;
1426 m[0][2] = tmp1 + tmp2;
1427 tmp1 = y*z*t;
1428 tmp2 = x*s;
1429 m[2][1] = tmp1 + tmp2;
1430 m[1][2] = tmp1 - tmp2;
1431
1432}
1433
1434void rotate_v2v_axisAngled(double* axis, double* angle, double *orig, double *result)
1435{
1436 double cvl;
1437 double dv[3], iv[3], cv[3];
1438
1439 dv[0] = 0;
1440 dv[1] = 0;
1441 dv[2] = 0;
1442
1443 iv[0] = 0;
1444 iv[1] = 0;
1445 iv[2] = 0;
1446
1447 cv[0] = 0;
1448 cv[1] = 0;
1449 cv[2] = 0;
1450
1451 /* step 1 get sin of angle between 2 vectors using cross product rule: ||u x v|| = ||u||*||v||*sin(theta) */
1452 vecnormald(dv,orig); /*normalizes vector to unit length U -> u^ (length 1) */
1453 vecnormald(iv,result);
1454
1455 veccrossd(cv,dv,iv); /*the axis of rotation cv = dv X iv*/
1456 cvl = vecnormald(cv,cv); /* cvl = ||u x v|| / ||u^||*||v^|| = ||u x v|| = sin(theta)*/
1457 veccopyd(axis,cv);
1458
1459 /* if(cvl == 0) { */
1460 if(APPROX(cvl, 0)) {
1461 cv[2] = 1.0;
1462 }
1463 /* step 2 get cos of angle between 2 vectors using dot product rule: u dot v = ||u||*||v||*cos(theta) or cos(theta) = u dot v/( ||u|| ||v||)
1464 or, since U,V already unit length from being normalized, cos(theta) = u dot v */
1465 *angle = atan2(cvl,vecdotd(dv,iv)); /*the angle theta = arctan(rise/run) = atan2(sin_theta,cos_theta) in radians*/
1466}
1467/*puts dv back on iv - return the 4x4 rotation matrix that will rotate vector dv onto iv*/
1468double matrotate2v(GLDOUBLE* res, struct point_XYZ iv/*original*/, struct point_XYZ dv/*result*/) {
1469 struct point_XYZ cv;
1470 double cvl,a;
1471
1472 /* step 1 get sin of angle between 2 vectors using cross product rule: ||u x v|| = ||u||*||v||*sin(theta) */
1473 vecnormal(&dv,&dv); /*normalizes vector to unit length U -> u^ (length 1) */
1474 vecnormal(&iv,&iv);
1475
1476 veccross(&cv,dv,iv); /*the axis of rotation cv = dv X iv*/
1477 cvl = vecnormal(&cv,&cv); /* cvl = ||u x v|| / ||u^||*||v^|| = ||u x v|| = sin(theta)*/
1478 /* if(cvl == 0) { */
1479 if(APPROX(cvl, 0)) {
1480 cv.z = 1;
1481 }
1482 /* step 2 get cos of angle between 2 vectors using dot product rule: u dot v = ||u||*||v||*cos(theta) or cos(theta) = u dot v/( ||u|| ||v||)
1483 or, since U,V already unit length from being normalized, cos(theta) = u dot v */
1484 a = atan2(cvl,vecdot(&dv,&iv)); /*the angle theta = arctan(rise/run) = atan2(sin_theta,cos_theta) in radians*/
1485 /* step 3 convert rotation angle around unit directional vector of rotation into an equivalent rotation matrix 4x4 */
1486 matrotate(res,a,cv.x,cv.y,cv.z);
1487 return a;
1488}
1489double matrotate2vd(GLDOUBLE* res, double * iv/*original*/, double * dv/*result*/) {
1490 struct point_XYZ piv, pdv;
1491 double2pointxyz(&piv,iv);
1492 double2pointxyz(&pdv,dv);
1493 return matrotate2v(res,piv,pdv);
1494}
1495
1496#define SHOW_NONSINGULARS 0 //or 1 for noisy
1497/****
1498 * hacked from a graphics gem
1499 * Returned value:
1500 * TRUE if input matrix is nonsingular
1501 * FALSE otherwise
1502 *
1503 ***/
1504
1505BOOL matrix3x3_inverse_float(float *inn, float *outt)
1506{
1507 /*FLOPs 40 float: det3 12, 1/det 1, adj3x3 9x3=27 */
1508
1509 float det_1;
1510 float pos, /* neg, */ temp;
1511 float *in[3], *out[3];
1512
1513/*#define ACCUMULATE \
1514// if (temp >= 0.0) \
1515// pos += temp; \
1516// else \
1517 neg += temp;
1518*/
1519
1520#define ACCUMULATE pos += temp;
1521
1522//#define PRECISION_LIMIT 1.0e-7 //(1.0e-15)
1523 in[0] = &inn[0];
1524 in[1] = &inn[3];
1525 in[2] = &inn[6];
1526 out[0] = &outt[0];
1527 out[1] = &outt[3];
1528 out[2] = &outt[6];
1529
1530 /*
1531 * Calculate the determinant of submatrix A and determine if the
1532 * the matrix is singular as limited by the double precision
1533 * floating-point data representation.
1534 */
1535 pos = 0.0f; //neg = 0.0;
1536 temp = in[0][0] * in[1][1] * in[2][2];
1537 ACCUMULATE
1538 temp = in[0][1] * in[1][2] * in[2][0];
1539 ACCUMULATE
1540 temp = in[0][2] * in[1][0] * in[2][1];
1541 ACCUMULATE
1542 temp = -in[0][2] * in[1][1] * in[2][0];
1543 ACCUMULATE
1544 temp = -in[0][1] * in[1][0] * in[2][2];
1545 ACCUMULATE
1546 temp = -in[0][0] * in[1][2] * in[2][1];
1547 ACCUMULATE
1548 det_1 = pos; // + neg;
1549
1550 /* Is the submatrix A singular? */
1551 //if ((det_1 == 0.0) || (abs(det_1 / (pos - neg)) < PRECISION_LIMIT)) {
1552 if(APPROX(det_1,0.0f)){
1553
1554 /* Matrix M has no inverse */
1555
1556 if(SHOW_NONSINGULARS) fprintf (stderr, "affine_matrix4_inverse: singular matrix\n");
1557 return FALSE;
1558 }
1559
1560 else {
1561
1562 /* Calculate inverse(A) = adj(A) / det(A) */
1563 det_1 = 1.0f / det_1;
1564 out[0][0] = (in[1][1] * in[2][2] - in[1][2] * in[2][1] ) * det_1;
1565 out[1][0] = -(in[1][0] * in[2][2] - in[1][2] * in[2][0] ) * det_1;
1566 out[2][0] = (in[1][0] * in[2][1] - in[1][1] * in[2][0] ) * det_1;
1567 out[0][1] = -(in[0][1] * in[2][2] - in[0][2] * in[2][1] ) * det_1;
1568 out[1][1] = (in[0][0] * in[2][2] - in[0][2] * in[2][0] ) * det_1;
1569 out[2][1] = -(in[0][0] * in[2][1] - in[0][1] * in[2][0] ) * det_1;
1570 out[0][2] = (in[0][1] * in[1][2] - in[0][2] * in[1][1] ) * det_1;
1571 out[1][2] = -(in[0][0] * in[1][2] - in[0][2] * in[1][0] ) * det_1;
1572 out[2][2] = (in[0][0] * in[1][1] - in[0][1] * in[1][0] ) * det_1;
1573
1574 return TRUE;
1575 }
1576}
1577float * mat423f(float *out3x3, float *in4x4)
1578{
1579 int i,j;
1580 for(i=0;i<3;i++){
1581 for(j=0;j<3;j++)
1582 out3x3[i*3 + j] = in4x4[i*4 + j];
1583 }
1584 return out3x3;
1585}
1586float * matinverse3f(float *out3x3, float *in3x3)
1587{
1588 matrix3x3_inverse_float(in3x3,out3x3);
1589 return out3x3;
1590}
1591
1592float * transform3x3f(float *out3, float *in3, float *mat3x3){
1593 int i,j;
1594 float t3[3];
1595 memcpy(t3,in3,3*sizeof(float));
1596 for(i=0;i<3;i++){
1597 out3[i] = 0.0f;
1598 for(j=0;j<3;j++)
1599 out3[i] += t3[j]*mat3x3[j*3 + i];
1600 }
1601
1602 return out3;
1603}
1604
1605BOOL affine_matrix4x4_inverse_float(float *inn, float *outt)
1606{
1607 /*FLOPs 49 float: det3 12, 1/det 1, adj3x3 9x3=27, INV*T=9 */
1608 /* faithful transcription of GraphicsGem II, p.604, Wu
1609 use this for modelview matrix which has no perspectives (vs FULL 4x4 100 FLOPs)
1610 */
1611
1612 float det_1;
1613 float pos, /* neg, */ temp;
1614 float *in[4], *out[4];
1615
1616/*#define ACCUMULATE \
1617// if (temp >= 0.0) \
1618// pos += temp; \
1619// else \
1620 neg += temp;
1621*/
1622
1623#define ACCUMULATE pos += temp;
1624
1625//#define PRECISION_LIMIT 1.0e-7 //(1.0e-15)
1626 in[0] = &inn[0];
1627 in[1] = &inn[4];
1628 in[2] = &inn[8];
1629 in[3] = &inn[12];
1630 out[0] = &outt[0];
1631 out[1] = &outt[4];
1632 out[2] = &outt[8];
1633 out[3] = &outt[12];
1634
1635 /*
1636 * Calculate the determinant of submatrix A and determine if the
1637 * the matrix is singular as limited by the double precision
1638 * floating-point data representation.
1639 */
1640 pos = 0.0f; //neg = 0.0;
1641 temp = in[0][0] * in[1][1] * in[2][2];
1642 ACCUMULATE
1643 temp = in[0][1] * in[1][2] * in[2][0];
1644 ACCUMULATE
1645 temp = in[0][2] * in[1][0] * in[2][1];
1646 ACCUMULATE
1647 temp = -in[0][2] * in[1][1] * in[2][0];
1648 ACCUMULATE
1649 temp = -in[0][1] * in[1][0] * in[2][2];
1650 ACCUMULATE
1651 temp = -in[0][0] * in[1][2] * in[2][1];
1652 ACCUMULATE
1653 det_1 = pos; // + neg;
1654
1655 /* Is the submatrix A singular? */
1656 //if ((det_1 == 0.0) || (abs(det_1 / (pos - neg)) < PRECISION_LIMIT)) {
1657 if(APPROX(det_1,0.0f)){
1658
1659 /* Matrix M has no inverse */
1660 if(SHOW_NONSINGULARS) fprintf (stderr, "affine_matrix4_inverse: singular matrix\n");
1661 return FALSE;
1662 }
1663
1664 else {
1665
1666 /* Calculate inverse(A) = adj(A) / det(A) */
1667 det_1 = 1.0f / det_1;
1668 out[0][0] = (in[1][1] * in[2][2] - in[1][2] * in[2][1] ) * det_1;
1669 out[1][0] = -(in[1][0] * in[2][2] - in[1][2] * in[2][0] ) * det_1;
1670 out[2][0] = (in[1][0] * in[2][1] - in[1][1] * in[2][0] ) * det_1;
1671 out[0][1] = -(in[0][1] * in[2][2] - in[0][2] * in[2][1] ) * det_1;
1672 out[1][1] = (in[0][0] * in[2][2] - in[0][2] * in[2][0] ) * det_1;
1673 out[2][1] = -(in[0][0] * in[2][1] - in[0][1] * in[2][0] ) * det_1;
1674 out[0][2] = (in[0][1] * in[1][2] - in[0][2] * in[1][1] ) * det_1;
1675 out[1][2] = -(in[0][0] * in[1][2] - in[0][2] * in[1][0] ) * det_1;
1676 out[2][2] = (in[0][0] * in[1][1] - in[0][1] * in[1][0] ) * det_1;
1677
1678 /* Calculat -C * inverse(A) */
1679 out[3][0] = -(in[3][0] * out[0][0] + in[3][1]*out[1][0] + in[3][2]*out[2][0]);
1680 out[3][1] = -(in[3][0] * out[0][1] + in[3][1]*out[1][1] + in[3][2]*out[2][1]);
1681 out[3][2] = -(in[3][0] * out[0][2] + in[3][1]*out[1][2] + in[3][2]*out[2][2]);
1682
1683 /* Fill in last column */
1684 out[0][3] = out[1][3] = out[2][3] = 0.0f;
1685 out[3][3] = 1.0f;
1686 return TRUE;
1687 }
1688}
1689
1690BOOL affine_matrix4x4_inverse_double(double *inn, double *outt)
1691{
1692 /*FLOPs 49 double: det3 12, 1/det 1, adj3x3 9x3=27, INV*T=9 */
1693 /* faithful transcription of Graphics Gems II, p.604, Wu, FAST MATRIX INVERSION
1694 use this for modelview matrix which has no perspectives (vs FULL 4x4 inverse 102 FLOPs)
1695 */
1696 double det_1;
1697 double pos, /* neg, */ temp;
1698 double *in[4], *out[4];
1699
1700/*#define ACCUMULATE \
1701// if (temp >= 0.0) \
1702// pos += temp; \
1703// else \
1704 neg += temp;
1705*/
1706
1707#define ACCUMULATE pos += temp;
1708
1709//#define PRECISION_LIMIT 1.0e-7 //(1.0e-15)
1710 in[0] = &inn[0];
1711 in[1] = &inn[4];
1712 in[2] = &inn[8];
1713 in[3] = &inn[12];
1714 out[0] = &outt[0];
1715 out[1] = &outt[4];
1716 out[2] = &outt[8];
1717 out[3] = &outt[12];
1718
1719 /*
1720 * Calculate the determinant of submatrix A and determine if the
1721 * the matrix is singular as limited by the double precision
1722 * floating-point data representation.
1723 */
1724 pos = 0.0; //neg = 0.0;
1725 temp = in[0][0] * in[1][1] * in[2][2];
1726 ACCUMULATE
1727 temp = in[0][1] * in[1][2] * in[2][0];
1728 ACCUMULATE
1729 temp = in[0][2] * in[1][0] * in[2][1];
1730 ACCUMULATE
1731 temp = -in[0][2] * in[1][1] * in[2][0];
1732 ACCUMULATE
1733 temp = -in[0][1] * in[1][0] * in[2][2];
1734 ACCUMULATE
1735 temp = -in[0][0] * in[1][2] * in[2][1];
1736 ACCUMULATE
1737 det_1 = pos; // + neg;
1738
1739 /* Is the submatrix A singular? */
1740 //if ((det_1 == 0.0) || (abs(det_1 / (pos - neg)) < PRECISION_LIMIT)) {
1741 if(APPROX(det_1,0.0)){
1742
1743 /* Matrix M has no inverse */
1744 if(SHOW_NONSINGULARS) fprintf (stderr, "affine_matrix4_inverse: singular matrix\n");
1745 return FALSE;
1746 }
1747
1748 else {
1749
1750 /* Calculate inverse(A) = adj(A) / det(A) */
1751 det_1 = 1.0 / det_1;
1752 out[0][0] = (in[1][1] * in[2][2] - in[1][2] * in[2][1] ) * det_1;
1753 out[1][0] = -(in[1][0] * in[2][2] - in[1][2] * in[2][0] ) * det_1;
1754 out[2][0] = (in[1][0] * in[2][1] - in[1][1] * in[2][0] ) * det_1;
1755 out[0][1] = -(in[0][1] * in[2][2] - in[0][2] * in[2][1] ) * det_1;
1756 out[1][1] = (in[0][0] * in[2][2] - in[0][2] * in[2][0] ) * det_1;
1757 out[2][1] = -(in[0][0] * in[2][1] - in[0][1] * in[2][0] ) * det_1;
1758 out[0][2] = (in[0][1] * in[1][2] - in[0][2] * in[1][1] ) * det_1;
1759 out[1][2] = -(in[0][0] * in[1][2] - in[0][2] * in[1][0] ) * det_1;
1760 out[2][2] = (in[0][0] * in[1][1] - in[0][1] * in[1][0] ) * det_1;
1761
1762 /* Calculat -C * inverse(A) */
1763 out[3][0] = -(in[3][0] * out[0][0] + in[3][1]*out[1][0] + in[3][2]*out[2][0]);
1764 out[3][1] = -(in[3][0] * out[0][1] + in[3][1]*out[1][1] + in[3][2]*out[2][1]);
1765 out[3][2] = -(in[3][0] * out[0][2] + in[3][1]*out[1][2] + in[3][2]*out[2][2]);
1766
1767 /* Fill in last column */
1768 out[0][3] = out[1][3] = out[2][3] = 0.0;
1769 out[3][3] = 1.0;
1770 return TRUE;
1771 }
1772}
1773GLDOUBLE* matinverseAFFINE(GLDOUBLE* res, GLDOUBLE* mm){
1774 affine_matrix4x4_inverse_double(mm, res);
1775 return res;
1776}
1777GLDOUBLE* matinverseFULL(GLDOUBLE* res, GLDOUBLE* mm){
1778 matinverse98(res,mm);
1779 return res;
1780}
1781GLDOUBLE* matinverse(GLDOUBLE* res, GLDOUBLE* mm){
1782 matinverseFULL(res,mm);
1783 return res;
1784}
1785
1786
1787
1788#ifdef COMMENT
1789/*fast crossproduct using references, that checks for auto-assignments */
1790GLDOUBLE* veccross(GLDOUBLE* r, GLDOUBLE* v1, GLDOUBLE* v2)
1791{
1792 /*check against self-assignment. */
1793 if(r != v1) {
1794 if(r != v2) {
1795 r[0] = v1[1]*v2[2] - v1[2]*v2[1];
1796 r[1] = v1[2]*v2[0] - v1[0]*v2[2];
1797 r[2] = v1[0]*v2[1] - v1[1]*v2[0];
1798 } else { /* r == v2 */
1799 GLDOUBLE v2c[3] = {v2[0],v2[1],v2[2]};
1800 r[0] = v1[1]*v2c[2] - v1[2]*v2c[1];
1801 r[1] = v1[2]*v2c[0] - v1[0]*v2c[2];
1802 r[2] = v1[0]*v2c[1] - v1[1]*v2c[0];
1803 }
1804 } else { /* r == v1 */
1805 GLDOUBLE v1c[3] = {v1[0],v1[1],v1[2]};
1806 r[0] = v1c[1]*v2[2] - v1c[2]*v2[1];
1807 r[1] = v1c[2]*v2[0] - v1c[0]*v2[2];
1808 r[2] = v1c[0]*v2[1] - v1c[1]*v2[0];
1809 }
1810 return r;
1811}
1812
1813
1814
1815#endif
1816
1817
1818/*
1819 * apply a scale to the matrix given. Assumes matrix is valid...
1820 *
1821 */
1822
1823void scale_to_matrix (double *mat, struct point_XYZ *scale) {
1824/* copy the definitions from quaternion.h... */
1825#define MAT00 mat[0]
1826#define MAT01 mat[1]
1827#define MAT02 mat[2]
1828#if DJ_KEEP_COMPILER_WARNING
1829#define MAT03 mat[3]
1830#endif
1831#define MAT10 mat[4]
1832#define MAT11 mat[5]
1833#define MAT12 mat[6]
1834#if DJ_KEEP_COMPILER_WARNING
1835#define MAT13 mat[7]
1836#endif
1837#define MAT20 mat[8]
1838#define MAT21 mat[9]
1839#define MAT22 mat[10]
1840#if DJ_KEEP_COMPILER_WARNING
1841#define MAT23 mat[11]
1842#define MAT30 mat[12]
1843#define MAT31 mat[13]
1844#define MAT32 mat[14]
1845#define MAT33 mat[15]
1846#endif
1847
1848 MAT00 *=scale->x;
1849 MAT01 *=scale->x;
1850 MAT02 *=scale->x;
1851 MAT10 *=scale->y;
1852 MAT11 *=scale->y;
1853 MAT12 *=scale->y;
1854 MAT20 *=scale->z;
1855 MAT21 *=scale->z;
1856 MAT22 *=scale->z;
1857}
1858
1859static double identity[] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };
1860
1861void loadIdentityMatrix (double *mat) {
1862 memcpy((void *)mat, (void *)identity, sizeof(double)*16);
1863}
1864double *matcopy(double *r, double*mat){
1865 memcpy((void*)r, (void*)mat,sizeof(double)*16);
1866 return r;
1867}
1868float *matdouble2float4(float *rmat4, double *dmat4){
1869 int i;
1870 /* convert GLDOUBLE to float */
1871 for (i=0; i<16; i++) {
1872 rmat4[i] = (float)dmat4[i];
1873 }
1874 return rmat4;
1875}
1876void printmatrix3(GLDOUBLE *mat, char *description, int row_major){
1877 int i,j;
1878 printf("mat %s {\n",description);
1879 if(row_major){
1880 //prints in C row-major order, element numbers remain correct/same
1881 for(i = 0; i< 4; i++) {
1882 printf("mat [%2d-%2d] = ",i*4,(i*4)+3);
1883 for(j=0;j<4;j++)
1884 printf(" %lf ",mat[(i*4)+j]);
1885 printf("\n");
1886 }
1887 }
1888 if(!row_major){
1889 //prints in opengl column-major order, element numbers remain correct/same
1890 for(i=0;i<4;i++){
1891 printf("mat [%2d %2d %2d %2d] =",i,i+4,i+8,i+12);
1892 printf(" %lf %lf %lf %lf\n",mat[i],mat[i+4],mat[i+8],mat[i+12]);
1893 }
1894 }
1895 printf("}\n");
1896}
1897void printmatrix2(GLDOUBLE* mat,char* description ) {
1898 static int row_major = FALSE;
1899 printmatrix3(mat,description,row_major);
1900}
1901
1902float *veclerp3f(float *T, float *A, float *B, float alpha){
1903 int i;
1904 for(i=0;i<3;i++){
1905 T[i] = (1.0f - alpha)*A[i] + alpha*B[i];
1906 }
1907 return T;
1908}
1909float *veclerp2f(float *T, float *A, float *B, float alpha){
1910 int i;
1911 for(i=0;i<2;i++){
1912 T[i] = (1.0f - alpha)*A[i] + alpha*B[i];
1913 }
1914 return T;
1915}
1916double *veclerpd(double *T, double *A, double *B, double alpha){
1917 int i;
1918 for(i=0;i<3;i++){
1919 T[i] = (1.0f - alpha)*A[i] + alpha*B[i];
1920 }
1921 return T;
1922}
1923
1924void general_slerp(double *ret, double *p1, double *p2, int size, const double t)
1925{
1926 //not tested as of July16,2011
1927 //goal start slow, speed up in the middle, and slow down when stopping
1928 // (like a sine or cosine wave)
1929 //let omega = t*pi
1930 //then cos omega goes from 1 to -1 natively
1931 //we want scale0 to go from 1 to 0
1932 //scale0 = .5(1+cos(t*pi)) should be in the 1 to 0 range,
1933 //and be 'fastest' in the middle ie at pi/2
1934 //then scale1 = 1 - scale0
1935 double scale0, scale1, omega;
1936 int i;
1937 /* calculate coefficients */
1938 if (0) {
1939 //if ( t > .05 || t < .95 ) {
1940 /* standard case (SLERP) */
1941 omega = t*PI;
1942 scale0 = 0.5*(1.0 + cos(omega));
1943 scale1 = 1.0 - scale0;
1944 } else {
1945 /* p1 & p2 are very close, so do linear interpolation */
1946 scale0 = 1.0 - t;
1947 scale1 = t;
1948 }
1949 for(i=0;i<size;i++)
1950 ret[i] = scale0 * p1[i] + scale1 * p2[i];
1951}
1952void point_XYZ_slerp(struct point_XYZ *ret, struct point_XYZ *p1, struct point_XYZ *p2, const double t){
1953 double pret[3], pp1[3], pp2[3];
1954 pointxyz2double(pp1, p1);
1955 pointxyz2double(pp2, p2);
1956 general_slerp(pret, pp1, pp2, 3, t);
1957 double2pointxyz(ret,pret);
1958}