34#include <libFreeWRL.h>
36#include "../vrml_parser/Structs.h"
37#include "../main/headers.h"
39#include "LinearAlgebra.h"
41float fclamp(
float fval,
float fstart,
float fend) {
43 fret = fval > fend? fend : fval;
44 fret = fret < fstart ? fstart : fret;
47float *vecclamp2f(
float *fval,
float *fstart,
float *fend){
50 if(fstart[i] <= fend[i])
51 fval[i] = fclamp(fval[i],fstart[i],fend[i]);
55float *vecclamp3f(
float *fval,
float *fstart,
float *fend){
58 if(fstart[i] <= fend[i])
59 fval[i] = fclamp(fval[i],fstart[i],fend[i]);
63float *fvecclamp3f(
float *fval,
float fstart,
float fend){
67 fval[i] = fclamp(fval[i],fstart,fend);
72int approx3f(
float *a,
float *b){
73 float tol = 0.00000001;
76 iret = iret && (fabs(a[i] - b[i]) < tol) ? iret : FALSE;
80int approx4f(
float *a,
float *b){
81 float tol = 0.00000001;
84 iret = iret && (fabs(a[i] - b[i]) < tol) ? iret : FALSE;
91double angleNormalized(
double angle){
93 return atan2(sin(angle),cos(angle));
97#define DJ_KEEP_COMPILER_WARNING 0
98void vecprint3fb(
char *name,
float *p,
char *eol){
99 printf(
"%s %f %f %f %s",name,p[0],p[1],p[2],eol);
101void vecprint4fb(
char *name,
float *p,
char *eol){
102 printf(
"%s %f %f %f %f %s",name,p[0],p[1],p[2],p[3],eol);
104void vecprint3db(
char *name,
double *p,
char *eol){
105 printf(
"%s %lf %lf %lf %s",name,p[0],p[1],p[2],eol);
107void vecprint4db(
char *name,
double *p,
char *eol){
108 printf(
"%s %lf %lf %lf %lf %s",name,p[0],p[1],p[2],p[3],eol);
110double signd(
double val){
111 return val < 0.0 ? -1.0 : val > 0.0 ? 1.0 : 0;
113double * vecsignd(
double *b,
double *a){
115 for (i = 0; i<3; i++) b[i] = signd(a[i]);
118double * vecmuld(
double *c,
double *a,
double *b){
124double * vecsetd(
double *b,
double x,
double y,
double z){
125 b[0] = x, b[1] = y; b[2] = z;
128double* vecset2d(
double* b,
double x,
double y) {
133double * vecset4d(
double *b,
double x,
double y,
double z,
double a){
134 b[0] = x, b[1] = y; b[2] = z; b[3] = a;
137float *double2float(
float *b,
const double *a,
int n){
139 for(i=0;i<n;i++) b[i] = (
float)a[i];
142double *float2double(
double *b,
float *a,
int n){
144 for(i=0;i<n;i++) b[i] = (
double)a[i];
147double * vecadd2d(
double *c,
double *a,
double *b){
153double *vecdif2d(
double *c,
double* a,
double *b){
158double* veccopy2d(
double* c,
double* a) {
164double veclength2d(
double *p ){
165 return sqrt(p[0]*p[0] + p[1]*p[1]);
167double vecdot2d(
double *a,
double *b){
168 return a[0]*b[0] + a[1]*b[1];
171double* vecscale2d(
double* r,
double* v,
double s){
177double vecnormal2d(
double *r,
double *v){
178 double ret = sqrt(vecdot2d(v,v));
180 if (APPROX(ret, 0))
return 0.;
181 vecscale2d(r,v,1./ret);
185float * vecadd2f(
float *c,
float *a,
float *b){
191float *vecdif2f(
float *c,
float* a,
float *b){
196float veclength2f(
float *p ){
197 return (
float) sqrt(p[0]*p[0] + p[1]*p[1]);
199float vecdot2f(
float *a,
float *b){
200 return a[0]*b[0] + a[1]*b[1];
203float* vecscale2f(
float* r,
float* v,
float s){
209float vecnormal2f(
float *r,
float *v){
210 float ret = (float)sqrt(vecdot2f(v,v));
212 if (APPROX(ret, 0))
return 0.0f;
213 vecscale2f(r,v,1.0f/ret);
221double *vecaddd(
double *c,
double* a,
double *b)
228float *vecadd3f(
float *c,
float *a,
float *b)
235double *vecdifd(
double *c,
double* a,
double *b)
242double *vecdif4d(
double *c,
double* a,
double *b)
250float *vecdif3f(
float *c,
float *a,
float *b)
257double *veccopyd(
double *c,
double *a)
264double *vecnegated(
double *b,
double *a)
271double *vecswizzle2d(
double *inout ){
272 double tmp = inout[0];
278double *veccopy4d(
double *c,
double *a)
288float *veccopy3f(
float *b,
float *a)
295float *vecnegate3f(
float *b,
float *a)
302int vecsame2f(
float *b,
float *a){
303 return a[0] == b[0] && a[1] == b[1] ? TRUE : FALSE;
305float *veccopy2f(
float *b,
float *a)
311float *vecset2f(
float *b,
float x,
float y)
316double * veccrossd(
double *c,
double *a,
double *b)
321 c[0] = aa[1] * bb[2] - aa[2] * bb[1];
322 c[1] = aa[2] * bb[0] - aa[0] * bb[2];
323 c[2] = aa[0] * bb[1] - aa[1] * bb[0];
326double veclengthd(
double *p )
328 return sqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2]);
330double veclength4d(
double *p )
332 return sqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2] + p[3]*p[3]);
334double vecdotd(
double *a,
double *b)
336 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
338double* vecscaled(
double* r,
double* v,
double s)
346double vecnormald(
double *r,
double *v)
348 double ret = sqrt(vecdotd(v,v));
350 if (APPROX(ret, 0))
return 0.;
351 vecscaled(r,v,1./ret);
357 c->x = a.y * b.z - a.z * b.y;
358 c->y = a.z * b.x - a.x * b.z;
359 c->z = a.x * b.y - a.y * b.x;
361float *veccross3f(
float *c,
float *a,
float *b)
364 c[0] = a[1]*b[2] - a[2]*b[1];
365 c[1] = a[2]*b[0] - a[0]*b[2];
366 c[2] = a[0]*b[1] - a[1]*b[0];
371 return (
float) sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
373float vecdot3f(
float *a,
float *b )
376 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
378float vecdot4f(
float *a,
float *b )
380 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + + a[3]*b[3];
382double vecdot4d(
double* a,
double* b)
384 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + +a[3] * b[3];
387float *vecset3f(
float *b,
float x,
float y,
float z)
389 b[0] = x; b[1] = y; b[2] = z;
392float *vecset4f(
float *b,
float x,
float y,
float z,
float a)
394 b[0] = x; b[1] = y; b[2] = z; b[3] = a;
397float veclength3f(
float *a){
398 return (
float)sqrt(vecdot3f(a, a));
402 return acos((V1->x*V2->x + V1->y*V2->y +V1->z*V2->z) /
403 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) ) );
405float vecangle2f(
float * V1,
float * V2) {
407 float det, dot, angle;
408 dot = V1[0]*V2[0] + V1[1]*V2[1];
409 det = V1[0]*V2[1] - V1[1]*V2[0];
410 angle = atan2(det, dot);
414float *veccopy4f(
float *b,
float *a)
422float calc_angle_between_two_vectors3f(
float * a,
float * b){
424 float an[3], bn[3], dotf, anglef, flen;
425 flen = veclength3f(a);
426 if(flen <= 0.0f)
return 0.0f;
427 flen = veclength3f(b);
428 if(flen <= 0.0f)
return 0.0f;
429 vecnormalize3f(an,a);
430 vecnormalize3f(bn,b);
431 dotf = vecdot3f(an,bn);
437 float length_a, length_b, scalar, temp;
438 scalar = (float) calc_vector_scalar_product(a,b);
439 length_a = calc_vector_length(a);
440 length_b = calc_vector_length(b);
445 if (APPROX(scalar, 0)) {
446 return (
float) (M_PI/2.0);
449 if ( (length_a <= 0) || (length_b <= 0)){
450 printf(
"Divide by 0 in calc_angle_between_two_vectors(): No can do! \n");
454 temp = scalar /(length_a * length_b);
459 if ((temp >= 1) || (temp <= -1)){
460 if (temp < 0.0f)
return 3.141526f;
463 return (
float) acos(temp);
465int vecapprox3f(
float *a,
float *b,
float tol){
467 return veclength3f(vecdif3f(tmp,a,b)) < tol ? TRUE : FALSE;
469int vecapprox2f(
float *a,
float *b,
float tol){
471 return veclength2f(vecdif2f(tmp,a,b)) < tol ? TRUE : FALSE;
473int vecsame3f(
float *a,
float *b){
476 if(a[i] != b[i]) isame = FALSE;
479int vecclose3f(
float* a,
float* b,
float tol) {
482 for (
int i = 0; i < 3; i++)
483 if (fabs(a[i] - b[i]) > tol) isame = FALSE;
487int vecsame4f(
float *a,
float *b){
490 if(a[i] != b[i]) isame = FALSE;
493int vecsamed(
double *a,
double *b){
496 if(a[i] != b[i]) isame = FALSE;
502 GLDOUBLE ret = sqrt(vecdot(v,v));
504 if (APPROX(ret, 0))
return 0.;
505 vecscale(r,v,1./ret);
508float vecnormsquared3f(
float *a){
509 return a[0] * a[0] + a[1] * a[1] + a[2] * a[2];
511float *vecscale3f(
float *b,
float *a,
float scale){
517float *vecscale4f(
float *b,
float *a,
float scale){
524double* vecscale4d(
double* b,
double* a,
double scale) {
531float *vecmult3f(
float *c,
float *a,
float *b){
534 for(;i<3;i++) c[i] = a[i]*b[i];
537double* vecmult3d(
double* c,
double* a,
double* b) {
540 for (; i < 3; i++) c[i] = a[i] * b[i];
544float *vecmult2f(
float *c,
float *a,
float *b){
547 for(;i<2;i++) c[i] = a[i]*b[i];
550double* vecmult2d(
double* c,
double* a,
double* b) {
553 for (; i < 2; i++) c[i] = a[i] * b[i];
557float *vecnormalize3f(
float *b,
float *a)
559 float norm = veclength3f(a);
560 if (APPROX(norm, 0.0f)){
561 b[0] = 1.0f; b[1] = 0.0f; b[2] = 0.0f;
563 vecscale3f(b, a, 1.0f / norm);
569GLDOUBLE det3x3(GLDOUBLE* data)
571 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];
573float det3f(
float *a,
float *b,
float *c)
577 return vecdot3f(a,veccross3f(temp,b, c));
579double det3d(
double *a,
double *b,
double *c)
583 return vecdotd(a,veccrossd(temp,b, c));
592 r->x = b[0]*a->x +b[4]*a->y +b[8]*a->z +b[12];
593 r->y = b[1]*a->x +b[5]*a->y +b[9]*a->z +b[13];
594 r->z = b[2]*a->x +b[6]*a->y +b[10]*a->z +b[14];
597 tmp.x = a->x; tmp.y = a->y; tmp.z = a->z;
598 r->x = b[0]*tmp.x +b[4]*tmp.y +b[8]*tmp.z +b[12];
599 r->y = b[1]*tmp.x +b[5]*tmp.y +b[9]*tmp.z +b[13];
600 r->z = b[2]*tmp.x +b[6]*tmp.y +b[10]*tmp.z +b[14];
604GLDOUBLE* transformUPPER3X3d(
double *r,
double *a,
const GLDOUBLE* b){
607 r[0] = b[0]*tmp[0] +b[4]*tmp[1] +b[8]*tmp[2];
608 r[1] = b[1]*tmp[0] +b[5]*tmp[1] +b[9]*tmp[2];
609 r[2] = b[2]*tmp[0] +b[6]*tmp[1] +b[10]*tmp[2];
615 return transform(r,a,b);
617GLDOUBLE* pointxyz2double(
double* r,
struct point_XYZ *p){
618 r[0] = p->x; r[1] = p->y; r[2] = p->z;
622 r->x = p[0]; r->y = p[1]; r->z = p[2];
625double * matrixAFFINE2RotationMatrix(
double* rotmat,
double *fullmat){
633 double sx,sy,sz, pp[3], q[3],p[3],d[3], matinv[16], matt[16], matr[16], mats[16];
636 matinverseAFFINE(matinv,fullmat);
637 vecsetd(pp,0.0,0.0,0.0);
638 transformAFFINEd(pp,pp,matinv);
639 mattranslate(matt,pp[0],pp[1],pp[2]);
640 matmultiplyAFFINE(matr,matt,fullmat);
644 vecsetd(p,0.0,0.0,0.0);
645 transformAFFINEd(p,p,matr);
646 vecsetd(q,1.0,0.0,0.0);
647 transformAFFINEd(q,q,matr);
648 sx = 1.0/veclengthd(vecdifd(d,q,p));
649 vecsetd(q,0.0,1.0,0.0);
650 transformAFFINEd(q,q,matr);
652 sy = 1.0/veclengthd(vecdifd(d,q,p));
653 vecsetd(q,0.0,0.0,1.0);
654 transformAFFINEd(q,q,matr);
655 sz = 1.0/veclengthd(vecdifd(d,q,p));
657 matscale(mats,sx,sy,sz);
658 matmultiplyAFFINE(rotmat,mats,matr);
666void RotationMatrixtoAxisAngle(
double *axisangle,
double *mat4) {
669 double angle, x, y, z;
670 double epsilon = 0.01;
671 double epsilon2 = 0.1;
678 if ((abs(m[0][1] - m[1][0]) < epsilon)
679 && (abs(m[0][2] - m[2][0]) < epsilon)
680 && (abs(m[1][2] - m[2][1]) < epsilon)) {
684 if ((abs(m[0][1] + m[1][0]) < epsilon2)
685 && (abs(m[0][2] + m[2][0]) < epsilon2)
686 && (abs(m[1][2] + m[2][1]) < epsilon2)
687 && (abs(m[0][0] + m[1][1] + m[2][2] - 3) < epsilon2)) {
689 vecsetd(axisangle, 0.0, 1.0, 0.0);
695 double xx = (m[0][0] + 1) / 2;
696 double yy = (m[1][1] + 1) / 2;
697 double zz = (m[2][2] + 1) / 2;
698 double xy = (m[0][1] + m[1][0]) / 4;
699 double xz = (m[0][2] + m[2][0]) / 4;
700 double yz = (m[1][2] + m[2][1]) / 4;
701 if ((xx > yy) && (xx > zz)) {
737 vecsetd(axisangle, x, y, z);
738 axisangle[3] = angle;
742 double s = sqrt((m[2][1] - m[1][2]) * (m[2][1] - m[1][2])
743 + (m[0][2] - m[2][0]) * (m[0][2] - m[2][0])
744 + (m[1][0] - m[0][1]) * (m[1][0] - m[0][1]));
745 if (abs(s) < 0.001) s = 1;
748 angle = acos((m[0][0] + m[1][1] + m[2][2] - 1) / 2);
749 x = (m[2][1] - m[1][2]) / s;
750 y = (m[0][2] - m[2][0]) / s;
751 z = (m[1][0] - m[0][1]) / s;
752 vecsetd(axisangle, x, y, z);
753 axisangle[3] = angle;
756void AFFINEmatrix2axisangled(
double* axisangle,
double* matrix4) {
759 matrixAFFINE2RotationMatrix(rotmat4, matrix4);
761 RotationMatrixtoAxisAngle(axisangle, rotmat4);
763double *transformAFFINEd(
double *r,
double *a,
const GLDOUBLE* mat){
766 double2pointxyz(&pa,a);
767 transformAFFINE(&pr,&pa,mat);
768 pointxyz2double(r,&pr);
771double *transformFULL4d(
double *r,
double *a,
double *mat){
774 for (i=0; i<4; i++) {
783float* transformf(
float* r,
const float* a,
const GLDOUBLE* b)
789 r[0] = (float) (b[0]*a[0] +b[4]*a[1] +b[8]*a[2] +b[12]);
790 r[1] = (float) (b[1]*a[0] +b[5]*a[1] +b[9]*a[2] +b[13]);
791 r[2] = (float) (b[2]*a[0] +b[6]*a[1] +b[10]*a[2] +b[14]);
793 tmp[0] =a[0]; tmp[1] = a[1]; tmp[2] = a[2];
794 r[0] = (float) (b[0]*tmp[0] +b[4]*tmp[1] +b[8]*tmp[2] +b[12]);
795 r[1] = (float) (b[1]*tmp[0] +b[5]*tmp[1] +b[9]*tmp[2] +b[13]);
796 r[2] = (float) (b[2]*tmp[0] +b[6]*tmp[1] +b[10]*tmp[2] +b[14]);
800float* matmultvec4f(
float* r4,
float *mat4,
float* a4 )
804 memcpy(t4,a4,4*
sizeof(
float));
809 r4[i] += b[i][j]*t4[j];
813double* matmultvec4d(
double* r4,
double* mat4,
double* a4)
816 double t4[4], * b[4];
817 memcpy(t4, a4, 4 *
sizeof(
double));
818 for (i = 0; i < 4; i++) {
821 for (j = 0; j < 4; j++)
822 r4[i] += b[i][j] * t4[j];
826float* vecmultmat4f_broken(
float* r4,
float* a4,
float *mat4 )
830 memcpy(t4,a4,4*
sizeof(
float));
835 r4[i] += t4[j]*b[j][i];
839float* vecmultmat4f(
float* r4,
float* a4,
float *mat4 )
843 memcpy(t4,a4,4*
sizeof(
float));
853double* vecmultmat4d(
double* r4,
double* a4,
double* mat4)
857 memcpy(t4, a4, 4 *
sizeof(
double));
858 for (i = 0; i < 4; i++) {
860 for (j = 0; j < 4; j++)
861 r4[i] += t4[j] * mat4[j * 4 + i];
866float* matmultvec3f(
float* r3,
float *mat3,
float* a3 )
870 memcpy(t3,a3,3*
sizeof(
float));
875 r3[i] += b[i][j]*t3[j];
879float* vecmultmat3f(
float* r3,
float* a3,
float *mat3 )
883 memcpy(t3,a3,3*
sizeof(
float));
888 r3[i] += t3[j]*b[j][i];
894double* matmultvec3d(
double* r3,
double* mat3,
double* a3)
897 double t3[3], * b[3];
898 memcpy(t3, a3, 3 *
sizeof(
double));
899 for (i = 0; i < 3; i++) {
902 for (j = 0; j < 3; j++)
903 r3[i] += b[i][j] * t3[j];
907double* vecmultmat3d(
double* r3,
double* a3,
double* mat3)
910 double t3[3], * b[3];
911 memcpy(t3, a3, 3 *
sizeof(
double));
912 for (i = 0; i < 3; i++) b[i] = &mat3[i * 3];
913 for (i = 0; i < 3; i++) {
916 for (j = 0; j < 3; j++)
917 r3[i] += t3[j] * b[j][i];
929 r->x = b[0]*a->x +b[4]*a->y +b[8]*a->z;
930 r->y = b[1]*a->x +b[5]*a->y +b[9]*a->z;
931 r->z = b[2]*a->x +b[6]*a->y +b[10]*a->z;
934 tmp.x = a->x; tmp.y = a->y; tmp.z = a->z;
935 r->x = b[0]*tmp.x +b[4]*tmp.y +b[8]*tmp.z;
936 r->y = b[1]*tmp.x +b[5]*tmp.y +b[9]*tmp.z;
937 r->z = b[2]*tmp.x +b[6]*tmp.y +b[10]*tmp.z;
951 return (a->x*b->x) + (a->y*b->y) + (a->z*b->z);
970double closest_point_of_segment_to_y_axis_segment(
double y1,
double y2,
struct point_XYZ p1,
struct point_XYZ p2) {
972 double imin = (y1- p1.y) / (p2.y - p1.y);
973 double imax = (y2- p1.y) / (p2.y - p1.y);
976 double x12 = (p1.x - p2.x);
977 double z12 = (p1.z - p2.z);
978 double q = ( x12*x12 + z12*z12 );
981 double i = ((APPROX(q, 0)) ? 0 : (p1.x * x12 + p1.z * z12) / q);
983 printf(
"imin=%f, imax=%f => ",imin,imax);
992 if(imin < 0) imin = 0;
993 if(imax > 1) imax = 1;
995 printf(
"imin=%f, imax=%f\n",imin,imax);
998 if(i < imin) i = imin;
999 if(i > imax) i = imax;
1003BOOL line_intersect_line_3f(
float *p1,
float *v1,
float *p2,
float *v2,
float *t,
float *s,
float *x1,
float *x2)
1011 float t1[3], cross[3];
1012 float crosslength2, ss, tt;
1013 veccross3f(cross, v1, v2);
1014 crosslength2 = vecnormsquared3f(cross);
1015 if (APPROX(crosslength2, 0.0f))
return FALSE;
1016 crosslength2 = 1.0f / crosslength2;
1017 tt = det3f(vecdif3f(t1, p2, p1), v2, cross) * crosslength2;
1018 ss = det3f(vecdif3f(t1, p2, p1), v1, cross) * crosslength2;
1020 vecadd3f(x1, p1, vecscale3f(t1, v1, tt));
1022 vecadd3f(x2, p2, vecscale3f(t1, v2, ss));
1030BOOL line_intersect_planed_3f(
float *p,
float *v,
float *N,
float d,
float *pi,
float *t)
1036 float t1[3], t2[3], nd, tt;
1037 nd = vecdot3f(N, v);
1038 if (APPROX(nd, 0.0f))
return FALSE;
1039 tt = -(d + vecdot3f(N, p)) / nd;
1040 vecadd3f(t2, p, vecscale3f(t1, v, tt));
1042 if (pi) veccopy3f(pi, t2);
1045BOOL line_intersect_plane_3f(
float *p,
float *v,
float *N,
float *pp,
float *pi,
float *t)
1048 d = vecdot3f(N, pp);
1049 return line_intersect_planed_3f(p, v, N, d, pi, t);
1052BOOL line_intersect_planed_3d(
double *p,
double *v,
double *N,
double d,
double *pi,
double *t)
1058 double t1[3], t2[3], nd, tt;
1060 if (APPROX(nd, 0.0))
return FALSE;
1061 tt = -(d + vecdotd(N, p)) / nd;
1062 vecaddd(t2, p, vecscaled(t1, v, tt));
1064 if (pi) veccopyd(pi, t2);
1067BOOL line_intersect_plane_3d(
double *p,
double *v,
double *N,
double *pp,
double *pi,
double *t)
1071 return line_intersect_planed_3d(p, v, N, d, pi, t);
1073BOOL line_intersect_cylinder_3f(
float *p,
float *v,
float radius,
float *pi)
1082 float a = dx*dx + dz*dz;
1083 float b = 2.0f*(dx * p[0] + dz * p[2]);
1084 float c = p[0] * p[0] + p[2] * p[2] - radius*radius;
1086 if (APPROX(a, 0.0f))
return FALSE;
1092 float sol2 = (-b-(float) sqrt(und))/2;
1094 vecadd3f(pp, p, vecscale3f(t2, v, sol));
1095 if (pi) veccopy3f(pi, pp);
1104 r->x = v->x + v2->x;
1105 r->y = v->y + v2->y;
1106 r->z = v->z + v2->z;
1112 r->x = v->x - v2->x;
1113 r->y = v->y - v2->y;
1114 r->z = v->z - v2->z;
1125 if(fabs(n.x) <= fabs(n.y) && fabs(n.x) <= fabs(n.z)) {
1130 j->x = n.y*n.y + n.z*n.z;
1133 }
else if(fabs(n.y) <= fabs(n.x) && fabs(n.y) <= fabs(n.z)) {
1139 j->y = n.x*n.x + n.z*n.z;
1148 j->z = n.x*n.x + n.y*n.y;
1153GLDOUBLE* mattranspose(GLDOUBLE* res, GLDOUBLE* mm)
1161 memcpy(mcpy,m,
sizeof(GLDOUBLE)*16);
1165 for (i = 0; i < 4; i++) {
1169 for (j = 0; j < 4; j++) {
1170 res[i*4+j] = m[j*4+i];
1176float* mattranspose4f(
float* res,
float* mm)
1184 memcpy(mcpy,m,
sizeof(
float)*16);
1188 for (i = 0; i < 4; i++) {
1189 for (j = 0; j < 4; j++) {
1190 res[i*4+j] = m[j*4+i];
1195float* mattranspose3f(
float* res,
float* mm)
1203 memcpy(mcpy,m,
sizeof(
float)*9);
1207 for (i = 0; i < 3; i++) {
1208 for (j = 0; j < 3; j++) {
1209 res[i*3+j] = m[j*3+i];
1214double* mattranspose3d(
double* res,
double* mm)
1222 memcpy(mcpy, m,
sizeof(
double) * 9);
1226 for (i = 0; i < 3; i++) {
1227 for (j = 0; j < 3; j++) {
1228 res[i * 3 + j] = m[j * 3 + i];
1234GLDOUBLE* matinverse98(GLDOUBLE* res, GLDOUBLE* mm)
1246 memcpy(mcpy,m,
sizeof(GLDOUBLE)*16);
1251 if(APPROX(Deta,0.0))
1255 res[0] = (-m[9]*m[6] +m[5]*m[10])*Deta;
1256 res[4] = (+m[8]*m[6] -m[4]*m[10])*Deta;
1257 res[8] = (-m[8]*m[5] +m[4]*m[9])*Deta;
1258 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;
1260 res[1] = (+m[9]*m[2] -m[1]*m[10])*Deta;
1261 res[5] = (-m[8]*m[2] +m[0]*m[10])*Deta;
1262 res[9] = (+m[8]*m[1] -m[0]*m[9])*Deta;
1263 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;
1265 res[2] = (-m[5]*m[2] +m[1]*m[6])*Deta;
1266 res[6] = (+m[4]*m[2] -m[0]*m[6])*Deta;
1267 res[10] = (-m[4]*m[1] +m[0]*m[5])*Deta;
1268 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;
1270 res[3] = (+m[5]*m[2]*m[11] -m[1]*m[6]*m[11])*Deta;
1271 res[7] = (-m[4]*m[2]*m[11] +m[0]*m[6]*m[11])*Deta;
1272 res[11] = (+m[4]*m[1]*m[11] -m[0]*m[5]*m[11])*Deta;
1273 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;
1277float* matinverse4f(
float* res,
float* mm)
1287 memcpy(mcpy,m,
sizeof(
float)*16);
1291 Deta = det3f(&m[0],&m[4],&m[8]);
1294 res[0] = (-m[9]*m[6] +m[5]*m[10])*Deta;
1295 res[4] = (+m[8]*m[6] -m[4]*m[10])*Deta;
1296 res[8] = (-m[8]*m[5] +m[4]*m[9])*Deta;
1297 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;
1299 res[1] = (+m[9]*m[2] -m[1]*m[10])*Deta;
1300 res[5] = (-m[8]*m[2] +m[0]*m[10])*Deta;
1301 res[9] = (+m[8]*m[1] -m[0]*m[9])*Deta;
1302 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;
1304 res[2] = (-m[5]*m[2] +m[1]*m[6])*Deta;
1305 res[6] = (+m[4]*m[2] -m[0]*m[6])*Deta;
1306 res[10] = (-m[4]*m[1] +m[0]*m[5])*Deta;
1307 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;
1309 res[3] = (+m[5]*m[2]*m[11] -m[1]*m[6]*m[11])*Deta;
1310 res[7] = (-m[4]*m[2]*m[11] +m[0]*m[6]*m[11])*Deta;
1311 res[11] = (+m[4]*m[1]*m[11] -m[0]*m[5]*m[11])*Deta;
1312 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;
1320 VECDIFF(*p2,*p1,v1);
1321 VECDIFF(*p3,*p1,v2);
1339 return polynormal(r,pp+0,pp+1,pp+2);
1343GLDOUBLE* matrotate(GLDOUBLE* Result,
double Theta,
double x,
double y,
double z)
1345 GLDOUBLE CosTheta = cos(Theta);
1346 GLDOUBLE SinTheta = sin(Theta);
1348 Result[0] = x*x + (y*y+z*z)*CosTheta;
1349 Result[1] = x*y - x*y*CosTheta + z*SinTheta;
1350 Result[2] = x*z - x*z*CosTheta - y*SinTheta;
1351 Result[4] = x*y - x*y*CosTheta - z*SinTheta;
1352 Result[5] = y*y + (x*x+z*z)*CosTheta;
1353 Result[6] = z*y - z*y*CosTheta + x*SinTheta;
1354 Result[8] = z*x - z*x*CosTheta + y*SinTheta;
1355 Result[9] = z*y - z*y*CosTheta - x*SinTheta;
1356 Result[10]= z*z + (x*x+y*y)*CosTheta;
1368GLDOUBLE* mattranslate(GLDOUBLE* r,
double dx,
double dy,
double dz)
1370 r[0] = r[5] = r[10] = r[15] = 1;
1371 r[1] = r[2] = r[3] = r[4] =
1372 r[6] = r[7] = r[8] = r[9] =
1379GLDOUBLE* matscale(GLDOUBLE* r,
double sx,
double sy,
double sz)
1382 r[0] = r[5] = r[10] = r[15] =
1383 r[1] = r[2] = r[3] = r[4] =
1384 r[6] = r[7] = r[8] = r[9] =
1385 r[11] = r[12] = r[13] = r[14] = 0.0;
1393GLDOUBLE* matmultiplyFULL(GLDOUBLE* r, GLDOUBLE* mm , GLDOUBLE* nn)
1399 GLDOUBLE tm[16],tn[16];
1406 memcpy(tm,m,
sizeof(GLDOUBLE)*16);
1410 memcpy(tn,n,
sizeof(GLDOUBLE)*16);
1414 if(1)
for(i=0;i<3;i++){
1415 if(mm[i +12] != 0.0){
1416 double p = mm[i +12];
1417 printf(
"Ft[%d]%f ",i,p);
1419 if(nn[i + 12] != 0.0){
1420 double p = nn[i +12];
1421 printf(
"FT[%d]%f ",i,p);
1424 if(1)
for(i=0;i<3;i++){
1425 if(mm[i*4 + 3] != 0.0){
1426 double p = mm[i*4 + 3];
1427 printf(
"Fp[%d]%f ",i,p);
1429 if(nn[i*4 + 3] != 0.0){
1430 double p = nn[i*4 + 3];
1431 printf(
"FP[%d]%f ",i,p);
1441 r[i*4+j] += m[i*4+k]*n[k*4+j];
1446GLDOUBLE* matmultiplyAFFINE(GLDOUBLE* r, GLDOUBLE* nn , GLDOUBLE* mm)
1452 GLDOUBLE tm[16],tn[16];
1459 memcpy(tm,m,
sizeof(GLDOUBLE)*16);
1463 memcpy(tn,n,
sizeof(GLDOUBLE)*16);
1467 if(0)
for(i=0;i<3;i++){
1468 if(mm[i +12] != 0.0){
1469 double p = mm[i +12];
1470 printf(
"At[%d]%lf ",i,p);
1472 if(nn[i + 12] != 0.0){
1473 double p = nn[i +12];
1474 printf(
"AT[%d]%lf ",i,p);
1477 if(1)
for(i=0;i<3;i++){
1478 if(mm[i*4 + 3] != 0.0){
1479 double p = mm[i*4 + 3];
1480 printf(
"Ap[%d]%lf ",i,p);
1482 if(nn[i*4 + 3] != 0.0){
1483 double p = nn[i*4 + 3];
1484 printf(
"AP[%d]%lf ",i,p);
1491 r[0] = m[0]*n[0] +m[4]*n[1] +m[8]*n[2];
1492 r[4] = m[0]*n[4] +m[4]*n[5] +m[8]*n[6];
1493 r[8] = m[0]*n[8] +m[4]*n[9] +m[8]*n[10];
1494 r[12]= m[0]*n[12]+m[4]*n[13]+m[8]*n[14] +m[12];
1496 r[1] = m[1]*n[0] +m[5]*n[1] +m[9]*n[2];
1497 r[5] = m[1]*n[4] +m[5]*n[5] +m[9]*n[6];
1498 r[9] = m[1]*n[8] +m[5]*n[9] +m[9]*n[10];
1499 r[13]= m[1]*n[12]+m[5]*n[13]+m[9]*n[14] +m[13];
1501 r[2] = m[2]*n[0]+ m[6]*n[1] +m[10]*n[2];
1502 r[6] = m[2]*n[4]+ m[6]*n[5] +m[10]*n[6];
1503 r[10]= m[2]*n[8]+ m[6]*n[9] +m[10]*n[10];
1504 r[14]= m[2]*n[12]+m[6]*n[13]+m[10]*n[14] +m[14];
1506 r[3] = r[7] = r[11] = 0.0;
1511GLDOUBLE* matmultiply(GLDOUBLE* r, GLDOUBLE* mm , GLDOUBLE* nn){
1512 return matmultiplyFULL(r,mm,nn);
1515float* matmultiply4f(
float* r,
float* mm ,
float* nn)
1520 float tm[16],tn[16];
1527 memcpy(tm,m,
sizeof(
float)*16);
1531 memcpy(tn,n,
sizeof(
float)*16);
1540 r[i*4+j] += m[i*4+k]*n[k*4+j];
1544float* matmultiply3f(
float* r,
float* mm ,
float* nn)
1556 memcpy(tm,m,
sizeof(
float)*9);
1560 memcpy(tn,n,
sizeof(
float)*9);
1569 r[i*3+j] += m[i*3+k]*n[k*3+j];
1573double* matmultiply3d(
double* r,
double* mm,
double* nn)
1578 double tm[9], tn[9];
1585 memcpy(tm, m,
sizeof(
double) * 9);
1589 memcpy(tn, n,
sizeof(
double) * 9);
1593 for (i = 0; i < 3; i++)
1594 for (j = 0; j < 3; j++)
1597 for (k = 0; k < 3; k++)
1598 r[i * 3 + j] += m[i * 3 + k] * n[k * 3 + j];
1602float *axisangle_rotate3f(
float* b,
float *a,
float *axisangle)
1610 float cosine, sine, cross[3], dot, theta, *axis, t1[3], t2[3],t3[3],t4[3];
1611 theta = axisangle[3];
1613 cosine = (float)cos(theta);
1614 sine = (float)sin(theta);
1615 veccross3f(cross,axis, a);
1616 dot = vecdot3f(axis, a);
1617 vecadd3f(b,vecscale3f(t1, a, cosine), vecadd3f(t2, vecscale3f(t3, cross, sine), vecscale3f(t4, axis, dot*(1.0f - cosine))));
1620double* axisangle_rotate3d(
double* b,
double* a,
float* axisangle)
1628 double cosine, sine, cross[3], dot, theta, axis[3], t1[3], t2[3], t3[3], t4[3];
1629 theta = axisangle[3];
1630 float2double(axis,axisangle,3);
1631 cosine = cos(theta);
1632 sine = (float)sin(theta);
1633 veccrossd(cross, axis, a);
1634 dot = vecdotd(axis, a);
1635 vecaddd(b, vecscaled(t1, a, cosine), vecaddd(t2, vecscaled(t3, cross, sine), vecscaled(t4, axis, dot * (1.0 - cosine))));
1640float *axisangle_rotate4f(
float* axisAngleC,
float *axisAngleA,
float *axisAngleB)
1645 veccopy4f(A.c,axisAngleA);
1646 veccopy4f(B.c,axisAngleB);
1647 sfrotation_multiply(&C,&A,&B);
1648 veccopy4f(axisAngleC,C.c);
1685 veccopy4f(axisAngleC,axisAngleA);
1689double *matidentity4d(
double *b){
1701double *mattranslate4d(
double *mat,
double* xyz){
1704 matidentity4d(mtemp);
1708 matmultiplyFULL(mat,mtemp,mat);
1711double* matscale4d(
double* mat,
double* sxyz) {
1714 matidentity4d(mtemp);
1717 mtemp[10] = sxyz[2];
1718 matmultiplyFULL(mat, mtemp, mat);
1721float *matidentity4f(
float *b){
1733float *matidentity3f(
float *b){
1736 for(i=0;i<9;i++) b[i] = 0.0f;
1737 for(i=0;i<3;i++) b[i*3 +i] = 1.0f;
1740double* matidentity3d(
double* b) {
1743 for (i = 0; i < 9; i++) b[i] = 0.0;
1744 for (i = 0; i < 3; i++) b[i * 3 + i] = 1.0;
1747float *axisangle2matrix4f(
float *b,
float *axisangle){
1756 axisangle_rotate3f(mat[i], mat[i], axisangle);
1761void matrixFromAxisAngle4d(
double *mat,
double rangle,
double x,
double y,
double z)
1780 for(i=0;i<16;i++) mat[i] = 0.0;
1781 for(i=0;i<4;i++) m[i][i] = 1.0;
1789 m[0][0] = c + x*x*t;
1790 m[1][1] = c + y*y*t;
1791 m[2][2] = c + z*z*t;
1796 m[1][0] = tmp1 + tmp2;
1797 m[0][1] = tmp1 - tmp2;
1800 m[2][0] = tmp1 - tmp2;
1801 m[0][2] = tmp1 + tmp2;
1804 m[2][1] = tmp1 + tmp2;
1805 m[1][2] = tmp1 - tmp2;
1809void rotate_v2v_axisAngled(
double* axis,
double* angle,
double *orig,
double *result)
1812 double dv[3], iv[3], cv[3];
1827 vecnormald(dv,orig);
1828 vecnormald(iv,result);
1830 veccrossd(cv,dv,iv);
1831 cvl = vecnormald(cv,cv);
1835 if(APPROX(cvl, 0)) {
1840 *angle = atan2(cvl,vecdotd(dv,iv));
1851 veccross(&cv,dv,iv);
1852 cvl = vecnormal(&cv,&cv);
1854 if(APPROX(cvl, 0)) {
1859 a = atan2(cvl,vecdot(&dv,&iv));
1861 matrotate(res,a,cv.x,cv.y,cv.z);
1864double matrotate2vd(GLDOUBLE* res,
double * iv,
double * dv) {
1866 double2pointxyz(&piv,iv);
1867 double2pointxyz(&pdv,dv);
1868 return matrotate2v(res,piv,pdv);
1871#define SHOW_NONSINGULARS 0
1880BOOL matrix3x3_inverse_float(
float *inn,
float *outt)
1886 float *in[3], *out[3];
1895#define ACCUMULATE pos += temp;
1911 temp = in[0][0] * in[1][1] * in[2][2];
1913 temp = in[0][1] * in[1][2] * in[2][0];
1915 temp = in[0][2] * in[1][0] * in[2][1];
1917 temp = -in[0][2] * in[1][1] * in[2][0];
1919 temp = -in[0][1] * in[1][0] * in[2][2];
1921 temp = -in[0][0] * in[1][2] * in[2][1];
1927 if(APPROX(det_1,0.0f)){
1931 if(SHOW_NONSINGULARS) fprintf (stderr,
"affine_matrix4_inverse: singular matrix\n");
1938 det_1 = 1.0f / det_1;
1939 out[0][0] = (in[1][1] * in[2][2] - in[1][2] * in[2][1] ) * det_1;
1940 out[1][0] = -(in[1][0] * in[2][2] - in[1][2] * in[2][0] ) * det_1;
1941 out[2][0] = (in[1][0] * in[2][1] - in[1][1] * in[2][0] ) * det_1;
1942 out[0][1] = -(in[0][1] * in[2][2] - in[0][2] * in[2][1] ) * det_1;
1943 out[1][1] = (in[0][0] * in[2][2] - in[0][2] * in[2][0] ) * det_1;
1944 out[2][1] = -(in[0][0] * in[2][1] - in[0][1] * in[2][0] ) * det_1;
1945 out[0][2] = (in[0][1] * in[1][2] - in[0][2] * in[1][1] ) * det_1;
1946 out[1][2] = -(in[0][0] * in[1][2] - in[0][2] * in[1][0] ) * det_1;
1947 out[2][2] = (in[0][0] * in[1][1] - in[0][1] * in[1][0] ) * det_1;
1952BOOL matrix3x3_inverse_double(
double* inn,
double* outt)
1958 double* in[3], * out[3];
1967#define ACCUMULATE pos += temp;
1983 temp = in[0][0] * in[1][1] * in[2][2];
1985 temp = in[0][1] * in[1][2] * in[2][0];
1987 temp = in[0][2] * in[1][0] * in[2][1];
1989 temp = -in[0][2] * in[1][1] * in[2][0];
1991 temp = -in[0][1] * in[1][0] * in[2][2];
1993 temp = -in[0][0] * in[1][2] * in[2][1];
1999 if (APPROX(det_1, 0.0)) {
2003 if (SHOW_NONSINGULARS) printf(
"affine_matrix4_inverse: singular matrix\n");
2010 det_1 = 1.0f / det_1;
2011 out[0][0] = (in[1][1] * in[2][2] - in[1][2] * in[2][1]) * det_1;
2012 out[1][0] = -(in[1][0] * in[2][2] - in[1][2] * in[2][0]) * det_1;
2013 out[2][0] = (in[1][0] * in[2][1] - in[1][1] * in[2][0]) * det_1;
2014 out[0][1] = -(in[0][1] * in[2][2] - in[0][2] * in[2][1]) * det_1;
2015 out[1][1] = (in[0][0] * in[2][2] - in[0][2] * in[2][0]) * det_1;
2016 out[2][1] = -(in[0][0] * in[2][1] - in[0][1] * in[2][0]) * det_1;
2017 out[0][2] = (in[0][1] * in[1][2] - in[0][2] * in[1][1]) * det_1;
2018 out[1][2] = -(in[0][0] * in[1][2] - in[0][2] * in[1][0]) * det_1;
2019 out[2][2] = (in[0][0] * in[1][1] - in[0][1] * in[1][0]) * det_1;
2024float * mat423f(
float *out3x3,
float *in4x4)
2029 out3x3[i*3 + j] = in4x4[i*4 + j];
2033float * matinverse3f(
float *out3x3,
float *in3x3)
2035 matrix3x3_inverse_float(in3x3,out3x3);
2038double* matinverse3d(
double* out3x3,
double* in3x3)
2040 matrix3x3_inverse_double(in3x3, out3x3);
2044float * transform3x3f(
float *out3,
float *in3,
float *mat3x3){
2047 memcpy(t3,in3,3*
sizeof(
float));
2051 out3[i] += t3[j]*mat3x3[j*3 + i];
2057BOOL affine_matrix4x4_inverse_float(
float *inn,
float *outt)
2066 float *in[4], *out[4];
2075#define ACCUMULATE pos += temp;
2093 temp = in[0][0] * in[1][1] * in[2][2];
2095 temp = in[0][1] * in[1][2] * in[2][0];
2097 temp = in[0][2] * in[1][0] * in[2][1];
2099 temp = -in[0][2] * in[1][1] * in[2][0];
2101 temp = -in[0][1] * in[1][0] * in[2][2];
2103 temp = -in[0][0] * in[1][2] * in[2][1];
2109 if(APPROX(det_1,0.0f)){
2112 if(SHOW_NONSINGULARS) fprintf (stderr,
"affine_matrix4_inverse: singular matrix\n");
2119 det_1 = 1.0f / det_1;
2120 out[0][0] = (in[1][1] * in[2][2] - in[1][2] * in[2][1] ) * det_1;
2121 out[1][0] = -(in[1][0] * in[2][2] - in[1][2] * in[2][0] ) * det_1;
2122 out[2][0] = (in[1][0] * in[2][1] - in[1][1] * in[2][0] ) * det_1;
2123 out[0][1] = -(in[0][1] * in[2][2] - in[0][2] * in[2][1] ) * det_1;
2124 out[1][1] = (in[0][0] * in[2][2] - in[0][2] * in[2][0] ) * det_1;
2125 out[2][1] = -(in[0][0] * in[2][1] - in[0][1] * in[2][0] ) * det_1;
2126 out[0][2] = (in[0][1] * in[1][2] - in[0][2] * in[1][1] ) * det_1;
2127 out[1][2] = -(in[0][0] * in[1][2] - in[0][2] * in[1][0] ) * det_1;
2128 out[2][2] = (in[0][0] * in[1][1] - in[0][1] * in[1][0] ) * det_1;
2131 out[3][0] = -(in[3][0] * out[0][0] + in[3][1]*out[1][0] + in[3][2]*out[2][0]);
2132 out[3][1] = -(in[3][0] * out[0][1] + in[3][1]*out[1][1] + in[3][2]*out[2][1]);
2133 out[3][2] = -(in[3][0] * out[0][2] + in[3][1]*out[1][2] + in[3][2]*out[2][2]);
2136 out[0][3] = out[1][3] = out[2][3] = 0.0f;
2142BOOL affine_matrix4x4_inverse_double(
double *inn,
double *outt)
2150 double *in[4], *out[4];
2159#define ACCUMULATE pos += temp;
2177 temp = in[0][0] * in[1][1] * in[2][2];
2179 temp = in[0][1] * in[1][2] * in[2][0];
2181 temp = in[0][2] * in[1][0] * in[2][1];
2183 temp = -in[0][2] * in[1][1] * in[2][0];
2185 temp = -in[0][1] * in[1][0] * in[2][2];
2187 temp = -in[0][0] * in[1][2] * in[2][1];
2193 if(APPROX(det_1,0.0)){
2196 if(SHOW_NONSINGULARS) fprintf (stderr,
"affine_matrix4_inverse: singular matrix\n");
2203 det_1 = 1.0 / det_1;
2204 out[0][0] = (in[1][1] * in[2][2] - in[1][2] * in[2][1] ) * det_1;
2205 out[1][0] = -(in[1][0] * in[2][2] - in[1][2] * in[2][0] ) * det_1;
2206 out[2][0] = (in[1][0] * in[2][1] - in[1][1] * in[2][0] ) * det_1;
2207 out[0][1] = -(in[0][1] * in[2][2] - in[0][2] * in[2][1] ) * det_1;
2208 out[1][1] = (in[0][0] * in[2][2] - in[0][2] * in[2][0] ) * det_1;
2209 out[2][1] = -(in[0][0] * in[2][1] - in[0][1] * in[2][0] ) * det_1;
2210 out[0][2] = (in[0][1] * in[1][2] - in[0][2] * in[1][1] ) * det_1;
2211 out[1][2] = -(in[0][0] * in[1][2] - in[0][2] * in[1][0] ) * det_1;
2212 out[2][2] = (in[0][0] * in[1][1] - in[0][1] * in[1][0] ) * det_1;
2215 out[3][0] = -(in[3][0] * out[0][0] + in[3][1]*out[1][0] + in[3][2]*out[2][0]);
2216 out[3][1] = -(in[3][0] * out[0][1] + in[3][1]*out[1][1] + in[3][2]*out[2][1]);
2217 out[3][2] = -(in[3][0] * out[0][2] + in[3][1]*out[1][2] + in[3][2]*out[2][2]);
2220 out[0][3] = out[1][3] = out[2][3] = 0.0;
2225GLDOUBLE* matinverseAFFINE(GLDOUBLE* res, GLDOUBLE* mm){
2226 affine_matrix4x4_inverse_double(mm, res);
2229GLDOUBLE* matinverseFULL(GLDOUBLE* res, GLDOUBLE* mm){
2230 matinverse98(res,mm);
2233GLDOUBLE* matinverse(GLDOUBLE* res, GLDOUBLE* mm){
2234 matinverseFULL(res,mm);
2242GLDOUBLE* veccross(GLDOUBLE* r, GLDOUBLE* v1, GLDOUBLE* v2)
2247 r[0] = v1[1]*v2[2] - v1[2]*v2[1];
2248 r[1] = v1[2]*v2[0] - v1[0]*v2[2];
2249 r[2] = v1[0]*v2[1] - v1[1]*v2[0];
2251 GLDOUBLE v2c[3] = {v2[0],v2[1],v2[2]};
2252 r[0] = v1[1]*v2c[2] - v1[2]*v2c[1];
2253 r[1] = v1[2]*v2c[0] - v1[0]*v2c[2];
2254 r[2] = v1[0]*v2c[1] - v1[1]*v2c[0];
2257 GLDOUBLE v1c[3] = {v1[0],v1[1],v1[2]};
2258 r[0] = v1c[1]*v2[2] - v1c[2]*v2[1];
2259 r[1] = v1c[2]*v2[0] - v1c[0]*v2[2];
2260 r[2] = v1c[0]*v2[1] - v1c[1]*v2[0];
2275void scale_to_matrix (
double *mat,
struct point_XYZ *scale) {
2280#if DJ_KEEP_COMPILER_WARNING
2286#if DJ_KEEP_COMPILER_WARNING
2291#define MAT22 mat[10]
2292#if DJ_KEEP_COMPILER_WARNING
2293#define MAT23 mat[11]
2294#define MAT30 mat[12]
2295#define MAT31 mat[13]
2296#define MAT32 mat[14]
2297#define MAT33 mat[15]
2311static 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 };
2313void loadIdentityMatrix (
double *mat) {
2314 memcpy((
void *)mat, (
void *)identity,
sizeof(
double)*16);
2316double *matcopy(
double *r,
double*mat){
2317 memcpy((
void*)r, (
void*)mat,
sizeof(
double)*16);
2320float *matdouble2float4(
float *rmat4,
double *dmat4){
2323 for (i=0; i<16; i++) {
2324 rmat4[i] = (float)dmat4[i];
2328void printmatrix3(GLDOUBLE *mat,
char *description,
int row_major){
2330 printf(
"mat %s {\n",description);
2333 for(i = 0; i< 4; i++) {
2334 printf(
"mat [%2d-%2d] = ",i*4,(i*4)+3);
2336 printf(
" %lf ",mat[(i*4)+j]);
2343 printf(
"mat [%2d %2d %2d %2d] =",i,i+4,i+8,i+12);
2344 printf(
" %lf %lf %lf %lf\n",mat[i],mat[i+4],mat[i+8],mat[i+12]);
2349void printmatrix2(GLDOUBLE* mat,
char* description ) {
2350 static int row_major = FALSE;
2351 printmatrix3(mat,description,row_major);
2354float *veclerp3f(
float *T,
float *A,
float *B,
float alpha){
2357 T[i] = (1.0f - alpha)*A[i] + alpha*B[i];
2361float *veclerp2f(
float *T,
float *A,
float *B,
float alpha){
2364 T[i] = (1.0f - alpha)*A[i] + alpha*B[i];
2368double *veclerpd(
double *T,
double *A,
double *B,
double alpha){
2371 T[i] = (1.0f - alpha)*A[i] + alpha*B[i];
2376void general_slerp(
double *ret,
double *p1,
double *p2,
int size,
const double t)
2387 double scale0, scale1, omega;
2394 scale0 = 0.5*(1.0 + cos(omega));
2395 scale1 = 1.0 - scale0;
2402 ret[i] = scale0 * p1[i] + scale1 * p2[i];
2405 double pret[3], pp1[3], pp2[3];
2406 pointxyz2double(pp1, p1);
2407 pointxyz2double(pp2, p2);
2408 general_slerp(pret, pp1, pp2, 3, t);
2409 double2pointxyz(ret,pret);