1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
|
/*
cvecmat.c: C library for vector and matrix manipulations
Copyright (C) 2001 CCLRC, Martyn Winn
This library is free software: you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public License
version 3, modified in accordance with the provisions of the
license to address the requirements of UK law.
You should have received a copy of the modified GNU Lesser General
Public License along with this library. If not, copies may be
downloaded from http://www.ccp4.ac.uk/ccp4license.php
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU Lesser General Public License for more details.
*/
/** @file cvecmat.c
* C library for vector and matrix manipulations.
* Martyn Winn
*/
#include <math.h>
#include "cvecmat.h"
/* rcsid[] = "$Id$" */
/* c = a X b */
void ccp4_dcross(const double a[3], const double b[3], double c[3])
{
c[0] = a[1]*b[2] - b[1]*a[2];
c[1] = a[2]*b[0] - b[2]*a[0];
c[2] = a[0]*b[1] - b[0]*a[1];
}
void ccp4_3matmul(double c[3][3], const double a[3][3], const double b[3][3])
{
int i,j,k;
for ( i = 0; i < 3; i++ )
for ( j = 0; j < 3; j++ ) {
c[i][j] = 0.0;
for ( k = 0; k < 3; k++ )
c[i][j] += a[i][k]*b[k][j];
}
}
void ccp4_4matmul( float c[4][4], const float a[4][4], const float b[4][4])
{
int i,j,k;
for ( i = 0; i < 4; i++ )
for ( j = 0; j < 4; j++ ) {
c[i][j] = 0.0;
for ( k = 0; k < 4; k++ )
c[i][j] += a[i][k]*b[k][j];
}
}
/* A (I) 3*3 matrix to be inverted */
/* AI (O) inverse matrix */
/* returns determinant */
double invert3matrix(const double a[3][3], double ai[3][3])
{ int i,j;
double c[3][3],d;
ccp4_dcross(a[1],a[2],c[0]);
ccp4_dcross(a[2],a[0],c[1]);
ccp4_dcross(a[0],a[1],c[2]);
d = a[0][0]*c[0][0] + a[0][1]*c[0][1] + a[0][2]*c[0][2];
if (fabs(d) > 1.0e-30) {
for ( i = 0; i < 3; i++ )
for ( j = 0; j < 3; j++ )
ai[i][j] = c[j][i] / d;
} else {
return 0.0;
}
return d;
}
/* A (I) 4*4 matrix to be inverted */
/* AI (O) inverse matrix */
/* returns determinant */
float invert4matrix(const float a[4][4], float ai[4][4])
{
double c[4][4], d;
int i, j;
double x[3][3];
int i1, j1, i2 ;
double am, q;
int ii, jj;
/* Function Body */
for (ii = 0; ii < 4; ++ii) {
for (jj = 0; jj < 4; ++jj) {
ai[ii][jj] = 0.0;
i = -1;
for (i1 = 0; i1 < 4; ++i1) {
if (i1 != ii) {
++i;
j = -1;
for (j1 = 0; j1 < 4; ++j1) {
if (j1 != jj) {
++j;
x[i][j] = a[i1][j1];
}
}
}
}
am = x[0][0]*x[1][1]*x[2][2] - x[0][0]*x[1][2]*x[2][1] +
x[0][1]*x[1][2]*x[2][0] - x[0][1]*x[1][0]*x[2][2] +
x[0][2]*x[1][0]*x[2][1] - x[0][2]*x[1][1]*x[2][0];
i2 = ii + jj;
c[ii][jj] = ccp4_pow_ii(-1.0, i2) * am;
}
}
/* ---- Calculate determinant */
d = 0.0;
for (i = 0; i < 4; ++i) {
d = a[i][0] * c[i][0] + d;
}
/* ---- Get inverse matrix */
if (fabs(d) > 1.0e-30) {
q = 1.0/d;
for (i = 0; i < 4; ++i) {
for (j = 0; j < 4; ++j) {
ai[i][j] = (float) (c[j][i] * q);
}
}
} else {
return 0.0;
}
return ((float) d);
}
float ccp4_pow_ii(const float base, const int power) {
int i = 0;
float pow = 1;
while (++i <= power)
pow *= base;
return pow;
}
|
