1
2
3 /*
4 A* -------------------------------------------------------------------
5 B* This file contains source code for the PyMOL computer program
6 C* Copyright (c) Schrodinger, LLC.
7 D* -------------------------------------------------------------------
8 E* It is unlawful to modify or remove this copyright notice.
9 F* -------------------------------------------------------------------
10 G* Please see the accompanying LICENSE file for further information.
11 H* -------------------------------------------------------------------
12 I* Additional authors of this source file include:
13 -*
14 -*
15 -*
16 Z* -------------------------------------------------------------------
17 */
18 #include"os_predef.h"
19 #include"os_std.h"
20
21 #include"Base.h"
22 #include"Vector.h"
23 #include"Matrix.h"
24
25 #define MASK_01010101 (((unsigned long)(-1))/3)
26 #define MASK_00110011 (((unsigned long)(-1))/5)
27 #define MASK_00001111 (((unsigned long)(-1))/17)
28 #define MASK_11111111 (((unsigned long)(-1))/257)
29
30 #define MASK_01010101010101010101010101010101 MASK_01010101
31 #define MASK_00110011001100110011001100110011 MASK_00110011
32 #define MASK_00001111000011110000111100001111 MASK_00001111
33 #define MASK_00000000111111110000000011111111 MASK_11111111
34 #define MASK_00000000000000001111111111111111 (((unsigned long)(-1))/65537)
35
countBits(unsigned long bits)36 short countBits(unsigned long bits){
37 unsigned long n = bits;
38 n = (n & MASK_01010101010101010101010101010101) + ((n >> 1) & MASK_01010101010101010101010101010101) ;
39 n = (n & MASK_00110011001100110011001100110011) + ((n >> 2) & MASK_00110011001100110011001100110011) ;
40 n = (n & MASK_00001111000011110000111100001111) + ((n >> 4) & MASK_00001111000011110000111100001111) ;
41 n = (n & MASK_00000000111111110000000011111111) + ((n >> 8) & MASK_00000000111111110000000011111111) ;
42 n = (n & MASK_00000000000000001111111111111111) + ((n >> 16) & MASK_00000000000000001111111111111111) ;
43 return (n % 255);
44 }
countBitsInt(int bits)45 short countBitsInt(int bits){
46 /* This could be improved to not splitting
47 the bits into 4 variables in a loop which are 16 bits each */
48 unsigned long n = bits & 65535;
49 short nbits = 0;
50 n = (n & MASK_01010101) + ((n >> 1) & MASK_01010101) ;
51 n = (n & MASK_00110011) + ((n >> 2) & MASK_00110011) ;
52 n = (n & MASK_00001111) + ((n >> 4) & MASK_00001111) ;
53 nbits += (n % 255);
54 return nbits;
55 }
56
57 #ifndef R_SMALL
58 #define R_SMALL 0.000000001
59 #endif
60
61 #ifndef R_MED
62 #define R_MED 0.00001
63 #endif
64
65 #define cPI 3.14159265358979323846 /* pi */
66
67 static const float _0 = 0.0F;
68 static const float _1 = 1.0F;
69 static const double _d0 = 0.0;
70 static const double _d1 = 1.0;
71
mix3f(const float * v1,const float * v2,const float fxn,float * v3)72 void mix3f(const float *v1, const float *v2, const float fxn, float *v3)
73 {
74 float fxn_1 = 1.0F - fxn;
75 v3[0] = v1[0] * fxn_1 + v2[0] * fxn;
76 v3[1] = v1[1] * fxn_1 + v2[1] * fxn;
77 v3[2] = v1[2] * fxn_1 + v2[2] * fxn;
78 }
79
mix3d(const double * v1,const double * v2,const double fxn,double * v3)80 void mix3d(const double *v1, const double *v2, const double fxn, double *v3)
81 {
82 double fxn_1 = 1.0F - fxn;
83 v3[0] = v1[0] * fxn_1 + v2[0] * fxn;
84 v3[1] = v1[1] * fxn_1 + v2[1] * fxn;
85 v3[2] = v1[2] * fxn_1 + v2[2] * fxn;
86 }
87
optimizer_workaround1u(unsigned int value)88 unsigned int optimizer_workaround1u(unsigned int value)
89 {
90 return value;
91 }
92
get_random0to1f()93 float get_random0to1f()
94 {
95 return (rand() / (_1 + RAND_MAX));
96 }
97
pymol_roundf(float f)98 int pymol_roundf(float f)
99 {
100 if(f > 0.0F)
101 return (int) (f + 0.49999F);
102 else
103 return (int) (f - 0.49999F);
104 }
105
dump3i(const int * v,const char * prefix)106 void dump3i(const int *v, const char *prefix)
107 { /* for debugging */
108 printf("%s %8i %8i %8i\n", prefix, v[0], v[1], v[2]);
109 }
110
dump3f(const float * v,const char * prefix)111 void dump3f(const float *v, const char *prefix)
112 { /* for debugging */
113 printf("%s %8.3f %8.3f %8.3f\n", prefix, v[0], v[1], v[2]);
114 }
115
dump2f(const float * v,const char * prefix)116 void dump2f(const float *v, const char *prefix)
117 { /* for debugging */
118 printf("%s %8.3f %8.3f\n", prefix, v[0], v[1]);
119 }
120
dump3d(const double * v,const char * prefix)121 void dump3d(const double *v, const char *prefix)
122 { /* for debugging */
123 printf("%s %8.3f %8.3f %8.3f\n", prefix, v[0], v[1], v[2]);
124 }
125
dump4f(const float * v,const char * prefix)126 void dump4f(const float *v, const char *prefix)
127 { /* for debugging */
128 printf("%s %8.3f %8.3f %8.3f %8.3f\n", prefix, v[0], v[1], v[2], v[3]);
129 }
130
dump33f(const float * m,const char * prefix)131 void dump33f(const float *m, const char *prefix)
132 { /* for debugging */
133 if(m) {
134 printf("%s:0 %8.3f %8.3f %8.3f\n", prefix, m[0], m[1], m[2]);
135 printf("%s:1 %8.3f %8.3f %8.3f\n", prefix, m[3], m[4], m[5]);
136 printf("%s:2 %8.3f %8.3f %8.3f\n", prefix, m[6], m[7], m[8]);
137 } else {
138 printf("%s: (null matrix pointer)\n", prefix);
139 }
140 }
141
dump44f(const float * m,const char * prefix)142 void dump44f(const float *m, const char *prefix)
143 { /* for debugging */
144 if(m) {
145 if(prefix) {
146 printf("%s:0 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[0], m[1], m[2], m[3]);
147 printf("%s:1 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[4], m[5], m[6], m[7]);
148 printf("%s:2 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[8], m[9], m[10], m[11]);
149 printf("%s:3 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[12], m[13], m[14], m[15]);
150 } else {
151 }
152 } else {
153 printf("%s: (null matrix pointer)\n", prefix);
154 }
155
156 }
157
dump44d(const double * m,const char * prefix)158 void dump44d(const double *m, const char *prefix)
159 { /* for debugging */
160 if(m) {
161 printf("%s:0 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[0], m[1], m[2], m[3]);
162 printf("%s:1 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[4], m[5], m[6], m[7]);
163 printf("%s:2 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[8], m[9], m[10], m[11]);
164 printf("%s:3 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[12], m[13], m[14], m[15]);
165 } else {
166 printf("%s: (null matrix pointer)\n", prefix);
167 }
168 }
169
dump33d(const double * m,const char * prefix)170 void dump33d(const double *m, const char *prefix)
171 { /* for debugging */
172 printf("%s:0 %8.3f %8.3f %8.3f\n", prefix, m[0], m[1], m[2]);
173 printf("%s:1 %8.3f %8.3f %8.3f\n", prefix, m[3], m[4], m[5]);
174 printf("%s:2 %8.3f %8.3f %8.3f\n", prefix, m[6], m[7], m[8]);
175 }
176
get_divergent3f(const float * src,float * dst)177 void get_divergent3f(const float *src, float *dst)
178 {
179 if(src[0] != _0) {
180 *(dst++) = -*(src++);
181 *(dst++) = *(src++) + 0.1F;
182 *(dst++) = *(src++);
183 } else if(src[1] != _0) {
184 *(dst++) = *(src++) + 0.1F;
185 *(dst++) = -*(src++);
186 *(dst++) = *(src++);
187 } else {
188 *(dst++) = *(src++) + 0.1F;
189 *(dst++) = *(src++);
190 *(dst++) = -*(src++);
191 }
192 }
193
equal3f(const float * v1,const float * v2)194 int equal3f(const float *v1, const float *v2)
195 {
196 return ((fabs(v1[0] - v2[0]) < R_SMALL) &&
197 (fabs(v1[1] - v2[1]) < R_SMALL) && (fabs(v1[2] - v2[2]) < R_SMALL));
198 }
199
get_random3f(float * x)200 void get_random3f(float *x)
201 { /* this needs to be fixed as in Tinker */
202 x[0] = 0.5F - (rand() / (_1 + RAND_MAX));
203 x[1] = 0.5F - (rand() / (_1 + RAND_MAX));
204 x[2] = 0.5F - (rand() / (_1 + RAND_MAX));
205 normalize3f(x);
206 }
207
get_system3f(float * x,float * y,float * z)208 void get_system3f(float *x, float *y, float *z)
209 { /* make random system */
210 get_random3f(x);
211 get_divergent3f(x, y);
212 cross_product3f(x, y, z);
213 normalize3f(z);
214 cross_product3f(z, x, y);
215 normalize3f(y);
216 normalize3f(x);
217 }
218
get_system1f3f(float * x,float * y,float * z)219 void get_system1f3f(float *x, float *y, float *z)
220 { /* make system in direction of x */
221 get_divergent3f(x, y);
222 cross_product3f(x, y, z);
223 normalize3f(z);
224 cross_product3f(z, x, y);
225 normalize3f(y);
226 normalize3f(x);
227 }
228
get_system2f3f(float * x,float * y,float * z)229 void get_system2f3f(float *x, float *y, float *z)
230 { /* make system in direction of x */
231 cross_product3f(x, y, z);
232 normalize3f(z);
233 cross_product3f(z, x, y);
234 normalize3f(y);
235 normalize3f(x);
236 }
237
extrapolate3f(const float * v1,const float * unit,float * result)238 void extrapolate3f(const float *v1, const float *unit, float *result)
239 {
240 float lsq = lengthsq3f(v1);
241 float dp = dot_product3f(v1, unit);
242 if(dp != 0.0F) {
243 float l2 = lsq / dp;
244 scale3f(unit, l2, result);
245 }
246 }
247
scatter3f(float * v,float weight)248 void scatter3f(float *v, float weight)
249 {
250 float r[3];
251 get_random3f(r);
252 scale3f(r, weight, r);
253 add3f(r, v, v);
254 normalize3f(v);
255 }
256
wiggle3f(float * v,const float * p,const float * s)257 void wiggle3f(float *v, const float *p, const float *s)
258 {
259 float q[3];
260 q[0] = (float) cos((p[0] + p[1] + p[2]) * s[1]);
261 q[1] = (float) cos((p[0] - p[1] + p[2]) * s[1]);
262 q[2] = (float) cos((p[0] + p[1] - p[2]) * s[1]);
263 scale3f(q, s[0], q);
264 add3f(q, v, v);
265 normalize3f(v);
266
267 }
268
copy3d3f(const double * v1,float * v2)269 void copy3d3f(const double *v1, float *v2)
270 {
271 v2[0] = (float) v1[0];
272 v2[1] = (float) v1[1];
273 v2[2] = (float) v1[2];
274 }
275
copy3f3d(const float * v1,double * v2)276 void copy3f3d(const float *v1, double *v2)
277 {
278 v2[0] = (double) v1[0];
279 v2[1] = (double) v1[1];
280 v2[2] = (double) v1[2];
281 }
282
min3f(const float * v1,const float * v2,float * v3)283 void min3f(const float *v1, const float *v2, float *v3)
284 {
285 (v3)[0] = ((v1)[0] < (v2)[0] ? (v1)[0] : (v2)[0]);
286 (v3)[1] = ((v1)[1] < (v2)[1] ? (v1)[1] : (v2)[1]);
287 (v3)[2] = ((v1)[2] < (v2)[2] ? (v1)[2] : (v2)[2]);
288 }
289
max3f(const float * v1,const float * v2,float * v3)290 void max3f(const float *v1, const float *v2, float *v3)
291 {
292 (v3)[0] = ((v1)[0] > (v2)[0] ? (v1)[0] : (v2)[0]);
293 (v3)[1] = ((v1)[1] > (v2)[1] ? (v1)[1] : (v2)[1]);
294 (v3)[2] = ((v1)[2] > (v2)[2] ? (v1)[2] : (v2)[2]);
295 }
296
dot_product3d(const double * v1,const double * v2)297 double dot_product3d(const double *v1, const double *v2)
298 {
299 return (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]);
300 }
301
identity33f(float * m)302 void identity33f(float *m)
303 {
304 m[0] = _1;
305 m[1] = _0;
306 m[2] = _0;
307 m[3] = _0;
308 m[4] = _1;
309 m[5] = _0;
310 m[6] = _0;
311 m[7] = _0;
312 m[8] = _1;
313 }
314
identity33d(double * m)315 void identity33d(double *m)
316 {
317 m[0] = _d1;
318 m[1] = _d0;
319 m[2] = _d0;
320 m[3] = _d0;
321 m[4] = _d1;
322 m[5] = _d0;
323 m[6] = _d0;
324 m[7] = _d0;
325 m[8] = _d1;
326 }
327
identity44f(float * m1)328 void identity44f(float *m1)
329 {
330 int a;
331 for(a = 0; a < 16; a++)
332 m1[a] = _0;
333 for(a = 0; a < 16; a = a + 5)
334 m1[a] = _1;
335 }
336
identity44d(double * m1)337 void identity44d(double *m1)
338 {
339 int a;
340 for(a = 0; a < 16; a++)
341 m1[a] = _d0;
342 for(a = 0; a < 16; a = a + 5)
343 m1[a] = _d1;
344 }
345
346 /*
347 * Check a nxn matrix for identity
348 */
is_identityf(int n,const float * m,float threshold)349 bool is_identityf(int n, const float *m, float threshold)
350 {
351 int n_sq = n * n, stride = n + 1;
352 for (int a = 0; a < n_sq; a++) {
353 float e = (a % stride) ? _0 : _1;
354 if (fabsf(m[a] - e) > threshold)
355 return false;
356 }
357 return true;
358 }
359
360 /*
361 * Check if two matrices are the same. The two matrices may have different
362 * number of rows and columns, in that case only the overlaying upper left
363 * (nrow x min(ncol1, ncol2)) submatrix is compared.
364 */
is_allclosef(int nrow,const float * m1,int ncol1,const float * m2,int ncol2,float threshold)365 bool is_allclosef(int nrow,
366 const float *m1, int ncol1,
367 const float *m2, int ncol2, float threshold)
368 {
369 int ncol = (ncol1 < ncol2) ? ncol1 : ncol2;
370 for (int i = 0; i < nrow; i++) {
371 for (int j = 0; j < ncol; j++) {
372 int a1 = i * ncol1 + j, a2 = i * ncol2 + j;
373 if (fabsf(m1[a1] - m2[a2]) > threshold)
374 return false;
375 }
376 }
377 return true;
378 }
379
380 /*
381 * Check a nxm matrix is a diagonal matrix (non-diagonal elements are zero)
382 */
is_diagonalf(int nrow,const float * m,int ncol,float threshold)383 bool is_diagonalf(int nrow,
384 const float *m, int ncol, float threshold)
385 {
386 if (!ncol)
387 ncol = nrow;
388 for (int i = 0; i < nrow; ++i) {
389 for (int j = 0; j < ncol; ++j) {
390 if (i != j && fabsf(m[i * ncol + j]) > threshold)
391 return false;
392 }
393 }
394 return true;
395 }
396
397 /*
398 * Determinant of the upper left 3x3 submatrix.
399 */
determinant33f(const float * m,int ncol)400 double determinant33f(const float *m, int ncol)
401 {
402 int &a = ncol, b = ncol * 2;
403 return
404 m[0] * (m[a + 1] * (double) m[b + 2] - m[a + 2] * (double) m[b + 1]) -
405 m[1] * (m[a + 0] * (double) m[b + 2] - m[a + 2] * (double) m[b + 0]) +
406 m[2] * (m[a + 0] * (double) m[b + 1] - m[a + 1] * (double) m[b + 0]);
407 }
408
glOrtho44f(float * m1,GLfloat left,GLfloat right,GLfloat bottom,GLfloat top,GLfloat nearVal,GLfloat farVal)409 void glOrtho44f(float *m1,
410 GLfloat left, GLfloat right,
411 GLfloat bottom, GLfloat top,
412 GLfloat nearVal, GLfloat farVal){
413 int a;
414 /* this is set in column major order */
415 for(a = 0; a < 16; a++)
416 m1[a] = _0;
417 m1[0] = 2.f / (right-left);
418 m1[5] = 2.f / (top-bottom);
419 m1[10] = -2.f/(farVal-nearVal);
420 m1[12] = - (right+left) / (right-left);
421 m1[13] = - (top+bottom) / (top-bottom);
422 m1[14] = - (farVal+nearVal) / (farVal-nearVal);
423 m1[15] = _1;
424 }
425
glFrustum44f(float * m1,GLfloat left,GLfloat right,GLfloat bottom,GLfloat top,GLfloat nearVal,GLfloat farVal)426 void glFrustum44f(float *m1,
427 GLfloat left, GLfloat right,
428 GLfloat bottom, GLfloat top,
429 GLfloat nearVal, GLfloat farVal){
430 int a;
431 /* this is set in column major order */
432 for(a = 0; a < 16; a++)
433 m1[a] = _0;
434 m1[0] = 2.f * nearVal / (right-left);
435 m1[5] = 2.f * nearVal / (top-bottom);
436 m1[8] = (right+left) / (right-left);
437 m1[9] = (top+bottom) / (top-bottom);
438 m1[10] = -(farVal+nearVal) / (farVal-nearVal);
439 m1[11] = -1.f;
440 m1[14] = -2.f * (farVal * nearVal) / (farVal - nearVal);
441 }
442
copy44f(const float * src,float * dst)443 void copy44f(const float *src, float *dst)
444 {
445 *(dst++) = *(src++);
446 *(dst++) = *(src++);
447 *(dst++) = *(src++);
448 *(dst++) = *(src++);
449
450 *(dst++) = *(src++);
451 *(dst++) = *(src++);
452 *(dst++) = *(src++);
453 *(dst++) = *(src++);
454
455 *(dst++) = *(src++);
456 *(dst++) = *(src++);
457 *(dst++) = *(src++);
458 *(dst++) = *(src++);
459
460 *(dst++) = *(src++);
461 *(dst++) = *(src++);
462 *(dst++) = *(src++);
463 *(dst++) = *(src++);
464 }
465
copy44d(const double * src,double * dst)466 void copy44d(const double *src, double *dst)
467 {
468 *(dst++) = *(src++);
469 *(dst++) = *(src++);
470 *(dst++) = *(src++);
471 *(dst++) = *(src++);
472
473 *(dst++) = *(src++);
474 *(dst++) = *(src++);
475 *(dst++) = *(src++);
476 *(dst++) = *(src++);
477
478 *(dst++) = *(src++);
479 *(dst++) = *(src++);
480 *(dst++) = *(src++);
481 *(dst++) = *(src++);
482
483 *(dst++) = *(src++);
484 *(dst++) = *(src++);
485 *(dst++) = *(src++);
486 *(dst++) = *(src++);
487 }
488
copy44d33f(const double * src,float * dst)489 void copy44d33f(const double *src, float *dst)
490 {
491 *(dst++) = (float) *(src++);
492 *(dst++) = (float) *(src++);
493 *(dst++) = (float) *(src++);
494 src++;
495
496 *(dst++) = (float) *(src++);
497 *(dst++) = (float) *(src++);
498 *(dst++) = (float) *(src++);
499 src++;
500
501 *(dst++) = (float) *(src++);
502 *(dst++) = (float) *(src++);
503 *(dst++) = (float) *(src++);
504 src++;
505 }
506
copy44f33f(const float * src,float * dst)507 void copy44f33f(const float *src, float *dst)
508 {
509 *(dst++) = *(src++);
510 *(dst++) = *(src++);
511 *(dst++) = *(src++);
512 src++;
513
514 *(dst++) = *(src++);
515 *(dst++) = *(src++);
516 *(dst++) = *(src++);
517 src++;
518
519 *(dst++) = *(src++);
520 *(dst++) = *(src++);
521 *(dst++) = *(src++);
522 src++;
523 }
524
copy44f44d(const float * src,double * dst)525 void copy44f44d(const float *src, double *dst)
526 {
527 *(dst++) = (double) *(src++);
528 *(dst++) = (double) *(src++);
529 *(dst++) = (double) *(src++);
530 *(dst++) = (double) *(src++);
531
532 *(dst++) = (double) *(src++);
533 *(dst++) = (double) *(src++);
534 *(dst++) = (double) *(src++);
535 *(dst++) = (double) *(src++);
536
537 *(dst++) = (double) *(src++);
538 *(dst++) = (double) *(src++);
539 *(dst++) = (double) *(src++);
540 *(dst++) = (double) *(src++);
541
542 *(dst++) = (double) *(src++);
543 *(dst++) = (double) *(src++);
544 *(dst++) = (double) *(src++);
545 *(dst++) = (double) *(src++);
546 }
547
copy44d44f(const double * src,float * dst)548 void copy44d44f(const double *src, float *dst)
549 {
550 *(dst++) = (float) *(src++);
551 *(dst++) = (float) *(src++);
552 *(dst++) = (float) *(src++);
553 *(dst++) = (float) *(src++);
554
555 *(dst++) = (float) *(src++);
556 *(dst++) = (float) *(src++);
557 *(dst++) = (float) *(src++);
558 *(dst++) = (float) *(src++);
559
560 *(dst++) = (float) *(src++);
561 *(dst++) = (float) *(src++);
562 *(dst++) = (float) *(src++);
563 *(dst++) = (float) *(src++);
564
565 *(dst++) = (float) *(src++);
566 *(dst++) = (float) *(src++);
567 *(dst++) = (float) *(src++);
568 *(dst++) = (float) *(src++);
569 }
570
copy33f44d(const float * src,double * dst)571 void copy33f44d(const float *src, double *dst)
572 {
573 const double _0 = 0.0;
574 *(dst++) = (double) *(src++);
575 *(dst++) = (double) *(src++);
576 *(dst++) = (double) *(src++);
577 *(dst++) = _0;
578
579 *(dst++) = (double) *(src++);
580 *(dst++) = (double) *(src++);
581 *(dst++) = (double) *(src++);
582 *(dst++) = _0;
583
584 *(dst++) = (double) *(src++);
585 *(dst++) = (double) *(src++);
586 *(dst++) = (double) *(src++);
587 *(dst++) = _0;
588
589 *(dst++) = _0;
590 *(dst++) = _0;
591 *(dst++) = _0;
592 *(dst++) = _1;
593 }
594
copy33f44f(const float * src,float * dst)595 void copy33f44f(const float *src, float *dst)
596 {
597 const float _0 = 0.0;
598 *(dst++) = *(src++);
599 *(dst++) = *(src++);
600 *(dst++) = *(src++);
601 *(dst++) = _0;
602
603 *(dst++) = *(src++);
604 *(dst++) = *(src++);
605 *(dst++) = *(src++);
606 *(dst++) = _0;
607
608 *(dst++) = *(src++);
609 *(dst++) = *(src++);
610 *(dst++) = *(src++);
611 *(dst++) = _0;
612
613 *(dst++) = _0;
614 *(dst++) = _0;
615 *(dst++) = _0;
616 *(dst++) = _1;
617 }
618 /* Transformation: MatMult( (3x3), (3x1) = (3x1) ) */
transform33f3f(const float * m1,const float * m2,float * m3)619 void transform33f3f(const float *m1, const float *m2, float *m3)
620 {
621 float m2r0 = m2[0];
622 float m2r1 = m2[1];
623 float m2r2 = m2[2];
624 m3[0] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2;
625 m3[1] = m1[3] * m2r0 + m1[4] * m2r1 + m1[5] * m2r2;
626 m3[2] = m1[6] * m2r0 + m1[7] * m2r1 + m1[8] * m2r2;
627 }
628
transpose33f33f(const float * m1,float * m2)629 void transpose33f33f(const float *m1, float *m2)
630 {
631 m2[0] = m1[0];
632 m2[1] = m1[3];
633 m2[2] = m1[6];
634 m2[3] = m1[1];
635 m2[4] = m1[4];
636 m2[5] = m1[7];
637 m2[6] = m1[2];
638 m2[7] = m1[5];
639 m2[8] = m1[8];
640 }
641
transpose33d33d(const double * m1,double * m2)642 void transpose33d33d(const double *m1, double *m2)
643 {
644 m2[0] = m1[0];
645 m2[1] = m1[3];
646 m2[2] = m1[6];
647 m2[3] = m1[1];
648 m2[4] = m1[4];
649 m2[5] = m1[7];
650 m2[6] = m1[2];
651 m2[7] = m1[5];
652 m2[8] = m1[8];
653 }
654
transpose44f44f(const float * m1,float * m2)655 void transpose44f44f(const float *m1, float *m2)
656 {
657 m2[0] = m1[0];
658 m2[1] = m1[4];
659 m2[2] = m1[8];
660 m2[3] = m1[12];
661
662 m2[4] = m1[1];
663 m2[5] = m1[5];
664 m2[6] = m1[9];
665 m2[7] = m1[13];
666
667 m2[8] = m1[2];
668 m2[9] = m1[6];
669 m2[10] = m1[10];
670 m2[11] = m1[14];
671
672 m2[12] = m1[3];
673 m2[13] = m1[7];
674 m2[14] = m1[11];
675 m2[15] = m1[15];
676 }
677
transpose44d44d(const double * m1,double * m2)678 void transpose44d44d(const double *m1, double *m2)
679 {
680 m2[0] = m1[0];
681 m2[1] = m1[4];
682 m2[2] = m1[8];
683 m2[3] = m1[12];
684
685 m2[4] = m1[1];
686 m2[5] = m1[5];
687 m2[6] = m1[9];
688 m2[7] = m1[13];
689
690 m2[8] = m1[2];
691 m2[9] = m1[6];
692 m2[10] = m1[10];
693 m2[11] = m1[14];
694
695 m2[12] = m1[3];
696 m2[13] = m1[7];
697 m2[14] = m1[11];
698 m2[15] = m1[15];
699 }
700
transform33Tf3f(const float * m1,const float * m2,float * m3)701 void transform33Tf3f(const float *m1, const float *m2, float *m3)
702 {
703 float m2r0 = m2[0];
704 float m2r1 = m2[1];
705 float m2r2 = m2[2];
706 m3[0] = m1[0] * m2r0 + m1[3] * m2r1 + m1[6] * m2r2;
707 m3[1] = m1[1] * m2r0 + m1[4] * m2r1 + m1[7] * m2r2;
708 m3[2] = m1[2] * m2r0 + m1[5] * m2r1 + m1[8] * m2r2;
709 }
710 /* multiply the upper-left 3x3 of a 4x4, m, by m2 and put result into m3:
711 * m3 = A x m2 */
transform44f3f(const float * m1,const float * m2,float * m3)712 void transform44f3f(const float *m1, const float *m2, float *m3)
713 {
714 float m2r0 = m2[0];
715 float m2r1 = m2[1];
716 float m2r2 = m2[2];
717 m3[0] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2 + m1[3];
718 m3[1] = m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2 + m1[7];
719 m3[2] = m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2 + m1[11];
720 }
721 /* Multi the upper left 3x3 of a 4x4 by a 3x1 vector; so effectively, [3x3]*[3x1] = [3x1] */
transform44d3f(const double * m1,const float * m2,float * m3)722 void transform44d3f(const double *m1, const float *m2, float *m3)
723 {
724 double m2r0 = m2[0];
725 double m2r1 = m2[1];
726 double m2r2 = m2[2];
727 m3[0] = (float) (m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2 + m1[3]);
728 m3[1] = (float) (m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2 + m1[7]);
729 m3[2] = (float) (m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2 + m1[11]);
730 }
731
transform44d3d(const double * m1,const double * m2,double * m3)732 void transform44d3d(const double *m1, const double *m2, double *m3)
733 {
734 double m2r0 = m2[0];
735 double m2r1 = m2[1];
736 double m2r2 = m2[2];
737 m3[0] = (float) (m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2 + m1[3]);
738 m3[1] = (float) (m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2 + m1[7]);
739 m3[2] = (float) (m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2 + m1[11]);
740 }
741
transform44d3fas33d3f(const double * m1,const float * m2,float * m3)742 void transform44d3fas33d3f(const double *m1, const float *m2, float *m3)
743 {
744 double m2r0 = m2[0];
745 double m2r1 = m2[1];
746 double m2r2 = m2[2];
747 m3[0] = (float) (m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2);
748 m3[1] = (float) (m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2);
749 m3[2] = (float) (m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2);
750 }
751
752 // same as MatrixInvTransformC44fAs33f3f
transform44f3fas33f3f(const float * m1,const float * m2,float * m3)753 void transform44f3fas33f3f(const float *m1, const float *m2, float *m3)
754 {
755 float m2r0 = m2[0];
756 float m2r1 = m2[1];
757 float m2r2 = m2[2];
758 m3[0] = (m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2);
759 m3[1] = (m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2);
760 m3[2] = (m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2);
761 }
762
inverse_transformC44f3f(const float * m1,const float * m2,float * m3)763 void inverse_transformC44f3f(const float *m1, const float *m2, float *m3)
764 {
765 float m2r0 = m2[0] - m1[12];
766 float m2r1 = m2[1] - m1[13];
767 float m2r2 = m2[2] - m1[14];
768 m3[0] = (float) (m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2);
769 m3[1] = (float) (m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2);
770 m3[2] = (float) (m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2);
771 }
772
inverse_transform44f3f(const float * m1,const float * m2,float * m3)773 void inverse_transform44f3f(const float *m1, const float *m2, float *m3)
774 {
775 float m2r0 = m2[0] - m1[3];
776 float m2r1 = m2[1] - m1[7];
777 float m2r2 = m2[2] - m1[11];
778 m3[0] = (float) (m1[0] * m2r0 + m1[4] * m2r1 + m1[8] * m2r2);
779 m3[1] = (float) (m1[1] * m2r0 + m1[5] * m2r1 + m1[9] * m2r2);
780 m3[2] = (float) (m1[2] * m2r0 + m1[6] * m2r1 + m1[10] * m2r2);
781 }
782
inverse_transform44d3f(const double * m1,const float * m2,float * m3)783 void inverse_transform44d3f(const double *m1, const float *m2, float *m3)
784 {
785 double m2r0 = m2[0] - m1[3];
786 double m2r1 = m2[1] - m1[7];
787 double m2r2 = m2[2] - m1[11];
788 m3[0] = (float) (m1[0] * m2r0 + m1[4] * m2r1 + m1[8] * m2r2);
789 m3[1] = (float) (m1[1] * m2r0 + m1[5] * m2r1 + m1[9] * m2r2);
790 m3[2] = (float) (m1[2] * m2r0 + m1[6] * m2r1 + m1[10] * m2r2);
791 }
792
inverse_transform44d3d(const double * m1,const double * m2,double * m3)793 void inverse_transform44d3d(const double *m1, const double *m2, double *m3)
794 {
795 double m2r0 = m2[0] - m1[3];
796 double m2r1 = m2[1] - m1[7];
797 double m2r2 = m2[2] - m1[11];
798 m3[0] = (float) (m1[0] * m2r0 + m1[4] * m2r1 + m1[8] * m2r2);
799 m3[1] = (float) (m1[1] * m2r0 + m1[5] * m2r1 + m1[9] * m2r2);
800 m3[2] = (float) (m1[2] * m2r0 + m1[6] * m2r1 + m1[10] * m2r2);
801 }
802
transform44f4f(const float * m1,const float * m2,float * m3)803 void transform44f4f(const float *m1, const float *m2, float *m3)
804 {
805 float m2r0 = m2[0];
806 float m2r1 = m2[1];
807 float m2r2 = m2[2];
808 float m2r3 = m2[3];
809 m3[0] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2 + m1[3] * m2r3;
810 m3[1] = m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2 + m1[7] * m2r3;
811 m3[2] = m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2 + m1[11] * m2r3;
812 m3[3] = m1[12] * m2r0 + m1[13] * m2r1 + m1[14] * m2r2 + m1[15] * m2r3;
813 }
814
initializeTTT44f(float * m)815 void initializeTTT44f(float *m)
816 {
817 int a;
818 for(a = 0; a < 16; a++)
819 m[a] = _0;
820 for(a = 0; a < 4; a++)
821 m[4 * a + a] = _1;
822 }
823
combineTTT44f44f(const float * m1,const float * m2,float * m3)824 void combineTTT44f44f(const float *m1, const float *m2, float *m3)
825
826
827 /* WARNING: this routine is ill-conceived and essentially broken */
828
829 /* NOTE: this is NOT equivalent to 4x4 matrix multiplication.
830 TTTs are designed for easily creating movies of rotating
831 bodies! */
832 {
833 float m1_homo[16];
834 float m2_homo[16];
835 const float *src;
836 float *dst;
837 float pre[3], post[3];
838
839 /* convert the existing TTT into a homogenous transformation matrix */
840
841 convertTTTfR44f(m1, m1_homo);
842 convertTTTfR44f(m2, m2_homo);
843
844 /* combine the matrices */
845
846 left_multiply44f44f(m1_homo, m2_homo);
847
848 /* now use the origin from the most recent TTT */
849
850 src = m1 + 12;
851 invert3f3f(src, pre);
852
853 transform44f3fas33f3f(m2_homo, pre, post);
854
855 m2_homo[3] += post[0];
856 m2_homo[7] += post[1];
857 m2_homo[11] += post[2];
858
859 dst = m2_homo + 12;
860
861 copy3f(src, dst);
862 copy44f(m2_homo, m3);
863
864 }
865
convertTTTfR44d(const float * ttt,double * homo)866 void convertTTTfR44d(const float *ttt, double *homo)
867 {
868 /* takes the PyMOL-specific TTT matrix and
869 makes a homogenous 4x4 txf matrix homo of it */
870
871 double ttt_3 = (double) ttt[3];
872 double ttt_7 = (double) ttt[7];
873 double ttt_11 = (double) ttt[11];
874 double ttt_12 = (double) ttt[12];
875 double ttt_13 = (double) ttt[13];
876 double ttt_14 = (double) ttt[14];
877
878 /* dump44f(ttt,"ttt"); */
879
880 homo[0] = (double) ttt[0];
881 homo[1] = (double) ttt[1];
882 homo[2] = (double) ttt[2];
883 homo[4] = (double) ttt[4];
884 homo[5] = (double) ttt[5];
885 homo[6] = (double) ttt[6];
886 homo[8] = (double) ttt[8];
887 homo[9] = (double) ttt[9];
888 homo[10] = (double) ttt[10];
889
890 homo[3] = (homo[0] * ttt_12) + (homo[1] * ttt_13) + (homo[2] * ttt_14) + ttt_3;
891 homo[7] = (homo[4] * ttt_12) + (homo[5] * ttt_13) + (homo[6] * ttt_14) + ttt_7;
892 homo[11] = (homo[8] * ttt_12) + (homo[9] * ttt_13) + (homo[10] * ttt_14) + ttt_11;
893
894 homo[12] = 0.0;
895 homo[13] = 0.0;
896 homo[14] = 0.0;
897 homo[15] = 1.0;
898
899 /* dump44d(homo, "homo"); */
900
901 }
902
convertTTTfR44f(const float * ttt,float * homo)903 void convertTTTfR44f(const float *ttt, float *homo)
904 {
905 /* takes the PyMOL-specific TTT matrix and
906 makes a homogenous 4x4 txf matrix homo of it */
907
908 float ttt_3 = ttt[3];
909 float ttt_7 = ttt[7];
910 float ttt_11 = ttt[11];
911 float ttt_12 = ttt[12];
912 float ttt_13 = ttt[13];
913 float ttt_14 = ttt[14];
914
915 homo[0] = ttt[0];
916 homo[1] = ttt[1];
917 homo[2] = ttt[2];
918 homo[4] = ttt[4];
919 homo[5] = ttt[5];
920 homo[6] = ttt[6];
921 homo[8] = ttt[8];
922 homo[9] = ttt[9];
923 homo[10] = ttt[10];
924
925 homo[3] = (homo[0] * ttt_12) + (homo[1] * ttt_13) + (homo[2] * ttt_14) + ttt_3;
926 homo[7] = (homo[4] * ttt_12) + (homo[5] * ttt_13) + (homo[6] * ttt_14) + ttt_7;
927 homo[11] = (homo[8] * ttt_12) + (homo[9] * ttt_13) + (homo[10] * ttt_14) + ttt_11;
928
929 homo[12] = 0.0;
930 homo[13] = 0.0;
931 homo[14] = 0.0;
932 homo[15] = 1.0;
933
934 }
935
convert44d44f(const double * dbl,float * flt)936 void convert44d44f(const double *dbl, float *flt)
937 {
938 flt[0] = (float) dbl[0];
939 flt[1] = (float) dbl[1];
940 flt[2] = (float) dbl[2];
941 flt[3] = (float) dbl[3];
942 flt[4] = (float) dbl[4];
943 flt[5] = (float) dbl[5];
944 flt[6] = (float) dbl[6];
945 flt[7] = (float) dbl[7];
946 flt[8] = (float) dbl[8];
947 flt[9] = (float) dbl[9];
948 flt[10] = (float) dbl[10];
949 flt[11] = (float) dbl[11];
950 flt[12] = (float) dbl[12];
951 flt[13] = (float) dbl[13];
952 flt[14] = (float) dbl[14];
953 flt[15] = (float) dbl[15];
954 }
955
convert44f44d(const float * flt,double * dbl)956 void convert44f44d(const float *flt, double *dbl)
957 {
958 dbl[0] = (double) flt[0];
959 dbl[1] = (double) flt[1];
960 dbl[2] = (double) flt[2];
961 dbl[3] = (double) flt[3];
962 dbl[4] = (double) flt[4];
963 dbl[5] = (double) flt[5];
964 dbl[6] = (double) flt[6];
965 dbl[7] = (double) flt[7];
966 dbl[8] = (double) flt[8];
967 dbl[9] = (double) flt[9];
968 dbl[10] = (double) flt[10];
969 dbl[11] = (double) flt[11];
970 dbl[12] = (double) flt[12];
971 dbl[13] = (double) flt[13];
972 dbl[14] = (double) flt[14];
973 dbl[15] = (double) flt[15];
974 }
975
convertR44dTTTf(const double * homo,float * ttt)976 void convertR44dTTTf(const double *homo, float *ttt)
977 {
978 /* nowadays, homogeneous matrices with (0,0,0,1) in 4th row are TTT
979 compatible */
980 convert44d44f(homo, ttt);
981 }
982
multiply44d44d44d(const double * left,const double * right,double * product)983 void multiply44d44d44d(const double *left, const double *right, double *product)
984 {
985 double rA = right[0];
986 double rB = right[4];
987 double rC = right[8];
988 double rD = right[12];
989
990 product[0] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
991 product[4] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
992 product[8] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
993 product[12] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
994
995 rA = right[1];
996 rB = right[5];
997 rC = right[9];
998 rD = right[13];
999
1000 product[1] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1001 product[5] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1002 product[9] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1003 product[13] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1004
1005 rA = right[2];
1006 rB = right[6];
1007 rC = right[10];
1008 rD = right[14];
1009
1010 product[2] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1011 product[6] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1012 product[10] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1013 product[14] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1014
1015 rA = right[3];
1016 rB = right[7];
1017 rC = right[11];
1018 rD = right[15];
1019
1020 product[3] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1021 product[7] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1022 product[11] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1023 product[15] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1024 }
1025
left_multiply44d44d(const double * left,double * right)1026 void left_multiply44d44d(const double *left, double *right)
1027 {
1028 double rA = right[0];
1029 double rB = right[4];
1030 double rC = right[8];
1031 double rD = right[12];
1032
1033 right[0] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1034 right[4] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1035 right[8] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1036 right[12] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1037
1038 rA = right[1];
1039 rB = right[5];
1040 rC = right[9];
1041 rD = right[13];
1042
1043 right[1] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1044 right[5] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1045 right[9] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1046 right[13] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1047
1048 rA = right[2];
1049 rB = right[6];
1050 rC = right[10];
1051 rD = right[14];
1052
1053 right[2] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1054 right[6] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1055 right[10] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1056 right[14] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1057
1058 rA = right[3];
1059 rB = right[7];
1060 rC = right[11];
1061 rD = right[15];
1062
1063 right[3] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1064 right[7] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1065 right[11] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1066 right[15] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1067 }
1068
right_multiply44d44d(double * left,const double * right)1069 void right_multiply44d44d(double *left, const double *right)
1070 {
1071 double cA = left[0];
1072 double cB = left[1];
1073 double cC = left[2];
1074 double cD = left[3];
1075
1076 left[0] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
1077 left[1] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
1078 left[2] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
1079 left[3] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
1080
1081 cA = left[4];
1082 cB = left[5];
1083 cC = left[6];
1084 cD = left[7];
1085
1086 left[4] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
1087 left[5] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
1088 left[6] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
1089 left[7] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
1090
1091 cA = left[8];
1092 cB = left[9];
1093 cC = left[10];
1094 cD = left[11];
1095
1096 left[8] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
1097 left[9] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
1098 left[10] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
1099 left[11] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
1100
1101 cA = left[12];
1102 cB = left[13];
1103 cC = left[14];
1104 cD = left[15];
1105
1106 left[12] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
1107 left[13] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
1108 left[14] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
1109 left[15] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
1110
1111 }
1112
multiply44f44f44f(const float * left,const float * right,float * product)1113 void multiply44f44f44f(const float *left, const float *right, float *product)
1114 {
1115 float rA = right[0];
1116 float rB = right[4];
1117 float rC = right[8];
1118 float rD = right[12];
1119
1120 product[0] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1121 product[4] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1122 product[8] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1123 product[12] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1124
1125 rA = right[1];
1126 rB = right[5];
1127 rC = right[9];
1128 rD = right[13];
1129
1130 product[1] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1131 product[5] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1132 product[9] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1133 product[13] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1134
1135 rA = right[2];
1136 rB = right[6];
1137 rC = right[10];
1138 rD = right[14];
1139
1140 product[2] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1141 product[6] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1142 product[10] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1143 product[14] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1144
1145 rA = right[3];
1146 rB = right[7];
1147 rC = right[11];
1148 rD = right[15];
1149
1150 product[3] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1151 product[7] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1152 product[11] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1153 product[15] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1154 }
1155
left_multiply44f44f(const float * left,float * right)1156 void left_multiply44f44f(const float *left, float *right)
1157 {
1158 float rA = right[0];
1159 float rB = right[4];
1160 float rC = right[8];
1161 float rD = right[12];
1162
1163 right[0] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1164 right[4] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1165 right[8] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1166 right[12] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1167
1168 rA = right[1];
1169 rB = right[5];
1170 rC = right[9];
1171 rD = right[13];
1172
1173 right[1] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1174 right[5] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1175 right[9] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1176 right[13] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1177
1178 rA = right[2];
1179 rB = right[6];
1180 rC = right[10];
1181 rD = right[14];
1182
1183 right[2] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1184 right[6] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1185 right[10] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1186 right[14] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1187
1188 rA = right[3];
1189 rB = right[7];
1190 rC = right[11];
1191 rD = right[15];
1192
1193 right[3] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
1194 right[7] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
1195 right[11] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
1196 right[15] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
1197 }
1198
right_multiply44f44f(float * left,const float * right)1199 void right_multiply44f44f(float *left, const float *right)
1200 {
1201 float cA = left[0];
1202 float cB = left[1];
1203 float cC = left[2];
1204 float cD = left[3];
1205
1206 left[0] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
1207 left[1] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
1208 left[2] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
1209 left[3] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
1210
1211 cA = left[4];
1212 cB = left[5];
1213 cC = left[6];
1214 cD = left[7];
1215
1216 left[4] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
1217 left[5] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
1218 left[6] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
1219 left[7] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
1220
1221 cA = left[8];
1222 cB = left[9];
1223 cC = left[10];
1224 cD = left[11];
1225
1226 left[8] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
1227 left[9] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
1228 left[10] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
1229 left[11] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
1230
1231 cA = left[12];
1232 cB = left[13];
1233 cC = left[14];
1234 cD = left[15];
1235
1236 left[12] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
1237 left[13] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
1238 left[14] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
1239 left[15] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
1240
1241 }
1242
invert_special44d44d(const double * orig,double * inv)1243 void invert_special44d44d(const double *orig, double *inv)
1244 {
1245 /* inverse of the rotation matrix */
1246
1247 inv[0] = orig[0];
1248 inv[1] = orig[4];
1249 inv[2] = orig[8];
1250 inv[4] = orig[1];
1251 inv[5] = orig[5];
1252 inv[6] = orig[9];
1253 inv[8] = orig[2];
1254 inv[9] = orig[6];
1255 inv[10] = orig[10];
1256
1257 /* invert the translation portion */
1258
1259 inv[3] = -(orig[3] * orig[0] + orig[7] * orig[4] + orig[11] * orig[8]);
1260 inv[7] = -(orig[3] * orig[1] + orig[7] * orig[5] + orig[11] * orig[9]);
1261 inv[11] = -(orig[3] * orig[2] + orig[7] * orig[6] + orig[11] * orig[10]);
1262
1263 inv[12] = 0.0;
1264 inv[13] = 0.0;
1265 inv[14] = 0.0;
1266 inv[15] = 1.0;
1267
1268 }
1269
invert_special44f44f(const float * orig,float * inv)1270 void invert_special44f44f(const float *orig, float *inv)
1271 {
1272 /* inverse of the rotation matrix */
1273
1274 inv[0] = orig[0];
1275 inv[1] = orig[4];
1276 inv[2] = orig[8];
1277 inv[4] = orig[1];
1278 inv[5] = orig[5];
1279 inv[6] = orig[9];
1280 inv[8] = orig[2];
1281 inv[9] = orig[6];
1282 inv[10] = orig[10];
1283
1284 /* invert the translation portion */
1285
1286 inv[3] = -(orig[3] * orig[0] + orig[7] * orig[4] + orig[11] * orig[8]);
1287 inv[7] = -(orig[3] * orig[1] + orig[7] * orig[5] + orig[11] * orig[9]);
1288 inv[11] = -(orig[3] * orig[2] + orig[7] * orig[6] + orig[11] * orig[10]);
1289
1290 inv[12] = 0.0F;
1291 inv[13] = 0.0F;
1292 inv[14] = 0.0F;
1293 inv[15] = 1.0F;
1294
1295 }
1296
normalize3dp(double * v1,double * v2,double * v3)1297 static void normalize3dp(double *v1, double *v2, double *v3)
1298 {
1299 double vlen = sqrt1d((v1[0] * v1[0]) + (v2[0] * v2[0]) + (v3[0] * v3[0]));
1300 if(vlen > R_SMALL) {
1301 v1[0] /= vlen;
1302 v2[0] /= vlen;
1303 v3[0] /= vlen;
1304 } else {
1305 v1[0] = _0;
1306 v2[1] = _0;
1307 v3[2] = _0;
1308 }
1309 }
1310
1311
1312 /* unused at present
1313 static void normalize3df( float *v1, float *v2, float *v3 )
1314 {
1315 float vlen = (float)sqrt1f((v1[0]*v1[0]) +
1316 (v2[0]*v2[0]) +
1317 (v3[0]*v3[0]));
1318 if(vlen>R_SMALL)
1319 {
1320 v1[0]/=vlen;
1321 v2[0]/=vlen;
1322 v3[0]/=vlen;
1323 }
1324 else
1325 {
1326 v1[0]=_0;
1327 v2[1]=_0;
1328 v3[2]=_0;
1329 }
1330 }
1331 */
scale3d(const double * v1,const double v0,double * v2)1332 void scale3d(const double *v1, const double v0, double *v2)
1333 {
1334 v2[0] = v1[0] * v0;
1335 v2[1] = v1[1] * v0;
1336 v2[2] = v1[2] * v0;
1337 }
1338
add3d(const double * v1,const double * v2,double * v3)1339 void add3d(const double *v1, const double *v2, double *v3)
1340 {
1341 v3[0] = v1[0] + v2[0];
1342 v3[1] = v1[1] + v2[1];
1343 v3[2] = v1[2] + v2[2];
1344 }
1345
cross_product3d(const double * v1,const double * v2,double * cross)1346 void cross_product3d(const double *v1, const double *v2, double *cross)
1347 {
1348 cross[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);
1349 cross[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);
1350 cross[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);
1351 }
1352
remove_component3d(const double * v1,const double * unit,double * result)1353 void remove_component3d(const double *v1, const double *unit, double *result)
1354 {
1355 double dot;
1356
1357 dot = v1[0] * unit[0] + v1[1] * unit[1] + v1[2] * unit[2];
1358 result[0] = v1[0] - unit[0] * dot;
1359 result[1] = v1[1] - unit[1] * dot;
1360 result[2] = v1[2] - unit[2] * dot;
1361 }
1362
reorient44d(double * matrix)1363 void reorient44d(double *matrix)
1364 {
1365 double tmp[16];
1366 int a;
1367
1368 /* restore orthogonality and recondition */
1369
1370 for(a = 0; a < 3; a++) {
1371 normalize3d(matrix);
1372 normalize3d(matrix + 4);
1373 normalize3d(matrix + 8);
1374 cross_product3d(matrix + 4, matrix + 8, tmp);
1375 cross_product3d(matrix + 8, matrix, tmp + 4);
1376 cross_product3d(matrix, matrix + 4, tmp + 8);
1377 normalize3d(tmp);
1378 normalize3d(tmp + 4);
1379 normalize3d(tmp + 8);
1380 scale3d(tmp, 2.0, tmp);
1381 scale3d(tmp + 4, 2.0, tmp + 4);
1382 scale3d(tmp + 8, 2.0, tmp + 8);
1383 add3d(matrix, tmp, tmp);
1384 add3d(matrix + 4, tmp + 4, tmp + 4);
1385 add3d(matrix + 8, tmp + 8, tmp + 8);
1386 copy3d(tmp, matrix);
1387 copy3d(tmp + 4, matrix + 4);
1388 copy3d(tmp + 8, matrix + 8);
1389 }
1390
1391 normalize3d(matrix);
1392 normalize3d(matrix + 4);
1393 normalize3d(matrix + 8);
1394
1395 copy3d(matrix, tmp);
1396 remove_component3d(matrix + 4, tmp, tmp + 4);
1397 cross_product3d(tmp, tmp + 4, tmp + 8);
1398 normalize3d(tmp + 4);
1399 normalize3d(tmp + 8);
1400
1401 recondition44d(tmp);
1402
1403 copy3d(tmp, matrix);
1404 copy3d(tmp + 4, matrix + 4);
1405 copy3d(tmp + 8, matrix + 8);
1406
1407 }
1408
recondition33d(double * matrix)1409 void recondition33d(double *matrix)
1410 {
1411 normalize3d(matrix);
1412 normalize3d(matrix + 3);
1413 normalize3d(matrix + 6);
1414 normalize3dp(matrix + 0, matrix + 3, matrix + 6);
1415 normalize3dp(matrix + 1, matrix + 4, matrix + 7);
1416 normalize3dp(matrix + 2, matrix + 5, matrix + 8);
1417 normalize3d(matrix);
1418 normalize3d(matrix + 3);
1419 normalize3d(matrix + 6);
1420 normalize3dp(matrix + 0, matrix + 3, matrix + 6);
1421 normalize3dp(matrix + 1, matrix + 4, matrix + 7);
1422 normalize3dp(matrix + 2, matrix + 5, matrix + 8);
1423 normalize3d(matrix);
1424 normalize3d(matrix + 3);
1425 normalize3d(matrix + 6);
1426 }
1427
recondition44d(double * matrix)1428 void recondition44d(double *matrix)
1429 {
1430 normalize3d(matrix);
1431 normalize3d(matrix + 4);
1432 normalize3d(matrix + 8);
1433 normalize3dp(matrix + 0, matrix + 4, matrix + 8);
1434 normalize3dp(matrix + 1, matrix + 5, matrix + 9);
1435 normalize3dp(matrix + 2, matrix + 6, matrix + 10);
1436 normalize3d(matrix);
1437 normalize3d(matrix + 4);
1438 normalize3d(matrix + 8);
1439 normalize3dp(matrix + 0, matrix + 4, matrix + 8);
1440 normalize3dp(matrix + 1, matrix + 5, matrix + 9);
1441 normalize3dp(matrix + 2, matrix + 6, matrix + 10);
1442 normalize3d(matrix);
1443 normalize3d(matrix + 4);
1444 normalize3d(matrix + 8);
1445 }
1446
invert_rotation_only44d44d(const double * orig,double * inv)1447 void invert_rotation_only44d44d(const double *orig, double *inv)
1448 {
1449 /* inverse of the rotation matrix */
1450
1451 inv[0] = orig[0];
1452 inv[1] = orig[4];
1453 inv[2] = orig[8];
1454 inv[4] = orig[1];
1455 inv[5] = orig[5];
1456 inv[6] = orig[9];
1457 inv[8] = orig[2];
1458 inv[9] = orig[6];
1459 inv[10] = orig[10];
1460
1461 inv[3] = 0.0;
1462 inv[7] = 0.0;
1463 inv[11] = 0.0;
1464
1465 inv[12] = 0.0;
1466 inv[13] = 0.0;
1467 inv[14] = 0.0;
1468 inv[15] = 1.0;
1469
1470 }
1471
transformTTT44f3f(const float * m1,const float * m2,float * m3)1472 void transformTTT44f3f(const float *m1, const float *m2, float *m3)
1473 {
1474 float m2r0 = m2[0] + m1[12];
1475 float m2r1 = m2[1] + m1[13];
1476 float m2r2 = m2[2] + m1[14];
1477 m3[0] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2 + m1[3];
1478 m3[1] = m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2 + m1[7];
1479 m3[2] = m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2 + m1[11];
1480 }
1481
transform_normalTTT44f3f(const float * m1,const float * m2,float * m3)1482 void transform_normalTTT44f3f(const float *m1, const float *m2, float *m3)
1483 {
1484 float m2r0 = m2[0];
1485 float m2r1 = m2[1];
1486 float m2r2 = m2[2];
1487 m3[0] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2;
1488 m3[1] = m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2;
1489 m3[2] = m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2;
1490 }
1491
multiply33f33f(const float * m1,const float * m2,float * m3)1492 void multiply33f33f(const float *m1, const float *m2, float *m3)
1493 { /* m2 and m3 can be the same matrix */
1494 int a;
1495 float m2r0, m2r1, m2r2;
1496 for(a = 0; a < 3; a++) {
1497 m2r0 = m2[a];
1498 m2r1 = m2[3 + a];
1499 m2r2 = m2[6 + a];
1500 m3[a] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2;
1501 m3[3 + a] = m1[3] * m2r0 + m1[4] * m2r1 + m1[5] * m2r2;
1502 m3[6 + a] = m1[6] * m2r0 + m1[7] * m2r1 + m1[8] * m2r2;
1503 }
1504 }
1505
multiply33d33d(const double * m1,const double * m2,double * m3)1506 void multiply33d33d(const double *m1, const double *m2, double *m3)
1507 { /* m2 and m3 can be the same matrix */
1508 int a;
1509 double m2r0, m2r1, m2r2;
1510 for(a = 0; a < 3; a++) {
1511 m2r0 = m2[a];
1512 m2r1 = m2[3 + a];
1513 m2r2 = m2[6 + a];
1514 m3[a] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2;
1515 m3[3 + a] = m1[3] * m2r0 + m1[4] * m2r1 + m1[5] * m2r2;
1516 m3[6 + a] = m1[6] * m2r0 + m1[7] * m2r1 + m1[8] * m2r2;
1517 }
1518 }
1519
matrix_multiply33f33f(Matrix33f m1,Matrix33f m2,Matrix33f m3)1520 void matrix_multiply33f33f(Matrix33f m1, Matrix33f m2, Matrix33f m3)
1521 {
1522 multiply33f33f((float *) m1, (float *) m2, (float *) m3);
1523 }
1524
matrix_multiply33d33d(Matrix33d m1,Matrix33d m2,Matrix33d m3)1525 void matrix_multiply33d33d(Matrix33d m1, Matrix33d m2, Matrix33d m3)
1526 {
1527 multiply33d33d((double *) m1[0], (double *) m2, (double *) m3);
1528 }
1529
deg_to_rad(float angle)1530 float deg_to_rad(float angle)
1531 {
1532 return ((float) ((angle * cPI) / 180.0));
1533 }
1534
rad_to_deg(float angle)1535 float rad_to_deg(float angle)
1536 {
1537 return ((float) (180.0 * (angle / cPI)));
1538 }
1539
get_rotation_about3f3fTTTf(float angle,const float * dir,const float * origin,float * ttt)1540 void get_rotation_about3f3fTTTf(float angle, const float *dir, const float *origin, float *ttt)
1541 {
1542 float rot[9];
1543 rotation_matrix3f(angle, dir[0], dir[1], dir[2], rot);
1544 ttt[0] = rot[0];
1545 ttt[1] = rot[1];
1546 ttt[2] = rot[2];
1547 ttt[4] = rot[3];
1548 ttt[5] = rot[4];
1549 ttt[6] = rot[5];
1550 ttt[8] = rot[6];
1551 ttt[9] = rot[7];
1552 ttt[10] = rot[8];
1553 ttt[12] = -origin[0];
1554 ttt[13] = -origin[1];
1555 ttt[14] = -origin[2];
1556 ttt[3] = origin[0];
1557 ttt[7] = origin[1];
1558 ttt[11] = origin[2];
1559 ttt[15] = 1.0F;
1560 }
1561
rotation_to_matrix33f(const float * axis,float angle,Matrix33f mat)1562 void rotation_to_matrix33f(const float *axis, float angle, Matrix33f mat)
1563 {
1564 rotation_matrix3f(angle, axis[0], axis[1], axis[2], &mat[0][0]);
1565 }
1566
rotation_matrix3f(float angle,float x,float y,float z,float * m)1567 void rotation_matrix3f(float angle, float x, float y, float z, float *m)
1568 {
1569 /* returns a row-major rotation matrix */
1570
1571 int a, b;
1572
1573 /* This function contributed by Erich Boleyn (erich@uruk.org) */
1574 float mag, s, c;
1575 float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
1576
1577 s = (float) sin(angle);
1578 c = (float) cos(angle);
1579
1580 mag = (float) sqrt1f(x * x + y * y + z * z);
1581 if(mag >= R_SMALL) {
1582 x /= mag;
1583 y /= mag;
1584 z /= mag;
1585
1586 #define M(row,col) m[row*3+col]
1587
1588 xx = x * x;
1589 yy = y * y;
1590 zz = z * z;
1591 xy = x * y;
1592 yz = y * z;
1593 zx = z * x;
1594 xs = x * s;
1595 ys = y * s;
1596 zs = z * s;
1597 one_c = _1 - c;
1598
1599 M(0, 0) = (one_c * xx) + c;
1600 M(0, 1) = (one_c * xy) - zs;
1601 M(0, 2) = (one_c * zx) + ys;
1602
1603 M(1, 0) = (one_c * xy) + zs;
1604 M(1, 1) = (one_c * yy) + c;
1605 M(1, 2) = (one_c * yz) - xs;
1606
1607 M(2, 0) = (one_c * zx) - ys;
1608 M(2, 1) = (one_c * yz) + xs;
1609 M(2, 2) = (one_c * zz) + c;
1610 } else {
1611 for(a = 0; a < 3; a++)
1612 for(b = 0; b < 3; b++)
1613 M(a, b) = 0;
1614 M(0, 0) = _1;
1615 M(1, 1) = _1;
1616 M(2, 2) = _1;
1617 }
1618
1619 }
1620
1621 #define get_angle USED_TO_RETURN_DEGREES
1622
get_dihedral3f(const float * v0,const float * v1,const float * v2,const float * v3)1623 float get_dihedral3f(const float *v0, const float *v1, const float *v2, const float *v3)
1624 {
1625 Vector3f d01, d21, d32, dd1, dd3, pos_d;
1626 float result = _0;
1627
1628 subtract3f(v2, v1, d21);
1629 subtract3f(v0, v1, d01);
1630 subtract3f(v3, v2, d32);
1631 if(length3f(d21) < R_SMALL) {
1632 result = get_angle3f(d01, d32);
1633 } else {
1634 cross_product3f(d21, d01, dd1);
1635 cross_product3f(d21, d32, dd3);
1636 if((length3f(dd1) < R_SMALL) || (length3f(dd3) < R_SMALL)) { /* degenerate cases */
1637 result = get_angle3f(d01, d32); /* fall back to angle between vectors */
1638 } else {
1639 result = get_angle3f(dd1, dd3);
1640 cross_product3f(d21, dd1, pos_d);
1641 if(dot_product3f(dd3, pos_d) < _0)
1642 result = -result;
1643 }
1644 }
1645 return (result);
1646 }
1647
get_angle3f(const float * v1,const float * v2)1648 float get_angle3f(const float *v1, const float *v2)
1649 {
1650 double denom;
1651 double result;
1652 double arg1, arg2;
1653 arg1 = ((v1[0] * (double)v1[0]) + (v1[1] * (double)v1[1]) + (v1[2] * (double)v1[2]));
1654 arg2 = ((v2[0] * (double)v2[0]) + (v2[1] * (double)v2[1]) + (v2[2] * (double)v2[2]));
1655 denom = sqrt1d(arg1) * sqrt1d(arg2);
1656
1657 if(denom > R_SMALL){
1658 arg1 = (v1[0] * (double)v2[0] + v1[1] * (double)v2[1] + v1[2] * (double)v2[2]);
1659 result = arg1 / denom;
1660 } else
1661 result = _0;
1662 if(result < -_1)
1663 result = -_1;
1664 else if(result > _1)
1665 result = _1;
1666
1667 return acosf(result);
1668 }
1669
normalize23f(const float * v1,float * v2)1670 void normalize23f(const float *v1, float *v2)
1671 {
1672 double vlen;
1673 vlen = length3f(v1);
1674 if(vlen > R_SMALL) {
1675 v2[0] = (float) (v1[0] / vlen);
1676 v2[1] = (float) (v1[1] / vlen);
1677 v2[2] = (float) (v1[2] / vlen);
1678 } else {
1679 v2[0] = _0;
1680 v2[1] = _0;
1681 v2[2] = _0;
1682 }
1683 }
1684
clamp3f(float * v1)1685 void clamp3f(float *v1)
1686 {
1687 if(v1[0] < _0)
1688 v1[0] = _0;
1689 if(v1[0] > _1)
1690 v1[0] = _1;
1691 if(v1[1] < _0)
1692 v1[1] = _0;
1693 if(v1[1] > _1)
1694 v1[1] = _1;
1695 if(v1[2] < _0)
1696 v1[2] = _0;
1697 if(v1[2] > _1)
1698 v1[2] = _1;
1699 }
1700
normalize3d(double * v1)1701 void normalize3d(double *v1)
1702 {
1703 double vlen;
1704 vlen = length3d(v1);
1705 if(vlen > R_SMALL) {
1706 v1[0] /= vlen;
1707 v1[1] /= vlen;
1708 v1[2] /= vlen;
1709 } else {
1710 v1[0] = _0;
1711 v1[1] = _0;
1712 v1[2] = _0;
1713 }
1714 }
1715
normalize2f(float * v1)1716 void normalize2f(float *v1)
1717 {
1718 double vlen;
1719 vlen = length2f(v1);
1720 if(vlen > R_SMALL) {
1721 v1[0] /= vlen;
1722 v1[1] /= vlen;
1723 } else {
1724 v1[0] = _0;
1725 v1[1] = _0;
1726 }
1727 }
1728
normalize4f(float * v1)1729 void normalize4f(float *v1)
1730 {
1731 v1[0] /= v1[3];
1732 v1[1] /= v1[3];
1733 v1[2] /= v1[3];
1734 v1[3] = 1.f;
1735 }
1736
length3d(const double * v1)1737 double length3d(const double *v1)
1738 {
1739 return (sqrt1d((v1[0] * v1[0]) + (v1[1] * v1[1]) + (v1[2] * v1[2])));
1740 }
1741
distance_line2point3f(const float * base,const float * normal,const float * point,float * alongNormalSq)1742 double distance_line2point3f(const float *base, const float *normal, const float *point,
1743 float *alongNormalSq)
1744 {
1745 float hyp[3], adj[3];
1746 double result;
1747
1748 hyp[0] = point[0] - base[0];
1749 hyp[1] = point[1] - base[1];
1750 hyp[2] = point[2] - base[2];
1751
1752 project3f(hyp, normal, adj);
1753
1754 (*alongNormalSq) = ((adj[0] * adj[0]) + (adj[1] * adj[1]) + (adj[2] * adj[2]));
1755 result = ((hyp[0] * hyp[0]) + (hyp[1] * hyp[1]) + (hyp[2] * hyp[2]))
1756 - (*alongNormalSq);
1757 if(result <= _0)
1758 return (_0);
1759 else
1760 return (sqrt1d(result));
1761
1762 }
1763
distance_halfline2point3f(const float * base,const float * normal,const float * point,float * alongNormalSq)1764 double distance_halfline2point3f(const float *base, const float *normal, const float *point,
1765 float *alongNormalSq)
1766 {
1767 float hyp[3], adj[3];
1768 double result;
1769
1770 hyp[0] = point[0] - base[0];
1771 hyp[1] = point[1] - base[1];
1772 hyp[2] = point[2] - base[2];
1773
1774 if(project3f(hyp, normal, adj) > _0) {
1775 (*alongNormalSq) = ((adj[0] * adj[0]) + (adj[1] * adj[1]) + (adj[2] * adj[2]));
1776 result = ((hyp[0] * hyp[0]) + (hyp[1] * hyp[1]) + (hyp[2] * hyp[2]))
1777 - (*alongNormalSq);
1778 if(result <= _0)
1779 return (_0);
1780 else
1781 return (sqrt1d(result));
1782 } else {
1783 return FLT_MAX;
1784 }
1785 }
1786
matrix_transform33f3f(const Matrix33f m1,const float * v1,float * v2)1787 void matrix_transform33f3f(const Matrix33f m1, const float *v1, float *v2)
1788 {
1789 v2[0] = m1[0][0] * v1[0] + m1[0][1] * v1[1] + m1[0][2] * v1[2];
1790 v2[1] = m1[1][0] * v1[0] + m1[1][1] * v1[1] + m1[1][2] * v1[2];
1791 v2[2] = m1[2][0] * v1[0] + m1[2][1] * v1[1] + m1[2][2] * v1[2];
1792 }
1793
matrix_inverse_transform33f3f(const Matrix33f m1,const float * v1,float * v2)1794 void matrix_inverse_transform33f3f(const Matrix33f m1, const float *v1, float *v2)
1795 {
1796 v2[0] = m1[0][0] * v1[0] + m1[1][0] * v1[1] + m1[2][0] * v1[2];
1797 v2[1] = m1[0][1] * v1[0] + m1[1][1] * v1[1] + m1[2][1] * v1[2];
1798 v2[2] = m1[0][2] * v1[0] + m1[1][2] * v1[1] + m1[2][2] * v1[2];
1799 }
1800
1801 #if 0
1802 double matdiffsq(float *v1, oMatrix5f m, float *v2)
1803 {
1804 double dx, dy, dz;
1805 float vx, vy, vz;
1806
1807 dx = v2[0] - m[3][0];
1808 dy = v2[1] - m[3][1];
1809 dz = v2[2] - m[3][2];
1810
1811 vx = m[0][0] * dx + m[0][1] * dy + m[0][2] * dz;
1812 vy = m[1][0] * dx + m[1][1] * dy + m[1][2] * dz;
1813 vz = m[2][0] * dx + m[2][1] * dy + m[2][2] * dz;
1814
1815 dx = (v1[0] - (vx + m[4][0]));
1816 dy = (v1[1] - (vy + m[4][1]));
1817 dz = (v1[2] - (vz + m[4][2]));
1818
1819 return (dx * dx + dy * dy + dz * dz);
1820
1821 }
1822 #endif
1823
transform5f3f(const oMatrix5f m,const float * v1,float * v2)1824 void transform5f3f(const oMatrix5f m, const float *v1, float *v2)
1825 {
1826 double dx, dy, dz;
1827 double vx, vy, vz;
1828
1829 dx = v1[0] - m[3][0];
1830 dy = v1[1] - m[3][1];
1831 dz = v1[2] - m[3][2];
1832
1833 vx = m[0][0] * dx + m[0][1] * dy + m[0][2] * dz;
1834 vy = m[1][0] * dx + m[1][1] * dy + m[1][2] * dz;
1835 vz = m[2][0] * dx + m[2][1] * dy + m[2][2] * dz;
1836
1837 v2[0] = (((float) vx) + m[4][0]);
1838 v2[1] = (((float) vy) + m[4][1]);
1839 v2[2] = (((float) vz) + m[4][2]);
1840
1841 }
1842
transform3d3f(const oMatrix3d m1,const float * v1,float * v2)1843 void transform3d3f(const oMatrix3d m1, const float *v1, float *v2)
1844 {
1845 int b;
1846 for(b = 0; b < 3; b++)
1847 v2[b] = m1[b][0] * v1[0] + m1[b][1] * v1[1] + m1[b][2] * v1[2];
1848 }
1849
transform33d3f(const Matrix33d m1,const float * v1,float * v2)1850 void transform33d3f(const Matrix33d m1, const float *v1, float *v2)
1851 {
1852 int b;
1853 for(b = 0; b < 3; b++)
1854 v2[b] = (float) (m1[b][0] * v1[0] + m1[b][1] * v1[1] + m1[b][2] * v1[2]);
1855 }
1856
1857
1858 /*
1859
1860 void matcopy ( oMatrix5f to,oMatrix5f from )
1861 {
1862 int a,b;
1863 for(a=0;a<5;a++)
1864 for(b=0;b<3;b++)
1865 to[a][b] = from[a][b];
1866 }
1867
1868 void mattran ( oMatrix5f nm, oMatrix5f om, int axis, float dist )
1869 {
1870 matcopy(nm,om);
1871 nm[4][axis] += dist;
1872 }
1873
1874 void matrot ( oMatrix5f nm, oMatrix5f om, int axis, float angle )
1875 {
1876 oMatrix5f rm;
1877
1878 float ca,sa;
1879 int a,b;
1880
1881 ca = cos(angle);
1882 sa = sin(angle);
1883
1884 switch(axis)
1885 {
1886 case 0:
1887 rm[0][0] = _1; rm[0][1] = _0; rm[0][2] = _0;
1888 rm[1][0] = _0; rm[1][1] = ca; rm[1][2] = sa;
1889 rm[2][0] = _0; rm[2][1] = -sa; rm[2][2] = ca;
1890 break;
1891 case 1:
1892 rm[0][0] = ca; rm[0][1] = _0; rm[0][2] = -sa;
1893 rm[1][0] = _0; rm[1][1] = _1; rm[1][2] = _0;
1894 rm[2][0] = sa; rm[2][1] = _0; rm[2][2] = ca;
1895 break;
1896 case 2:
1897 rm[0][0] = ca; rm[0][1] = sa; rm[0][2] = _0;
1898 rm[1][0] = -sa; rm[1][1] = ca; rm[1][2] = _0;
1899 rm[2][0] = _0; rm[2][1] = _0; rm[2][2] = _1;
1900 break;
1901 }
1902 for(a=0;a<3;a++)
1903 {
1904 nm[3][a] = om[3][a];
1905 nm[4][a] = om[4][a];
1906 for(b=0;b<3;b++)
1907 nm[a][b] =
1908 rm[a][0]*om[0][b] +
1909 rm[a][1]*om[1][b] +
1910 rm[a][2]*om[2][b];
1911 }
1912
1913 normalize3f(nm[0]);
1914 normalize3f(nm[1]);
1915 normalize3f(nm[2]);
1916
1917 }
1918 */
1919
rotation_to_matrix(Matrix53f rot,const float * axis,float angle)1920 void rotation_to_matrix(Matrix53f rot, const float *axis, float angle)
1921 {
1922 rotation_matrix3f(angle, axis[0], axis[1], axis[2], &rot[0][0]);
1923 }
1924
find_axis(Matrix33d a,float * axis)1925 static void find_axis(Matrix33d a, float *axis)
1926 {
1927 doublereal at[3][3], v[3][3], vt[3][3], fv1[3][3];
1928 integer iv1[3];
1929 integer ierr;
1930 integer nm, n, matz;
1931 doublereal wr[3], wi[3];
1932 /*p[3][3]; */
1933 int x, y;
1934
1935 nm = 3;
1936 n = 3;
1937 matz = 1;
1938
1939 recondition33d(&a[0][0]); /* IMPORTANT! */
1940
1941 for(x = 0; x < 3; x++) {
1942 for(y = 0; y < 3; y++) {
1943 at[y][x] = a[x][y];
1944 }
1945 }
1946
1947 pymol_rg_(&nm, &n, &at[0][0], wr, wi, &matz, &vt[0][0], iv1, &fv1[0][0], &ierr);
1948
1949 for(x = 0; x < 3; x++) {
1950 for(y = 0; y < 3; y++) {
1951 v[y][x] = vt[x][y];
1952 }
1953 }
1954
1955 axis[0] = 0.0F;
1956 axis[1] = 0.0F;
1957 axis[2] = 0.0F;
1958
1959 {
1960 doublereal max_real = 0.0F, test_real;
1961 doublereal min_imag = 1.0F, test_imag;
1962 float test_inp[3], test_out[3];
1963
1964 for(x = 0; x < 3; x++) { /* looking for an eigvalue of (1,0) */
1965 /* printf("wr %8.3f wi %8.3f\n",wr[x],wi[x]);
1966 printf("%8.3f %8.3f %8.3f\n",
1967 v[0][x],v[1][x],v[2][x]); */
1968 test_real = fabs(wr[x]);
1969 test_imag = fabs(wi[x]);
1970
1971 if((test_real >= max_real) && (test_imag <= min_imag)) {
1972 for(y = 0; y < 3; y++)
1973 test_inp[y] = (float) v[y][x];
1974 transform33d3f(a, test_inp, test_out); /* confirm that axis is invariant to rotation */
1975 test_out[0] -= test_inp[0];
1976 test_out[1] -= test_inp[1];
1977 test_out[2] -= test_inp[2];
1978 if((test_out[0] * test_out[0] +
1979 test_out[1] * test_out[1] + test_out[2] * test_out[2]) < 0.1) {
1980 for(y = 0; y < 3; y++)
1981 axis[y] = test_inp[y];
1982 max_real = test_real;
1983 min_imag = test_imag;
1984 }
1985 } else {
1986 /*for(y=0;y<3;y++)
1987 v[y][x]=_0; */
1988 }
1989 }
1990 }
1991 /*
1992 printf("eigenvectors\n%8.3f %8.3f %8.3f\n",v[0][0],v[0][1],v[0][2]);
1993 printf("%8.3f %8.3f %8.3f\n",v[1][0],v[1][1],v[1][2]);
1994 printf("%8.3f %8.3f %8.3f\n",v[2][0],v[2][1],v[2][2]);
1995
1996 printf("eigenvalues\n%8.3f %8.3f %8.3f\n",wr[0],wr[1],wr[2]);
1997 printf("%8.3f %8.3f %8.3f\n",wi[0],wi[1],wi[2]);
1998 */
1999
2000 /* matrix_multiply33d33d(a,v,p);
2001
2002 printf("invariance\n");
2003 printf("%8.3f %8.3f %8.3f\n",p[0][0],p[0][1],p[0][2]);
2004 printf("%8.3f %8.3f %8.3f\n",p[1][0],p[1][1],p[1][2]);
2005 printf("%8.3f %8.3f %8.3f\n",p[2][0],p[2][1],p[2][2]);
2006 */
2007
2008 }
2009
matrix_to_rotation(Matrix53f rot,float * axis,float * angle)2010 void matrix_to_rotation(Matrix53f rot, float *axis, float *angle)
2011 {
2012 float perp[3], tmp[3], rperp[3], dirck[3];
2013 Matrix33d rot3d;
2014 Matrix53f rotck;
2015 int a, b;
2016
2017 #ifdef MATCHK
2018 printf("starting matrix\n");
2019 for(a = 0; a < 3; a++)
2020 printf("%8.3f %8.3f %8.3f\n", rot[a][0], rot[a][1], rot[a][2]);
2021 #endif
2022
2023 for(a = 0; a < 3; a++)
2024 for(b = 0; b < 3; b++)
2025 rot3d[a][b] = (double) rot[a][b];
2026
2027 find_axis(rot3d, axis);
2028
2029 /* find a perpendicular vector */
2030
2031 perp[0] = axis[1] * axis[0] - axis[2] * axis[2];
2032 perp[1] = axis[2] * axis[1] - axis[0] * axis[0];
2033 perp[2] = axis[0] * axis[2] - axis[1] * axis[1];
2034
2035 if(length3f(perp) < R_SMALL) {
2036 tmp[0] = axis[0];
2037 tmp[1] = -2 * axis[1];
2038 tmp[2] = axis[2];
2039 cross_product3f(axis, tmp, perp);
2040 }
2041
2042 normalize3f(perp);
2043
2044 transform33d3f(rot3d, perp, rperp);
2045
2046 *angle = get_angle3f(perp, rperp);
2047
2048 cross_product3f(perp, rperp, dirck);
2049 if(((dirck[0] * axis[0]) + (dirck[1] * axis[1]) + (dirck[2] * axis[2])) < _0)
2050 *angle = -*angle;
2051
2052 /* printf("angle %8.3f \n",*angle); */
2053
2054 rotation_to_matrix(rotck, axis, *angle);
2055
2056 #ifdef MATCHK
2057 printf("reconstructed matrix: \n");
2058 for(a = 0; a < 3; a++)
2059 printf("%8.3f %8.3f %8.3f\n", rotck[a][0], rotck[a][1], rotck[a][2]);
2060 printf("\n");
2061 #endif
2062
2063 }
mult3f(const float * vsrc,const float val,float * vdest)2064 void mult3f(const float *vsrc, const float val, float *vdest){
2065 vdest[0] = vsrc[0] * val;
2066 vdest[1] = vsrc[1] * val;
2067 vdest[2] = vsrc[2] * val;
2068 }
2069
mult4f(const float * vsrc,const float val,float * vdest)2070 void mult4f(const float *vsrc, const float val, float *vdest){
2071 vdest[0] = vsrc[0] * val;
2072 vdest[1] = vsrc[1] * val;
2073 vdest[2] = vsrc[2] * val;
2074 vdest[3] = vsrc[3] * val;
2075 }
2076
max3(float val1,float val2,float val3)2077 float max3(float val1, float val2, float val3){
2078 if (val1>val2){
2079 if (val1>val3){
2080 return val1;
2081 } else {
2082 return val3;
2083 }
2084 } else {
2085 if (val2>val3){
2086 return val2;
2087 } else {
2088 return val3;
2089 }
2090 }
2091 }
ave3(float val1,float val2,float val3)2092 float ave3(float val1, float val2, float val3){
2093 return ((val1+val2+val3)/3.f);
2094 }
ave2(float val1,float val2)2095 float ave2(float val1, float val2){
2096 return ((val1+val2)/2.f);
2097 }
2098
white4f(float * rgba,float value)2099 void white4f(float *rgba, float value){
2100 rgba[0] = value;
2101 rgba[1] = value;
2102 rgba[2] = value;
2103 rgba[3] = 1.0F;
2104 }
add4f(const float * v1,const float * v2,float * v3)2105 void add4f(const float *v1, const float *v2, float *v3)
2106 {
2107 v3[0] = v1[0] + v2[0];
2108 v3[1] = v1[1] + v2[1];
2109 v3[2] = v1[2] + v2[2];
2110 v3[3] = v1[3] + v2[3];
2111 }
2112
countchrs(const char * str,char ch)2113 int countchrs(const char *str, char ch){
2114 int cnt = 0;
2115 const char *tmp = str;
2116 while((tmp = strchr(tmp, ch))) {
2117 cnt++;
2118 tmp++;
2119 }
2120 return cnt;
2121 }
2122
2123 /*
2124 * sigmoid smoothstep function with
2125 * smooth(0) = 0
2126 * smooth(1) = 1
2127 * smooth'(0) = 0
2128 * smooth'(1) = 0
2129 */
smooth(float x,float power)2130 float smooth(float x, float power)
2131 {
2132
2133 if(x <= 0.5F) {
2134 if(x <= 0.0F)
2135 return 0.0F;
2136 return 0.5F * powf(2.0F * x, power);
2137 }
2138 if(x >= 1.0F)
2139 return 1.0F;
2140 return 1.0F - (0.5F * powf(2.0F * (1.0F - x), power));
2141 }
2142
2143 /* Divides the unit circle radially into n segments with n >= 3. */
subdivide(int n,float * x,float * y)2144 void subdivide(int n, float *x, float *y)
2145 {
2146 int a;
2147 if(n < 3) {
2148 n = 3;
2149 }
2150 for(a = 0; a <= n; a++) {
2151 x[a] = (float) cos(a * 2 * PI / n);
2152 y[a] = (float) sin(a * 2 * PI / n);
2153 }
2154 }
2155