GzMatrix GzMatrix::transpose() {
GzMatrix res;
res.resize(nCol(), nRow());
for (int i=0; i<nRow(); i++)
for (int j=0; j<nCol(); j++) res[j][i]=at(i)[j];
return res;
}
bool GeneralMatrix::CutIn2_Vertical(GeneralMatrix& Left,GeneralMatrix& Right,int iCol)
{
int i,j;
if (nCol()==0)
{
Left.ReSize(0,0);
Right.ReSize(0,0);
return TRUE;
}
if(iCol>nCol()||iCol<0) return FALSE;
if(!Left.ReSize(nRow(),iCol,ERRORVAL))
return FALSE;
if(!Right.ReSize(nRow(),nCol()-iCol,ERRORVAL))
return FALSE;
for(i=0;i<nRow();i++)
for(j=0;j<iCol;j++)
Left[i][j] = GetElem(i,j);
for(i=0;i<nRow();i++)
for(j=0;j<Right.nCol();j++)
Right[i][j] = GetElem(i,j+iCol);
return TRUE;
}
bool GeneralMatrix::CutIn2_Across(GeneralMatrix& Upper,GeneralMatrix& Lower,int iRow)
{
int i,j;
if (nCol()==0)
{
Upper.ReSize(0,0);
Lower.ReSize(0,0);
return TRUE;
}
if(iRow>nRow()||iRow<0) return FALSE;
if(!Upper.ReSize(iRow,nCol(),ERRORVAL))
return FALSE;
if(!Lower.ReSize(nRow()-iRow,nCol(),ERRORVAL))
return FALSE;
for(i=0;i<iRow;i++)
for(j=0;j<nCol();j++)
Upper[i][j] = GetElem(i,j);
for(i=0;i<Lower.nRow();i++)
for(j=0;j<nCol();j++)
Lower[i][j] = GetElem(i+iRow,j);
return TRUE;
}
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::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;
}
ELEMTYPE GeneralMatrix::Distance_E(GeneralMatrix& other) const
{
int i,j;
ELEMTYPE result = ERRORVAL;
if(nRow()!=other.nRow()||nCol()!=other.nCol()) return result;
for(result=0,i=0;i<nRow();i++)
for(j=0;j<nCol();j++)
result += pow(GetElem(i,j)-other[i][j],2);
return sqrt(result);
}
bool GeneralMatrix::Paste(GeneralMatrix& other,int top,int left) const
{
int i,j,iRow,iCol;
iRow = other.nRow()>(nRow()-top) ? (nRow()-top) : other.nRow();
iCol = other.nCol()>(nCol()-left) ? (nCol()-left): other.nCol();
for(i=0;i<iRow;i++)
for(j=0;j<iCol;j++)
GetElem(i+top,j+left) = other[i][j];
return TRUE;
}
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());
}
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;
}
void GeneralMatrix::Print()
{
if (nRow()==0||nCol()==0)
{
return;
}
for (int i=0;i<nRow();i++)
{
for (int j=0;j<nCol();j++)
{
cout<<GetElem(i,j)<<" ";
}
cout<<endl;
}
cout<<endl;
}
bool GeneralMatrix::Zero() const
{
for(int i=0;i<nRow();i++)
for(int j=0;j<nCol();j++)
GetElem(i,j) = 0.0;
return TRUE;
}
bool GeneralMatrix::Jointer_bottom(GeneralMatrix& other)
{
if(other.nCol()!=nCol()) return FALSE;
if(!AddRows(other.nRow(),ERRORVAL))
return FALSE;
return Paste(other,nRow()-other.nRow(),0);
}
bool GeneralMatrix::Fabs() const
{
int i,j;
for(i=0;i<nRow();i++)
for(j=0;j<nCol();j++)
GetElem(i,j) = fabs(GetElem(i,j));
return TRUE;
}
ELEMTYPE GeneralMatrix::Addup(int iPower) const
{
int i,j;
ELEMTYPE result=0;
for(i=0;i<nRow();i++)
for(j=0;j<nCol();j++)
result += pow(GetElem(i,j),iPower);
return result;
}
bool GeneralMatrix::Jointer_Right(GeneralMatrix& other)
{
if(other.nRow()!=nRow()) return FALSE;
if(!AddCols(other.nCol(),ERRORVAL))
return FALSE;
Paste(other,0,nCol()-other.nCol());
return TRUE;
}
bool GeneralMatrix::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;
}
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;
}
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::GetRowVector(int iRow) const
{
int j;
GeneralMatrix Vector(1,nCol(),ERRORVAL);
if(iRow<0||iRow>nRow()-1||nCol()==0) return Vector;
for(j=0;j<nCol();j++)
Vector[0][j] = GetElem(iRow,j);
return Vector;
}
//////////////////////////////////////////////////////////////////////
// 重载运算符==，判断矩阵是否相等
//
// 参数：
// 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;
}
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;
}
bool GeneralMatrix::ExchangeCols(int iCol1,int iCol2) const
{
int i;
ELEMTYPE temp;
if(this->nCol()<2||iCol1==iCol2)
return TRUE;
if(iCol1>=nCol()||iCol2>=nCol())
return FALSE;
for(i=0;i<nRow();i++)
{
temp = GetElem(i,iCol1);
GetElem(i,iCol1) = GetElem(i,iCol2);
GetElem(i,iCol2) = 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;
}
void LpSub::addVars(
ArrayBuffer<Variable*> &vars,
ArrayBuffer<FSVarStat*> &fsVarStat,
ArrayBuffer<double> &lb,
ArrayBuffer<double> &ub)
{
// LpSub::addVars(): local variables
ArrayBuffer<int> delVar(vars.size(),false); //!< the eliminated variables
Array<double> rhsDelta(0,sub_->nCon()-1, 0.0); //!< the correction of the rhs
double vValue;
double coeff;
bool modifyRhs = false; //!< if \a true the modification of rhs required
int oldNCol = trueNCol();
int n = trueNCol();
Variable *v;
// divide the added variables in eliminable and non-eliminable ones
int nVariables = vars.size();
for (int i = 0; i < nVariables; i++) {
v = vars[i];
if(fsVarStat[i]->fixedOrSet()) {
if (eliminable(i)) {
//! the new variable is eliminated
delVar.push(i);
vValue = elimVal(fsVarStat[i], lb[i], ub[i]);
valueAdd_ += v->obj() * vValue;
orig2lp_[nOrigVar_++] = -1;
const int nCon = sub_->nCon();
for (int c = 0; c < nCon; c++) {
coeff = sub_->constraint(c)->coeff(v);
if (fabs(coeff) > master_->eps()) {
rhsDelta[c] += vValue * coeff;
modifyRhs = true;
}
}
}
else {
// the new variable is fixed in the LP
orig2lp_[nOrigVar_++] = n;
lp2orig_[n] = oldNCol + i;
++n;
lb[i] = ub[i] = elimVal(fsVarStat[i], lb[i], ub[i]);
}
}
else {
// the new variable will be added to the LP explicitly
orig2lp_[nOrigVar_++] = n;
lp2orig_[n] = oldNCol + i;
++n;
}
}
// remove the fixed and set added variables
if (delVar.size()) {
vars.leftShift(delVar);
fsVarStat.leftShift(delVar);
lb.leftShift(delVar);
ub.leftShift(delVar);
}
// generate the column of the added variable and add them to the LP
ArrayBuffer<Column*> newCols(vars.size(),false);
//!< new columns added to the constraint matrix
Column colBuf(master_, nRow()); //!< buffer for generated columns
Column *col;
nVariables = vars.size();
for(int i = 0; i < nVariables; i++) {
vars[i]->genColumn(sub_->actCon(), colBuf);
col = new Column(master_, colBuf.nnz());
col->copy(colBuf);
col->obj(colBuf.obj());
col->uBound(colBuf.uBound());
col->lBound(colBuf.lBound());
newCols.push(col);
colBuf.clear();
}
LP::addCols(newCols);
// modify the right hand side if fixed or set variables are added
if (modifyRhs) {
const int nCon = sub_->nCon();
Array<double> newRhs(nCon);
for(int c = 0; c < nCon; c++)
newRhs[c] = rhs(c) - rhsDelta[c];
changeRhs(newRhs);
}
// LpSub::addVars(): clean up
for(int i = 0; i < nVariables; i++)
delete newCols[i];
}
// operate on Array -- handy for PhysicalOperator::execute() overloads
inline size_t nRow(std::shared_ptr<Array>& array, bool transpose=false) { return nRow(array->getArrayDesc(), transpose); }
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;
}
extern
void printError(const char *errorstring, ...) {
std::cerr << "error:" << nRow() << "," << nCol() << ": " << convertTokens(errorstring) << std::endl;
exit(0);
}
// operate on ArrayDesc -- handy for LogicalOperator::inferSchema() overloads
inline size_t nRow(const ArrayDesc& desc, bool transpose=false) { return nRow(desc.getDimensions(), transpose); }