34#include <libFreeWRL.h>
36#include "../vrml_parser/Structs.h"
37#include "../main/headers.h"
39#include "LinearAlgebra.h"
41double angleNormalized(
double angle){
43 return atan2(sin(angle),cos(angle));
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);
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);
54void vecprint3db(
char *name,
double *p,
char *eol){
55 printf(
"%s %lf %lf %lf %s",name,p[0],p[1],p[2],eol);
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);
60double signd(
double val){
61 return val < 0.0 ? -1.0 : val > 0.0 ? 1.0 : 0;
63double * vecsignd(
double *b,
double *a){
65 for (i = 0; i<3; i++) b[i] = signd(a[i]);
68double * vecmuld(
double *c,
double *a,
double *b){
74double * vecsetd(
double *b,
double x,
double y,
double z){
75 b[0] = x, b[1] = y; b[2] = z;
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;
82float *double2float(
float *b,
const double *a,
int n){
84 for(i=0;i<n;i++) b[i] = (
float)a[i];
87double *float2double(
double *b,
float *a,
int n){
89 for(i=0;i<n;i++) b[i] = (
double)a[i];
92double * vecadd2d(
double *c,
double *a,
double *b){
98double *vecdif2d(
double *c,
double* a,
double *b){
103double veclength2d(
double *p ){
104 return sqrt(p[0]*p[0] + p[1]*p[1]);
106double vecdot2d(
double *a,
double *b){
107 return a[0]*b[0] + a[1]*b[1];
110double* vecscale2d(
double* r,
double* v,
double s){
116double vecnormal2d(
double *r,
double *v){
117 double ret = sqrt(vecdot2d(v,v));
119 if (APPROX(ret, 0))
return 0.;
120 vecscale2d(r,v,1./ret);
124float * vecadd2f(
float *c,
float *a,
float *b){
130float *vecdif2f(
float *c,
float* a,
float *b){
135float veclength2f(
float *p ){
136 return (
float) sqrt(p[0]*p[0] + p[1]*p[1]);
138float vecdot2f(
float *a,
float *b){
139 return a[0]*b[0] + a[1]*b[1];
142float* vecscale2f(
float* r,
float* v,
float s){
148float vecnormal2f(
float *r,
float *v){
149 float ret = (float)sqrt(vecdot2f(v,v));
151 if (APPROX(ret, 0))
return 0.0f;
152 vecscale2f(r,v,1.0f/ret);
160double *vecaddd(
double *c,
double* a,
double *b)
167float *vecadd3f(
float *c,
float *a,
float *b)
174double *vecdifd(
double *c,
double* a,
double *b)
181double *vecdif4d(
double *c,
double* a,
double *b)
189float *vecdif3f(
float *c,
float *a,
float *b)
196double *veccopyd(
double *c,
double *a)
203double *vecnegated(
double *b,
double *a)
210double *vecswizzle2d(
double *inout ){
211 double tmp = inout[0];
217double *veccopy4d(
double *c,
double *a)
227float *veccopy3f(
float *b,
float *a)
234float *vecnegate3f(
float *b,
float *a)
241int vecsame2f(
float *b,
float *a){
242 return a[0] == b[0] && a[1] == b[1] ? TRUE : FALSE;
244float *veccopy2f(
float *b,
float *a)
250float *vecset2f(
float *b,
float x,
float y)
255double * veccrossd(
double *c,
double *a,
double *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];
265double veclengthd(
double *p )
267 return sqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2]);
269double veclength4d(
double *p )
271 return sqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2] + p[3]*p[3]);
273double vecdotd(
double *a,
double *b)
275 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
277double* vecscaled(
double* r,
double* v,
double s)
285double vecnormald(
double *r,
double *v)
287 double ret = sqrt(vecdotd(v,v));
289 if (APPROX(ret, 0))
return 0.;
290 vecscaled(r,v,1./ret);
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;
300float *veccross3f(
float *c,
float *a,
float *b)
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];
310 return (
float) sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
312float vecdot3f(
float *a,
float *b )
315 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
317float vecdot4f(
float *a,
float *b )
319 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + + a[3]*b[3];
321float *vecset3f(
float *b,
float x,
float y,
float z)
323 b[0] = x; b[1] = y; b[2] = z;
326float *vecset4f(
float *b,
float x,
float y,
float z,
float a)
328 b[0] = x; b[1] = y; b[2] = z; b[3] = a;
331float veclength3f(
float *a){
332 return (
float)sqrt(vecdot3f(a, a));
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) ) );
340float *veccopy4f(
float *b,
float *a)
348float calc_angle_between_two_vectors3f(
float * a,
float * b){
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);
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);
371 if (APPROX(scalar, 0)) {
372 return (
float) (M_PI/2.0);
375 if ( (length_a <= 0) || (length_b <= 0)){
376 printf(
"Divide by 0 in calc_angle_between_two_vectors(): No can do! \n");
380 temp = scalar /(length_a * length_b);
385 if ((temp >= 1) || (temp <= -1)){
386 if (temp < 0.0f)
return 3.141526f;
389 return (
float) acos(temp);
392int vecsame3f(
float *a,
float *b){
395 if(a[i] != b[i]) isame = FALSE;
398int vecsame4f(
float *a,
float *b){
401 if(a[i] != b[i]) isame = FALSE;
404int vecsamed(
double *a,
double *b){
407 if(a[i] != b[i]) isame = FALSE;
413 GLDOUBLE ret = sqrt(vecdot(v,v));
415 if (APPROX(ret, 0))
return 0.;
416 vecscale(r,v,1./ret);
419float vecnormsquared3f(
float *a){
420 return a[0] * a[0] + a[1] * a[1] + a[2] * a[2];
422float *vecscale3f(
float *b,
float *a,
float scale){
428float *vecscale4f(
float *b,
float *a,
float scale){
435float *vecmult3f(
float *c,
float *a,
float *b){
438 for(;i<3;i++) c[i] = a[i]*b[i];
441float *vecmult2f(
float *c,
float *a,
float *b){
444 for(;i<2;i++) c[i] = a[i]*b[i];
448float *vecnormalize3f(
float *b,
float *a)
450 float norm = veclength3f(a);
451 if (APPROX(norm, 0.0f)){
452 b[0] = 1.0f; b[1] = 0.0f; b[2] = 0.0f;
454 vecscale3f(b, a, 1.0f / norm);
460GLDOUBLE det3x3(GLDOUBLE* data)
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];
464float det3f(
float *a,
float *b,
float *c)
468 return vecdot3f(a,veccross3f(temp,b, c));
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];
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];
490GLDOUBLE* transformUPPER3X3d(
double *r,
double *a,
const GLDOUBLE* b){
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];
501 return transform(r,a,b);
503GLDOUBLE* pointxyz2double(
double* r,
struct point_XYZ *p){
504 r[0] = p->x; r[1] = p->y; r[2] = p->z;
508 r->x = p[0]; r->y = p[1]; r->z = p[2];
511double * matrixAFFINE2RotationMatrix(
double* rotmat,
double *fullmat){
519 double sx,sy,sz, pp[3], q[3],p[3],d[3], matinv[16], matt[16], matr[16], mats[16];
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);
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);
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));
543 matscale(mats,sx,sy,sz);
544 matmultiplyAFFINE(rotmat,mats,matr);
550double *transformAFFINEd(
double *r,
double *a,
const GLDOUBLE* mat){
553 double2pointxyz(&pa,a);
554 transformAFFINE(&pr,&pa,mat);
555 pointxyz2double(r,&pr);
558double *transformFULL4d(
double *r,
double *a,
double *mat){
561 for (i=0; i<4; i++) {
570float* transformf(
float* r,
const float* a,
const GLDOUBLE* b)
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]);
580 tmp[0] =a[0]; tmp[1] = a[1]; tmp[2] = a[2];
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]);
587float* matmultvec4f(
float* r4,
float *mat4,
float* a4 )
591 memcpy(t4,a4,4*
sizeof(
float));
596 r4[i] += b[i][j]*t4[j];
600float* vecmultmat4f_broken(
float* r4,
float* a4,
float *mat4 )
604 memcpy(t4,a4,4*
sizeof(
float));
609 r4[i] += t4[j]*b[j][i];
613float* vecmultmat4f(
float* r4,
float* a4,
float *mat4 )
617 memcpy(t4,a4,4*
sizeof(
float));
626float* matmultvec3f(
float* r3,
float *mat3,
float* a3 )
630 memcpy(t3,a3,3*
sizeof(
float));
635 r3[i] += b[i][j]*t3[j];
639float* vecmultmat3f(
float* r3,
float* a3,
float *mat3 )
643 memcpy(t3,a3,3*
sizeof(
float));
648 r3[i] += t3[j]*b[j][i];
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;
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;
682 return (a->x*b->x) + (a->y*b->y) + (a->z*b->z);
701double closest_point_of_segment_to_y_axis_segment(
double y1,
double y2,
struct point_XYZ p1,
struct point_XYZ p2) {
703 double imin = (y1- p1.y) / (p2.y - p1.y);
704 double imax = (y2- p1.y) / (p2.y - p1.y);
707 double x12 = (p1.x - p2.x);
708 double z12 = (p1.z - p2.z);
709 double q = ( x12*x12 + z12*z12 );
712 double i = ((APPROX(q, 0)) ? 0 : (p1.x * x12 + p1.z * z12) / q);
714 printf(
"imin=%f, imax=%f => ",imin,imax);
723 if(imin < 0) imin = 0;
724 if(imax > 1) imax = 1;
726 printf(
"imin=%f, imax=%f\n",imin,imax);
729 if(i < imin) i = imin;
730 if(i > imax) i = imax;
734BOOL line_intersect_line_3f(
float *p1,
float *v1,
float *p2,
float *v2,
float *t,
float *s,
float *x1,
float *x2)
742 float t1[3], cross[3];
743 float crosslength2, ss, tt;
744 veccross3f(cross, v1, v2);
745 crosslength2 = vecnormsquared3f(cross);
746 if (APPROX(crosslength2, 0.0f))
return FALSE;
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;
751 vecadd3f(x1, p1, vecscale3f(t1, v1, tt));
753 vecadd3f(x2, p2, vecscale3f(t1, v2, ss));
761BOOL line_intersect_planed_3f(
float *p,
float *v,
float *N,
float d,
float *pi,
float *t)
767 float t1[3], t2[3], nd, tt;
769 if (APPROX(nd, 0.0f))
return FALSE;
770 tt = -(d + vecdot3f(N, p)) / nd;
771 vecadd3f(t2, p, vecscale3f(t1, v, tt));
773 if (pi) veccopy3f(pi, t2);
776BOOL line_intersect_plane_3f(
float *p,
float *v,
float *N,
float *pp,
float *pi,
float *t)
780 return line_intersect_planed_3f(p, v, N, d, pi, t);
783BOOL line_intersect_cylinder_3f(
float *p,
float *v,
float radius,
float *pi)
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;
796 if (APPROX(a, 0.0f))
return FALSE;
802 float sol2 = (-b-(float) sqrt(und))/2;
804 vecadd3f(pp, p, vecscale3f(t2, v, sol));
805 if (pi) veccopy3f(pi, pp);
835 if(fabs(n.x) <= fabs(n.y) && fabs(n.x) <= fabs(n.z)) {
840 j->x = n.y*n.y + n.z*n.z;
843 }
else if(fabs(n.y) <= fabs(n.x) && fabs(n.y) <= fabs(n.z)) {
849 j->y = n.x*n.x + n.z*n.z;
858 j->z = n.x*n.x + n.y*n.y;
863GLDOUBLE* mattranspose(GLDOUBLE* res, GLDOUBLE* mm)
871 memcpy(mcpy,m,
sizeof(GLDOUBLE)*16);
875 for (i = 0; i < 4; i++) {
879 for (j = 0; j < 4; j++) {
880 res[i*4+j] = m[j*4+i];
886float* mattranspose4f(
float* res,
float* mm)
894 memcpy(mcpy,m,
sizeof(
float)*16);
898 for (i = 0; i < 4; i++) {
899 for (j = 0; j < 4; j++) {
900 res[i*4+j] = m[j*4+i];
905float* mattranspose3f(
float* res,
float* mm)
913 memcpy(mcpy,m,
sizeof(
float)*9);
917 for (i = 0; i < 3; i++) {
918 for (j = 0; j < 3; j++) {
919 res[i*3+j] = m[j*3+i];
925GLDOUBLE* matinverse98(GLDOUBLE* res, GLDOUBLE* mm)
937 memcpy(mcpy,m,
sizeof(GLDOUBLE)*16);
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;
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;
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;
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;
968float* matinverse4f(
float* res,
float* mm)
978 memcpy(mcpy,m,
sizeof(
float)*16);
982 Deta = det3f(&m[0],&m[4],&m[8]);
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;
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;
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;
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;
1011 VECDIFF(*p2,*p1,v1);
1012 VECDIFF(*p3,*p1,v2);
1030 return polynormal(r,pp+0,pp+1,pp+2);
1034GLDOUBLE* matrotate(GLDOUBLE* Result,
double Theta,
double x,
double y,
double z)
1036 GLDOUBLE CosTheta = cos(Theta);
1037 GLDOUBLE SinTheta = sin(Theta);
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;
1059GLDOUBLE* mattranslate(GLDOUBLE* r,
double dx,
double dy,
double dz)
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] =
1070GLDOUBLE* matscale(GLDOUBLE* r,
double sx,
double sy,
double sz)
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;
1083GLDOUBLE* matmultiplyFULL(GLDOUBLE* r, GLDOUBLE* mm , GLDOUBLE* nn)
1089 GLDOUBLE tm[16],tn[16];
1096 memcpy(tm,m,
sizeof(GLDOUBLE)*16);
1100 memcpy(tn,n,
sizeof(GLDOUBLE)*16);
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);
1109 if(nn[i + 12] != 0.0){
1110 double p = nn[i +12];
1111 printf(
"FT[%d]%f ",i,p);
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);
1119 if(nn[i*4 + 3] != 0.0){
1120 double p = nn[i*4 + 3];
1121 printf(
"FP[%d]%f ",i,p);
1131 r[i*4+j] += m[i*4+k]*n[k*4+j];
1136GLDOUBLE* matmultiplyAFFINE(GLDOUBLE* r, GLDOUBLE* nn , GLDOUBLE* mm)
1142 GLDOUBLE tm[16],tn[16];
1149 memcpy(tm,m,
sizeof(GLDOUBLE)*16);
1153 memcpy(tn,n,
sizeof(GLDOUBLE)*16);
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);
1162 if(nn[i + 12] != 0.0){
1163 double p = nn[i +12];
1164 printf(
"AT[%d]%lf ",i,p);
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);
1172 if(nn[i*4 + 3] != 0.0){
1173 double p = nn[i*4 + 3];
1174 printf(
"AP[%d]%lf ",i,p);
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];
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];
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];
1196 r[3] = r[7] = r[11] = 0.0;
1201GLDOUBLE* matmultiply(GLDOUBLE* r, GLDOUBLE* mm , GLDOUBLE* nn){
1202 return matmultiplyFULL(r,mm,nn);
1205float* matmultiply4f(
float* r,
float* mm ,
float* nn)
1210 float tm[16],tn[16];
1217 memcpy(tm,m,
sizeof(
float)*16);
1221 memcpy(tn,n,
sizeof(
float)*16);
1230 r[i*4+j] += m[i*4+k]*n[k*4+j];
1234float* matmultiply3f(
float* r,
float* mm ,
float* nn)
1246 memcpy(tm,m,
sizeof(
float)*9);
1250 memcpy(tn,n,
sizeof(
float)*9);
1259 r[i*3+j] += m[i*3+k]*n[k*3+j];
1263float *axisangle_rotate3f(
float* b,
float *a,
float *axisangle)
1271 float cosine, sine, cross[3], dot, theta, *axis, t1[3], t2[3],t3[3],t4[3];
1272 theta = axisangle[3];
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))));
1282float *axisangle_rotate4f(
float* axisAngleC,
float *axisAngleA,
float *axisAngleB)
1287 veccopy4f(A.c,axisAngleA);
1288 veccopy4f(B.c,axisAngleB);
1289 sfrotation_multiply(&C,&A,&B);
1290 veccopy4f(axisAngleC,C.c);
1327 veccopy4f(axisAngleC,axisAngleA);
1331double *matidentity4d(
double *b){
1343double *mattranslate4d(
double *mat,
double* xyz){
1346 matidentity4d(mtemp);
1350 matmultiplyFULL(mat,mtemp,mat);
1353float *matidentity4f(
float *b){
1365float *matidentity3f(
float *b){
1368 for(i=0;i<9;i++) b[i] = 0.0f;
1369 for(i=0;i<3;i++) b[i*3 +i] = 1.0f;
1372float *axisangle2matrix4f(
float *b,
float *axisangle){
1381 axisangle_rotate3f(mat[i], mat[i], axisangle);
1386void matrixFromAxisAngle4d(
double *mat,
double rangle,
double x,
double y,
double z)
1405 for(i=0;i<16;i++) mat[i] = 0.0;
1406 for(i=0;i<4;i++) m[i][i] = 1.0;
1414 m[0][0] = c + x*x*t;
1415 m[1][1] = c + y*y*t;
1416 m[2][2] = c + z*z*t;
1421 m[1][0] = tmp1 + tmp2;
1422 m[0][1] = tmp1 - tmp2;
1425 m[2][0] = tmp1 - tmp2;
1426 m[0][2] = tmp1 + tmp2;
1429 m[2][1] = tmp1 + tmp2;
1430 m[1][2] = tmp1 - tmp2;
1434void rotate_v2v_axisAngled(
double* axis,
double* angle,
double *orig,
double *result)
1437 double dv[3], iv[3], cv[3];
1452 vecnormald(dv,orig);
1453 vecnormald(iv,result);
1455 veccrossd(cv,dv,iv);
1456 cvl = vecnormald(cv,cv);
1460 if(APPROX(cvl, 0)) {
1465 *angle = atan2(cvl,vecdotd(dv,iv));
1476 veccross(&cv,dv,iv);
1477 cvl = vecnormal(&cv,&cv);
1479 if(APPROX(cvl, 0)) {
1484 a = atan2(cvl,vecdot(&dv,&iv));
1486 matrotate(res,a,cv.x,cv.y,cv.z);
1489double matrotate2vd(GLDOUBLE* res,
double * iv,
double * dv) {
1491 double2pointxyz(&piv,iv);
1492 double2pointxyz(&pdv,dv);
1493 return matrotate2v(res,piv,pdv);
1496#define SHOW_NONSINGULARS 0
1505BOOL matrix3x3_inverse_float(
float *inn,
float *outt)
1511 float *in[3], *out[3];
1520#define ACCUMULATE pos += temp;
1536 temp = in[0][0] * in[1][1] * in[2][2];
1538 temp = in[0][1] * in[1][2] * in[2][0];
1540 temp = in[0][2] * in[1][0] * in[2][1];
1542 temp = -in[0][2] * in[1][1] * in[2][0];
1544 temp = -in[0][1] * in[1][0] * in[2][2];
1546 temp = -in[0][0] * in[1][2] * in[2][1];
1552 if(APPROX(det_1,0.0f)){
1556 if(SHOW_NONSINGULARS) fprintf (stderr,
"affine_matrix4_inverse: singular matrix\n");
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;
1577float * mat423f(
float *out3x3,
float *in4x4)
1582 out3x3[i*3 + j] = in4x4[i*4 + j];
1586float * matinverse3f(
float *out3x3,
float *in3x3)
1588 matrix3x3_inverse_float(in3x3,out3x3);
1592float * transform3x3f(
float *out3,
float *in3,
float *mat3x3){
1595 memcpy(t3,in3,3*
sizeof(
float));
1599 out3[i] += t3[j]*mat3x3[j*3 + i];
1605BOOL affine_matrix4x4_inverse_float(
float *inn,
float *outt)
1614 float *in[4], *out[4];
1623#define ACCUMULATE pos += temp;
1641 temp = in[0][0] * in[1][1] * in[2][2];
1643 temp = in[0][1] * in[1][2] * in[2][0];
1645 temp = in[0][2] * in[1][0] * in[2][1];
1647 temp = -in[0][2] * in[1][1] * in[2][0];
1649 temp = -in[0][1] * in[1][0] * in[2][2];
1651 temp = -in[0][0] * in[1][2] * in[2][1];
1657 if(APPROX(det_1,0.0f)){
1660 if(SHOW_NONSINGULARS) fprintf (stderr,
"affine_matrix4_inverse: singular matrix\n");
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;
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]);
1684 out[0][3] = out[1][3] = out[2][3] = 0.0f;
1690BOOL affine_matrix4x4_inverse_double(
double *inn,
double *outt)
1698 double *in[4], *out[4];
1707#define ACCUMULATE pos += temp;
1725 temp = in[0][0] * in[1][1] * in[2][2];
1727 temp = in[0][1] * in[1][2] * in[2][0];
1729 temp = in[0][2] * in[1][0] * in[2][1];
1731 temp = -in[0][2] * in[1][1] * in[2][0];
1733 temp = -in[0][1] * in[1][0] * in[2][2];
1735 temp = -in[0][0] * in[1][2] * in[2][1];
1741 if(APPROX(det_1,0.0)){
1744 if(SHOW_NONSINGULARS) fprintf (stderr,
"affine_matrix4_inverse: singular matrix\n");
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;
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]);
1768 out[0][3] = out[1][3] = out[2][3] = 0.0;
1773GLDOUBLE* matinverseAFFINE(GLDOUBLE* res, GLDOUBLE* mm){
1774 affine_matrix4x4_inverse_double(mm, res);
1777GLDOUBLE* matinverseFULL(GLDOUBLE* res, GLDOUBLE* mm){
1778 matinverse98(res,mm);
1781GLDOUBLE* matinverse(GLDOUBLE* res, GLDOUBLE* mm){
1782 matinverseFULL(res,mm);
1790GLDOUBLE* veccross(GLDOUBLE* r, GLDOUBLE* v1, GLDOUBLE* 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];
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];
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];
1823void scale_to_matrix (
double *mat,
struct point_XYZ *scale) {
1828#if DJ_KEEP_COMPILER_WARNING
1834#if DJ_KEEP_COMPILER_WARNING
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]
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 };
1861void loadIdentityMatrix (
double *mat) {
1862 memcpy((
void *)mat, (
void *)identity,
sizeof(
double)*16);
1864double *matcopy(
double *r,
double*mat){
1865 memcpy((
void*)r, (
void*)mat,
sizeof(
double)*16);
1868float *matdouble2float4(
float *rmat4,
double *dmat4){
1871 for (i=0; i<16; i++) {
1872 rmat4[i] = (float)dmat4[i];
1876void printmatrix3(GLDOUBLE *mat,
char *description,
int row_major){
1878 printf(
"mat %s {\n",description);
1881 for(i = 0; i< 4; i++) {
1882 printf(
"mat [%2d-%2d] = ",i*4,(i*4)+3);
1884 printf(
" %lf ",mat[(i*4)+j]);
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]);
1897void printmatrix2(GLDOUBLE* mat,
char* description ) {
1898 static int row_major = FALSE;
1899 printmatrix3(mat,description,row_major);
1902float *veclerp3f(
float *T,
float *A,
float *B,
float alpha){
1905 T[i] = (1.0f - alpha)*A[i] + alpha*B[i];
1909float *veclerp2f(
float *T,
float *A,
float *B,
float alpha){
1912 T[i] = (1.0f - alpha)*A[i] + alpha*B[i];
1916double *veclerpd(
double *T,
double *A,
double *B,
double alpha){
1919 T[i] = (1.0f - alpha)*A[i] + alpha*B[i];
1924void general_slerp(
double *ret,
double *p1,
double *p2,
int size,
const double t)
1935 double scale0, scale1, omega;
1942 scale0 = 0.5*(1.0 + cos(omega));
1943 scale1 = 1.0 - scale0;
1950 ret[i] = scale0 * p1[i] + scale1 * p2[i];
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);