69 lines
1.9 KiB
C++
69 lines
1.9 KiB
C++
#include <cstdlib>
|
|
#include <cstring>
|
|
|
|
#include <sys/types.h>
|
|
|
|
#include "linear-algebra.h"
|
|
|
|
namespace ewald {
|
|
|
|
void matrix_vector_product(double const* A, double* X) {
|
|
double Y[] = { 0.0, 0.0, 0.0 };
|
|
|
|
for (size_t i = 0; i < 3; ++i)
|
|
for (size_t j = 0; j < 3; ++j)
|
|
Y[i] += A[3*i+j] * X[j];
|
|
|
|
memcpy(X, Y, 3 * sizeof(double));
|
|
}
|
|
|
|
void matrix_vector_product_transpose(double const* A, double* X) {
|
|
double Y[] = { 0.0, 0.0, 0.0 };
|
|
|
|
for (size_t i = 0; i < 3; ++i)
|
|
for (size_t j = 0; j < 3; ++j)
|
|
Y[i] += A[3*j+i] * X[j];
|
|
|
|
memcpy(X, Y, 3 * sizeof(double));
|
|
}
|
|
|
|
void matrix_matrix_product(double const* A, double const* B, double* C) {
|
|
memset(C, 0, 3 * 3 * sizeof(double));
|
|
|
|
for (size_t i = 0; i < 3; ++i)
|
|
for (size_t j = 0; j < 3; ++j)
|
|
for (size_t k = 0; k < 3; ++k)
|
|
C[3*i+j] += A[3*i+k] * B[3*k+j];
|
|
}
|
|
|
|
double determinant(double const* A) {
|
|
return A[3*0+0] * (A[3*1+1] * A[3*2+2] - A[3*1+2] * A[3*2+1])
|
|
- A[3*0+1] * (A[3*1+0] * A[3*2+2] - A[3*1+2] * A[3*2+0])
|
|
+ A[3*0+2] * (A[3*1+0] * A[3*2+1] - A[3*1+1] * A[3*2+0]);
|
|
}
|
|
|
|
void matrix_inverse(double const* A, double* Ainv) {
|
|
const double invdet = 1.0 / determinant(A);
|
|
memset(Ainv, 0, 3 * 3 * sizeof(double));
|
|
Ainv[3*0+0] = ( A[3*1+1]*A[3*2+2] - A[3*1+2]*A[3*2+1]) * invdet;
|
|
Ainv[3*0+1] = (-A[3*0+1]*A[3*2+2] + A[3*0+2]*A[3*2+1]) * invdet;
|
|
Ainv[3*0+2] = ( A[3*0+1]*A[3*1+2] - A[3*0+2]*A[3*1+1]) * invdet;
|
|
Ainv[3*1+0] = (-A[3*1+0]*A[3*2+2] + A[3*1+2]*A[3*2+0]) * invdet;
|
|
Ainv[3*1+1] = ( A[3*0+0]*A[3*2+2] - A[3*0+2]*A[3*2+0]) * invdet;
|
|
Ainv[3*1+2] = (-A[3*0+0]*A[3*1+2] + A[3*0+2]*A[3*1+0]) * invdet;
|
|
Ainv[3*2+0] = ( A[3*1+0]*A[3*2+1] - A[3*1+1]*A[3*2+0]) * invdet;
|
|
Ainv[3*2+1] = (-A[3*0+0]*A[3*2+1] + A[3*0+1]*A[3*2+0]) * invdet;
|
|
Ainv[3*2+2] = ( A[3*0+0]*A[3*1+1] - A[3*0+1]*A[3*1+0]) * invdet;
|
|
}
|
|
|
|
double dot_product(double const* u, double const* v) {
|
|
double result = 0.0;
|
|
|
|
for (size_t k = 0; k < 3; ++k)
|
|
result += u[k] * v[k];
|
|
|
|
return result;
|
|
}
|
|
|
|
}
|