bool SMatrix::Jointer_Right(SMatrix& other)
{
if(other.nRow()!=nRow()) return FALSE;
if(!AddCols(other.nCol(),ERRORVAL))
return FALSE;
Paste(other,0,nCol()-other.nCol());
return TRUE;
}
bool SMatrix::ReSize(int iRow,int iCol, ELEMTYPE Val)const
{
if(iRow<0||iCol<0) return FALSE;
if(iRow==0) iCol = 0;
AddRows(iRow-nRow(),Val);
AddCols(iCol-nCol(),Val);
return TRUE;
}
GeneralMatrix GeneralMatrix::GetColVector(int iCol) const
{
int i;
GeneralMatrix Vector(nRow(),1,ERRORVAL);
if(iCol<0||iCol>nCol()-1||nRow()==0) return Vector;
for(i=0;i<nRow();i++)
Vector[i][0] = GetElem(i,iCol);
return Vector;
}
GeneralMatrix GeneralMatrix::Invert()
{
GeneralMatrix result(nRow(),nRow(),ERRORVAL);
if(nRow()!=nCol()) return result;
result = *this;
result.Invert_Self();
return result;
}
bool GeneralMatrix::MakeUnit() const
{
if(nRow()!=nCol())
return FALSE;
Zero();
for (int i=0;i<nRow();i++)
GetElem(i,i) = 1.0;
return TRUE;
}
GeneralMatrix GeneralMatrix::GetDiagonalVector() const
{
int i;
GeneralMatrix Vector(nRow(),1,ERRORVAL);
if (nRow()!=nCol())
return Vector;
for(i=0;i<nRow();i++)
Vector[i][0] = GetElem(i,i);
return Vector;
}
bool GeneralMatrix::Jointer_Diagonal(GeneralMatrix& other,ELEMTYPE Val)
{
int iCol;
iCol = nRow()==0 ? other.nCol()-1 : other.nCol();
if(!AddRows(other.nRow(),Val))
return FALSE;
if(!AddCols(iCol,Val))
return FALSE;
return Paste(other,nRow()-other.nRow(),nCol()-other.nCol());
}
GzVertex GzMatrix::toVertex() {
assert((nRow()==4)&&(nCol()==1));
//Convert to vertex, remember to divide X, Y, Z coordinates by W
//See http://en.wikipedia.org/wiki/Homogeneous_coordinates#Use_in_computer_graphics
// http://en.wikipedia.org/wiki/Transformation_matrix
//Or google: "homogeneous coordinates"
GzVertex v;
GzReal w=at(3)[0];
v[X]=at(0)[0]/w;
v[Y]=at(1)[0]/w;
v[Z]=at(2)[0]/w;
return v;
}
GeneralMatrix GeneralMatrix::GetPart(int left,int top,int bottom,int right) const
{
int i,j;
GeneralMatrix result;
if(left>right||top>bottom||right>=nCol()||bottom>=nRow()||left<0||top<0||right<0||bottom<0) return result;
if(!result.ReSize(bottom-top+1,right-left+1,ERRORVAL))
return result;
for(i=0;i<result.nRow();i++)
for(j=0;j<result.nCol();j++)
result[i][j] = GetElem(i+left,j+top);
return result;
}
//////////////////////////////////////////////////////////////////////
// 重载运算符==，判断矩阵是否相等
//
// 参数：
// 1. const GeneralMatrix& other - 用于比较的矩阵
//
// 返回值：BOOL 型，两个矩阵相等则为TRUE，否则为FALSE
//////////////////////////////////////////////////////////////////////
bool GeneralMatrix::operator==(const GeneralMatrix& other) const
{
// 首先检查行列数是否相等
if (this->nCol() != other.nCol() || this->nRow() != other.nRow())
return FALSE;
for (int i=0; i<nRow(); ++i)
{
for (int j=0; j<nCol(); ++j)
{
if (this->GetElem(i,j)!=other.GetElem(i,j))
return FALSE;
}
}
return TRUE;
}
bool GeneralMatrix::ExchangeRows(int iRow1,int iRow2) const
{
int j;
ELEMTYPE temp;
if(this->nRow()<2||iRow1==iRow2)
return TRUE;
if(iRow1>=nRow()||iRow2>=nRow())
return FALSE;
for(j=0;j<nCol();j++)
{
temp = GetElem(iRow1,j);
GetElem(iRow1,j) = GetElem(iRow2,j);
GetElem(iRow2,j) = temp;
}
return TRUE;
}
GzMatrix GzMatrix::inverse3x3() {
assert((nRow()==3)&&(nCol()==3));
GzVector x0, x1, x2;
x0[0]=at(0)[0]; x1[0]=at(0)[1]; x2[0]=at(0)[2];
x0[1]=at(1)[0]; x1[1]=at(1)[1]; x2[1]=at(1)[2];
x0[2]=at(2)[0]; x1[2]=at(2)[1]; x2[2]=at(2)[2];
GzReal detA=dotProduct(x0, crossProduct(x1, x2));
assert(fabs(detA)>1e-6);
GzVector r0, r1, r2;
r0=crossProduct(x1, x2);
r1=crossProduct(x2, x0);
r2=crossProduct(x0, x1);
GzMatrix res;
res.resize(3, 3);
res[0][0]=r0[0]/detA; res[0][1]=r0[1]/detA; res[0][2]=r0[2]/detA;
res[1][0]=r1[0]/detA; res[1][1]=r1[1]/detA; res[1][2]=r1[2]/detA;
res[2][0]=r2[0]/detA; res[2][1]=r2[1]/detA; res[2][2]=r2[2]/detA;
return res;
}
extern
void printError(const char *errorstring, ...) {
std::cerr << "error:" << nRow() << "," << nCol() << ": " << convertTokens(errorstring) << std::endl;
exit(0);
}
bool GeneralMatrix::Invert_Self() const
{
int k,i,j,icol,irow;
ELEMTYPE elem,d;
icol = nCol();
irow = nRow();
if (icol!=irow)
return FALSE;
int *IS = new int[irow]; //记忆行交换空间
int *JS = new int[irow]; //记忆列交换空间
for (k=0; k<irow; k++)
{
d = 0.0;
for (i=k; i<irow; i++)
for (j=k; j<irow; j++)
{
elem = fabs(GetElem(i,j));
if(elem > d) { d = elem; IS[k] = i; JS[k] = j; } //记忆行列交换
}
if(fabs(d) < 1e-19) //矩阵奇异，错误返回
{
delete []IS;
delete []JS;
return FALSE;
}
if(IS[k]!=k) //行交换
ExchangeRows(k,IS[k]);
if(JS[k]!=k) //列变换
ExchangeCols(k,JS[k]);
GetElem(k,k) = 1.0 / GetElem(k,k); //归一化
for(j=0; j<irow; j++) //归一化计算Akj*Akk=>Akj
if(j != k)
GetElem(k,j) = GetElem(k,k) * GetElem(k,j);
for(i=0; i<irow; i++) //消元计算Aij-Aik*Akj=>Aij
if(i != k)
for (j=0; j<irow; j++)
if (j != k)
GetElem(i,j) = GetElem(i,j) - GetElem(i,k) * GetElem(k,j);
for(i=0; i<irow; i++) //消元计算-Aik*Akk=>Aik
if(i != k)
GetElem(i,k) = GetElem(i,k) * GetElem(k,k) * -1.0;
}
for (k=irow-1; k>=0; k--)
{
if(JS[k] != k)
ExchangeRows(k,JS[k]);
if(IS[k] != k)
ExchangeCols(k,IS[k]);
}
delete []IS;
delete []JS;
return TRUE;
}
inline size_t nCol(std::shared_ptr<Array>& array, bool transpose=false) { return nCol(array->getArrayDesc(), transpose); }
inline size_t nCol(const ArrayDesc& desc, bool transpose=false) { return nCol(desc.getDimensions(), transpose); }
