pub_soft/pymol-open-source
3
0
Fork 0
mirror of https://github.com/schrodinger/pymol-open-source.git synced 2026-06-04 20:04:21 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
e58d6ac20732f6bf7bd39072f7d2eb2cf323723a
pymol-open-source/layer0/Vector.cpp
Thomas Holder e58d6ac207 Fix warning: -Wimplicit-int-float-conversion 
implicit conversion from 'int' to 'float' changes value from
2147483647 to 2147483648 [-Wimplicit-int-float-conversion]
2020-07-03 14:55:52 +02:00

2155 lines
54 KiB
C++
Raw Blame History

/*
A* -------------------------------------------------------------------
B* This file contains source code for the PyMOL computer program
C* Copyright (c) Schrodinger, LLC.
D* -------------------------------------------------------------------
E* It is unlawful to modify or remove this copyright notice.
F* -------------------------------------------------------------------
G* Please see the accompanying LICENSE file for further information.
H* -------------------------------------------------------------------
I* Additional authors of this source file include:
-*
-*
-*
Z* -------------------------------------------------------------------
*/
#include"os_predef.h"
#include"os_std.h"
#include"Base.h"
#include"Vector.h"
#include"Matrix.h"
#define MASK_01010101 (((unsigned long)(-1))/3)
#define MASK_00110011 (((unsigned long)(-1))/5)
#define MASK_00001111 (((unsigned long)(-1))/17)
#define MASK_11111111 (((unsigned long)(-1))/257)
#define MASK_01010101010101010101010101010101 MASK_01010101
#define MASK_00110011001100110011001100110011 MASK_00110011
#define MASK_00001111000011110000111100001111 MASK_00001111
#define MASK_00000000111111110000000011111111 MASK_11111111
#define MASK_00000000000000001111111111111111 (((unsigned long)(-1))/65537)
short countBits(unsigned long bits){
unsigned long n = bits;
n = (n & MASK_01010101010101010101010101010101) + ((n >> 1) & MASK_01010101010101010101010101010101) ;
n = (n & MASK_00110011001100110011001100110011) + ((n >> 2) & MASK_00110011001100110011001100110011) ;
n = (n & MASK_00001111000011110000111100001111) + ((n >> 4) & MASK_00001111000011110000111100001111) ;
n = (n & MASK_00000000111111110000000011111111) + ((n >> 8) & MASK_00000000111111110000000011111111) ;
n = (n & MASK_00000000000000001111111111111111) + ((n >> 16) & MASK_00000000000000001111111111111111) ;
return (n % 255);
}
short countBitsInt(int bits){
/* This could be improved to not splitting
the bits into 4 variables in a loop which are 16 bits each */
unsigned long n = bits & 65535;
short nbits = 0;
n = (n & MASK_01010101) + ((n >> 1) & MASK_01010101) ;
n = (n & MASK_00110011) + ((n >> 2) & MASK_00110011) ;
n = (n & MASK_00001111) + ((n >> 4) & MASK_00001111) ;
nbits += (n % 255);
return nbits;
}
#ifndef R_SMALL
#define R_SMALL 0.000000001
#endif
#ifndef R_MED
#define R_MED 0.00001
#endif
#define cPI 3.14159265358979323846 /* pi */
static const float _0 = 0.0F;
static const float _1 = 1.0F;
static const double _d0 = 0.0;
static const double _d1 = 1.0;
void mix3f(const float *v1, const float *v2, const float fxn, float *v3)
{
float fxn_1 = 1.0F - fxn;
v3[0] = v1[0] * fxn_1 + v2[0] * fxn;
v3[1] = v1[1] * fxn_1 + v2[1] * fxn;
v3[2] = v1[2] * fxn_1 + v2[2] * fxn;
}
void mix3d(const double *v1, const double *v2, const double fxn, double *v3)
{
double fxn_1 = 1.0F - fxn;
v3[0] = v1[0] * fxn_1 + v2[0] * fxn;
v3[1] = v1[1] * fxn_1 + v2[1] * fxn;
v3[2] = v1[2] * fxn_1 + v2[2] * fxn;
}
unsigned int optimizer_workaround1u(unsigned int value)
{
return value;
}
float get_random0to1f()
{
return rand() / (1.0 + RAND_MAX);
}
int pymol_roundf(float f)
{
if(f > 0.0F)
return (int) (f + 0.49999F);
else
return (int) (f - 0.49999F);
}
void dump3i(const int *v, const char *prefix)
{ /* for debugging */
printf("%s %8i %8i %8i\n", prefix, v[0], v[1], v[2]);
}
void dump3f(const float *v, const char *prefix)
{ /* for debugging */
printf("%s %8.3f %8.3f %8.3f\n", prefix, v[0], v[1], v[2]);
}
void dump2f(const float *v, const char *prefix)
{ /* for debugging */
printf("%s %8.3f %8.3f\n", prefix, v[0], v[1]);
}
void dump3d(const double *v, const char *prefix)
{ /* for debugging */
printf("%s %8.3f %8.3f %8.3f\n", prefix, v[0], v[1], v[2]);
}
void dump4f(const float *v, const char *prefix)
{ /* for debugging */
printf("%s %8.3f %8.3f %8.3f %8.3f\n", prefix, v[0], v[1], v[2], v[3]);
}
void dump33f(const float *m, const char *prefix)
{ /* for debugging */
if(m) {
printf("%s:0 %8.3f %8.3f %8.3f\n", prefix, m[0], m[1], m[2]);
printf("%s:1 %8.3f %8.3f %8.3f\n", prefix, m[3], m[4], m[5]);
printf("%s:2 %8.3f %8.3f %8.3f\n", prefix, m[6], m[7], m[8]);
} else {
printf("%s: (null matrix pointer)\n", prefix);
}
}
void dump44f(const float *m, const char *prefix)
{ /* for debugging */
if(m) {
if(prefix) {
printf("%s:0 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[0], m[1], m[2], m[3]);
printf("%s:1 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[4], m[5], m[6], m[7]);
printf("%s:2 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[8], m[9], m[10], m[11]);
printf("%s:3 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[12], m[13], m[14], m[15]);
} else {
}
} else {
printf("%s: (null matrix pointer)\n", prefix);
}
}
void dump44d(const double *m, const char *prefix)
{ /* for debugging */
if(m) {
printf("%s:0 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[0], m[1], m[2], m[3]);
printf("%s:1 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[4], m[5], m[6], m[7]);
printf("%s:2 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[8], m[9], m[10], m[11]);
printf("%s:3 %8.3f %8.3f %8.3f %8.3f\n", prefix, m[12], m[13], m[14], m[15]);
} else {
printf("%s: (null matrix pointer)\n", prefix);
}
}
void dump33d(const double *m, const char *prefix)
{ /* for debugging */
printf("%s:0 %8.3f %8.3f %8.3f\n", prefix, m[0], m[1], m[2]);
printf("%s:1 %8.3f %8.3f %8.3f\n", prefix, m[3], m[4], m[5]);
printf("%s:2 %8.3f %8.3f %8.3f\n", prefix, m[6], m[7], m[8]);
}
void get_divergent3f(const float *src, float *dst)
{
if(src[0] != _0) {
*(dst++) = -*(src++);
*(dst++) = *(src++) + 0.1F;
*(dst++) = *(src++);
} else if(src[1] != _0) {
*(dst++) = *(src++) + 0.1F;
*(dst++) = -*(src++);
*(dst++) = *(src++);
} else {
*(dst++) = *(src++) + 0.1F;
*(dst++) = *(src++);
*(dst++) = -*(src++);
}
}
int equal3f(const float *v1, const float *v2)
{
return ((fabs(v1[0] - v2[0]) < R_SMALL) &&
(fabs(v1[1] - v2[1]) < R_SMALL) && (fabs(v1[2] - v2[2]) < R_SMALL));
}
void get_random3f(float *x)
{ /* this needs to be fixed as in Tinker */
x[0] = 0.5F - get_random0to1f();
x[1] = 0.5F - get_random0to1f();
x[2] = 0.5F - get_random0to1f();
normalize3f(x);
}
void get_system3f(float *x, float *y, float *z)
{ /* make random system */
get_random3f(x);
get_divergent3f(x, y);
cross_product3f(x, y, z);
normalize3f(z);
cross_product3f(z, x, y);
normalize3f(y);
normalize3f(x);
}
void get_system1f3f(float *x, float *y, float *z)
{ /* make system in direction of x */
get_divergent3f(x, y);
cross_product3f(x, y, z);
normalize3f(z);
cross_product3f(z, x, y);
normalize3f(y);
normalize3f(x);
}
void get_system2f3f(float *x, float *y, float *z)
{ /* make system in direction of x */
cross_product3f(x, y, z);
normalize3f(z);
cross_product3f(z, x, y);
normalize3f(y);
normalize3f(x);
}
void extrapolate3f(const float *v1, const float *unit, float *result)
{
float lsq = lengthsq3f(v1);
float dp = dot_product3f(v1, unit);
if(dp != 0.0F) {
float l2 = lsq / dp;
scale3f(unit, l2, result);
}
}
void scatter3f(float *v, float weight)
{
float r[3];
get_random3f(r);
scale3f(r, weight, r);
add3f(r, v, v);
normalize3f(v);
}
void wiggle3f(float *v, const float *p, const float *s)
{
float q[3];
q[0] = (float) cos((p[0] + p[1] + p[2]) * s[1]);
q[1] = (float) cos((p[0] - p[1] + p[2]) * s[1]);
q[2] = (float) cos((p[0] + p[1] - p[2]) * s[1]);
scale3f(q, s[0], q);
add3f(q, v, v);
normalize3f(v);
}
void copy3d3f(const double *v1, float *v2)
{
v2[0] = (float) v1[0];
v2[1] = (float) v1[1];
v2[2] = (float) v1[2];
}
void copy3f3d(const float *v1, double *v2)
{
v2[0] = (double) v1[0];
v2[1] = (double) v1[1];
v2[2] = (double) v1[2];
}
void min3f(const float *v1, const float *v2, float *v3)
{
(v3)[0] = ((v1)[0] < (v2)[0] ? (v1)[0] : (v2)[0]);
(v3)[1] = ((v1)[1] < (v2)[1] ? (v1)[1] : (v2)[1]);
(v3)[2] = ((v1)[2] < (v2)[2] ? (v1)[2] : (v2)[2]);
}
void max3f(const float *v1, const float *v2, float *v3)
{
(v3)[0] = ((v1)[0] > (v2)[0] ? (v1)[0] : (v2)[0]);
(v3)[1] = ((v1)[1] > (v2)[1] ? (v1)[1] : (v2)[1]);
(v3)[2] = ((v1)[2] > (v2)[2] ? (v1)[2] : (v2)[2]);
}
double dot_product3d(const double *v1, const double *v2)
{
return (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]);
}
void identity33f(float *m)
{
m[0] = _1;
m[1] = _0;
m[2] = _0;
m[3] = _0;
m[4] = _1;
m[5] = _0;
m[6] = _0;
m[7] = _0;
m[8] = _1;
}
void identity33d(double *m)
{
m[0] = _d1;
m[1] = _d0;
m[2] = _d0;
m[3] = _d0;
m[4] = _d1;
m[5] = _d0;
m[6] = _d0;
m[7] = _d0;
m[8] = _d1;
}
void identity44f(float *m1)
{
int a;
for(a = 0; a < 16; a++)
m1[a] = _0;
for(a = 0; a < 16; a = a + 5)
m1[a] = _1;
}
void identity44d(double *m1)
{
int a;
for(a = 0; a < 16; a++)
m1[a] = _d0;
for(a = 0; a < 16; a = a + 5)
m1[a] = _d1;
}
/*
* Check a nxn matrix for identity
*/
bool is_identityf(int n, const float *m, float threshold)
{
int n_sq = n * n, stride = n + 1;
for (int a = 0; a < n_sq; a++) {
float e = (a % stride) ? _0 : _1;
if (fabsf(m[a] - e) > threshold)
return false;
}
return true;
}
/*
* Check if two matrices are the same. The two matrices may have different
* number of rows and columns, in that case only the overlaying upper left
* (nrow x min(ncol1, ncol2)) submatrix is compared.
*/
bool is_allclosef(int nrow,
const float *m1, int ncol1,
const float *m2, int ncol2, float threshold)
{
int ncol = (ncol1 < ncol2) ? ncol1 : ncol2;
for (int i = 0; i < nrow; i++) {
for (int j = 0; j < ncol; j++) {
int a1 = i * ncol1 + j, a2 = i * ncol2 + j;
if (fabsf(m1[a1] - m2[a2]) > threshold)
return false;
}
}
return true;
}
/*
* Check a nxm matrix is a diagonal matrix (non-diagonal elements are zero)
*/
bool is_diagonalf(int nrow,
const float *m, int ncol, float threshold)
{
if (!ncol)
ncol = nrow;
for (int i = 0; i < nrow; ++i) {
for (int j = 0; j < ncol; ++j) {
if (i != j && fabsf(m[i * ncol + j]) > threshold)
return false;
}
}
return true;
}
/*
* Determinant of the upper left 3x3 submatrix.
*/
double determinant33f(const float *m, int ncol)
{
int &a = ncol, b = ncol * 2;
return
m[0] * (m[a + 1] * (double) m[b + 2] - m[a + 2] * (double) m[b + 1]) -
m[1] * (m[a + 0] * (double) m[b + 2] - m[a + 2] * (double) m[b + 0]) +
m[2] * (m[a + 0] * (double) m[b + 1] - m[a + 1] * (double) m[b + 0]);
}
void glOrtho44f(float *m1,
GLfloat left, GLfloat right,
GLfloat bottom, GLfloat top,
GLfloat nearVal, GLfloat farVal){
int a;
/* this is set in column major order */
for(a = 0; a < 16; a++)
m1[a] = _0;
m1[0] = 2.f / (right-left);
m1[5] = 2.f / (top-bottom);
m1[10] = -2.f/(farVal-nearVal);
m1[12] = - (right+left) / (right-left);
m1[13] = - (top+bottom) / (top-bottom);
m1[14] = - (farVal+nearVal) / (farVal-nearVal);
m1[15] = _1;
}
void glFrustum44f(float *m1,
GLfloat left, GLfloat right,
GLfloat bottom, GLfloat top,
GLfloat nearVal, GLfloat farVal){
int a;
/* this is set in column major order */
for(a = 0; a < 16; a++)
m1[a] = _0;
m1[0] = 2.f * nearVal / (right-left);
m1[5] = 2.f * nearVal / (top-bottom);
m1[8] = (right+left) / (right-left);
m1[9] = (top+bottom) / (top-bottom);
m1[10] = -(farVal+nearVal) / (farVal-nearVal);
m1[11] = -1.f;
m1[14] = -2.f * (farVal * nearVal) / (farVal - nearVal);
}
void copy44f(const float *src, float *dst)
{
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
}
void copy44d(const double *src, double *dst)
{
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
}
void copy44d33f(const double *src, float *dst)
{
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
src++;
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
src++;
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
src++;
}
void copy44f33f(const float *src, float *dst)
{
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
src++;
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
src++;
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
src++;
}
void copy44f44d(const float *src, double *dst)
{
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
}
void copy44d44f(const double *src, float *dst)
{
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
*(dst++) = (float) *(src++);
}
void copy33f44d(const float *src, double *dst)
{
const double _0 = 0.0;
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = _0;
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = _0;
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = (double) *(src++);
*(dst++) = _0;
*(dst++) = _0;
*(dst++) = _0;
*(dst++) = _0;
*(dst++) = _1;
}
void copy33f44f(const float *src, float *dst)
{
const float _0 = 0.0;
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = _0;
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = _0;
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = *(src++);
*(dst++) = _0;
*(dst++) = _0;
*(dst++) = _0;
*(dst++) = _0;
*(dst++) = _1;
}
/* Transformation: MatMult( (3x3), (3x1) = (3x1) ) */
void transform33f3f(const float *m1, const float *m2, float *m3)
{
float m2r0 = m2[0];
float m2r1 = m2[1];
float m2r2 = m2[2];
m3[0] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2;
m3[1] = m1[3] * m2r0 + m1[4] * m2r1 + m1[5] * m2r2;
m3[2] = m1[6] * m2r0 + m1[7] * m2r1 + m1[8] * m2r2;
}
void transpose33f33f(const float *m1, float *m2)
{
m2[0] = m1[0];
m2[1] = m1[3];
m2[2] = m1[6];
m2[3] = m1[1];
m2[4] = m1[4];
m2[5] = m1[7];
m2[6] = m1[2];
m2[7] = m1[5];
m2[8] = m1[8];
}
void transpose33d33d(const double *m1, double *m2)
{
m2[0] = m1[0];
m2[1] = m1[3];
m2[2] = m1[6];
m2[3] = m1[1];
m2[4] = m1[4];
m2[5] = m1[7];
m2[6] = m1[2];
m2[7] = m1[5];
m2[8] = m1[8];
}
void transpose44f44f(const float *m1, float *m2)
{
m2[0] = m1[0];
m2[1] = m1[4];
m2[2] = m1[8];
m2[3] = m1[12];
m2[4] = m1[1];
m2[5] = m1[5];
m2[6] = m1[9];
m2[7] = m1[13];
m2[8] = m1[2];
m2[9] = m1[6];
m2[10] = m1[10];
m2[11] = m1[14];
m2[12] = m1[3];
m2[13] = m1[7];
m2[14] = m1[11];
m2[15] = m1[15];
}
void transpose44d44d(const double *m1, double *m2)
{
m2[0] = m1[0];
m2[1] = m1[4];
m2[2] = m1[8];
m2[3] = m1[12];
m2[4] = m1[1];
m2[5] = m1[5];
m2[6] = m1[9];
m2[7] = m1[13];
m2[8] = m1[2];
m2[9] = m1[6];
m2[10] = m1[10];
m2[11] = m1[14];
m2[12] = m1[3];
m2[13] = m1[7];
m2[14] = m1[11];
m2[15] = m1[15];
}
void transform33Tf3f(const float *m1, const float *m2, float *m3)
{
float m2r0 = m2[0];
float m2r1 = m2[1];
float m2r2 = m2[2];
m3[0] = m1[0] * m2r0 + m1[3] * m2r1 + m1[6] * m2r2;
m3[1] = m1[1] * m2r0 + m1[4] * m2r1 + m1[7] * m2r2;
m3[2] = m1[2] * m2r0 + m1[5] * m2r1 + m1[8] * m2r2;
}
/* multiply the upper-left 3x3 of a 4x4, m, by m2 and put result into m3:
* m3 = A x m2 */
void transform44f3f(const float *m1, const float *m2, float *m3)
{
float m2r0 = m2[0];
float m2r1 = m2[1];
float m2r2 = m2[2];
m3[0] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2 + m1[3];
m3[1] = m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2 + m1[7];
m3[2] = m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2 + m1[11];
}
/* Multi the upper left 3x3 of a 4x4 by a 3x1 vector; so effectively, [3x3]*[3x1] = [3x1] */
void transform44d3f(const double *m1, const float *m2, float *m3)
{
double m2r0 = m2[0];
double m2r1 = m2[1];
double m2r2 = m2[2];
m3[0] = (float) (m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2 + m1[3]);
m3[1] = (float) (m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2 + m1[7]);
m3[2] = (float) (m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2 + m1[11]);
}
void transform44d3d(const double *m1, const double *m2, double *m3)
{
double m2r0 = m2[0];
double m2r1 = m2[1];
double m2r2 = m2[2];
m3[0] = (float) (m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2 + m1[3]);
m3[1] = (float) (m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2 + m1[7]);
m3[2] = (float) (m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2 + m1[11]);
}
void transform44d3fas33d3f(const double *m1, const float *m2, float *m3)
{
double m2r0 = m2[0];
double m2r1 = m2[1];
double m2r2 = m2[2];
m3[0] = (float) (m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2);
m3[1] = (float) (m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2);
m3[2] = (float) (m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2);
}
// same as MatrixInvTransformC44fAs33f3f
void transform44f3fas33f3f(const float *m1, const float *m2, float *m3)
{
float m2r0 = m2[0];
float m2r1 = m2[1];
float m2r2 = m2[2];
m3[0] = (m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2);
m3[1] = (m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2);
m3[2] = (m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2);
}
void inverse_transformC44f3f(const float *m1, const float *m2, float *m3)
{
float m2r0 = m2[0] - m1[12];
float m2r1 = m2[1] - m1[13];
float m2r2 = m2[2] - m1[14];
m3[0] = (float) (m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2);
m3[1] = (float) (m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2);
m3[2] = (float) (m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2);
}
void inverse_transform44f3f(const float *m1, const float *m2, float *m3)
{
float m2r0 = m2[0] - m1[3];
float m2r1 = m2[1] - m1[7];
float m2r2 = m2[2] - m1[11];
m3[0] = (float) (m1[0] * m2r0 + m1[4] * m2r1 + m1[8] * m2r2);
m3[1] = (float) (m1[1] * m2r0 + m1[5] * m2r1 + m1[9] * m2r2);
m3[2] = (float) (m1[2] * m2r0 + m1[6] * m2r1 + m1[10] * m2r2);
}
void inverse_transform44d3f(const double *m1, const float *m2, float *m3)
{
double m2r0 = m2[0] - m1[3];
double m2r1 = m2[1] - m1[7];
double m2r2 = m2[2] - m1[11];
m3[0] = (float) (m1[0] * m2r0 + m1[4] * m2r1 + m1[8] * m2r2);
m3[1] = (float) (m1[1] * m2r0 + m1[5] * m2r1 + m1[9] * m2r2);
m3[2] = (float) (m1[2] * m2r0 + m1[6] * m2r1 + m1[10] * m2r2);
}
void inverse_transform44d3d(const double *m1, const double *m2, double *m3)
{
double m2r0 = m2[0] - m1[3];
double m2r1 = m2[1] - m1[7];
double m2r2 = m2[2] - m1[11];
m3[0] = (float) (m1[0] * m2r0 + m1[4] * m2r1 + m1[8] * m2r2);
m3[1] = (float) (m1[1] * m2r0 + m1[5] * m2r1 + m1[9] * m2r2);
m3[2] = (float) (m1[2] * m2r0 + m1[6] * m2r1 + m1[10] * m2r2);
}
void transform44f4f(const float *m1, const float *m2, float *m3)
{
float m2r0 = m2[0];
float m2r1 = m2[1];
float m2r2 = m2[2];
float m2r3 = m2[3];
m3[0] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2 + m1[3] * m2r3;
m3[1] = m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2 + m1[7] * m2r3;
m3[2] = m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2 + m1[11] * m2r3;
m3[3] = m1[12] * m2r0 + m1[13] * m2r1 + m1[14] * m2r2 + m1[15] * m2r3;
}
void initializeTTT44f(float *m)
{
int a;
for(a = 0; a < 16; a++)
m[a] = _0;
for(a = 0; a < 4; a++)
m[4 * a + a] = _1;
}
void combineTTT44f44f(const float *m1, const float *m2, float *m3)
/* WARNING: this routine is ill-conceived and essentially broken */
/* NOTE: this is NOT equivalent to 4x4 matrix multiplication.
TTTs are designed for easily creating movies of rotating
bodies! */
{
float m1_homo[16];
float m2_homo[16];
const float *src;
float *dst;
float pre[3], post[3];
/* convert the existing TTT into a homogenous transformation matrix */
convertTTTfR44f(m1, m1_homo);
convertTTTfR44f(m2, m2_homo);
/* combine the matrices */
left_multiply44f44f(m1_homo, m2_homo);
/* now use the origin from the most recent TTT */
src = m1 + 12;
invert3f3f(src, pre);
transform44f3fas33f3f(m2_homo, pre, post);
m2_homo[3] += post[0];
m2_homo[7] += post[1];
m2_homo[11] += post[2];
dst = m2_homo + 12;
copy3f(src, dst);
copy44f(m2_homo, m3);
}
void convertTTTfR44d(const float *ttt, double *homo)
{
/* takes the PyMOL-specific TTT matrix and
makes a homogenous 4x4 txf matrix homo of it */
double ttt_3 = (double) ttt[3];
double ttt_7 = (double) ttt[7];
double ttt_11 = (double) ttt[11];
double ttt_12 = (double) ttt[12];
double ttt_13 = (double) ttt[13];
double ttt_14 = (double) ttt[14];
/* dump44f(ttt,"ttt"); */
homo[0] = (double) ttt[0];
homo[1] = (double) ttt[1];
homo[2] = (double) ttt[2];
homo[4] = (double) ttt[4];
homo[5] = (double) ttt[5];
homo[6] = (double) ttt[6];
homo[8] = (double) ttt[8];
homo[9] = (double) ttt[9];
homo[10] = (double) ttt[10];
homo[3] = (homo[0] * ttt_12) + (homo[1] * ttt_13) + (homo[2] * ttt_14) + ttt_3;
homo[7] = (homo[4] * ttt_12) + (homo[5] * ttt_13) + (homo[6] * ttt_14) + ttt_7;
homo[11] = (homo[8] * ttt_12) + (homo[9] * ttt_13) + (homo[10] * ttt_14) + ttt_11;
homo[12] = 0.0;
homo[13] = 0.0;
homo[14] = 0.0;
homo[15] = 1.0;
/* dump44d(homo, "homo"); */
}
void convertTTTfR44f(const float *ttt, float *homo)
{
/* takes the PyMOL-specific TTT matrix and
makes a homogenous 4x4 txf matrix homo of it */
float ttt_3 = ttt[3];
float ttt_7 = ttt[7];
float ttt_11 = ttt[11];
float ttt_12 = ttt[12];
float ttt_13 = ttt[13];
float ttt_14 = ttt[14];
homo[0] = ttt[0];
homo[1] = ttt[1];
homo[2] = ttt[2];
homo[4] = ttt[4];
homo[5] = ttt[5];
homo[6] = ttt[6];
homo[8] = ttt[8];
homo[9] = ttt[9];
homo[10] = ttt[10];
homo[3] = (homo[0] * ttt_12) + (homo[1] * ttt_13) + (homo[2] * ttt_14) + ttt_3;
homo[7] = (homo[4] * ttt_12) + (homo[5] * ttt_13) + (homo[6] * ttt_14) + ttt_7;
homo[11] = (homo[8] * ttt_12) + (homo[9] * ttt_13) + (homo[10] * ttt_14) + ttt_11;
homo[12] = 0.0;
homo[13] = 0.0;
homo[14] = 0.0;
homo[15] = 1.0;
}
void convert44d44f(const double *dbl, float *flt)
{
flt[0] = (float) dbl[0];
flt[1] = (float) dbl[1];
flt[2] = (float) dbl[2];
flt[3] = (float) dbl[3];
flt[4] = (float) dbl[4];
flt[5] = (float) dbl[5];
flt[6] = (float) dbl[6];
flt[7] = (float) dbl[7];
flt[8] = (float) dbl[8];
flt[9] = (float) dbl[9];
flt[10] = (float) dbl[10];
flt[11] = (float) dbl[11];
flt[12] = (float) dbl[12];
flt[13] = (float) dbl[13];
flt[14] = (float) dbl[14];
flt[15] = (float) dbl[15];
}
void convert44f44d(const float *flt, double *dbl)
{
dbl[0] = (double) flt[0];
dbl[1] = (double) flt[1];
dbl[2] = (double) flt[2];
dbl[3] = (double) flt[3];
dbl[4] = (double) flt[4];
dbl[5] = (double) flt[5];
dbl[6] = (double) flt[6];
dbl[7] = (double) flt[7];
dbl[8] = (double) flt[8];
dbl[9] = (double) flt[9];
dbl[10] = (double) flt[10];
dbl[11] = (double) flt[11];
dbl[12] = (double) flt[12];
dbl[13] = (double) flt[13];
dbl[14] = (double) flt[14];
dbl[15] = (double) flt[15];
}
void convertR44dTTTf(const double *homo, float *ttt)
{
/* nowadays, homogeneous matrices with (0,0,0,1) in 4th row are TTT
compatible */
convert44d44f(homo, ttt);
}
void multiply44d44d44d(const double *left, const double *right, double *product)
{
double rA = right[0];
double rB = right[4];
double rC = right[8];
double rD = right[12];
product[0] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
product[4] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
product[8] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
product[12] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
rA = right[1];
rB = right[5];
rC = right[9];
rD = right[13];
product[1] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
product[5] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
product[9] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
product[13] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
rA = right[2];
rB = right[6];
rC = right[10];
rD = right[14];
product[2] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
product[6] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
product[10] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
product[14] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
rA = right[3];
rB = right[7];
rC = right[11];
rD = right[15];
product[3] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
product[7] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
product[11] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
product[15] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
}
void left_multiply44d44d(const double *left, double *right)
{
double rA = right[0];
double rB = right[4];
double rC = right[8];
double rD = right[12];
right[0] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
right[4] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
right[8] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
right[12] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
rA = right[1];
rB = right[5];
rC = right[9];
rD = right[13];
right[1] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
right[5] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
right[9] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
right[13] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
rA = right[2];
rB = right[6];
rC = right[10];
rD = right[14];
right[2] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
right[6] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
right[10] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
right[14] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
rA = right[3];
rB = right[7];
rC = right[11];
rD = right[15];
right[3] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
right[7] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
right[11] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
right[15] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
}
void right_multiply44d44d(double *left, const double *right)
{
double cA = left[0];
double cB = left[1];
double cC = left[2];
double cD = left[3];
left[0] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
left[1] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
left[2] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
left[3] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
cA = left[4];
cB = left[5];
cC = left[6];
cD = left[7];
left[4] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
left[5] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
left[6] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
left[7] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
cA = left[8];
cB = left[9];
cC = left[10];
cD = left[11];
left[8] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
left[9] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
left[10] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
left[11] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
cA = left[12];
cB = left[13];
cC = left[14];
cD = left[15];
left[12] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
left[13] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
left[14] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
left[15] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
}
void multiply44f44f44f(const float *left, const float *right, float *product)
{
float rA = right[0];
float rB = right[4];
float rC = right[8];
float rD = right[12];
product[0] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
product[4] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
product[8] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
product[12] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
rA = right[1];
rB = right[5];
rC = right[9];
rD = right[13];
product[1] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
product[5] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
product[9] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
product[13] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
rA = right[2];
rB = right[6];
rC = right[10];
rD = right[14];
product[2] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
product[6] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
product[10] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
product[14] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
rA = right[3];
rB = right[7];
rC = right[11];
rD = right[15];
product[3] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
product[7] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
product[11] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
product[15] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
}
void left_multiply44f44f(const float *left, float *right)
{
float rA = right[0];
float rB = right[4];
float rC = right[8];
float rD = right[12];
right[0] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
right[4] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
right[8] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
right[12] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
rA = right[1];
rB = right[5];
rC = right[9];
rD = right[13];
right[1] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
right[5] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
right[9] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
right[13] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
rA = right[2];
rB = right[6];
rC = right[10];
rD = right[14];
right[2] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
right[6] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
right[10] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
right[14] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
rA = right[3];
rB = right[7];
rC = right[11];
rD = right[15];
right[3] = left[0] * rA + left[1] * rB + left[2] * rC + left[3] * rD;
right[7] = left[4] * rA + left[5] * rB + left[6] * rC + left[7] * rD;
right[11] = left[8] * rA + left[9] * rB + left[10] * rC + left[11] * rD;
right[15] = left[12] * rA + left[13] * rB + left[14] * rC + left[15] * rD;
}
void right_multiply44f44f(float *left, const float *right)
{
float cA = left[0];
float cB = left[1];
float cC = left[2];
float cD = left[3];
left[0] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
left[1] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
left[2] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
left[3] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
cA = left[4];
cB = left[5];
cC = left[6];
cD = left[7];
left[4] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
left[5] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
left[6] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
left[7] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
cA = left[8];
cB = left[9];
cC = left[10];
cD = left[11];
left[8] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
left[9] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
left[10] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
left[11] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
cA = left[12];
cB = left[13];
cC = left[14];
cD = left[15];
left[12] = cA * right[0] + cB * right[4] + cC * right[8] + cD * right[12];
left[13] = cA * right[1] + cB * right[5] + cC * right[9] + cD * right[13];
left[14] = cA * right[2] + cB * right[6] + cC * right[10] + cD * right[14];
left[15] = cA * right[3] + cB * right[7] + cC * right[11] + cD * right[15];
}
void invert_special44d44d(const double *orig, double *inv)
{
/* inverse of the rotation matrix */
inv[0] = orig[0];
inv[1] = orig[4];
inv[2] = orig[8];
inv[4] = orig[1];
inv[5] = orig[5];
inv[6] = orig[9];
inv[8] = orig[2];
inv[9] = orig[6];
inv[10] = orig[10];
/* invert the translation portion */
inv[3] = -(orig[3] * orig[0] + orig[7] * orig[4] + orig[11] * orig[8]);
inv[7] = -(orig[3] * orig[1] + orig[7] * orig[5] + orig[11] * orig[9]);
inv[11] = -(orig[3] * orig[2] + orig[7] * orig[6] + orig[11] * orig[10]);
inv[12] = 0.0;
inv[13] = 0.0;
inv[14] = 0.0;
inv[15] = 1.0;
}
void invert_special44f44f(const float *orig, float *inv)
{
/* inverse of the rotation matrix */
inv[0] = orig[0];
inv[1] = orig[4];
inv[2] = orig[8];
inv[4] = orig[1];
inv[5] = orig[5];
inv[6] = orig[9];
inv[8] = orig[2];
inv[9] = orig[6];
inv[10] = orig[10];
/* invert the translation portion */
inv[3] = -(orig[3] * orig[0] + orig[7] * orig[4] + orig[11] * orig[8]);
inv[7] = -(orig[3] * orig[1] + orig[7] * orig[5] + orig[11] * orig[9]);
inv[11] = -(orig[3] * orig[2] + orig[7] * orig[6] + orig[11] * orig[10]);
inv[12] = 0.0F;
inv[13] = 0.0F;
inv[14] = 0.0F;
inv[15] = 1.0F;
}
static void normalize3dp(double *v1, double *v2, double *v3)
{
double vlen = sqrt1d((v1[0] * v1[0]) + (v2[0] * v2[0]) + (v3[0] * v3[0]));
if(vlen > R_SMALL) {
v1[0] /= vlen;
v2[0] /= vlen;
v3[0] /= vlen;
} else {
v1[0] = _0;
v2[1] = _0;
v3[2] = _0;
}
}
/* unused at present
static void normalize3df( float *v1, float *v2, float *v3 )
{
float vlen = (float)sqrt1f((v1[0]*v1[0]) +
(v2[0]*v2[0]) +
(v3[0]*v3[0]));
if(vlen>R_SMALL)
{
v1[0]/=vlen;
v2[0]/=vlen;
v3[0]/=vlen;
}
else
{
v1[0]=_0;
v2[1]=_0;
v3[2]=_0;
}
}
*/
void scale3d(const double *v1, const double v0, double *v2)
{
v2[0] = v1[0] * v0;
v2[1] = v1[1] * v0;
v2[2] = v1[2] * v0;
}
void add3d(const double *v1, const double *v2, double *v3)
{
v3[0] = v1[0] + v2[0];
v3[1] = v1[1] + v2[1];
v3[2] = v1[2] + v2[2];
}
void cross_product3d(const double *v1, const double *v2, double *cross)
{
cross[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);
cross[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);
cross[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);
}
void remove_component3d(const double *v1, const double *unit, double *result)
{
double dot;
dot = v1[0] * unit[0] + v1[1] * unit[1] + v1[2] * unit[2];
result[0] = v1[0] - unit[0] * dot;
result[1] = v1[1] - unit[1] * dot;
result[2] = v1[2] - unit[2] * dot;
}
void reorient44d(double *matrix)
{
double tmp[16];
int a;
/* restore orthogonality and recondition */
for(a = 0; a < 3; a++) {
normalize3d(matrix);
normalize3d(matrix + 4);
normalize3d(matrix + 8);
cross_product3d(matrix + 4, matrix + 8, tmp);
cross_product3d(matrix + 8, matrix, tmp + 4);
cross_product3d(matrix, matrix + 4, tmp + 8);
normalize3d(tmp);
normalize3d(tmp + 4);
normalize3d(tmp + 8);
scale3d(tmp, 2.0, tmp);
scale3d(tmp + 4, 2.0, tmp + 4);
scale3d(tmp + 8, 2.0, tmp + 8);
add3d(matrix, tmp, tmp);
add3d(matrix + 4, tmp + 4, tmp + 4);
add3d(matrix + 8, tmp + 8, tmp + 8);
copy3d(tmp, matrix);
copy3d(tmp + 4, matrix + 4);
copy3d(tmp + 8, matrix + 8);
}
normalize3d(matrix);
normalize3d(matrix + 4);
normalize3d(matrix + 8);
copy3d(matrix, tmp);
remove_component3d(matrix + 4, tmp, tmp + 4);
cross_product3d(tmp, tmp + 4, tmp + 8);
normalize3d(tmp + 4);
normalize3d(tmp + 8);
recondition44d(tmp);
copy3d(tmp, matrix);
copy3d(tmp + 4, matrix + 4);
copy3d(tmp + 8, matrix + 8);
}
void recondition33d(double *matrix)
{
normalize3d(matrix);
normalize3d(matrix + 3);
normalize3d(matrix + 6);
normalize3dp(matrix + 0, matrix + 3, matrix + 6);
normalize3dp(matrix + 1, matrix + 4, matrix + 7);
normalize3dp(matrix + 2, matrix + 5, matrix + 8);
normalize3d(matrix);
normalize3d(matrix + 3);
normalize3d(matrix + 6);
normalize3dp(matrix + 0, matrix + 3, matrix + 6);
normalize3dp(matrix + 1, matrix + 4, matrix + 7);
normalize3dp(matrix + 2, matrix + 5, matrix + 8);
normalize3d(matrix);
normalize3d(matrix + 3);
normalize3d(matrix + 6);
}
void recondition44d(double *matrix)
{
normalize3d(matrix);
normalize3d(matrix + 4);
normalize3d(matrix + 8);
normalize3dp(matrix + 0, matrix + 4, matrix + 8);
normalize3dp(matrix + 1, matrix + 5, matrix + 9);
normalize3dp(matrix + 2, matrix + 6, matrix + 10);
normalize3d(matrix);
normalize3d(matrix + 4);
normalize3d(matrix + 8);
normalize3dp(matrix + 0, matrix + 4, matrix + 8);
normalize3dp(matrix + 1, matrix + 5, matrix + 9);
normalize3dp(matrix + 2, matrix + 6, matrix + 10);
normalize3d(matrix);
normalize3d(matrix + 4);
normalize3d(matrix + 8);
}
void invert_rotation_only44d44d(const double *orig, double *inv)
{
/* inverse of the rotation matrix */
inv[0] = orig[0];
inv[1] = orig[4];
inv[2] = orig[8];
inv[4] = orig[1];
inv[5] = orig[5];
inv[6] = orig[9];
inv[8] = orig[2];
inv[9] = orig[6];
inv[10] = orig[10];
inv[3] = 0.0;
inv[7] = 0.0;
inv[11] = 0.0;
inv[12] = 0.0;
inv[13] = 0.0;
inv[14] = 0.0;
inv[15] = 1.0;
}
void transformTTT44f3f(const float *m1, const float *m2, float *m3)
{
float m2r0 = m2[0] + m1[12];
float m2r1 = m2[1] + m1[13];
float m2r2 = m2[2] + m1[14];
m3[0] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2 + m1[3];
m3[1] = m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2 + m1[7];
m3[2] = m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2 + m1[11];
}
void transform_normalTTT44f3f(const float *m1, const float *m2, float *m3)
{
float m2r0 = m2[0];
float m2r1 = m2[1];
float m2r2 = m2[2];
m3[0] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2;
m3[1] = m1[4] * m2r0 + m1[5] * m2r1 + m1[6] * m2r2;
m3[2] = m1[8] * m2r0 + m1[9] * m2r1 + m1[10] * m2r2;
}
void multiply33f33f(const float *m1, const float *m2, float *m3)
{ /* m2 and m3 can be the same matrix */
int a;
float m2r0, m2r1, m2r2;
for(a = 0; a < 3; a++) {
m2r0 = m2[a];
m2r1 = m2[3 + a];
m2r2 = m2[6 + a];
m3[a] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2;
m3[3 + a] = m1[3] * m2r0 + m1[4] * m2r1 + m1[5] * m2r2;
m3[6 + a] = m1[6] * m2r0 + m1[7] * m2r1 + m1[8] * m2r2;
}
}
void multiply33d33d(const double *m1, const double *m2, double *m3)
{ /* m2 and m3 can be the same matrix */
int a;
double m2r0, m2r1, m2r2;
for(a = 0; a < 3; a++) {
m2r0 = m2[a];
m2r1 = m2[3 + a];
m2r2 = m2[6 + a];
m3[a] = m1[0] * m2r0 + m1[1] * m2r1 + m1[2] * m2r2;
m3[3 + a] = m1[3] * m2r0 + m1[4] * m2r1 + m1[5] * m2r2;
m3[6 + a] = m1[6] * m2r0 + m1[7] * m2r1 + m1[8] * m2r2;
}
}
void matrix_multiply33f33f(Matrix33f m1, Matrix33f m2, Matrix33f m3)
{
multiply33f33f((float *) m1, (float *) m2, (float *) m3);
}
void matrix_multiply33d33d(Matrix33d m1, Matrix33d m2, Matrix33d m3)
{
multiply33d33d((double *) m1[0], (double *) m2, (double *) m3);
}
float deg_to_rad(float angle)
{
return ((float) ((angle * cPI) / 180.0));
}
float rad_to_deg(float angle)
{
return ((float) (180.0 * (angle / cPI)));
}
void get_rotation_about3f3fTTTf(float angle, const float *dir, const float *origin, float *ttt)
{
float rot[9];
rotation_matrix3f(angle, dir[0], dir[1], dir[2], rot);
ttt[0] = rot[0];
ttt[1] = rot[1];
ttt[2] = rot[2];
ttt[4] = rot[3];
ttt[5] = rot[4];
ttt[6] = rot[5];
ttt[8] = rot[6];
ttt[9] = rot[7];
ttt[10] = rot[8];
ttt[12] = -origin[0];
ttt[13] = -origin[1];
ttt[14] = -origin[2];
ttt[3] = origin[0];
ttt[7] = origin[1];
ttt[11] = origin[2];
ttt[15] = 1.0F;
}
void rotation_to_matrix33f(const float *axis, float angle, Matrix33f mat)
{
rotation_matrix3f(angle, axis[0], axis[1], axis[2], &mat[0][0]);
}
void rotation_matrix3f(float angle, float x, float y, float z, float *m)
{
/* returns a row-major rotation matrix */
int a, b;
/* This function contributed by Erich Boleyn (erich@uruk.org) */
float mag, s, c;
float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
s = (float) sin(angle);
c = (float) cos(angle);
mag = (float) sqrt1f(x * x + y * y + z * z);
if(mag >= R_SMALL) {
x /= mag;
y /= mag;
z /= mag;
#define M(row,col) m[row*3+col]
xx = x * x;
yy = y * y;
zz = z * z;
xy = x * y;
yz = y * z;
zx = z * x;
xs = x * s;
ys = y * s;
zs = z * s;
one_c = _1 - c;
M(0, 0) = (one_c * xx) + c;
M(0, 1) = (one_c * xy) - zs;
M(0, 2) = (one_c * zx) + ys;
M(1, 0) = (one_c * xy) + zs;
M(1, 1) = (one_c * yy) + c;
M(1, 2) = (one_c * yz) - xs;
M(2, 0) = (one_c * zx) - ys;
M(2, 1) = (one_c * yz) + xs;
M(2, 2) = (one_c * zz) + c;
} else {
for(a = 0; a < 3; a++)
for(b = 0; b < 3; b++)
M(a, b) = 0;
M(0, 0) = _1;
M(1, 1) = _1;
M(2, 2) = _1;
}
}
#define get_angle USED_TO_RETURN_DEGREES
float get_dihedral3f(const float *v0, const float *v1, const float *v2, const float *v3)
{
Vector3f d01, d21, d32, dd1, dd3, pos_d;
float result = _0;
subtract3f(v2, v1, d21);
subtract3f(v0, v1, d01);
subtract3f(v3, v2, d32);
if(length3f(d21) < R_SMALL) {
result = get_angle3f(d01, d32);
} else {
cross_product3f(d21, d01, dd1);
cross_product3f(d21, d32, dd3);
if((length3f(dd1) < R_SMALL) || (length3f(dd3) < R_SMALL)) { /* degenerate cases */
result = get_angle3f(d01, d32); /* fall back to angle between vectors */
} else {
result = get_angle3f(dd1, dd3);
cross_product3f(d21, dd1, pos_d);
if(dot_product3f(dd3, pos_d) < _0)
result = -result;
}
}
return (result);
}
float get_angle3f(const float *v1, const float *v2)
{
double denom;
double result;
double arg1, arg2;
arg1 = ((v1[0] * (double)v1[0]) + (v1[1] * (double)v1[1]) + (v1[2] * (double)v1[2]));
arg2 = ((v2[0] * (double)v2[0]) + (v2[1] * (double)v2[1]) + (v2[2] * (double)v2[2]));
denom = sqrt1d(arg1) * sqrt1d(arg2);
if(denom > R_SMALL){
arg1 = (v1[0] * (double)v2[0] + v1[1] * (double)v2[1] + v1[2] * (double)v2[2]);
result = arg1 / denom;
} else
result = _0;
if(result < -_1)
result = -_1;
else if(result > _1)
result = _1;
return acosf(result);
}
void normalize23f(const float *v1, float *v2)
{
double vlen;
vlen = length3f(v1);
if(vlen > R_SMALL) {
v2[0] = (float) (v1[0] / vlen);
v2[1] = (float) (v1[1] / vlen);
v2[2] = (float) (v1[2] / vlen);
} else {
v2[0] = _0;
v2[1] = _0;
v2[2] = _0;
}
}
void clamp3f(float *v1)
{
if(v1[0] < _0)
v1[0] = _0;
if(v1[0] > _1)
v1[0] = _1;
if(v1[1] < _0)
v1[1] = _0;
if(v1[1] > _1)
v1[1] = _1;
if(v1[2] < _0)
v1[2] = _0;
if(v1[2] > _1)
v1[2] = _1;
}
void normalize3d(double *v1)
{
double vlen;
vlen = length3d(v1);
if(vlen > R_SMALL) {
v1[0] /= vlen;
v1[1] /= vlen;
v1[2] /= vlen;
} else {
v1[0] = _0;
v1[1] = _0;
v1[2] = _0;
}
}
void normalize2f(float *v1)
{
double vlen;
vlen = length2f(v1);
if(vlen > R_SMALL) {
v1[0] /= vlen;
v1[1] /= vlen;
} else {
v1[0] = _0;
v1[1] = _0;
}
}
void normalize4f(float *v1)
{
v1[0] /= v1[3];
v1[1] /= v1[3];
v1[2] /= v1[3];
v1[3] = 1.f;
}
double length3d(const double *v1)
{
return (sqrt1d((v1[0] * v1[0]) + (v1[1] * v1[1]) + (v1[2] * v1[2])));
}
double distance_line2point3f(const float *base, const float *normal, const float *point,
float *alongNormalSq)
{
float hyp[3], adj[3];
double result;
hyp[0] = point[0] - base[0];
hyp[1] = point[1] - base[1];
hyp[2] = point[2] - base[2];
project3f(hyp, normal, adj);
(*alongNormalSq) = ((adj[0] * adj[0]) + (adj[1] * adj[1]) + (adj[2] * adj[2]));
result = ((hyp[0] * hyp[0]) + (hyp[1] * hyp[1]) + (hyp[2] * hyp[2]))
- (*alongNormalSq);
if(result <= _0)
return (_0);
else
return (sqrt1d(result));
}
double distance_halfline2point3f(const float *base, const float *normal, const float *point,
float *alongNormalSq)
{
float hyp[3], adj[3];
double result;
hyp[0] = point[0] - base[0];
hyp[1] = point[1] - base[1];
hyp[2] = point[2] - base[2];
if(project3f(hyp, normal, adj) > _0) {
(*alongNormalSq) = ((adj[0] * adj[0]) + (adj[1] * adj[1]) + (adj[2] * adj[2]));
result = ((hyp[0] * hyp[0]) + (hyp[1] * hyp[1]) + (hyp[2] * hyp[2]))
- (*alongNormalSq);
if(result <= _0)
return (_0);
else
return (sqrt1d(result));
} else {
return FLT_MAX;
}
}
void matrix_transform33f3f(const Matrix33f m1, const float *v1, float *v2)
{
v2[0] = m1[0][0] * v1[0] + m1[0][1] * v1[1] + m1[0][2] * v1[2];
v2[1] = m1[1][0] * v1[0] + m1[1][1] * v1[1] + m1[1][2] * v1[2];
v2[2] = m1[2][0] * v1[0] + m1[2][1] * v1[1] + m1[2][2] * v1[2];
}
void matrix_inverse_transform33f3f(const Matrix33f m1, const float *v1, float *v2)
{
v2[0] = m1[0][0] * v1[0] + m1[1][0] * v1[1] + m1[2][0] * v1[2];
v2[1] = m1[0][1] * v1[0] + m1[1][1] * v1[1] + m1[2][1] * v1[2];
v2[2] = m1[0][2] * v1[0] + m1[1][2] * v1[1] + m1[2][2] * v1[2];
}
#if 0
double matdiffsq(float *v1, oMatrix5f m, float *v2)
{
double dx, dy, dz;
float vx, vy, vz;
dx = v2[0] - m[3][0];
dy = v2[1] - m[3][1];
dz = v2[2] - m[3][2];
vx = m[0][0] * dx + m[0][1] * dy + m[0][2] * dz;
vy = m[1][0] * dx + m[1][1] * dy + m[1][2] * dz;
vz = m[2][0] * dx + m[2][1] * dy + m[2][2] * dz;
dx = (v1[0] - (vx + m[4][0]));
dy = (v1[1] - (vy + m[4][1]));
dz = (v1[2] - (vz + m[4][2]));
return (dx * dx + dy * dy + dz * dz);
}
#endif
void transform5f3f(const oMatrix5f m, const float *v1, float *v2)
{
double dx, dy, dz;
double vx, vy, vz;
dx = v1[0] - m[3][0];
dy = v1[1] - m[3][1];
dz = v1[2] - m[3][2];
vx = m[0][0] * dx + m[0][1] * dy + m[0][2] * dz;
vy = m[1][0] * dx + m[1][1] * dy + m[1][2] * dz;
vz = m[2][0] * dx + m[2][1] * dy + m[2][2] * dz;
v2[0] = (((float) vx) + m[4][0]);
v2[1] = (((float) vy) + m[4][1]);
v2[2] = (((float) vz) + m[4][2]);
}
void transform3d3f(const oMatrix3d m1, const float *v1, float *v2)
{
int b;
for(b = 0; b < 3; b++)
v2[b] = m1[b][0] * v1[0] + m1[b][1] * v1[1] + m1[b][2] * v1[2];
}
void transform33d3f(const Matrix33d m1, const float *v1, float *v2)
{
int b;
for(b = 0; b < 3; b++)
v2[b] = (float) (m1[b][0] * v1[0] + m1[b][1] * v1[1] + m1[b][2] * v1[2]);
}
/*
void matcopy ( oMatrix5f to,oMatrix5f from )
{
int a,b;
for(a=0;a<5;a++)
for(b=0;b<3;b++)
to[a][b] = from[a][b];
}
void mattran ( oMatrix5f nm, oMatrix5f om, int axis, float dist )
{
matcopy(nm,om);
nm[4][axis] += dist;
}
void matrot ( oMatrix5f nm, oMatrix5f om, int axis, float angle )
{
oMatrix5f rm;
float ca,sa;
int a,b;
ca = cos(angle);
sa = sin(angle);
switch(axis)
{
case 0:
rm[0][0] = _1; rm[0][1] = _0; rm[0][2] = _0;
rm[1][0] = _0; rm[1][1] = ca; rm[1][2] = sa;
rm[2][0] = _0; rm[2][1] = -sa; rm[2][2] = ca;
break;
case 1:
rm[0][0] = ca; rm[0][1] = _0; rm[0][2] = -sa;
rm[1][0] = _0; rm[1][1] = _1; rm[1][2] = _0;
rm[2][0] = sa; rm[2][1] = _0; rm[2][2] = ca;
break;
case 2:
rm[0][0] = ca; rm[0][1] = sa; rm[0][2] = _0;
rm[1][0] = -sa; rm[1][1] = ca; rm[1][2] = _0;
rm[2][0] = _0; rm[2][1] = _0; rm[2][2] = _1;
break;
}
for(a=0;a<3;a++)
{
nm[3][a] = om[3][a];
nm[4][a] = om[4][a];
for(b=0;b<3;b++)
nm[a][b] =
rm[a][0]*om[0][b] +
rm[a][1]*om[1][b] +
rm[a][2]*om[2][b];
}
normalize3f(nm[0]);
normalize3f(nm[1]);
normalize3f(nm[2]);
}
*/
void rotation_to_matrix(Matrix53f rot, const float *axis, float angle)
{
rotation_matrix3f(angle, axis[0], axis[1], axis[2], &rot[0][0]);
}
static void find_axis(Matrix33d a, float *axis)
{
doublereal at[3][3], v[3][3], vt[3][3], fv1[3][3];
integer iv1[3];
integer ierr;
integer nm, n, matz;
doublereal wr[3], wi[3];
/*p[3][3]; */
int x, y;
nm = 3;
n = 3;
matz = 1;
recondition33d(&a[0][0]); /* IMPORTANT! */
for(x = 0; x < 3; x++) {
for(y = 0; y < 3; y++) {
at[y][x] = a[x][y];
}
}
pymol_rg_(&nm, &n, &at[0][0], wr, wi, &matz, &vt[0][0], iv1, &fv1[0][0], &ierr);
for(x = 0; x < 3; x++) {
for(y = 0; y < 3; y++) {
v[y][x] = vt[x][y];
}
}
axis[0] = 0.0F;
axis[1] = 0.0F;
axis[2] = 0.0F;
{
doublereal max_real = 0.0F, test_real;
doublereal min_imag = 1.0F, test_imag;
float test_inp[3], test_out[3];
for(x = 0; x < 3; x++) { /* looking for an eigvalue of (1,0) */
/* printf("wr %8.3f wi %8.3f\n",wr[x],wi[x]);
printf("%8.3f %8.3f %8.3f\n",
v[0][x],v[1][x],v[2][x]); */
test_real = fabs(wr[x]);
test_imag = fabs(wi[x]);
if((test_real >= max_real) && (test_imag <= min_imag)) {
for(y = 0; y < 3; y++)
test_inp[y] = (float) v[y][x];
transform33d3f(a, test_inp, test_out); /* confirm that axis is invariant to rotation */
test_out[0] -= test_inp[0];
test_out[1] -= test_inp[1];
test_out[2] -= test_inp[2];
if((test_out[0] * test_out[0] +
test_out[1] * test_out[1] + test_out[2] * test_out[2]) < 0.1) {
for(y = 0; y < 3; y++)
axis[y] = test_inp[y];
max_real = test_real;
min_imag = test_imag;
}
} else {
/*for(y=0;y<3;y++)
v[y][x]=_0; */
}
}
}
/*
printf("eigenvectors\n%8.3f %8.3f %8.3f\n",v[0][0],v[0][1],v[0][2]);
printf("%8.3f %8.3f %8.3f\n",v[1][0],v[1][1],v[1][2]);
printf("%8.3f %8.3f %8.3f\n",v[2][0],v[2][1],v[2][2]);
printf("eigenvalues\n%8.3f %8.3f %8.3f\n",wr[0],wr[1],wr[2]);
printf("%8.3f %8.3f %8.3f\n",wi[0],wi[1],wi[2]);
*/
/* matrix_multiply33d33d(a,v,p);
printf("invariance\n");
printf("%8.3f %8.3f %8.3f\n",p[0][0],p[0][1],p[0][2]);
printf("%8.3f %8.3f %8.3f\n",p[1][0],p[1][1],p[1][2]);
printf("%8.3f %8.3f %8.3f\n",p[2][0],p[2][1],p[2][2]);
*/
}
void matrix_to_rotation(Matrix53f rot, float *axis, float *angle)
{
float perp[3], tmp[3], rperp[3], dirck[3];
Matrix33d rot3d;
Matrix53f rotck;
int a, b;
#ifdef MATCHK
printf("starting matrix\n");
for(a = 0; a < 3; a++)
printf("%8.3f %8.3f %8.3f\n", rot[a][0], rot[a][1], rot[a][2]);
#endif
for(a = 0; a < 3; a++)
for(b = 0; b < 3; b++)
rot3d[a][b] = (double) rot[a][b];
find_axis(rot3d, axis);
/* find a perpendicular vector */
perp[0] = axis[1] * axis[0] - axis[2] * axis[2];
perp[1] = axis[2] * axis[1] - axis[0] * axis[0];
perp[2] = axis[0] * axis[2] - axis[1] * axis[1];
if(length3f(perp) < R_SMALL) {
tmp[0] = axis[0];
tmp[1] = -2 * axis[1];
tmp[2] = axis[2];
cross_product3f(axis, tmp, perp);
}
normalize3f(perp);
transform33d3f(rot3d, perp, rperp);
*angle = get_angle3f(perp, rperp);
cross_product3f(perp, rperp, dirck);
if(((dirck[0] * axis[0]) + (dirck[1] * axis[1]) + (dirck[2] * axis[2])) < _0)
*angle = -*angle;
/* printf("angle %8.3f \n",*angle); */
rotation_to_matrix(rotck, axis, *angle);
#ifdef MATCHK
printf("reconstructed matrix: \n");
for(a = 0; a < 3; a++)
printf("%8.3f %8.3f %8.3f\n", rotck[a][0], rotck[a][1], rotck[a][2]);
printf("\n");
#endif
}
void mult3f(const float *vsrc, const float val, float *vdest){
vdest[0] = vsrc[0] * val;
vdest[1] = vsrc[1] * val;
vdest[2] = vsrc[2] * val;
}
void mult4f(const float *vsrc, const float val, float *vdest){
vdest[0] = vsrc[0] * val;
vdest[1] = vsrc[1] * val;
vdest[2] = vsrc[2] * val;
vdest[3] = vsrc[3] * val;
}
float max3(float val1, float val2, float val3){
if (val1>val2){
if (val1>val3){
return val1;
} else {
return val3;
}
} else {
if (val2>val3){
return val2;
} else {
return val3;
}
}
}
float ave3(float val1, float val2, float val3){
return ((val1+val2+val3)/3.f);
}
float ave2(float val1, float val2){
return ((val1+val2)/2.f);
}
void white4f(float *rgba, float value){
rgba[0] = value;
rgba[1] = value;
rgba[2] = value;
rgba[3] = 1.0F;
}
void add4f(const float *v1, const float *v2, float *v3)
{
v3[0] = v1[0] + v2[0];
v3[1] = v1[1] + v2[1];
v3[2] = v1[2] + v2[2];
v3[3] = v1[3] + v2[3];
}
int countchrs(const char *str, char ch){
int cnt = 0;
const char *tmp = str;
while((tmp = strchr(tmp, ch))) {
cnt++;
tmp++;
}
return cnt;
}
/*
* sigmoid smoothstep function with
* smooth(0) = 0
* smooth(1) = 1
* smooth'(0) = 0
* smooth'(1) = 0
*/
float smooth(float x, float power)
{
if(x <= 0.5F) {
if(x <= 0.0F)
return 0.0F;
return 0.5F * powf(2.0F * x, power);
}
if(x >= 1.0F)
return 1.0F;
return 1.0F - (0.5F * powf(2.0F * (1.0F - x), power));
}
/* Divides the unit circle radially into n segments with n >= 3. */
void subdivide(int n, float *x, float *y)
{
int a;
if(n < 3) {
n = 3;
}
for(a = 0; a <= n; a++) {
x[a] = (float) cos(a * 2 * PI / n);
y[a] = (float) sin(a * 2 * PI / n);
}
}
Reference in New Issue View Git Blame Copy Permalink