int cpivoting(double *A, int nrow_A, int ncol_A, double *B, int nrow_B, int ncol_B, int csz){
int m,mi,n;
double *p, max;
nrow_A = nrow_A - csz;
for(m = 0; m < nrow_A; m++){
#ifdef DEBUG
printf("m = %d\n",m);
printm(A,nrow_A,ncol_A);
printf("\n");
#endif
p = A + m*(ncol_A + 1);
max = fabs(*p);
n = m;
for(mi = m + 1; mi < nrow_A; mi++){
p += ncol_A;
if (fabs(*p) > max){
max = fabs(*p);
n = mi;
}
}
if (n != m){
swap_rows(A, nrow_A, ncol_A, m, n);
swap_rows(B, nrow_B, ncol_B, m, n);
}
}
return 0;
}
/* Uses Gauss-Jordan elimination.
The elimination procedure works by applying elementary row
operations to our input matrix until the input matrix is reduced to
the identity matrix.
Simultaneously, we apply the same elementary row operations to a
separate identity matrix to produce the inverse matrix.
If this makes no sense, read wikipedia on Gauss-Jordan elimination.
This is not the fastest way to invert matrices, so this is quite
possibly the bottleneck. */
int destructive_invert_matrix(const Matrix_t * input, const Matrix_t * output)
{
uint8_t i,j,r;
double scalar, shear_needed;
set_identity_matrix(output);
/*
Convert input to the identity matrix via elementary row operations.
The ith pass through this loop turns the element at i,i to a 1
and turns all other elements in column i to a 0.
*/
for (i = 0; i < input->rows; ++i)
{
if (input->data[i][i] == 0.0)
{
/* We must swap rows to get a nonzero diagonal element. */
for (r = i + 1; r < input->rows; ++r)
{
if (input->data[r][i] != 0.0)
break;
}
if (r == input->rows)
/* Every remaining element in this column is zero, so this
matrix cannot be inverted. */
return 0;
swap_rows(input, i, r);
swap_rows(output, i, r);
}
/*
Scale this row to ensure a 1 along the diagonal.
We might need to worry about overflow from a huge scalar here. */
scalar = 1.0 / input->data[i][i];
scale_row(input, i, scalar);
scale_row(output, i, scalar);
/* Zero out the other elements in this column. */
for (j = 0; j < input->rows; ++j)
{
if (i == j)
continue;
shear_needed = -input->data[j][i];
shear_row(input, j, i, shear_needed);
shear_row(output, j, i, shear_needed);
}
}
return 1;
}
local void sort_matrix (entry** m,lie_Index n,lie_Index c)
{ if (n>=3)
{ lie_Index i=split_mat(m,n,c);
sort_matrix(m,i,c); sort_matrix(&m[i+1],n-i-1,c);
}
else if (n==2 && (*compare)(m[0],m[1],c)<0) swap_rows(m,m+1);
}
matrix* Unique(matrix* m, cmpfn_tp criterion)
{ lie_Index len=m->ncols; register entry** to=m->elm,** from=to,** end=to+m->nrows;
if (m->nrows<2) return m; heap_sort_m(m,criterion);
while (!eqrow(*++from,*to,len)) if (++to==end-1) return m;
while (++from<end) if (!eqrow(*from,*to,len)) swap_rows(++to,from);
m->nrows=to+1-m->elm; return m;
}
constexpr auto matrix_kernel(matrix< M, C, R > m){
using value_type = value_type_t< M >;
if(size_t(m.cols()) != size_t(m.rows())){
throw std::logic_error(
"matrix_kernel does up to now only work for square matrices"
);
}
auto result = make_vector_v< value_type >(
m.cols().as_row()
);
// Compiler may optimize with the compile time dimension
size_t const size = C == 0_C ? size_t(m.rows()) : size_t(m.cols());
for(size_t i = 0; i < size; ++i){
if(m(i, i) == 0){
// swap lines
size_t y = i + 1;
for(; y < size; ++y){
if(m(i, y) == 0) continue;
swap_rows(m, i, y);
break;
}
// matrix is not invertible
if(y == size){
break;
}
}
// eliminate col in lower rows
for(size_t y = i + 1; y < size; ++y){
auto factor = m(i, y) / m(i, i);
m(i, y) = 0;
for(size_t x = i + 1; x < size; ++x){
m(x, y) -= factor * m(x, i);
}
}
}
for(size_t i = 0; i < size; ++i){
auto y = size - i - 1;
for(size_t x = y + 1; x < size; ++x){
result[y] += m(x, y) * result[x];
}
if(m(y, y) == 0){
result[y] = 1;
}else{
result[y] /= -m(y, y);
}
}
return result;
}
int main(int argc, char * argv[]){
int threads = parse_number_threads(argc, argv);
double **A; int size;
Lab3LoadInput(&A, &size);
double storage[size];
// Initialize Times
double start_time;
double end_time;
GET_TIME(start_time);
int k;
for (k = 0; k < size; k++){
int max_indice = find_max_indice(k, A, size);
swap_rows(A, k, max_indice);
int i;
int j;
//printf("Current k value: %d \n",k);
//printf("----------------------------\n");
for (i = k+1; i < size; i++){
//printf("Current Row: %d \n",i);
double subtrahend_coefficient = (A[i][k]/A[k][k]);
//printf("Subtrahend Coefficient: %f \n",subtrahend_coefficient);
for (j = k; j < size + 1; j++){
//printf("Current Col: %d \n",j);
A[i][j] = A[i][j] - (subtrahend_coefficient* A[k][j]);
}
}
}
//Lab2_saveoutput(A, size, 10, "Gauss.txt");
for (k = size-1; k > 0; k--){
int i;
for (i = 0; i < k; i++){
//double result = A[i][size] - ( (A[i][k] / A[k][k]) * A[k][size]);;
//printf("A[%d][%d] = %f \n", i, size, result);
A[i][size] = A[i][size] - ( (A[i][k] / A[k][k]) * A[k][size]);
A[i][k] = A[i][k] - ( (A[i][k] / A[k][k]) * A[k][k]);
}
}
//Lab2_saveoutput(A, size, 10, "Jordan.txt");
// Retrieve elapsed time
GET_TIME(end_time);
get_result(storage, A, size);
printf("Total Time: %f \n",end_time-start_time);
Lab3SaveOutput(storage, size, end_time - start_time);
return 0;
}
local void heapify_m(matrix* m,lie_Index i,lie_Index n)
{ lie_Index l,len=m->ncols; entry** a=m->elm-1; /* array~$[1:n]$ */
while ((l=i<<1)<=n)
{ lie_Index d= (*compare)(a[i],a[l],len)<0 ? i : l;
if (l<n && (*compare)(a[d],a[l+1],len)>0) d=l+1;
/* lie_Index of minimum among $\{i,l,l+1\}$ */
if (d==i) return; /* stop if heap condition was already satisfied */
swap_rows(&a[i],&a[d]); i=d;
}
}
void Matrix::inverse() {
int maxrow;
float maxval;
float val;
Matrix temp;
temp.identity();
maxval = matrix[0][0];
maxrow = 0;
for(int i=0; i<4; i++){
maxval = matrix[i][i];
maxrow = i;
for(int j=i+1; j<4; j++){
val = matrix[j][i];
if(fabs(val) > fabs(maxval)){
maxval = val;
maxrow = j;
}
}
if(maxrow != i){
swap_rows(i, maxrow);
temp.swap_rows(i, maxrow);
}
for(int k=0; k<4; k++){
float div = 1.f / maxval;
matrix[i][k] *= div;
temp.matrix[i][k] *= div;
}
for(int j=i+1; j<4; j++){
val = matrix[j][i];
for(int k=0; k<4; k++){
matrix[j][k] -= matrix[i][k] * val;
temp.matrix[j][k] -= temp.matrix[i][k] * val;
}
}
}
for(int i=3; i>=0; i--) {
for(int j=i-1; j>=0; j--) {
val = matrix[j][i];
for(int k=0; k<4; k++) {
matrix[j][k] -= matrix[i][k] * val;
temp.matrix[j][k] -= temp.matrix[i][k] * val;
}
}
}
memcpy( matrix, temp.matrix, 16*sizeof(float));
}
void gaussian_elimination()
{
for(size_t r = 0; r < rows; ++r)
{
if(get(r, r) == 0)
{
for(size_t row_below = r + 1; row_below < rows; ++row_below)
{
if(get(row_below, r) != 0)
{
swap_rows(r, row_below);
}
}
}
if(get(r, r) == 0)
{
throw std::runtime_error("matrix is singular");
}
if(get(r, r) != 1)
{
uint8_t scale = galois.divide(1, get(r, r));
for(size_t c = 0; c < columns; ++c)
{
set(r, c, galois.multiply(get(r, c), scale));
}
}
for(size_t row_below = r + 1; row_below < rows; ++row_below)
{
if(get(row_below, r) != 0)
{
uint8_t scale = get(row_below, r);
for(size_t c = 0; c < columns; ++c)
{
set(row_below, c, get(row_below, c) ^ galois.multiply(scale, get(r, c)));
}
}
}
}
for(size_t d = 0; d < rows; ++d)
{
for(size_t row_above = 0; row_above < d; ++row_above)
{
if(get(row_above, d) != 0)
{
uint8_t scale = get(row_above, d);
for(size_t c = 0; c < columns; ++c)
{
set(row_above, c, get(row_above, c) ^ galois.multiply(scale, get(d, c)));
}
}
}
}
}
int
find_unit_element_and_swap(int flag, int i, int M, int Q, BitSequence **A)
{
int index, row_op=0;
if ( flag == MATRIX_FORWARD_ELIMINATION ) {
index = i+1;
while ( (index < M) && (A[index][i] == 0) )
index++;
if ( index < M )
row_op = swap_rows(i, index, Q, A);
}
else {
index = i-1;
while ( (index >= 0) && (A[index][i] == 0) )
index--;
if ( index >= 0 )
row_op = swap_rows(i, index, Q, A);
}
return row_op;
}
double lupp(int n, double **A, int *p) {
int i = 0;
int j = 0;
int k = 0;
int temp = 0;
double detA = 1;
double norma = norma_inf(A, n);
double mult = 0;
printf("epsilon=%.60lf\n", DBL_EPSILON * norma);
for (k = 0; k < n - 1; k++) {
i = k;
while (i < n && fabs(A[i][k]) < DBL_EPSILON * norma) i++;
if (i != k){
swap_rows(A, i, k, n);
temp = p[i];
p[i] = p[k];
p[k] = temp;
detA *= -1;
}
printf("Etapa %d: %d <-> %d\n", k, k, i);
detA *= A[k][k];
if (detA == 0) {
printf("La matriu A és singular.\n");
exit(1);
}
for (i = k + 1; i < n; i++) {
mult = A[i][k] / A[k][k];
for (j = k + 1; j < n; j++) {
A[i][j] -= mult * A[k][j];
}
A[i][k] = mult;
}
}
detA *= A[n - 1][n - 1];
if (detA == 0) {
printf("La matriu A és singular.\n");
exit(1);
}
return detA;
}
static void renumber_peripheral_curves(
Tetrahedron *tet)
{
int i,
j;
for (i = 0; i < 2; i++)
{
for (j = 0; j < 2; j++)
{
swap_rows (tet->curve[i][j], 2, 3);
swap_columns(tet->curve[i][j], 2, 3);
}
swap_sheets(tet->curve[i]);
}
}
void gaussian_elimination() {
int i, j, k;
double temp;
for(k = 0; k < size - 1; ++k) {
# pragma omp single
swap_rows(k, get_max_row(k));
# pragma omp for schedule(guided)
for(i = k + 1; i < size; ++i) {
for(j = k; j < size + 1; ++j) {
if( j == k )
temp = A[i][k] / A[k][k];
A[i][j] = A[i][j] - temp * A[k][j];
}
}
}
}
constexpr std_matrix< value_type_t< M >, C, R >
upper_triangular_matrix(matrix< M, C, R > const& in){
if(size_t(in.rows()) != size_t(in.cols())){
throw std::logic_error(
"lower_triangular_matrix with non square matrix"
);
}
// Compiler may optimize with the compile time dimension
size_t const size = C == 0_C ? size_t(in.rows()) : size_t(in.cols());
auto m = convert< value_type_t< M > >(in);
for(size_t i = 0; i < size; ++i){
if(m(i, i) == 0){
// swap lines
size_t y = i + 1;
for(; y < size; ++y){
if(m(i, y) == 0) continue;
swap_rows(m, i, y);
break;
}
// matrix is not invertible
if(y == size){
throw std::logic_error(
"lower_triangular_matrix with non invertible matrix"
);
}
}
// eliminate col in lower rows
for(size_t y = i + 1; y < size; ++y){
auto factor = m(i, y) / m(i, i);
m(i, y) = 0;
for(size_t x = i + 1; x < size; ++x){
m(x, y) -= factor * m(x, i);
}
}
}
return m;
}
Vector gauss(Matrix a) {
Matrix ac(a);
Vector xs(a.size());
for (int i = 0; i < xs.size(); ++i) xs[i] = i;
for (int i = 0; i < ac.size(); ++i) {
double max = fabs(a[i][i]), maxI = i, maxJ = i;
for (int l = i; l < ac.size(); ++l) {
for (int m = i; m < ac.size(); ++m) {
if (fabs(ac[l][m]) > max) {
max = fabs(ac[l][m]);
maxI = l;
maxJ = m;
}
}
}
if (maxI != i) ac = swap_rows(ac, i, maxI);
if (maxJ != i) ac = swap_cols(ac, i, maxJ);
swap(xs[i], xs[maxJ]);
for (int k = i + 1; k < ac.size(); ++k) {
double c = - ac[k][i] / ac[i][i];
for (int j = i; j < ac[0].size(); ++j) {
ac[k][j] += c * ac[i][j];
}
}
for (int j = ac[0].size() - 1; j >= i; --j) {
ac[i][j] /= ac[i][i];
}
}
Vector v(ac.size());
for (int i = ac.size() - 1; i >= 0; --i) {
v[xs[i]] = ac[i][ac[0].size()-1];
for (int j = i + 1; j < ac.size(); ++j) {
v[xs[i]] -= v[xs[j]] * ac[i][j];
}
}
return v;
}
void eliminate(double* m, int N, double* b)
{
for(int r=0; r<N-1; r++)
{
int p = find_pivot(m, N, r);
if(p != r)
{
swap_rows(m, N, r, p);
swap(b+r, b+p);
}
double piv = m[r*N+r];
// if(fabs(piv)<1e-20) {printf("matrix nearly singular ...."); abort();}
for(int rp = r+1; rp<N; rp++)
{
double fact = -m[rp*N+r]/piv;
for(int c=r; c<N; c++) m[rp*N+c]+=fact*m[r*N+c];
b[rp]+=fact*b[r];
}
}
}
void
job_dialog::on_up(wxCommandEvent &) {
std::vector<bool> selected;
bool first = true;
size_t i = 0;
size_t k;
for (k = 0; static_cast<size_t>(lv_jobs->GetItemCount()) > k; ++k) {
selected.push_back(lv_jobs->IsSelected(k));
if (lv_jobs->IsSelected(k) && first)
++i;
else
first = false;
}
for (; jobs.size() > i; i++)
if (selected[i]) {
swap_rows(i - 1, i, true);
selected[i - 1] = true;
selected[i] = false;
}
mdlg->save_job_queue();
}
void
job_dialog::on_down(wxCommandEvent &) {
std::vector<bool> selected;
bool first = true;
int i = lv_jobs->GetItemCount() - 1;
int k;
for (k = i; 0 <= k; --k) {
selected.insert(selected.begin(), lv_jobs->IsSelected(k));
if (lv_jobs->IsSelected(k) && first)
--i;
else
first = false;
}
for (; 0 <= i; --i)
if (selected[i]) {
swap_rows(i + 1, i, false);
selected[i + 1] = true;
selected[i] = false;
}
mdlg->save_job_queue();
}
void swap_terms(entry** w,bigint** coef,lie_Index i,lie_Index j)
{ swap_rows(&w[i],&w[j]);
if (coef!=NULL) { bigint* t=coef[i]; coef[i]=coef[j]; coef[j]=t; }
}
static int
gauss_jordan( GLU_complex M_1[ NCNC ] ,
const GLU_complex M[ NCNC ] )
{
__m128d a[ NCNC ] GLUalign ; // temporary space to overwrite matrix
register __m128d best , attempt , m1 , fac ;
size_t i , j , piv ;
// equate the necessary parts into double complex precision
for( i = 0 ; i < NCNC ; i++ ) {
a[ i ] = _mm_setr_pd( creal( M[i] ) , cimag( M[i] ) ) ;
M_1[ i ] = ( i%(NC+1) ) ? 0.0 :1.0 ;
}
// set these pointers, pB will be the inverse
__m128d *pB = (__m128d*)M_1 , *pA = (__m128d*)a ;
// loop over diagonal of the square matrix M
for( i = 0 ; i < NC-1 ; i++ ) {
// column pivot by selecting the largest in magnitude value
piv = i ;
best = absfac( *( pA + i*(NC+1) ) ) ;
for( j = i+1 ; j < NC ; j++ ) {
attempt = absfac( *( pB + i + j*NC ) ) ;
if( _mm_ucomilt_sd( best , attempt ) ) {
piv = j ;
best = attempt ;
}
}
// if we must pivot then we do
if( piv != i ) {
swap_rows( pA , pB , piv , i ) ;
}
// perform gaussian elimination to obtain the upper triangular
fac = _mm_div_pd( SSE2_CONJ( *( pA + i*(NC+1) ) ) , best ) ;
for( j = NC-1 ; j > i ; j-- ) { // go up in other columns
eliminate_column( pA , pB , fac , i , j ) ;
}
}
// a is upper triangular, do the same for the upper half
// no pivoting to be done here
for( i = NC-1 ; i > 0 ; i-- ) {
fac = SSE2_inverse( *( pA + i*(NC+1) ) ) ;
for( j = 0 ; j < i ; j++ ) {
eliminate_column( pA , pB , fac , i , j ) ;
}
}
// multiply each row by its M_1 diagonal
for( j = 0 ; j < NC ; j++ ) {
m1 = SSE2_inverse( *pA ) ;
for( i = 0 ; i < NC ; i++ ) {
*pB = SSE2_MUL( *pB , m1 ) ;
pB++ ;
}
pA += NC+1 ;
}
return GLU_SUCCESS ;
}
constexpr std_matrix< value_type_t< M >, C, R >
inverse(matrix< M, C, R > m){
using value_type = value_type_t< M >;
if(size_t(m.cols()) != size_t(m.rows())){
throw std::logic_error(
"inverse with non square matrix"
);
}
// Compiler may optimize with the compile time dimension
size_t const size = C == 0_C ? size_t(m.rows()) : size_t(m.cols());
auto r = make_matrix_v< value_type >(m.dims());
for(size_t i = 0; i < size; ++i){
r(i, i) = 1;
}
size_t swap_count = 0;
for(size_t i = 0; i < size; ++i){
if(m(i, i) == 0){
// swap lines
size_t y = i + 1;
for(; y < size; ++y){
if(m(i, y) == 0) continue;
swap_rows(m, i, y);
swap_rows(r, i, y);
++swap_count;
break;
}
// matrix is not invertible
if(y == size){
throw std::logic_error(
"inverse with non invertible matrix"
);
}
}
// eliminate col in lower rows
for(size_t y = i + 1; y < size; ++y){
auto factor = m(i, y) / m(i, i);
m(i, y) = 0;
for(size_t x = i + 1; x < size; ++x){
m(x, y) -= factor * m(x, i);
}
for(size_t x = 0; x < size; ++x){
r(x, y) -= factor * r(x, i);
}
}
for(size_t x = 0; x < size; ++x){
r(x, i) /= m(i, i);
}
for(size_t x = i + 1; x < size; ++x){
m(x, i) /= m(i, i);
}
// m(i, i) = 1; // TODO: remove this line
}
for(size_t i = 0; i < size; ++i){
auto j = size - i - 1;
for(size_t y = 0; y < j; ++y){
for(size_t x = 0; x < size; ++x){
r(x, y) -= m(j, y) * r(x, j);
}
// m(j, y) = 0; // TODO: remove this line
}
}
return r;
}
constexpr auto gaussian_elimination(
matrix< M1, C1, R1 > m,
col_vector< M2, R2 > v
){
using value_type = std::common_type_t<
value_type_t< M1 >, value_type_t< M2 >
>;
if(size_t(m.cols()) != size_t(m.rows()) || m.rows() != v.rows()){
throw std::logic_error(
"gaussian_elimination: incompatible dimensions"
);
}
auto b = convert< value_type >(v);
// Compiler may optimize with the compile time dimension
size_t const size = C1 != 0_C ?
size_t(m.cols()) : R1 != 0_R ? size_t(m.rows()) : size_t(b.rows());
size_t swap_count = 0;
for(size_t i = 0; i < size; ++i){
if(m(i, i) == 0){
// swap lines
size_t y = i + 1;
for(; y < size; ++y){
if(m(i, y) == 0) continue;
swap_rows(m, i, y);
swap_rows(b, i, y);
++swap_count;
break;
}
// matrix is not invertible
if(y == size){
throw std::logic_error(
"gaussian_elimination with non invertible matrix"
);
}
}
// eliminate col in lower rows
for(size_t y = i + 1; y < size; ++y){
auto factor = m(i, y) / m(i, i);
m(i, y) = 0;
for(size_t x = i + 1; x < size; ++x){
m(x, y) -= factor * m(x, i);
}
b[y] -= factor *b[i];
}
}
for(size_t i = 1; i < size; ++i){
auto x = size - i;
for(size_t y = 0; y < x; ++y){
auto factor = m(x, y) / m(x, x);
b[y] -= factor * b[x];
// m(x, y) = 0; // TODO: remove this line
}
}
for(size_t i = 0; i < size; ++i){
b[i] /= m(i, i);
}
return b;
}
local void heap_sort_m(matrix* m, cmpfn_tp criterion)
{ lie_Index i=m->nrows; entry** a=m->elm; if (i<2) return;
compare=set_ordering(criterion,m->ncols,defaultgrp); build_heap_m(m);
while (swap_rows(a,&a[--i]),i>1) heapify_m(m,1,i);
}
local lie_Index split_mat(entry** m,lie_Index n,lie_Index c)
{ entry* split_val= *m,** x=m,** end=m+n,** last_high=x;
while (++x<end) if ((*compare)(*x,split_val,c)>0) swap_rows(x,++last_high);
swap_rows(m,last_high); return last_high-m;
}
// solve system of linear equations
// the matrix mat will be changed. a inhomogenous solution will be written to sol and
// a basis of kernel(mat) will be written to ker, rnk will contain rank(mat) after computations
// allocates memory for sol {cols-1 doubles} and ker {(cols-1-(*rnk))*(cols-1) doubles)}
void solve_linearsystem(double** mat, const int rows, const int cols, const double eps_, double** sol, double*** ker, int* rnk)
{
int i,j,k,l;
int piv;
int lde;
int var;
int ker_var;
double frac;
(*sol) = (double*)malloc((cols-1)*sizeof(double));
for(j=0;j<cols-1;++j)
(*sol)[j] = 0.0;
assert(mat && cols>1 && rows>0);
(*rnk) =0;
// forward elimination
for(j=0; j<cols-1 && (*rnk)<rows; ++j)
{
//print_matrix(mat,3,3);
// determine pivot element
piv = argmax_abs(mat, rows, cols, j, (*rnk));
//printf("j=%i piv=%i rnk=%i pivval=%lf\n",j,piv,*rnk,mat[piv][j]);
if(dzero_eps(mat[piv][j],eps_)) continue;
swap_rows(mat, rows, cols, piv, (*rnk));
for(k=(*rnk)+1; k<rows; ++k)
{
frac = mat[k][j] / mat[(*rnk)][j];
mat[k][j] = 0.0;
for(l=j+1;l<cols;++l)
mat[k][l] -= frac*mat[(*rnk)][l];
}
(*rnk)++;
//print_matrix(mat,rows,cols);
}
// initialize kernel vectors
(*ker) = (double**)malloc( (cols-1-(*rnk))*sizeof(double*));
for(k=0; k<(cols-1-(*rnk)); ++k)
(*ker)[k] = (double*)malloc( (cols-1)*sizeof(double));
ker_var = cols-1-(*rnk)-1;
for(k=0; k < cols-1-(*rnk); ++k)
{
for(j=0;j<cols-1;++j)
(*ker)[k][j] = 0.0;
}
// backward elimination
var = cols-1;
for(i=rows-1; i>=0; --i)
{
//print_matrix(mat,rows,cols);
piv = 0; // determine first non zero entry
while( dzero_eps(mat[i][piv],eps_) && piv<cols)
piv++;
if( piv == cols) continue; // zero row
if( piv == cols-1) return; // unsolvable todo here...
frac = 1.0 / mat[i][piv];
for(k=i-1; k>=0; --k)
{
for(j=piv+1; j<cols;++j)
mat[k][j] -= mat[k][piv]*frac*mat[i][j];
mat[k][piv] = 0.0;
}
(*sol)[piv] = mat[i][cols-1]*frac;
//print_matrix(mat,rows,cols);
var = piv;
}
//print_matrix(mat,rows,cols);
// determine kernel
var = cols-1;
for(i=rows-1; i>=0; --i)
{
piv = 0; // determine first non zero entry
while( dzero_eps(mat[i][piv],eps_) && piv<cols)
piv++;
if( piv == cols) continue; // zero row
if(var - piv > 1) // are there free variables?
{
for(k=0; k<var-piv-1; ++k)
(*ker)[ker_var-k][var-k-1] = 1.0;
for(l=i; l>=0; --l)
{
// determine first non zero entry in row l
lde = 0;
while( dzero_eps(mat[l][lde],eps_) && lde<cols)
lde++;
frac = (-1.0)/mat[l][lde];
for(k=0; k<var-piv-1; ++k)
(*ker)[ker_var-k][lde] = mat[l][var-k-1]*frac;
}
ker_var -= var-piv-1;
}
var = piv;
//ker_var -= var-piv-1;
}
//print_matrix(mat,rows,cols);
//print_matrix(ker, cols-1-rnk, cols-1);
//print_vector(sol, cols-1);
}
bool matrix::inverse()
{
if (n!=m) return false;
matrix temp(n,n);
temp=*this;
matrix E(n,n);
for(size_t i=0; i<n; i++)
{
for(size_t j=0; j<n; j++)
{
(i==j)?E.p[n*i+j]=1.0:E.p[n*i+j]=0.0;
}
}
for(size_t k=0; k<n; k++)
{
size_t left_col,upper_row;
bool has_nozero=false;
for(size_t i=k; i<n; i++)
{
for(size_t j=k; j<n; j++)
{
if (p[i+j*n]!=0.0)
{
has_nozero=true;
upper_row=j;
break;
}
}
if (has_nozero)
{
left_col=i;
break;
}
}
if (!has_nozero)
{
*this=temp;
return false;
}
swap_cols(k,left_col);
E.swap_cols(k,left_col);
swap_rows(k,upper_row);
E.swap_rows(k,upper_row);
long double coeff=p[k*(n+1)];
divide_row(k,coeff);
E.divide_row(k,coeff);
for(size_t i=k+1; i<n; i++)
{
coeff=p[i*n+k];
sub_multed_row(k,i,coeff);
E.sub_multed_row(k,i,coeff);
}
}
for(size_t k=n-1; k>0; k--)
{
for(size_t i=k; i>0;)
{
long double coeff=p[--i*n+k];
sub_multed_row(k,i,coeff);
E.sub_multed_row(k,i,coeff);
}
}
*this=E;
return true;
}
void swap_ext_rows(int *arr, unsigned int rows, unsigned int columns) {
unsigned int min_r = row_with_min_negs(arr, rows, columns);
unsigned int max_r = row_with_max_negs(arr, rows, columns);
swap_rows(arr, min_r, max_r, rows, columns);
}
int
calculate_secret_bytes(unsigned char **bytes,unsigned char * bs, unsigned char * recover_bytes,int k )
{
int i, j;
unsigned char check1 = 0;
unsigned char check2 = 0;
row_t * rows = my_malloc(k*sizeof(row_t));
row_t row_pivot;
row_pivot.bytes = my_malloc((k * sizeof(char)));
memset(recover_bytes, 0, k * sizeof(unsigned char));
//Check if we have a row or colum with only ones! if so --> error
for(i = 0 ; i < k; ++i)
{
check1 = 0;
check2 = 0;
for (j = 0; j < k; ++j)
{
check1 = check1 | bytes[j][i];
check2 = check2 | bytes[i][j];
}
if(check1 == 0 || check2 ==0)
{
error("calculate_secret_bytes : there was a column or row with all ceros");
return ERROR;
}
rows[i].b = bs[i];
rows[i].bytes = bytes[i];
rows[i].index = i;
}
////
//Start gauss
for(i = 0 ; i < k- 1 ; ++i)
{
for(j = i ; j < k ; ++j)
{
if(rows[j].bytes[i] != 0)
{
swap_rows(&rows[i], &rows[j]);
break;
}
}
unsigned char x = 0;
for(j = (i + 1) ; j < k ; ++j)
{
if(rows[j].bytes[i] == 0)
continue;
memcpy(row_pivot.bytes, rows[i].bytes, k * sizeof(char));
row_pivot.b = rows[i].b;
x = divide(rows[j].bytes[i],row_pivot.bytes[i] );
x_mul_row( x,&row_pivot, k );
row_sub_row(&rows[j] , &row_pivot, k);
//TODO: Test if a row is all ceros and quit!!
int h;
int test = 0;
for(h = 0 ; h < k; ++h)
{
test = rows[j].bytes[h] | test;
}
if(test == 0 )
{
fprintf(stderr, "we are in trouble, the file generate a indeterminate system, finishing program \n");
exit(-1);
}
if(rows[j].bytes[i] != 0 )
{
fprintf(stderr, "calculate_secret_bytes: ERROR!!!");
}
}
}
//For every row sum all coeff * x_j and get the x_i
unsigned char suma;
for(i = (k-1); i >= 0 ; --i)
{
suma = 0;
for(j = i ; j < k ; ++j)
{
suma = sum(suma, mul(recover_bytes[j], rows[i].bytes[j]));
}
suma = sub(rows[i].b , suma);
recover_bytes[i] = divide(suma, rows[i].bytes[i]);
}
return OK;
}
static uchar mouse_axes_linear_equation_system() { // {{{
#define W 4
#define H 3
// Matrix of 3 lines and 4 columns
float m[H][W];
// The solution of this system
float sol[H];
uchar y;
fill_matrix_from_sensor(m);
// Gauss-Jordan elimination, based on:
// http://elonen.iki.fi/code/misc-notes/python-gaussj/index.html
for(y = 0; y < H; y++) {
uchar maxrow;
uchar y2;
float pivot;
// Finding the row with the maximum value at this column
maxrow = y;
pivot = fabs(m[maxrow][y]);
for(y2 = y+1; y2 < H; y2++) {
float newpivot;
newpivot = fabs(m[y2][y]);
if (newpivot > pivot) {
pivot = newpivot;
maxrow = y2;
}
}
// If the maximum value in this column is zero
if (pivot < 0.0009765625) { // 2**-10 == 0.0009765625
// Singular
return 0;
}
if (maxrow != y) {
swap_rows(m[y], m[maxrow]);
}
// Now we are ready to eliminate the column y, using m[y][y]
for(y2 = y+1; y2 < H; y2++) {
uchar x;
float c;
c = m[y2][y] / m[y][y];
// Subtracting from every element in row y2
for(x = y; x < W; x++) {
m[y2][x] -= m[y][x] * c;
}
}
}
// The matrix is now in "row echelon form" (it is a triangular matrix).
// Instead of using a loop for back-substituting all 3 elements from
// sol[], I'm calculating only 2 of them, as only those are needed.
sol[2] = m[2][3] / m[2][2];
sol[1] = m[1][3] / m[1][1] - m[1][2] * sol[2] / m[1][1];
// sol[0] is discarded
if ( sol[1] < -0.25
|| sol[1] > 1.25
|| sol[2] < -0.25
|| sol[2] > 1.25
) {
// Out-of-bounds
return 0;
}
mouse_report.x = apply_smoothing(0, &sol[1]);
mouse_report.y = apply_smoothing(1, &sol[2]);
return 1;
#undef W
#undef H
} // }}}
> constexpr auto gaussian_elimination(
matrix< M1, C1, R1 > a,
col_vector< M2, R2 > v,
col_vector< M3, R3 > defaults
){
using value_type = std::common_type_t<
value_type_t< M1 >, value_type_t< M2 >, value_type_t< M3 >
>;
if(
a.rows() == v.rows() &&
size_t(a.cols()) != size_t(v.rows() + defaults.rows())){
throw std::logic_error(
"gaussian_elimination: incompatible dimensions"
);
}
auto b = make_vector_fn(v.rows() + defaults.rows(),
[&v, &defaults](size_t i)->value_type{
if(i < v.rows()){
return v[i];
}else{
return defaults[i - v.rows()];
}
});
for(size_t i = size_t(a.rows()); i < size_t(b.rows()); ++i){
for(size_t y = 0; y < a.rows(); ++y){
b[y] -= a(y, y) * b[i];
}
}
auto m = make_matrix_fn(a.rows().as_col(), a.rows(),
[&a](size_t x, size_t y)->value_type{
return a(x, y);
});
// Compiler may optimize with the compile time dimension
size_t const size = C1 != 0_C ?
size_t(m.cols()) : R1 != 0_R ? size_t(m.rows()) : size_t(b.rows());
size_t swap_count = 0;
for(size_t i = 0; i < size; ++i){
if(m(i, i) == 0){
// swap lines
size_t y = i + 1;
for(; y < size; ++y){
if(m(i, y) == 0) continue;
swap_rows(m, i, y);
swap_rows(b, i, y);
++swap_count;
break;
}
// matrix is not invertible
if(y == size){
throw std::logic_error(
"gaussian_elimination with non invertible matrix"
);
}
}
// eliminate col in lower rows
for(size_t y = i + 1; y < size; ++y){
auto factor = m(i, y) / m(i, i);
m(i, y) = 0;
for(size_t x = i + 1; x < size; ++x){
m(x, y) -= factor * m(x, i);
}
b[y] -= factor *b[i];
}
}
for(size_t i = 1; i < size; ++i){
auto x = size - i;
for(size_t y = 0; y < x; ++y){
auto factor = m(x, y) / m(x, x);
b[y] -= factor * b[x];
// m(x, y) = 0; // TODO: remove this line
}
}
for(size_t i = 0; i < size; ++i){
b[i] /= m(i, i);
}
return b;
}
