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
41float fclamp(float fval, float fstart, float fend) {
42 float fret = fval;
43 fret = fval > fend? fend : fval; //min(fval,fend)
44 fret = fret < fstart ? fstart : fret; //max(fval,fstart)
45 return fret;
46}
47float *vecclamp2f(float *fval, float *fstart, float *fend){
48 int i;
49 for(i=0;i<2;i++){
50 if(fstart[i] <= fend[i])
51 fval[i] = fclamp(fval[i],fstart[i],fend[i]);
52 }
53 return fval; //so you can chain
54}
55float *vecclamp3f(float *fval, float *fstart, float *fend){
56 int i;
57 for(i=0;i<3;i++){
58 if(fstart[i] <= fend[i])
59 fval[i] = fclamp(fval[i],fstart[i],fend[i]);
60 }
61 return fval; //so you can chain
62}
63float *fvecclamp3f(float *fval, float fstart, float fend){
64 int i;
65 for(i=0;i<3;i++){
66 if(fstart <= fend)
67 fval[i] = fclamp(fval[i],fstart,fend);
68 }
69 return fval; //so you can chain
70}
71// #define APPROX(a,b) (fabs((a)-(b))<0.00000001)
72int approx3f(float *a, float *b){
73 float tol = 0.00000001;
74 int i, iret = TRUE;
75 for(i=0;i<3;i++){
76 iret = iret && (fabs(a[i] - b[i]) < tol) ? iret : FALSE;
77 }
78 return iret;
79}
80int approx4f(float *a, float *b){
81 float tol = 0.00000001;
82 int i, iret = TRUE;
83 for(i=0;i<4;i++){
84 iret = iret && (fabs(a[i] - b[i]) < tol) ? iret : FALSE;
85 }
86 return iret;
87}
88
89
90
91double angleNormalized(double angle){
92 //will normalize to +- 2*PI (+-180) range
93 return atan2(sin(angle),cos(angle));
94}
95
96
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);
100}
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);
103}
104void vecprint3db(char *name, double *p, char *eol){
105 printf("%s %lf %lf %lf %s",name,p[0],p[1],p[2],eol);
106}
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);
109}
110double signd(double val){
111 return val < 0.0 ? -1.0 : val > 0.0 ? 1.0 : 0;
112}
113double * vecsignd(double *b, double *a){
114 int i;
115 for (i = 0; i<3; i++) b[i] = signd(a[i]);
116 return b;
117}
118double * vecmuld(double *c, double *a, double *b){
119 int i;
120 for(i=0;i<3;i++)
121 c[i] = a[i]*b[i];
122 return c;
123}
124double * vecsetd(double *b, double x, double y, double z){
125 b[0] = x, b[1] = y; b[2] = z;
126 return b;
127}
128double* vecset2d(double* b, double x, double y) {
129 b[0] = x, b[1] = y;
130 return b;
131}
132
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;
135 return b;
136}
137float *double2float(float *b, const double *a, int n){
138 int i;
139 for(i=0;i<n;i++) b[i] = (float)a[i];
140 return b;
141}
142double *float2double(double *b, float *a, int n){
143 int i;
144 for(i=0;i<n;i++) b[i] = (double)a[i];
145 return b;
146}
147double * vecadd2d(double *c, double *a, double *b){
148 c[0] = a[0] + b[0];
149 c[1] = a[1] + b[1];
150 return c;
151}
152
153double *vecdif2d(double *c, double* a, double *b){
154 c[0] = a[0] - b[0];
155 c[1] = a[1] - b[1];
156 return c;
157}
158double* veccopy2d(double* c, double* a) {
159 c[0] = a[0];
160 c[1] = a[1];
161 return c;
162}
163
164double veclength2d( double *p ){
165 return sqrt(p[0]*p[0] + p[1]*p[1]);
166}
167double vecdot2d(double *a, double *b){
168 return a[0]*b[0] + a[1]*b[1];
169}
170
171double* vecscale2d(double* r, double* v, double s){
172 r[0] = v[0] * s;
173 r[1] = v[1] * s;
174 return r;
175}
176
177double vecnormal2d(double *r, double *v){
178 double ret = sqrt(vecdot2d(v,v));
179 /* if(ret == 0.) return 0.; */
180 if (APPROX(ret, 0)) return 0.;
181 vecscale2d(r,v,1./ret);
182 return ret;
183}
184
185float * vecadd2f(float *c, float *a, float *b){
186 c[0] = a[0] + b[0];
187 c[1] = a[1] + b[1];
188 return c;
189}
190
191float *vecdif2f(float *c, float* a, float *b){
192 c[0] = a[0] - b[0];
193 c[1] = a[1] - b[1];
194 return c;
195}
196float veclength2f( float *p ){
197 return (float) sqrt(p[0]*p[0] + p[1]*p[1]);
198}
199float vecdot2f(float *a, float *b){
200 return a[0]*b[0] + a[1]*b[1];
201}
202
203float* vecscale2f(float* r, float* v, float s){
204 r[0] = v[0] * s;
205 r[1] = v[1] * s;
206 return r;
207}
208
209float vecnormal2f(float *r, float *v){
210 float ret = (float)sqrt(vecdot2f(v,v));
211 /* if(ret == 0.) return 0.; */
212 if (APPROX(ret, 0)) return 0.0f;
213 vecscale2f(r,v,1.0f/ret);
214 return ret;
215}
216
217
218
219
220/* Altenate implemetations available, should merge them eventually */
221double *vecaddd(double *c, double* a, double *b)
222{
223 c[0] = a[0] + b[0];
224 c[1] = a[1] + b[1];
225 c[2] = a[2] + b[2];
226 return c;
227}
228float *vecadd3f(float *c, float *a, float *b)
229{
230 c[0] = a[0] + b[0];
231 c[1] = a[1] + b[1];
232 c[2] = a[2] + b[2];
233 return c;
234}
235double *vecdifd(double *c, double* a, double *b)
236{
237 c[0] = a[0] - b[0];
238 c[1] = a[1] - b[1];
239 c[2] = a[2] - b[2];
240 return c;
241}
242double *vecdif4d(double *c, double* a, double *b)
243{
244 c[0] = a[0] - b[0];
245 c[1] = a[1] - b[1];
246 c[2] = a[2] - b[2];
247 c[3] = a[3] - b[3];
248 return c;
249}
250float *vecdif3f(float *c, float *a, float *b)
251{
252 c[0] = a[0] - b[0];
253 c[1] = a[1] - b[1];
254 c[2] = a[2] - b[2];
255 return c;
256}
257double *veccopyd(double *c, double *a)
258{
259 c[0] = a[0];
260 c[1] = a[1];
261 c[2] = a[2];
262 return c;
263}
264double *vecnegated(double *b, double *a)
265{
266 b[0] = -a[0];
267 b[1] = -a[1];
268 b[2] = -a[2];
269 return b;
270}
271double *vecswizzle2d(double *inout ){
272 double tmp = inout[0];
273 inout[0] = inout[1];
274 inout[1] = tmp;
275 return inout;
276}
277
278double *veccopy4d(double *c, double *a)
279{
280 c[0] = a[0];
281 c[1] = a[1];
282 c[2] = a[2];
283 c[3] = a[3];
284 return c;
285}
286
287
288float *veccopy3f(float *b, float *a)
289{
290 b[0] = a[0];
291 b[1] = a[1];
292 b[2] = a[2];
293 return b;
294}
295float *vecnegate3f(float *b, float *a)
296{
297 b[0] = -a[0];
298 b[1] = -a[1];
299 b[2] = -a[2];
300 return b;
301}
302int vecsame2f(float *b, float *a){
303 return a[0] == b[0] && a[1] == b[1] ? TRUE : FALSE;
304}
305float *veccopy2f(float *b, float *a)
306{
307 b[0] = a[0];
308 b[1] = a[1];
309 return b;
310}
311float *vecset2f(float *b, float x, float y)
312{
313 b[0] = x; b[1] = y;
314 return b;
315}
316double * veccrossd(double *c, double *a, double *b)
317{
318 double aa[3], bb[3];
319 veccopyd(aa,a);
320 veccopyd(bb,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];
324 return c;
325}
326double veclengthd( double *p )
327{
328 return sqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2]);
329}
330double veclength4d( double *p )
331{
332 return sqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2] + p[3]*p[3]);
333}
334double vecdotd(double *a, double *b)
335{
336 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
337}
338double* vecscaled(double* r, double* v, double s)
339{
340 r[0] = v[0] * s;
341 r[1] = v[1] * s;
342 r[2] = v[2] * s;
343 return r;
344}
345/* returns vector length, too */
346double vecnormald(double *r, double *v)
347{
348 double ret = sqrt(vecdotd(v,v));
349 /* if(ret == 0.) return 0.; */
350 if (APPROX(ret, 0)) return 0.;
351 vecscaled(r,v,1./ret);
352 return ret;
353}
354
355void veccross(struct point_XYZ *c, struct point_XYZ a, struct point_XYZ b)
356{
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;
360}
361float *veccross3f(float *c, float *a, float *b)
362{
363 /*FLOPs 6 float*/
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];
367 return c;
368}
369float veclength( struct point_XYZ p )
370{
371 return (float) sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
372}
373float vecdot3f( float *a, float *b )
374{
375 /*FLOPs 3 float */
376 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
377}
378float vecdot4f( float *a, float *b )
379{
380 return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + + a[3]*b[3];
381}
382double vecdot4d(double* a, double* b)
383{
384 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + +a[3] * b[3];
385}
386
387float *vecset3f(float *b, float x, float y, float z)
388{
389 b[0] = x; b[1] = y; b[2] = z;
390 return b;
391}
392float *vecset4f(float *b, float x, float y, float z, float a)
393{
394 b[0] = x; b[1] = y; b[2] = z; b[3] = a;
395 return b;
396}
397float veclength3f(float *a){
398 return (float)sqrt(vecdot3f(a, a));
399}
400
401double vecangle(struct point_XYZ* V1, struct point_XYZ* V2) {
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) ) );
404};
405float vecangle2f(float * V1, float * V2) {
406 //full circle angele between 2 2D vectors
407 float det, dot, angle;
408 dot = V1[0]*V2[0] + V1[1]*V2[1]; // dot product
409 det = V1[0]*V2[1] - V1[1]*V2[0]; // determinant
410 angle = atan2(det, dot); // atan2(y, x) or atan2(sin, cos)
411 return angle;
412};
413
414float *veccopy4f(float *b, float *a)
415{
416 b[0] = a[0];
417 b[1] = a[1];
418 b[2] = a[2];
419 b[3] = a[3];
420 return b;
421}
422float calc_angle_between_two_vectors3f(float * a, float * b){
423 //scalar angle between 2 vectors (doesn't say which way on a great circle)
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);
432 anglef = acos(dotf);
433 return anglef;
434}
435float calc_angle_between_two_vectors(struct point_XYZ a, struct point_XYZ b)
436{
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);
441
442 /* printf("scalar: %f length_a: %f length_b: %f \n", scalar, length_a, length_b);*/
443
444 /* if (scalar == 0) */
445 if (APPROX(scalar, 0)) {
446 return (float) (M_PI/2.0);
447 }
448
449 if ( (length_a <= 0) || (length_b <= 0)){
450 printf("Divide by 0 in calc_angle_between_two_vectors(): No can do! \n");
451 return 0;
452 }
453
454 temp = scalar /(length_a * length_b);
455 /* printf("temp: %f", temp);*/
456
457 /*acos() appears to be unable to handle 1 and -1 */
458 /* fixed to handle border case where temp <=-1.0 for 0.39 JAS */
459 if ((temp >= 1) || (temp <= -1)){
460 if (temp < 0.0f) return 3.141526f;
461 return 0.0f;
462 }
463 return (float) acos(temp);
464}
465int vecapprox3f(float *a, float *b, float tol){
466 float tmp[3];
467 return veclength3f(vecdif3f(tmp,a,b)) < tol ? TRUE : FALSE;
468}
469int vecapprox2f(float *a, float *b, float tol){
470 float tmp[2];
471 return veclength2f(vecdif2f(tmp,a,b)) < tol ? TRUE : FALSE;
472}
473int vecsame3f(float *a, float *b){
474 int i,isame = TRUE;
475 for(i=0;i<3;i++)
476 if(a[i] != b[i]) isame = FALSE;
477 return isame;
478}
479int vecclose3f(float* a, float* b, float tol) {
480 int isame;
481 isame = TRUE;
482 for (int i = 0; i < 3; i++)
483 if (fabs(a[i] - b[i]) > tol) isame = FALSE;
484 return isame;
485}
486
487int vecsame4f(float *a, float *b){
488 int i,isame = TRUE;
489 for(i=0;i<4;i++)
490 if(a[i] != b[i]) isame = FALSE;
491 return isame;
492}
493int vecsamed(double *a, double *b){
494 int i,isame = TRUE;
495 for(i=0;i<3;i++)
496 if(a[i] != b[i]) isame = FALSE;
497 return isame;
498}
499/* returns vector length, too */
500GLDOUBLE vecnormal(struct point_XYZ*r, struct point_XYZ* v)
501{
502 GLDOUBLE ret = sqrt(vecdot(v,v));
503 /* if(ret == 0.) return 0.; */
504 if (APPROX(ret, 0)) return 0.;
505 vecscale(r,v,1./ret);
506 return ret;
507}
508float vecnormsquared3f(float *a){
509 return a[0] * a[0] + a[1] * a[1] + a[2] * a[2];
510}
511float *vecscale3f(float *b, float *a, float scale){
512 b[0] = a[0] * scale;
513 b[1] = a[1] * scale;
514 b[2] = a[2] * scale;
515 return b;
516}
517float *vecscale4f(float *b, float *a, float scale){
518 b[0] = a[0] * scale;
519 b[1] = a[1] * scale;
520 b[2] = a[2] * scale;
521 b[3] = a[3] * scale;
522 return b;
523}
524double* vecscale4d(double* b, double* a, double scale) {
525 b[0] = a[0] * scale;
526 b[1] = a[1] * scale;
527 b[2] = a[2] * scale;
528 b[3] = a[3] * scale;
529 return b;
530}
531float *vecmult3f(float *c, float *a, float *b){
532 /* c[i] = a[i]*b[i] */
533 int i=0;
534 for(;i<3;i++) c[i] = a[i]*b[i];
535 return c;
536}
537double* vecmult3d(double* c, double* a, double* b) {
538 /* c[i] = a[i]*b[i] */
539 int i = 0;
540 for (; i < 3; i++) c[i] = a[i] * b[i];
541 return c;
542}
543
544float *vecmult2f(float *c, float *a, float *b){
545 /* c[i] = a[i]*b[i] */
546 int i=0;
547 for(;i<2;i++) c[i] = a[i]*b[i];
548 return c;
549}
550double* vecmult2d(double* c, double* a, double* b) {
551 /* c[i] = a[i]*b[i] */
552 int i = 0;
553 for (; i < 2; i++) c[i] = a[i] * b[i];
554 return c;
555}
556
557float *vecnormalize3f(float *b, float *a)
558{
559 float norm = veclength3f(a);
560 if (APPROX(norm, 0.0f)){
561 b[0] = 1.0f; b[1] = 0.0f; b[2] = 0.0f;
562 }else{
563 vecscale3f(b, a, 1.0f / norm);
564 }
565 return b;
566}
567/*will add functions here as needed. */
568
569GLDOUBLE det3x3(GLDOUBLE* data)
570{
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];
572}
573float det3f(float *a, float *b, float *c)
574{
575 /*FLOPs 9 float: dot 3, cross 6 */
576 float temp[3];
577 return vecdot3f(a,veccross3f(temp,b, c));
578}
579double det3d(double *a, double *b, double *c)
580{
581 /*FLOPs 9 float: dot 3, cross 6 */
582 double temp[3];
583 return vecdotd(a,veccrossd(temp,b, c));
584}
585struct point_XYZ* transform(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b)
586{
587 //FLOPs 9 double
588 // r = a x b
589 struct point_XYZ tmp; /* JAS*/
590
591 if(r != a) { /*protect from self-assignments */
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];
595 } else {
596 /* JAS was - struct point_XYZ tmp = {a->x,a->y,a->z};*/
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];
601 }
602 return r;
603}
604GLDOUBLE* transformUPPER3X3d(double *r,double *a, const GLDOUBLE* b){
605 double tmp[3];
606 veccopyd(tmp,a);
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];
610 return r;
611}
612struct point_XYZ* transformAFFINE(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b){
613 //FLOPs 9 double
614 // r = a x b
615 return transform(r,a,b);
616}
617GLDOUBLE* pointxyz2double(double* r, struct point_XYZ *p){
618 r[0] = p->x; r[1] = p->y; r[2] = p->z;
619 return r;
620}
621struct point_XYZ* double2pointxyz(struct point_XYZ* r, double* p){
622 r->x = p[0]; r->y = p[1]; r->z = p[2];
623 return r;
624}
625double * matrixAFFINE2RotationMatrix(double* rotmat, double *fullmat){
626 //takes a full affine matrix
627 // and returns a pure rotation matrix
628 //could be used in background and/or billboard
629 //in freewrl terminology, an affine matrix has no perspectives (typical for modelview matrix):
630 //- perspectives are zero
631 //- has translations rotations, scale and shear
632 //- shear is not seen in web3d or very rare (an assymmetric offdiagonal); here we assume no shear
633 double sx,sy,sz, pp[3], q[3],p[3],d[3], matinv[16], matt[16], matr[16], mats[16];
634
635 //cancel / undo translation part
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);
641
642 //cancel/undo scale part, so that our background mesh stays at radius 1.0
643 /* Get scale */
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);
651 vecdifd(d,q,p);
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));
656 /* Undo the scale effects */
657 matscale(mats,sx,sy,sz);
658 matmultiplyAFFINE(rotmat,mats,matr);
659
660 //result should be pure rotation matrix (with possible rare shear)
661 return rotmat; //we return it too, in case you want to do fancy chain multiplication
662}
666void RotationMatrixtoAxisAngle(double *axisangle, double *mat4) {
667// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/
668 //matrix must be pure rotation matrix
669 double angle, x, y, z; // variables for result
670 double epsilon = 0.01; // margin to allow for rounding errors
671 double epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees
672 // optional check that input is pure rotation, 'isRotationMatrix' is defined at:
673 // https://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/
674 double* m[3];
675 m[0] = &mat4[0];
676 m[1] = &mat4[4];
677 m[2] = &mat4[8];
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)) {
681 // singularity found
682 // first check for identity matrix which must have +1 for all terms
683 // in leading diagonaland zero in other terms
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)) {
688 // this singularity is identity matrix so angle = 0
689 vecsetd(axisangle, 0.0, 1.0, 0.0);
690 axisangle[3] = 0.0;
691 return; // new axisAngle(0, 1, 0, 0); // zero angle, arbitrary axis
692 }
693 // otherwise this singularity is angle = 180
694 angle = PI;
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)) { // m[0][0] is the largest diagonal term
702 if (xx < epsilon) {
703 x = 0;
704 y = 0.7071;
705 z = 0.7071;
706 }
707 else {
708 x = sqrt(xx);
709 y = xy / x;
710 z = xz / x;
711 }
712 }
713 else if (yy > zz) { // m[1][1] is the largest diagonal term
714 if (yy < epsilon) {
715 x = 0.7071;
716 y = 0;
717 z = 0.7071;
718 }
719 else {
720 y = sqrt(yy);
721 x = xy / y;
722 z = yz / y;
723 }
724 }
725 else { // m[2][2] is the largest diagonal term so base result on this
726 if (zz < epsilon) {
727 x = 0.7071;
728 y = 0.7071;
729 z = 0;
730 }
731 else {
732 z = sqrt(zz);
733 x = xz / z;
734 y = yz / z;
735 }
736 }
737 vecsetd(axisangle, x, y, z);
738 axisangle[3] = angle;
739 return; // new axisAngle(angle, x, y, z); // return 180 deg rotation
740 }
741 // as we have reached here there are no singularities so we can handle normally
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])); // used to normalise
745 if (abs(s) < 0.001) s = 1;
746 // prevent divide by zero, should not happen if matrix is orthogonal and should be
747 // caught by singularity test above, but I've left it in just in case
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;
754 return; // new axisAngle(angle, x, y, z);
755}
756void AFFINEmatrix2axisangled(double* axisangle, double* matrix4) {
757 //convert to pure rotation matrix
758 double rotmat4[16];
759 matrixAFFINE2RotationMatrix(rotmat4, matrix4);
760 //get axis angle
761 RotationMatrixtoAxisAngle(axisangle, rotmat4);
762}
763double *transformAFFINEd(double *r, double *a, const GLDOUBLE* mat){
764 // r = a x mat
765 struct point_XYZ pa, pr;
766 double2pointxyz(&pa,a);
767 transformAFFINE(&pr,&pa,mat);
768 pointxyz2double(r,&pr);
769 return r;
770}
771double *transformFULL4d(double *r, double *a, double *mat){
772 //same as __gluMultMatrixVecd elsewhere
773 int i;
774 for (i=0; i<4; i++) {
775 r[i] =
776 a[0] * mat[0*4+i] +
777 a[1] * mat[1*4+i] +
778 a[2] * mat[2*4+i] +
779 a[3] * mat[3*4+i];
780 }
781 return r;
782}
783float* transformf(float* r, const float* a, const GLDOUBLE* b)
784{
785 //r = a x b
786 float tmp[3]; /* JAS*/
787
788 if(r != a) { /*protect from self-assignments */
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]);
792 } else {
793 tmp[0] =a[0]; tmp[1] = a[1]; tmp[2] = a[2]; /* JAS*/
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]);
797 }
798 return r;
799}
800float* matmultvec4f(float* r4, float *mat4, float* a4 )
801{
802 int i,j;
803 float t4[4], *b[4];
804 memcpy(t4,a4,4*sizeof(float));
805 for(i=0;i<4;i++){
806 r4[i] = 0.0f;
807 b[i] = &mat4[i*4];
808 for(j=0;j<4;j++)
809 r4[i] += b[i][j]*t4[j];
810 }
811 return r4;
812}
813double* matmultvec4d(double* r4, double* mat4, double* a4)
814{
815 int i, j;
816 double t4[4], * b[4];
817 memcpy(t4, a4, 4 * sizeof(double));
818 for (i = 0; i < 4; i++) {
819 r4[i] = 0.0f;
820 b[i] = &mat4[i * 4];
821 for (j = 0; j < 4; j++)
822 r4[i] += b[i][j] * t4[j];
823 }
824 return r4;
825}
826float* vecmultmat4f_broken(float* r4, float* a4, float *mat4 )
827{
828 int i,j;
829 float t4[4], *b[4];
830 memcpy(t4,a4,4*sizeof(float));
831 for(i=0;i<4;i++){
832 r4[i] = 0.0f;
833 b[i] = &mat4[i*4];
834 for(j=0;j<4;j++)
835 r4[i] += t4[j]*b[j][i];
836 }
837 return r4;
838}
839float* vecmultmat4f(float* r4, float* a4, float *mat4 )
840{
841 int i,j;
842 float t4[4], *b;
843 memcpy(t4,a4,4*sizeof(float));
844 for(i=0;i<4;i++){
845 r4[i] = 0.0f;
846 b = &mat4[i*4];
847 for(j=0;j<4;j++)
848 r4[i] += t4[j]*b[j];
849 }
850 return r4;
851}
852
853double* vecmultmat4d(double* r4, double* a4, double* mat4)
854{
855 int i, j;
856 double t4[4];
857 memcpy(t4, a4, 4 * sizeof(double));
858 for (i = 0; i < 4; i++) {
859 r4[i] = 0.0f;
860 for (j = 0; j < 4; j++)
861 r4[i] += t4[j] * mat4[j * 4 + i];
862 }
863 return r4;
864}
865
866float* matmultvec3f(float* r3, float *mat3, float* a3 )
867{
868 int i,j;
869 float t3[3], *b[3];
870 memcpy(t3,a3,3*sizeof(float));
871 for(i=0;i<3;i++){
872 r3[i] = 0.0f;
873 b[i] = &mat3[i*3];
874 for(j=0;j<3;j++)
875 r3[i] += b[i][j]*t3[j];
876 }
877 return r3;
878}
879float* vecmultmat3f(float* r3, float* a3, float *mat3 )
880{
881 int i,j;
882 float t3[3], *b[3];
883 memcpy(t3,a3,3*sizeof(float));
884 for(i=0;i<3;i++){
885 r3[i] = 0.0f;
886 b[i] = &mat3[i*4];
887 for(j=0;j<3;j++)
888 r3[i] += t3[j]*b[j][i];
889 }
890
891 return r3;
892}
893
894double* matmultvec3d(double* r3, double* mat3, double* a3)
895{
896 int i, j;
897 double t3[3], * b[3];
898 memcpy(t3, a3, 3 * sizeof(double));
899 for (i = 0; i < 3; i++) {
900 r3[i] = 0.0f;
901 b[i] = &mat3[i * 3];
902 for (j = 0; j < 3; j++)
903 r3[i] += b[i][j] * t3[j];
904 }
905 return r3;
906}
907double* vecmultmat3d(double* r3, double* a3, double* mat3)
908{
909 int i, j;
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++) {
914 r3[i] = 0.0f;
915 //b[i] = &mat3[i * 3];
916 for (j = 0; j < 3; j++)
917 r3[i] += t3[j] * b[j][i];
918 }
919
920 return r3;
921}
922
923/*transform point, but ignores translation.*/
924struct point_XYZ* transform3x3(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b)
925{
926 struct point_XYZ tmp;
927
928 if(r != a) { /*protect from self-assignments */
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;
932 } else {
933 /* JAS struct point_XYZ tmp = {a->x,a->y,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;
938 }
939 return r;
940}
941
942struct point_XYZ* vecscale(struct point_XYZ* r, struct point_XYZ* v, GLDOUBLE s)
943{
944 r->x = v->x * s;
945 r->y = v->y * s;
946 r->z = v->z * s;
947 return r;
948}
949double vecdot(struct point_XYZ* a, struct point_XYZ* b)
950{
951 return (a->x*b->x) + (a->y*b->y) + (a->z*b->z);
952}
953
954
955/* returns 0 if p1 is closest, 1 if p2 is closest, and a fraction if the closest point is in between */
956/* To get the closest point, use pclose = retval*p1 + (1-retval)p2; */
957/* y1 must be smaller than y2 */
958/*double closest_point_of_segment_to_y_axis_segment(double y1, double y2, struct point_XYZ p1, struct point_XYZ p2) {
959 double imin = (y1- p1.y) / (p2.y - p1.y);
960 double imax = (y2- p1.y) / (p2.y - p1.y);
961
962 double x21 = (p2.x - p1.x);
963 double z21 = (p2.z - p1.z);
964 double i = (p2.x * x21 + p2.z * z21) /
965 ( x21*x21 + z21*z21 );
966 return max(min(i,imax),imin);
967
968 }*/
969
970double closest_point_of_segment_to_y_axis_segment(double y1, double y2, struct point_XYZ p1, struct point_XYZ p2) {
971 /*cylinder constraints (to be between y1 and y2) */
972 double imin = (y1- p1.y) / (p2.y - p1.y);
973 double imax = (y2- p1.y) / (p2.y - p1.y);
974
975 /*the equation */
976 double x12 = (p1.x - p2.x);
977 double z12 = (p1.z - p2.z);
978 double q = ( x12*x12 + z12*z12 );
979
980 /* double i = ((q == 0.) ? 0 : (p1.x * x12 + p1.z * z12) / q); */
981 double i = ((APPROX(q, 0)) ? 0 : (p1.x * x12 + p1.z * z12) / q);
982
983 printf("imin=%f, imax=%f => ",imin,imax);
984
985 if(imin > imax) {
986 double tmp = imax;
987 imax = imin;
988 imin = tmp;
989 }
990
991 /*clamp constraints to segment*/
992 if(imin < 0) imin = 0;
993 if(imax > 1) imax = 1;
994
995 printf("imin=%f, imax=%f\n",imin,imax);
996
997 /*clamp result to constraints */
998 if(i < imin) i = imin;
999 if(i > imax) i = imax;
1000 return i;
1001
1002}
1003BOOL line_intersect_line_3f(float *p1, float *v1, float *p2, float *v2, float *t, float *s, float *x1, float *x2)
1004{
1005 //from Graphics Gems I, p.304 http://inis.jinr.ru/sl/vol1/CMC/Graphics_Gems_1,ed_A.Glassner.pdf
1006 //L1: P1 + V1*t
1007 //L2: P2 + V2*s
1008 //t and s are at the footpoint/point of closest passing
1009 //t = det(P2-P1,V2,V1xV2) / |V1 x V2|**2
1010 //s = det(P2-P1,V1,V1xV2) / |V1 x V2|**2
1011 float t1[3], cross[3]; //temp intermediate variables
1012 float crosslength2, ss, tt;
1013 veccross3f(cross, v1, v2);
1014 crosslength2 = vecnormsquared3f(cross);
1015 if (APPROX(crosslength2, 0.0f)) return FALSE; //lines are parallel, no intersection
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;
1019 if (x1)
1020 vecadd3f(x1, p1, vecscale3f(t1, v1, tt));
1021 if (x2)
1022 vecadd3f(x2, p2, vecscale3f(t1, v2, ss));
1023 if (t)
1024 *t = tt;
1025 if (s)
1026 *s = ss;
1027 return TRUE; //success we have 2 footpoints.
1028}
1029
1030BOOL line_intersect_planed_3f(float *p, float *v, float *N, float d, float *pi, float *t)
1031{
1032 //from graphics gems I, p.391 http://inis.jinr.ru/sl/vol1/CMC/Graphics_Gems_1,ed_A.Glassner.pdf
1033 // V dot N = d = const for points on a plane, or N dot P + d = 0
1034 // line/ray P1 + v1*t = P2 (intersection point)
1035 // combining t = -(d + N dot P1)/(N dot v1)
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));
1041 if (t) *t = tt;
1042 if (pi) veccopy3f(pi, t2);
1043 return TRUE;
1044}
1045BOOL line_intersect_plane_3f(float *p, float *v, float *N, float *pp, float *pi, float *t)
1046{
1047 float d;
1048 d = vecdot3f(N, pp);
1049 return line_intersect_planed_3f(p, v, N, d, pi, t);
1050}
1051
1052BOOL line_intersect_planed_3d(double *p, double *v, double *N, double d, double *pi, double *t)
1053{
1054 //from graphics gems I, p.391 http://inis.jinr.ru/sl/vol1/CMC/Graphics_Gems_1,ed_A.Glassner.pdf
1055 // V dot N = d = const for points on a plane, or N dot P + d = 0
1056 // line/ray P1 + v1*t = P2 (intersection point)
1057 // combining t = -(d + N dot P1)/(N dot v1)
1058 double t1[3], t2[3], nd, tt;
1059 nd = vecdotd(N, v);
1060 if (APPROX(nd, 0.0)) return FALSE;
1061 tt = -(d + vecdotd(N, p)) / nd;
1062 vecaddd(t2, p, vecscaled(t1, v, tt));
1063 if (t) *t = tt;
1064 if (pi) veccopyd(pi, t2);
1065 return TRUE;
1066}
1067BOOL line_intersect_plane_3d(double *p, double *v, double *N, double *pp, double *pi, double *t)
1068{
1069 double d;
1070 d = vecdotd(N, pp);
1071 return line_intersect_planed_3d(p, v, N, d, pi, t);
1072}
1073BOOL line_intersect_cylinder_3f(float *p, float *v, float radius, float *pi)
1074{
1075 //from rendray_Cylinder
1076 //intersects arbitrary ray (p,v) with cylinder of radius, origiin 0,0,0 and axis 0,1,0
1077 //april 2014: NOT TESTED, NOT USED - just hacked in and compiled
1078 // if((!XEQ) && (!ZEQ)) {
1079 float pp[3];
1080 float dx = v[0];
1081 float dz = v[2];
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;
1085 float und;
1086 if (APPROX(a, 0.0f))return FALSE;
1087 b /= a; c /= a;
1088 und = b*b - 4*c;
1089 if(und > 0) { /* HITS the infinite cylinder */
1090 float t2[3];
1091 //float sol1 = (-b+(float) sqrt(und))/2;
1092 float sol2 = (-b-(float) sqrt(und))/2;
1093 float sol = sol2;// sol1 < sol2 ? sol1 : sol2; //take the one closest to p (but should these be abs? what about direction
1094 vecadd3f(pp, p, vecscale3f(t2, v, sol));
1095 if (pi) veccopy3f(pi, pp);
1096 return TRUE;
1097 }
1098 // }
1099 return FALSE;
1100}
1101
1102struct point_XYZ* vecadd(struct point_XYZ* r, struct point_XYZ* v, struct point_XYZ* v2)
1103{
1104 r->x = v->x + v2->x;
1105 r->y = v->y + v2->y;
1106 r->z = v->z + v2->z;
1107 return r;
1108}
1109
1110struct point_XYZ* vecdiff(struct point_XYZ* r, struct point_XYZ* v, struct point_XYZ* v2)
1111{
1112 r->x = v->x - v2->x;
1113 r->y = v->y - v2->y;
1114 r->z = v->z - v2->z;
1115 return r;
1116}
1117
1118/*i,j,n will form an orthogonal vector space */
1119void make_orthogonal_vector_space(struct point_XYZ* i, struct point_XYZ* j, struct point_XYZ n) {
1120 /* optimal axis finding algorithm. the solution isn't unique.*/
1121 /* each of these three calculations doesn't work (or works poorly)*/
1122 /* in certain distinct cases. (gives zero vectors when two axes are 0)*/
1123 /* selecting the calculations according to smallest axis avoids this problem.*/
1124 /* (the two remaining axis are thus far from zero, if n is normal)*/
1125 if(fabs(n.x) <= fabs(n.y) && fabs(n.x) <= fabs(n.z)) { /* x smallest*/
1126 i->x = 0;
1127 i->y = n.z;
1128 i->z = -n.y;
1129 vecnormal(i,i);
1130 j->x = n.y*n.y + n.z*n.z;
1131 j->y = (-n.x)*n.y;
1132 j->z = (-n.x)*n.z;
1133 } else if(fabs(n.y) <= fabs(n.x) && fabs(n.y) <= fabs(n.z)) { /* y smallest*/
1134 i->x = -n.z;
1135 i->y = 0;
1136 i->z = n.x;
1137 vecnormal(i,i);
1138 j->x = (-n.x)*n.y;
1139 j->y = n.x*n.x + n.z*n.z;
1140 j->z = (-n.y)*n.z;
1141 } else { /* z smallest*/
1142 i->x = n.y;
1143 i->y = -n.x;
1144 i->z = 0;
1145 vecnormal(i,i);
1146 j->x = (-n.x)*n.z;
1147 j->y = (-n.y)*n.z;
1148 j->z = n.x*n.x + n.y*n.y;
1149 }
1150}
1151
1152
1153GLDOUBLE* mattranspose(GLDOUBLE* res, GLDOUBLE* mm)
1154{
1155 GLDOUBLE mcpy[16];
1156 int i, j;
1157 GLDOUBLE *m;
1158
1159 m = mm;
1160 if(res == m) {
1161 memcpy(mcpy,m,sizeof(GLDOUBLE)*16);
1162 m = mcpy;
1163 }
1164
1165 for (i = 0; i < 4; i++) {
1166 //for (j = i + 1; j < 4; j++) {
1167 // res[i*4+j] = m[j*4+i];
1168 //}
1169 for (j = 0; j < 4; j++) {
1170 res[i*4+j] = m[j*4+i];
1171 }
1172 }
1173 return res;
1174}
1175
1176float* mattranspose4f(float* res, float* mm)
1177{
1178 float mcpy[16];
1179 int i, j;
1180 float *m;
1181
1182 m = mm;
1183 if(res == m) {
1184 memcpy(mcpy,m,sizeof(float)*16);
1185 m = mcpy;
1186 }
1187
1188 for (i = 0; i < 4; i++) {
1189 for (j = 0; j < 4; j++) {
1190 res[i*4+j] = m[j*4+i];
1191 }
1192 }
1193 return res;
1194}
1195float* mattranspose3f(float* res, float* mm)
1196{
1197 float mcpy[9];
1198 int i, j;
1199 float *m;
1200
1201 m = mm;
1202 if(res == m) {
1203 memcpy(mcpy,m,sizeof(float)*9);
1204 m = mcpy;
1205 }
1206
1207 for (i = 0; i < 3; i++) {
1208 for (j = 0; j < 3; j++) {
1209 res[i*3+j] = m[j*3+i];
1210 }
1211 }
1212 return res;
1213}
1214double* mattranspose3d(double* res, double* mm)
1215{
1216 double mcpy[9];
1217 int i, j;
1218 double* m;
1219
1220 m = mm;
1221 if (res == m) {
1222 memcpy(mcpy, m, sizeof(double) * 9);
1223 m = mcpy;
1224 }
1225
1226 for (i = 0; i < 3; i++) {
1227 for (j = 0; j < 3; j++) {
1228 res[i * 3 + j] = m[j * 3 + i];
1229 }
1230 }
1231 return res;
1232}
1233
1234GLDOUBLE* matinverse98(GLDOUBLE* res, GLDOUBLE* mm)
1235{
1236 /*FLOPs 98 double: det3 9, 1/det 1, adj3x3 9x4=36, inv*T 13x4=52 */
1237 //July 2016 THIS IS WRONG DON'T USE
1238 //see the glu equivalent elsewhere
1239 //you can check with A*A-1 = I and this function doesn't give I
1240 double Deta;
1241 GLDOUBLE mcpy[16];
1242 GLDOUBLE *m;
1243
1244 m = mm;
1245 if(res == m) {
1246 memcpy(mcpy,m,sizeof(GLDOUBLE)*16);
1247 m = mcpy;
1248 }
1249
1250 Deta = det3x3(m);
1251 if(APPROX(Deta,0.0))
1252 printf("deta 0\n");
1253 Deta = 1.0 / Deta;
1254
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;
1259
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;
1264
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;
1269
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;
1274
1275 return res;
1276}
1277float* matinverse4f(float* res, float* mm)
1278{
1279 /*FLOPs 98 float: det3 9, 1/det 1, adj3x3 9x4=36, inv*T 13x4=52 */
1280
1281 float Deta;
1282 float mcpy[16];
1283 float *m;
1284
1285 m = mm;
1286 if(res == m) {
1287 memcpy(mcpy,m,sizeof(float)*16);
1288 m = mcpy;
1289 }
1290
1291 Deta = det3f(&m[0],&m[4],&m[8]); //det3x3(m);
1292 Deta = 1.0f / Deta;
1293
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;
1298
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;
1303
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;
1308
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;
1313
1314 return res;
1315}
1316
1317struct point_XYZ* polynormal(struct point_XYZ* r, struct point_XYZ* p1, struct point_XYZ* p2, struct point_XYZ* p3) {
1318 struct point_XYZ v1;
1319 struct point_XYZ v2;
1320 VECDIFF(*p2,*p1,v1);
1321 VECDIFF(*p3,*p1,v2);
1322 veccross(r,v1,v2);
1323 vecnormal(r,r);
1324 return r;
1325}
1326
1327/*simple wrapper for now. optimize later */
1328struct point_XYZ* polynormalf(struct point_XYZ* r, float* p1, float* p2, float* p3) {
1329 struct point_XYZ pp[3];
1330 pp[0].x = p1[0];
1331 pp[0].y = p1[1];
1332 pp[0].z = p1[2];
1333 pp[1].x = p2[0];
1334 pp[1].y = p2[1];
1335 pp[1].z = p2[2];
1336 pp[2].x = p3[0];
1337 pp[2].y = p3[1];
1338 pp[2].z = p3[2];
1339 return polynormal(r,pp+0,pp+1,pp+2);
1340}
1341
1342
1343GLDOUBLE* matrotate(GLDOUBLE* Result, double Theta, double x, double y, double z)
1344{
1345 GLDOUBLE CosTheta = cos(Theta);
1346 GLDOUBLE SinTheta = sin(Theta);
1347
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;
1357 Result[3] = 0;
1358 Result[7] = 0;
1359 Result[11] = 0;
1360 Result[12] = 0;
1361 Result[13] = 0;
1362 Result[14] = 0;
1363 Result[15] = 1;
1364
1365 return Result;
1366}
1367
1368GLDOUBLE* mattranslate(GLDOUBLE* r, double dx, double dy, double dz)
1369{
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] =
1373 r[11] = 0;
1374 r[12] = dx;
1375 r[13] = dy;
1376 r[14] = dz;
1377 return r;
1378}
1379GLDOUBLE* matscale(GLDOUBLE* r, double sx, double sy, double sz)
1380{
1381
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;
1386 r[0] = sx;
1387 r[5] = sy;
1388 r[10] = sz;
1389 r[15] = 1.0;
1390 return r;
1391}
1392
1393GLDOUBLE* matmultiplyFULL(GLDOUBLE* r, GLDOUBLE* mm , GLDOUBLE* nn)
1394{
1395 /* full 4x4, will do perspectives
1396 FLOPs 64 double: 4x4x4
1397 r = mm x nn
1398 */
1399 GLDOUBLE tm[16],tn[16];
1400 GLDOUBLE *m, *n;
1401 int i,j,k;
1402 /* prevent self-multiplication problems.*/
1403 m = mm;
1404 n = nn;
1405 if(r == m) {
1406 memcpy(tm,m,sizeof(GLDOUBLE)*16);
1407 m = tm;
1408 }
1409 if(r == n) {
1410 memcpy(tn,n,sizeof(GLDOUBLE)*16);
1411 n = tn;
1412 }
1413 if(0){
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);
1418 }
1419 if(nn[i + 12] != 0.0){
1420 double p = nn[i +12];
1421 printf("FT[%d]%f ",i,p);
1422 }
1423 }
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);
1428 }
1429 if(nn[i*4 + 3] != 0.0){
1430 double p = nn[i*4 + 3];
1431 printf("FP[%d]%f ",i,p);
1432 }
1433 }
1434 }
1435 /* assume 4x4 homgenous transform */
1436 for(i=0;i<4;i++)
1437 for(j=0;j<4;j++)
1438 {
1439 r[i*4+j] = 0.0;
1440 for(k=0;k<4;k++)
1441 r[i*4+j] += m[i*4+k]*n[k*4+j];
1442 }
1443 return r;
1444}
1445
1446GLDOUBLE* matmultiplyAFFINE(GLDOUBLE* r, GLDOUBLE* nn , GLDOUBLE* mm)
1447{
1448 /* AFFINE subset - ignores perspectives, use for MODELVIEW
1449 FLOPs 36 double: 3x3x4
1450 r = nn x mm
1451 */
1452 GLDOUBLE tm[16],tn[16];
1453 GLDOUBLE *m, *n;
1454 int i; //,j,k;
1455 /* prevent self-multiplication problems.*/
1456 m = mm;
1457 n = nn;
1458 if(r == m) {
1459 memcpy(tm,m,sizeof(GLDOUBLE)*16);
1460 m = tm;
1461 }
1462 if(r == n) {
1463 memcpy(tn,n,sizeof(GLDOUBLE)*16);
1464 n = tn;
1465 }
1466 if(0){
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);
1471 }
1472 if(nn[i + 12] != 0.0){
1473 double p = nn[i +12];
1474 printf("AT[%d]%lf ",i,p);
1475 }
1476 }
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);
1481 }
1482 if(nn[i*4 + 3] != 0.0){
1483 double p = nn[i*4 + 3];
1484 printf("AP[%d]%lf ",i,p);
1485 }
1486 }
1487 }
1488
1489
1490 /* this method ignors the perspectives */
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];
1495
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];
1500
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];
1505
1506 r[3] = r[7] = r[11] = 0.0;
1507 r[15] = 1.0;
1508
1509 return r;
1510}
1511GLDOUBLE* matmultiply(GLDOUBLE* r, GLDOUBLE* mm , GLDOUBLE* nn){
1512 return matmultiplyFULL(r,mm,nn);
1513}
1514
1515float* matmultiply4f(float* r, float* mm , float* nn)
1516{
1517 /* FLOPs 64 float: N^3 = 4x4x4
1518 r = mm x nn
1519 */
1520 float tm[16],tn[16];
1521 float *m, *n;
1522 int i,j,k;
1523 /* prevent self-multiplication problems.*/
1524 m = mm;
1525 n = nn;
1526 if(r == m) {
1527 memcpy(tm,m,sizeof(float)*16);
1528 m = tm;
1529 }
1530 if(r == n) {
1531 memcpy(tn,n,sizeof(float)*16);
1532 n = tn;
1533 }
1534 /* assume 4x4 homgenous transform */
1535 for(i=0;i<4;i++)
1536 for(j=0;j<4;j++)
1537 {
1538 r[i*4+j] = 0.0;
1539 for(k=0;k<4;k++)
1540 r[i*4+j] += m[i*4+k]*n[k*4+j];
1541 }
1542 return r;
1543}
1544float* matmultiply3f(float* r, float* mm , float* nn)
1545{
1546 /* FLOPs 27 float: N^3 = 3x3x3
1547 r = mm x nn
1548 */
1549 float tm[9],tn[9];
1550 float *m, *n;
1551 int i,j,k;
1552 /* prevent self-multiplication problems.*/
1553 m = mm;
1554 n = nn;
1555 if(r == m) {
1556 memcpy(tm,m,sizeof(float)*9);
1557 m = tm;
1558 }
1559 if(r == n) {
1560 memcpy(tn,n,sizeof(float)*9);
1561 n = tn;
1562 }
1563 /* assume 4x4 homgenous transform */
1564 for(i=0;i<3;i++)
1565 for(j=0;j<3;j++)
1566 {
1567 r[i*3+j] = 0.0;
1568 for(k=0;k<3;k++)
1569 r[i*3+j] += m[i*3+k]*n[k*3+j];
1570 }
1571 return r;
1572}
1573double* matmultiply3d(double* r, double* mm, double* nn)
1574{
1575 /* FLOPs 27 float: N^3 = 3x3x3
1576 r = mm x nn
1577 */
1578 double tm[9], tn[9];
1579 double* m, * n;
1580 int i, j, k;
1581 /* prevent self-multiplication problems.*/
1582 m = mm;
1583 n = nn;
1584 if (r == m) {
1585 memcpy(tm, m, sizeof(double) * 9);
1586 m = tm;
1587 }
1588 if (r == n) {
1589 memcpy(tn, n, sizeof(double) * 9);
1590 n = tn;
1591 }
1592 /* assume 4x4 homgenous transform */
1593 for (i = 0; i < 3; i++)
1594 for (j = 0; j < 3; j++)
1595 {
1596 r[i * 3 + j] = 0.0;
1597 for (k = 0; k < 3; k++)
1598 r[i * 3 + j] += m[i * 3 + k] * n[k * 3 + j];
1599 }
1600 return r;
1601}
1602float *axisangle_rotate3f(float* b, float *a, float *axisangle)
1603{
1604 /* http://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
1605 uses Rodrigues formula axisangle (axis,angle)
1606 somewhat expensive, so if tranforming many points with the same rotation,
1607 it might be more efficient to use another method (like axisangle -> matrix, then matrix transforms)
1608 b = a*cos(angle) + (axis cross a)*sin(theta) + axis*(axis dot a)*(1 - cos(theta))
1609 */
1610 float cosine, sine, cross[3], dot, theta, *axis, t1[3], t2[3],t3[3],t4[3];
1611 theta = axisangle[3];
1612 axis = axisangle;
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))));
1618 return b;
1619}
1620double* axisangle_rotate3d(double* b, double* a, float* axisangle)
1621{
1622 /* http://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
1623 uses Rodrigues formula axisangle (axis,angle)
1624 somewhat expensive, so if tranforming many points with the same rotation,
1625 it might be more efficient to use another method (like axisangle -> matrix, then matrix transforms)
1626 b = a*cos(angle) + (axis cross a)*sin(theta) + axis*(axis dot a)*(1 - cos(theta))
1627 */
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))));
1636 return b;
1637}
1638
1639struct SFRotation *sfrotation_multiply(struct SFRotation* T, struct SFRotation *A, struct SFRotation *B);
1640float *axisangle_rotate4f(float* axisAngleC, float *axisAngleA, float *axisAngleB)
1641{
1642 if(1){
1643 //this is a float[4] version of / wrapper for sfrotation_multiply
1644 struct SFRotation A,B,C;
1645 veccopy4f(A.c,axisAngleA);
1646 veccopy4f(B.c,axisAngleB);
1647 sfrotation_multiply(&C,&A,&B);
1648 veccopy4f(axisAngleC,C.c);
1649 }else{
1650 // Dec 2017, dug9: I think the rodrigues chain is too hard for us:
1651 // https://math.stackexchange.com/questions/382760/composition-of-two-axis-angle-rotations
1652 // and its math looks a lot like quaternion math anyway, I've read its equivalent
1653 // http://www.mathoman.com/en/index.php/1537-axis-and-angle-of-the-composition-of-two-rotations
1654 // one way I can visualize: rotating a point by 2 rotations, then using the start and end points
1655 // to get an angle between them and axis perpendicular to the new angle
1656 // dug9's (untested) formula
1657 // (uses full angles, not the half-angles seen in rodrigues or quaternions, so likely wrong)
1658 // for axis angles C = A * B
1659 // 1. find an arbitrary point p1 (xyz) on the perpendicular plane to B's axis, on unit sphere
1660 // i) using a cross product trick:
1661 // a) find the largest compoent of B.axis {x,y,z}
1662 // b) exhange that compoent with any other component ie a2 = {y,x,z)
1663 // c) p1 = normalize( a2 cross B.axis )
1664 // problem: what if A.axis is right on p1? then A.angle will have no effect / be 'missed' in result
1665 // ii) perpendicular to both axes
1666 // perpaxis = B.axis cross A.axis
1667 // if(length(perpaxis) == 0)
1668 // axes are parallel, just add angle
1669 // C = (B.axis, B.angle + A.angle)
1670 // else
1671 // p1 = normalize(perpaxis)
1672 // 2. rotate point p1 by B -> p2. (It will still be on B's perpendicular plane)
1673 // 3. rotate point p2 by A -> p3. (now we have 2 points on a sphere, p1, p3)
1674 // 4. find the perpendicular axis shared by origin O (0,0,0), p1 and p3.
1675 // axis_sinealpha = (p1-O) cross (p3 - O) = p1 cross p3 (cross product is scaled by sin(alpha))
1676 // sine_alpha = length(axis_sinealpha)
1677 // axis = axis_sinealpha / sine_alpha
1678 // 5. find the cosine_alpha = p3 dot p1
1679 // 6. find the full circle angle
1680 // angle = atan2(sine_alpha,cosine_alpha)
1681 // 7. C = (axis,angle)
1682 // Hypothesis: substituting half-angles in A,B before beginning, and doubling the final angle in C
1683 // will give the quaternion/rodrigues equivalent full angle
1684 // not implemented.
1685 veccopy4f(axisAngleC,axisAngleA);
1686 }
1687 return axisAngleC; //so can chain
1688}
1689double *matidentity4d(double *b){
1690 // zeros a 4x4 and puts 1's down the diagonal to make a 4x4 identity matrix
1691 int i,j;
1692 double *mat[4];
1693 for(i=0;i<4;i++){
1694 mat[i] = &b[i*4];
1695 for(j=0;j<4;j++)
1696 mat[i][j] = 0.0;
1697 mat[i][i] = 1.0;
1698 }
1699 return b;
1700}
1701double *mattranslate4d(double *mat, double* xyz){
1702 // untested, want it to work like fw_glTranslated
1703 double mtemp[16];
1704 matidentity4d(mtemp);
1705 mtemp[12] = xyz[0];
1706 mtemp[13] = xyz[1];
1707 mtemp[14] = xyz[2];
1708 matmultiplyFULL(mat,mtemp,mat);
1709 return mat;
1710}
1711double* matscale4d(double* mat, double* sxyz) {
1712 // untested, want it to work like fw_glTranslated
1713 double mtemp[16];
1714 matidentity4d(mtemp);
1715 mtemp[0] = sxyz[0];
1716 mtemp[5] = sxyz[1];
1717 mtemp[10] = sxyz[2];
1718 matmultiplyFULL(mat, mtemp, mat);
1719 return mat;
1720}
1721float *matidentity4f(float *b){
1722 // zeros a 4x4 and puts 1's down the diagonal to make a 4x4 identity matrix
1723 int i,j;
1724 float *mat[4];
1725 for(i=0;i<4;i++){
1726 mat[i] = &b[i*4];
1727 for(j=0;j<4;j++)
1728 mat[i][j] = 0.0f;
1729 mat[i][i] = 1.0f;
1730 }
1731 return b;
1732}
1733float *matidentity3f(float *b){
1734 // zeros a 3x3 and puts 1's down the diagonal to make a 4x4 identity matrix
1735 int i;
1736 for(i=0;i<9;i++) b[i] = 0.0f;
1737 for(i=0;i<3;i++) b[i*3 +i] = 1.0f;
1738 return b;
1739}
1740double* matidentity3d(double* b) {
1741 // zeros a 3x3 and puts 1's down the diagonal to make a 4x4 identity matrix
1742 int i;
1743 for (i = 0; i < 9; i++) b[i] = 0.0;
1744 for (i = 0; i < 3; i++) b[i * 3 + i] = 1.0;
1745 return b;
1746}
1747float *axisangle2matrix4f(float *b, float *axisangle){
1748 //untested as of july 2014
1749 int i; //,j;
1750 float *mat[4];
1751 for(i=0;i<4;i++)
1752 mat[i] = &b[i*4];
1753 matidentity4f(b);
1754 //rotate identity using axisangle
1755 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
1756 axisangle_rotate3f(mat[i], mat[i], axisangle);
1757 }
1758 return b;
1759}
1760
1761void matrixFromAxisAngle4d(double *mat, double rangle, double x, double y, double z)
1762{
1763
1764 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/
1765 int i;
1766 double c, s, t;
1767 double *m[4];
1768 double tmp1;
1769 double tmp2;
1770
1771 c = cos(rangle);
1772 s = sin(rangle);
1773 t = 1.0 - c;
1774 //row indexes
1775 m[0] = &mat[0];
1776 m[1] = &mat[4];
1777 m[2] = &mat[8];
1778 m[3] = &mat[12];
1779 //identity
1780 for(i=0;i<16;i++) mat[i] = 0.0;
1781 for(i=0;i<4;i++) m[i][i] = 1.0;
1782 // if axis is not already normalised then uncomment this
1783 // double magnitude = Math.sqrt(a1.x*a1.x + a1.y*a1.y + a1.z*a1.z);
1784 // if (magnitude==0) throw error;
1785 // a1.x /= magnitude;
1786 // a1.y /= magnitude;
1787 // a1.z /= magnitude;
1788
1789 m[0][0] = c + x*x*t;
1790 m[1][1] = c + y*y*t;
1791 m[2][2] = c + z*z*t;
1792
1793
1794 tmp1 = x*y*t;
1795 tmp2 = z*s;
1796 m[1][0] = tmp1 + tmp2;
1797 m[0][1] = tmp1 - tmp2;
1798 tmp1 = x*z*t;
1799 tmp2 = y*s;
1800 m[2][0] = tmp1 - tmp2;
1801 m[0][2] = tmp1 + tmp2;
1802 tmp1 = y*z*t;
1803 tmp2 = x*s;
1804 m[2][1] = tmp1 + tmp2;
1805 m[1][2] = tmp1 - tmp2;
1806
1807}
1808
1809void rotate_v2v_axisAngled(double* axis, double* angle, double *orig, double *result)
1810{
1811 double cvl;
1812 double dv[3], iv[3], cv[3];
1813
1814 dv[0] = 0;
1815 dv[1] = 0;
1816 dv[2] = 0;
1817
1818 iv[0] = 0;
1819 iv[1] = 0;
1820 iv[2] = 0;
1821
1822 cv[0] = 0;
1823 cv[1] = 0;
1824 cv[2] = 0;
1825
1826 /* step 1 get sin of angle between 2 vectors using cross product rule: ||u x v|| = ||u||*||v||*sin(theta) */
1827 vecnormald(dv,orig); /*normalizes vector to unit length U -> u^ (length 1) */
1828 vecnormald(iv,result);
1829
1830 veccrossd(cv,dv,iv); /*the axis of rotation cv = dv X iv*/
1831 cvl = vecnormald(cv,cv); /* cvl = ||u x v|| / ||u^||*||v^|| = ||u x v|| = sin(theta)*/
1832 veccopyd(axis,cv);
1833
1834 /* if(cvl == 0) { */
1835 if(APPROX(cvl, 0)) {
1836 cv[2] = 1.0;
1837 }
1838 /* 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||)
1839 or, since U,V already unit length from being normalized, cos(theta) = u dot v */
1840 *angle = atan2(cvl,vecdotd(dv,iv)); /*the angle theta = arctan(rise/run) = atan2(sin_theta,cos_theta) in radians*/
1841}
1842/*puts dv back on iv - return the 4x4 rotation matrix that will rotate vector dv onto iv*/
1843double matrotate2v(GLDOUBLE* res, struct point_XYZ iv/*original*/, struct point_XYZ dv/*result*/) {
1844 struct point_XYZ cv;
1845 double cvl,a;
1846
1847 /* step 1 get sin of angle between 2 vectors using cross product rule: ||u x v|| = ||u||*||v||*sin(theta) */
1848 vecnormal(&dv,&dv); /*normalizes vector to unit length U -> u^ (length 1) */
1849 vecnormal(&iv,&iv);
1850
1851 veccross(&cv,dv,iv); /*the axis of rotation cv = dv X iv*/
1852 cvl = vecnormal(&cv,&cv); /* cvl = ||u x v|| / ||u^||*||v^|| = ||u x v|| = sin(theta)*/
1853 /* if(cvl == 0) { */
1854 if(APPROX(cvl, 0)) {
1855 cv.z = 1;
1856 }
1857 /* 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||)
1858 or, since U,V already unit length from being normalized, cos(theta) = u dot v */
1859 a = atan2(cvl,vecdot(&dv,&iv)); /*the angle theta = arctan(rise/run) = atan2(sin_theta,cos_theta) in radians*/
1860 /* step 3 convert rotation angle around unit directional vector of rotation into an equivalent rotation matrix 4x4 */
1861 matrotate(res,a,cv.x,cv.y,cv.z);
1862 return a;
1863}
1864double matrotate2vd(GLDOUBLE* res, double * iv/*original*/, double * dv/*result*/) {
1865 struct point_XYZ piv, pdv;
1866 double2pointxyz(&piv,iv);
1867 double2pointxyz(&pdv,dv);
1868 return matrotate2v(res,piv,pdv);
1869}
1870
1871#define SHOW_NONSINGULARS 0 //or 1 for noisy
1872/****
1873 * hacked from a graphics gem
1874 * Returned value:
1875 * TRUE if input matrix is nonsingular
1876 * FALSE otherwise
1877 *
1878 ***/
1879
1880BOOL matrix3x3_inverse_float(float *inn, float *outt)
1881{
1882 /*FLOPs 40 float: det3 12, 1/det 1, adj3x3 9x3=27 */
1883
1884 float det_1;
1885 float pos, /* neg, */ temp;
1886 float *in[3], *out[3];
1887
1888/*#define ACCUMULATE \
1889// if (temp >= 0.0) \
1890// pos += temp; \
1891// else \
1892 neg += temp;
1893*/
1894
1895#define ACCUMULATE pos += temp;
1896
1897//#define PRECISION_LIMIT 1.0e-7 //(1.0e-15)
1898 in[0] = &inn[0];
1899 in[1] = &inn[3];
1900 in[2] = &inn[6];
1901 out[0] = &outt[0];
1902 out[1] = &outt[3];
1903 out[2] = &outt[6];
1904
1905 /*
1906 * Calculate the determinant of submatrix A and determine if the
1907 * the matrix is singular as limited by the double precision
1908 * floating-point data representation.
1909 */
1910 pos = 0.0f; //neg = 0.0;
1911 temp = in[0][0] * in[1][1] * in[2][2];
1912 ACCUMULATE
1913 temp = in[0][1] * in[1][2] * in[2][0];
1914 ACCUMULATE
1915 temp = in[0][2] * in[1][0] * in[2][1];
1916 ACCUMULATE
1917 temp = -in[0][2] * in[1][1] * in[2][0];
1918 ACCUMULATE
1919 temp = -in[0][1] * in[1][0] * in[2][2];
1920 ACCUMULATE
1921 temp = -in[0][0] * in[1][2] * in[2][1];
1922 ACCUMULATE
1923 det_1 = pos; // + neg;
1924
1925 /* Is the submatrix A singular? */
1926 //if ((det_1 == 0.0) || (abs(det_1 / (pos - neg)) < PRECISION_LIMIT)) {
1927 if(APPROX(det_1,0.0f)){
1928
1929 /* Matrix M has no inverse */
1930
1931 if(SHOW_NONSINGULARS) fprintf (stderr, "affine_matrix4_inverse: singular matrix\n");
1932 return FALSE;
1933 }
1934
1935 else {
1936
1937 /* Calculate inverse(A) = adj(A) / det(A) */
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;
1948
1949 return TRUE;
1950 }
1951}
1952BOOL matrix3x3_inverse_double(double* inn, double* outt)
1953{
1954 /*FLOPs 40 float: det3 12, 1/det 1, adj3x3 9x3=27 */
1955
1956 double det_1;
1957 double pos, /* neg, */ temp;
1958 double* in[3], * out[3];
1959
1960 /*#define ACCUMULATE \
1961 // if (temp >= 0.0) \
1962 // pos += temp; \
1963 // else \
1964 neg += temp;
1965 */
1966
1967#define ACCUMULATE pos += temp;
1968
1969 //#define PRECISION_LIMIT 1.0e-7 //(1.0e-15)
1970 in[0] = &inn[0];
1971 in[1] = &inn[3];
1972 in[2] = &inn[6];
1973 out[0] = &outt[0];
1974 out[1] = &outt[3];
1975 out[2] = &outt[6];
1976
1977 /*
1978 * Calculate the determinant of submatrix A and determine if the
1979 * the matrix is singular as limited by the double precision
1980 * floating-point data representation.
1981 */
1982 pos = 0.0f; //neg = 0.0;
1983 temp = in[0][0] * in[1][1] * in[2][2];
1984 ACCUMULATE
1985 temp = in[0][1] * in[1][2] * in[2][0];
1986 ACCUMULATE
1987 temp = in[0][2] * in[1][0] * in[2][1];
1988 ACCUMULATE
1989 temp = -in[0][2] * in[1][1] * in[2][0];
1990 ACCUMULATE
1991 temp = -in[0][1] * in[1][0] * in[2][2];
1992 ACCUMULATE
1993 temp = -in[0][0] * in[1][2] * in[2][1];
1994 ACCUMULATE
1995 det_1 = pos; // + neg;
1996
1997 /* Is the submatrix A singular? */
1998 //if ((det_1 == 0.0) || (abs(det_1 / (pos - neg)) < PRECISION_LIMIT)) {
1999 if (APPROX(det_1, 0.0)) {
2000
2001 /* Matrix M has no inverse */
2002
2003 if (SHOW_NONSINGULARS) printf("affine_matrix4_inverse: singular matrix\n");
2004 return FALSE;
2005 }
2006
2007 else {
2008
2009 /* Calculate inverse(A) = adj(A) / det(A) */
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;
2020
2021 return TRUE;
2022 }
2023}
2024float * mat423f(float *out3x3, float *in4x4)
2025{
2026 int i,j;
2027 for(i=0;i<3;i++){
2028 for(j=0;j<3;j++)
2029 out3x3[i*3 + j] = in4x4[i*4 + j];
2030 }
2031 return out3x3;
2032}
2033float * matinverse3f(float *out3x3, float *in3x3)
2034{
2035 matrix3x3_inverse_float(in3x3,out3x3);
2036 return out3x3;
2037}
2038double* matinverse3d(double* out3x3, double* in3x3)
2039{
2040 matrix3x3_inverse_double(in3x3, out3x3);
2041 return out3x3;
2042}
2043
2044float * transform3x3f(float *out3, float *in3, float *mat3x3){
2045 int i,j;
2046 float t3[3];
2047 memcpy(t3,in3,3*sizeof(float));
2048 for(i=0;i<3;i++){
2049 out3[i] = 0.0f;
2050 for(j=0;j<3;j++)
2051 out3[i] += t3[j]*mat3x3[j*3 + i];
2052 }
2053
2054 return out3;
2055}
2056
2057BOOL affine_matrix4x4_inverse_float(float *inn, float *outt)
2058{
2059 /*FLOPs 49 float: det3 12, 1/det 1, adj3x3 9x3=27, INV*T=9 */
2060 /* faithful transcription of GraphicsGem II, p.604, Wu
2061 use this for modelview matrix which has no perspectives (vs FULL 4x4 100 FLOPs)
2062 */
2063
2064 float det_1;
2065 float pos, /* neg, */ temp;
2066 float *in[4], *out[4];
2067
2068/*#define ACCUMULATE \
2069// if (temp >= 0.0) \
2070// pos += temp; \
2071// else \
2072 neg += temp;
2073*/
2074
2075#define ACCUMULATE pos += temp;
2076
2077//#define PRECISION_LIMIT 1.0e-7 //(1.0e-15)
2078 in[0] = &inn[0];
2079 in[1] = &inn[4];
2080 in[2] = &inn[8];
2081 in[3] = &inn[12];
2082 out[0] = &outt[0];
2083 out[1] = &outt[4];
2084 out[2] = &outt[8];
2085 out[3] = &outt[12];
2086
2087 /*
2088 * Calculate the determinant of submatrix A and determine if the
2089 * the matrix is singular as limited by the double precision
2090 * floating-point data representation.
2091 */
2092 pos = 0.0f; //neg = 0.0;
2093 temp = in[0][0] * in[1][1] * in[2][2];
2094 ACCUMULATE
2095 temp = in[0][1] * in[1][2] * in[2][0];
2096 ACCUMULATE
2097 temp = in[0][2] * in[1][0] * in[2][1];
2098 ACCUMULATE
2099 temp = -in[0][2] * in[1][1] * in[2][0];
2100 ACCUMULATE
2101 temp = -in[0][1] * in[1][0] * in[2][2];
2102 ACCUMULATE
2103 temp = -in[0][0] * in[1][2] * in[2][1];
2104 ACCUMULATE
2105 det_1 = pos; // + neg;
2106
2107 /* Is the submatrix A singular? */
2108 //if ((det_1 == 0.0) || (abs(det_1 / (pos - neg)) < PRECISION_LIMIT)) {
2109 if(APPROX(det_1,0.0f)){
2110
2111 /* Matrix M has no inverse */
2112 if(SHOW_NONSINGULARS) fprintf (stderr, "affine_matrix4_inverse: singular matrix\n");
2113 return FALSE;
2114 }
2115
2116 else {
2117
2118 /* Calculate inverse(A) = adj(A) / det(A) */
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;
2129
2130 /* Calculat -C * inverse(A) */
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]);
2134
2135 /* Fill in last column */
2136 out[0][3] = out[1][3] = out[2][3] = 0.0f;
2137 out[3][3] = 1.0f;
2138 return TRUE;
2139 }
2140}
2141
2142BOOL affine_matrix4x4_inverse_double(double *inn, double *outt)
2143{
2144 /*FLOPs 49 double: det3 12, 1/det 1, adj3x3 9x3=27, INV*T=9 */
2145 /* faithful transcription of Graphics Gems II, p.604, Wu, FAST MATRIX INVERSION
2146 use this for modelview matrix which has no perspectives (vs FULL 4x4 inverse 102 FLOPs)
2147 */
2148 double det_1;
2149 double pos, /* neg, */ temp;
2150 double *in[4], *out[4];
2151
2152/*#define ACCUMULATE \
2153// if (temp >= 0.0) \
2154// pos += temp; \
2155// else \
2156 neg += temp;
2157*/
2158
2159#define ACCUMULATE pos += temp;
2160
2161//#define PRECISION_LIMIT 1.0e-7 //(1.0e-15)
2162 in[0] = &inn[0];
2163 in[1] = &inn[4];
2164 in[2] = &inn[8];
2165 in[3] = &inn[12];
2166 out[0] = &outt[0];
2167 out[1] = &outt[4];
2168 out[2] = &outt[8];
2169 out[3] = &outt[12];
2170
2171 /*
2172 * Calculate the determinant of submatrix A and determine if the
2173 * the matrix is singular as limited by the double precision
2174 * floating-point data representation.
2175 */
2176 pos = 0.0; //neg = 0.0;
2177 temp = in[0][0] * in[1][1] * in[2][2];
2178 ACCUMULATE
2179 temp = in[0][1] * in[1][2] * in[2][0];
2180 ACCUMULATE
2181 temp = in[0][2] * in[1][0] * in[2][1];
2182 ACCUMULATE
2183 temp = -in[0][2] * in[1][1] * in[2][0];
2184 ACCUMULATE
2185 temp = -in[0][1] * in[1][0] * in[2][2];
2186 ACCUMULATE
2187 temp = -in[0][0] * in[1][2] * in[2][1];
2188 ACCUMULATE
2189 det_1 = pos; // + neg;
2190
2191 /* Is the submatrix A singular? */
2192 //if ((det_1 == 0.0) || (abs(det_1 / (pos - neg)) < PRECISION_LIMIT)) {
2193 if(APPROX(det_1,0.0)){
2194
2195 /* Matrix M has no inverse */
2196 if(SHOW_NONSINGULARS) fprintf (stderr, "affine_matrix4_inverse: singular matrix\n");
2197 return FALSE;
2198 }
2199
2200 else {
2201
2202 /* Calculate inverse(A) = adj(A) / det(A) */
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;
2213
2214 /* Calculat -C * inverse(A) */
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]);
2218
2219 /* Fill in last column */
2220 out[0][3] = out[1][3] = out[2][3] = 0.0;
2221 out[3][3] = 1.0;
2222 return TRUE;
2223 }
2224}
2225GLDOUBLE* matinverseAFFINE(GLDOUBLE* res, GLDOUBLE* mm){
2226 affine_matrix4x4_inverse_double(mm, res);
2227 return res;
2228}
2229GLDOUBLE* matinverseFULL(GLDOUBLE* res, GLDOUBLE* mm){
2230 matinverse98(res,mm);
2231 return res;
2232}
2233GLDOUBLE* matinverse(GLDOUBLE* res, GLDOUBLE* mm){
2234 matinverseFULL(res,mm);
2235 return res;
2236}
2237
2238
2239
2240#ifdef COMMENT
2241/*fast crossproduct using references, that checks for auto-assignments */
2242GLDOUBLE* veccross(GLDOUBLE* r, GLDOUBLE* v1, GLDOUBLE* v2)
2243{
2244 /*check against self-assignment. */
2245 if(r != v1) {
2246 if(r != 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];
2250 } else { /* r == v2 */
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];
2255 }
2256 } else { /* r == v1 */
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];
2261 }
2262 return r;
2263}
2264
2265
2266
2267#endif
2268
2269
2270/*
2271 * apply a scale to the matrix given. Assumes matrix is valid...
2272 *
2273 */
2274
2275void scale_to_matrix (double *mat, struct point_XYZ *scale) {
2276/* copy the definitions from quaternion.h... */
2277#define MAT00 mat[0]
2278#define MAT01 mat[1]
2279#define MAT02 mat[2]
2280#if DJ_KEEP_COMPILER_WARNING
2281#define MAT03 mat[3]
2282#endif
2283#define MAT10 mat[4]
2284#define MAT11 mat[5]
2285#define MAT12 mat[6]
2286#if DJ_KEEP_COMPILER_WARNING
2287#define MAT13 mat[7]
2288#endif
2289#define MAT20 mat[8]
2290#define MAT21 mat[9]
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]
2298#endif
2299
2300 MAT00 *=scale->x;
2301 MAT01 *=scale->x;
2302 MAT02 *=scale->x;
2303 MAT10 *=scale->y;
2304 MAT11 *=scale->y;
2305 MAT12 *=scale->y;
2306 MAT20 *=scale->z;
2307 MAT21 *=scale->z;
2308 MAT22 *=scale->z;
2309}
2310
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 };
2312
2313void loadIdentityMatrix (double *mat) {
2314 memcpy((void *)mat, (void *)identity, sizeof(double)*16);
2315}
2316double *matcopy(double *r, double*mat){
2317 memcpy((void*)r, (void*)mat,sizeof(double)*16);
2318 return r;
2319}
2320float *matdouble2float4(float *rmat4, double *dmat4){
2321 int i;
2322 /* convert GLDOUBLE to float */
2323 for (i=0; i<16; i++) {
2324 rmat4[i] = (float)dmat4[i];
2325 }
2326 return rmat4;
2327}
2328void printmatrix3(GLDOUBLE *mat, char *description, int row_major){
2329 int i,j;
2330 printf("mat %s {\n",description);
2331 if(row_major){
2332 //prints in C row-major order, element numbers remain correct/same
2333 for(i = 0; i< 4; i++) {
2334 printf("mat [%2d-%2d] = ",i*4,(i*4)+3);
2335 for(j=0;j<4;j++)
2336 printf(" %lf ",mat[(i*4)+j]);
2337 printf("\n");
2338 }
2339 }
2340 if(!row_major){
2341 //prints in opengl column-major order, element numbers remain correct/same
2342 for(i=0;i<4;i++){
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]);
2345 }
2346 }
2347 printf("}\n");
2348}
2349void printmatrix2(GLDOUBLE* mat,char* description ) {
2350 static int row_major = FALSE;
2351 printmatrix3(mat,description,row_major);
2352}
2353
2354float *veclerp3f(float *T, float *A, float *B, float alpha){
2355 int i;
2356 for(i=0;i<3;i++){
2357 T[i] = (1.0f - alpha)*A[i] + alpha*B[i];
2358 }
2359 return T;
2360}
2361float *veclerp2f(float *T, float *A, float *B, float alpha){
2362 int i;
2363 for(i=0;i<2;i++){
2364 T[i] = (1.0f - alpha)*A[i] + alpha*B[i];
2365 }
2366 return T;
2367}
2368double *veclerpd(double *T, double *A, double *B, double alpha){
2369 int i;
2370 for(i=0;i<3;i++){
2371 T[i] = (1.0f - alpha)*A[i] + alpha*B[i];
2372 }
2373 return T;
2374}
2375
2376void general_slerp(double *ret, double *p1, double *p2, int size, const double t)
2377{
2378 //not tested as of July16,2011
2379 //goal start slow, speed up in the middle, and slow down when stopping
2380 // (like a sine or cosine wave)
2381 //let omega = t*pi
2382 //then cos omega goes from 1 to -1 natively
2383 //we want scale0 to go from 1 to 0
2384 //scale0 = .5(1+cos(t*pi)) should be in the 1 to 0 range,
2385 //and be 'fastest' in the middle ie at pi/2
2386 //then scale1 = 1 - scale0
2387 double scale0, scale1, omega;
2388 int i;
2389 /* calculate coefficients */
2390 if (0) {
2391 //if ( t > .05 || t < .95 ) {
2392 /* standard case (SLERP) */
2393 omega = t*PI;
2394 scale0 = 0.5*(1.0 + cos(omega));
2395 scale1 = 1.0 - scale0;
2396 } else {
2397 /* p1 & p2 are very close, so do linear interpolation */
2398 scale0 = 1.0 - t;
2399 scale1 = t;
2400 }
2401 for(i=0;i<size;i++)
2402 ret[i] = scale0 * p1[i] + scale1 * p2[i];
2403}
2404void point_XYZ_slerp(struct point_XYZ *ret, struct point_XYZ *p1, struct point_XYZ *p2, const double t){
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);
2410}