/*
* invertMatrix
*
* I lifted this code from the skeleton code of a raytracer assignment
* from a different school. I taught that course too, so I figured it
* would be okay.
*/
Matrix4x4 Matrix4x4::invert() const
{
/* The algorithm is plain old Gauss-Jordan elimination
with partial pivoting. */
Matrix4x4 a(*this);
Matrix4x4 ret;
/* Loop over cols of a from left to right,
eliminating above and below diag */
/* Find largest pivot in column j among rows j..3 */
for(size_t j = 0; j < 4; ++j)
{
size_t i1 = j; /* Row with largest pivot candidate */
for(size_t i = j + 1; i < 4; ++i)
{
if(fabs(a[i][j]) > fabs(a[i1][j]))
{
i1 = i;
}
}
/* Swap rows i1 and j in a and ret to put pivot on diagonal */
swaprows(a, i1, j);
swaprows(ret, i1, j);
/* Scale row j to have a unit diagonal */
if(a[j][j] == 0.0)
{
// Theoretically throw an exception.
return ret;
}
dividerow(ret, j, a[j][j]);
dividerow(a, j, a[j][j]);
/* Eliminate off-diagonal elems in col j of a, doing identical
ops to b */
for(size_t i = 0; i < 4; ++i)
{
if(i != j)
{
submultrow(ret, i, j, a[i][j]);
submultrow(a, i, j, a[i][j]);
}
}
}
return ret;
}
//todo check sqr mtrx
void *gaussop(structmatrix *AM){//containts the rhs and lhs. in place
fpidx idx=getidxingfunc(AM);//(r,c,mat)
#define av(ri,ci) *(double*) idx(&ri,&ci,AM)
unsigned int ci;
for(ci=0;ci<AM->nrows;ci++){
//find pivot for col
//findmax
unsigned int imax;
double maxabsval=-DBL_MAX,absv;
for(imax=ci;imax<AM->nrows;imax++){
absv=abs(av(imax,ci));
if(absv>maxabsval){maxabsval=absv; continue;}
else{continue;}
}
//printf("max in col %d %lf",ci,maxabsval);
//argmax
for(imax=ci;imax<AM->nrows;imax++){
if(maxabsval==abs(av(imax,ci))){break;}
}
//printf("max ri %d \n",imax);
swaprows(ci,imax,AM);
unsigned int i;
for(i=ci+1;i<AM->nrows;i++){
unsigned int j;
for(j=ci+1;j<AM->ncols;j++){
av(i,j)=av(i,j)-av(ci,j)*(av(i,ci)/av(ci,ci));
}
av(i,ci)=0;//fill lower triangle with zeros
}
}
#undef av
}
void rotate(int img[N][N])
{
//transpose
for(int i=0; i<N; i++)
for(int j=i+1; j<N; j++)
swap(&img[i][j],&img[j][i]);
//flip rows
for(int i=0; i<N/2; i++)
swaprows(img[i],img[N-i-1],N);
}
// gjelim
void gjelim(float** lhs, float** rhs, long nrows, long ncolsrhs) {
// augment lhs array with rhs array and store in arr2
float** arr2 = new float*[nrows];
for (long row = 0; row < nrows; ++row)
arr2[row] = new float[nrows + ncolsrhs];
for (long row = 0; row < nrows; ++row) {
for (long col = 0; col < nrows; ++col) {
arr2[row][col] = lhs[row][col];
}
for (long col = nrows; col < nrows + ncolsrhs; ++col) {
arr2[row][col] = rhs[row][col - nrows];
}
}
// perform forward elimination to get arr2 in row-echelon form
for (long dindex = 0; dindex < nrows; ++dindex) {
// run along diagonal, swapping rows to move zeros in working position
// (along the diagonal) downwards
if ((dindex == (nrows - 1)) && (arr2[dindex][dindex] == 0)) {
return; // no solution
} else if (arr2[dindex][dindex] == 0) {
swaprows(arr2, dindex, dindex + 1);
}
// divide working row by value of working position to get a 1 on the
// diagonal
if (arr2[dindex][dindex] == 0.0) {
return;
} else {
float tempval = arr2[dindex][dindex];
for (long col = 0; col < nrows + ncolsrhs; ++col) {
arr2[dindex][col] /= tempval;
}
}
// eliminate value below working position by subtracting a multiple of
// the current row
for (long row = dindex + 1; row < nrows; ++row) {
float wval = arr2[row][dindex];
for (long col = 0; col < nrows + ncolsrhs; ++col) {
arr2[row][col] -= wval * arr2[dindex][col];
}
}
}
// backward substitution steps
for (long dindex = nrows - 1; dindex >= 0; --dindex) {
// eliminate value above working position by subtracting a multiple of
// the current row
for (long row = dindex - 1; row >= 0; --row) {
float wval = arr2[row][dindex];
for (long col = 0; col < nrows + ncolsrhs; ++col) {
arr2[row][col] -= wval * arr2[dindex][col];
}
}
}
// assign result to replace rhs
for (long row = 0; row < nrows; ++row) {
for (long col = 0; col < ncolsrhs; ++col) {
rhs[row][col] = arr2[row][col + nrows];
}
}
for (long row = 0; row < nrows; ++row)
delete[] arr2[row];
delete[] arr2;
}
