void elim ( short trans )
{
struct matrix * rp;
ROWP pp, ppp;
short val1, val2, div, gcd1;
#define myABS(A) ( ( (short)(A) >= (short)0 ) ? (A) : -(A) )
pp = oldmp->rowp;
for ( ppp = pp+(2*(*pp)+1) ; (short)(*(ppp++)) != (short)trans ; ppp++ );
val1 = *ppp;
for ( rp = oldmm; rp != NULL; rp = rp->next ) {
pp = rp->rowp;
for ( ppp = pp+(2*(*pp)+1) ;
(short)(*(ppp++)) != (short)trans ; ppp++ );
val2 = - *ppp;
div = gcd( val1, val2 );
{ int j, j1, j2; short val; register ROWP point, point1, point2;
for ( j = 0, gcd1 = 0, point = newrow + 1,
j1 = *(point1 = oldmp->rowp), point1++,
j2 = *(point2 = rp->rowp), point2++;
j1 > 0 && j2 > 0 ; ) {
if ( *point1 == *point2 ) {
j1--; j2--;
*(point++) = *(point1++); point2++;
if ( (short)( val =
(val2 * *(point1++) + val1 * *(point2++)) / div
) == (short)0 )
*(--point) = 0;
else{
*(point++) = val; j++;
if ( gcd1 != (short)1 ) {
if ( gcd1 == (short)0 ) gcd1 = myABS(val);
else gcd1 = gcd( myABS(val), gcd1 );
}
}
}
else if ( *point1 > *point2 ) {
j++; j2--;
*(point++) = *(point2++);
val = *(point++) = (short)( (val1 * *(point2++)) / div );
if ( gcd1 != (short)1 ) {
if ( gcd1 == (short)0 ) gcd1 = myABS(val);
else gcd1 = gcd( myABS(val), gcd1 );
}
}
else {
j++; j1--;
*(point++) = *(point1++);
val = *(point++) = (short)( (val2 * *(point1++)) / div );
if ( gcd1 != (short)1 ) {
if ( gcd1 == (short)0 ) gcd1 = myABS(val);
else gcd1 = gcd( myABS(val), gcd1 );
}
}
}
while ( j1-- > 0 ) {
j++;
*(point++) = *(point1++);
val = *(point++) = (short)( (val2 * *(point1++)) / div );
if ( gcd1 != (short)1 ) {
if ( gcd1 == (short)0 ) gcd1 = myABS(val);
else gcd1 = gcd( myABS(val), gcd1 );
}
}
while ( j2-- > 0 ) {
j++;
*(point++) = *(point2++);
val = *(point++) = (short)( (val1 * *(point2++)) / div );
if ( gcd1 != (short)1 ) {
if ( gcd1 == (short)0 ) gcd1 = myABS(val);
else gcd1 = gcd( myABS(val), gcd1 );
}
}
*newrow = j;
while ( ( (short)(*point1) != (short)0 ) &&
( (short)(*point2) != (short)0 ) ) {
if ( *point1 == *point2 ) {
*(point++) = *(point1++); point2++;
if ( (short)( val =
(val2 * *(point1++) + val1 * *(point2++)) / div
) == (short)0 )
*(--point) = 0;
else {
*(point++) = val; j++;
if ( gcd1 != (short)1 ) {
if ( gcd1 == (short)0 ) gcd1 = myABS(val);
else gcd1 = gcd( myABS(val), gcd1 );
}
}
}
else if ( *point1 > *point2 ) {
*(point++) = *(point2++); j++;
val = *(point++) = (short)( (val1 * *(point2++)) / div );
if ( gcd1 != (short)1 ) {
if ( gcd1 == (short)0 ) gcd1 = myABS(val);
else gcd1 = gcd( myABS(val), gcd1 );
}
}
else {
*(point++) = *(point1++); j++;
val = *(point++) = (short)( (val2 * *(point1++)) / div );
if ( gcd1 != (short)1 ) {
if ( gcd1 == (short)0 ) gcd1 = myABS(val);
else gcd1 = gcd( myABS(val), gcd1 );
}
}
}
while ( (short)(*point1) != (short)0 ) {
j++; *(point++) = *(point1++);
val = *(point++) = (short)( (val2 * *(point1++)) / div );
if ( gcd1 != (short)1 ) {
if ( gcd1 == (short)0 ) gcd1 = myABS(val);
else gcd1 = gcd( myABS(val), gcd1 );
}
}
while ( (short)(*point2) != (short)0 ) {
j++; *(point++) = *(point2++);
val = *(point++) = (short)( (val1 * *(point2++)) / div );
if ( gcd1 != (short)1 ) {
if ( gcd1 == (short)0 ) gcd1 = myABS(val);
else gcd1 = gcd( myABS(val), gcd1 );
}
}
*point = 0;
if ( test_min( newrow ) ) {
newm = (struct matrix *)emalloc(MAT_SIZE);
newm->next = mat; mat = newm;
newm->rowp = point = (ROWP)emalloc((unsigned)(2*(j+1)*ITM_SIZE));
for ( j = *(point1 = newrow), *(point++) = j, point1++;
j-- > 0 ;
*(point++) = *(point1++), *(point++) = *(point1++)/gcd1 );
while ( (short)(*point1) != (short)0 ) {
*(point++) = *(point1++);
if ( (*(point++) = *(point1++)/gcd1) > 0 )
++(n_pos[(int)*(point-2)]);
else
++(n_neg[(int)*(point-2)]);
}
*(point) = 0;
#ifdef DEBUG
printf("new row ");
for ( j = *(point = newm->rowp), printf(" %d :\n",(int)j), point++; j-- > 0 ;
printf(" %d",(int)(*(point++)) ), printf(" %d,",(int)(*(point++)) ) );
while ( (short)(*point) != (short)0 ) {
printf("; %d",(int)(*(point++)) );
printf(" %d",(int)(*(point++)) );
}
printf("\n");
#endif
}
}
}
}
mat_t* robolib_matrix_inverse(mat_t* m, mat_t* minv)
{
if (robolib_is_null_matrix(m)||m->rows!=m->cols) return NULL;
int i,j;
int maxi;
int nrc;
nrc = m->rows;
mat_t* mtmp = robolib_matrix_alloc(nrc, nrc);
mat_t* vtmp = robolib_vector_alloc(nrc);
double tmp;
robolib_matrix_copy(m, mtmp);
minv = robolib_matrix_new_unit(nrc);
if (mtmp==NULL || minv==NULL) return NULL;
for (i=0; i<nrc; i++) {
// maxi = 0; あれっこれ違…
maxi = i;
for (j=i+1; j<nrc; j++) {
if(myABS(robolib_matrix_get(mtmp,maxi,i)) < myABS(robolib_matrix_get(mtmp,j,i)))
maxi = j;
}
if (robolib_matrix_get(mtmp,maxi,i) == 0.0) {
printf("input matrix is invalid!\n");
break;
}
// 行入れ替え
robolib_matrix_copy_row(mtmp, i, vtmp, 0);
robolib_matrix_copy_row(mtmp, maxi, mtmp, i);
// robolib_matrix_copy_row(vtmp, 0, mtmp, i);
robolib_matrix_copy_row(vtmp, 0, mtmp, maxi);
// 行入れ替え
robolib_matrix_copy_row(minv, i, vtmp, 0);
robolib_matrix_copy_row(minv, maxi, minv, i);
// robolib_matrix_copy_row(vtmp, 0, minv, i);
robolib_matrix_copy_row(vtmp, 0, minv, maxi);
tmp = robolib_matrix_get(mtmp,i,i);
for (j=0; j<nrc; j++) {
// j:列番号
//*(mtmp->val+i*mtmp->cols+j) /= tmp;
//*(minv->val+i*mtmp->cols+j) /= tmp;
robolib_matrix_set(mtmp, i, j,
robolib_matrix_get(mtmp, i, j)/tmp);
robolib_matrix_set(minv, i, j,
robolib_matrix_get(minv, i, j)/tmp);
// printf("%s : %d\n",__FILE__,__LINE__);
// robolib_matrix_print(mtmp);
}
for (j=0; j<nrc; j++) {
// j:行番号
if (i!=j) {
// tmp = robolib_matrix_get(m,j,i);
tmp = robolib_matrix_get(mtmp,j,i);
robolib_matrix_row_linear_comb(-tmp, mtmp, i, 1.0, mtmp, j, mtmp, j);
robolib_matrix_row_linear_comb(-tmp, minv, i, 1.0, minv, j, minv, j);
// printf("%s : %d\n",__FILE__,__LINE__);
// robolib_matrix_print(mtmp);
}
}
}
robolib_vector_free(vtmp);
robolib_matrix_free(mtmp);
return minv;
}
