void doOneBeta(const EngineType& engine,
std::vector<size_t>& ne,
OpLibFactoryType& opLibFactory,
size_t site,
size_t sigma,
RealType beta)
{
RealType sum = 0;
RealType sum2 = 0;
RealType denominator = 0;
FreeFermions::Combinations combinations(engine.size(),ne[0]);
size_t factorToMakeItSmaller = 1;
for (size_t i = 0; i<combinations.size(); ++i) {
OpDiagonalFactoryType opDiagonalFactory(engine);
EtoTheBetaHType ebh(beta,engine,0);
DiagonalOperatorType& eibOp = opDiagonalFactory(ebh);
std::vector<size_t> vTmp(engine.size(),0);
for (size_t j=0;j<combinations(i).size();++j) vTmp[combinations(i)[j]]=1;
std::vector<std::vector<size_t> > occupations(1,vTmp);
HilbertStateType thisState(engine,occupations);
HilbertStateType phi = thisState;
eibOp.applyTo(phi);
denominator += scalarProduct(thisState,phi)*factorToMakeItSmaller;
LibraryOperatorType& myOp2 = opLibFactory(LibraryOperatorType::N,site,sigma);
myOp2.applyTo(phi);
sum += scalarProduct(thisState,phi)*factorToMakeItSmaller;
myOp2.applyTo(phi);
sum2 += scalarProduct(thisState,phi)*factorToMakeItSmaller;
}
std::cout<<beta<<" "<<sum<<" "<<denominator<<" "<<sum/denominator<<" "<<sum2/denominator<<"\n";
}
/* sum product */
double VectorSumProduct(const vector<double>& aV1, const vector<double>& aV2)
{
vector<double> vTmp(aV1);
VectorMult(vTmp, aV2);
double vRes = 0.;
for(size_t i=0; i<vTmp.size(); ++i)
vRes += vTmp[i];
return vRes;
}
int GetReplaceIndex(vector<vector<prob_t> >& vCorrelMat,vector< pair<float,int> >& vOrig,pair<float,int>& oP)
{
int iSz = vOrig.size() , iBestIDX = -1 , i = 0;
prob_t maxVal = KLDCorVal(vCorrelMat,vOrig);
prob_t curVal = maxVal;
for(i=0;i<iSz;i++)
{
vector< pair<float,int> > vTmp(vOrig);
vTmp[i]=oP;
if( (curVal=KLDCorVal(vCorrelMat,vTmp)) > maxVal )
{
maxVal = curVal;
iBestIDX = i;
}
}
return iBestIDX;
}
void restartSimpleQPSolve(Vector vBO, // in
Vector vP) // out
{
if (bQRFailed_QP) { vP.zero(); return; }
int i,k=0, *ii=vi_QP, nc2=vi_QP.sz();
double *lambda=vLastLambda_QP, *b=vBO;
Vector vTmp(nc2);
for (i=0; i<nc2; i++)
{
if (lambda[i]!=0.0)
{
b[k]=b[ii[i]];
k++;
}
}
vBO.setSize(k);
vBO.permutIn(vTmp,viPermut_QP);
mR_QP.solveInPlace(vTmp);
mQ_QP.multiply(vP,vTmp);
}
void NVUTCamera::SetProjParams(FLOAT fFOV, FLOAT fAspect, D3DXMATRIX mView, D3DXVECTOR3 vBBox[2])
{
D3DXMATRIX mViewInv;
D3DXMatrixInverse(&mViewInv, NULL, &mView);
D3DXVECTOR3 vView, vCorner[2], vEye;
D3DXVECTOR3 vTmp(0, 0, 1);
D3DXVec3TransformNormal(&vView, &vTmp, &mViewInv);
vTmp = D3DXVECTOR3(0, 0, 0);
D3DXVec3TransformCoord(&vEye, &vTmp, &mViewInv);
D3DXVec3Normalize(&vView, &vView);
for (int ic = 0; ic < 3; ++ic)
{
vCorner[0][ic] = (vView[ic] < 0) ? vBBox[0][ic] : vBBox[1][ic];
vCorner[1][ic] = (vView[ic] < 0) ? vBBox[1][ic] : vBBox[0][ic];
}
vTmp = vCorner[0] - vEye;
float fFar = max(0.01f, 1.2f * D3DXVec3Dot(&vTmp, &vView));
vTmp = vCorner[1] - vEye;
float fNear = max(fFar / 100, D3DXVec3Dot(&vTmp, &vView));
CModelViewerCamera::SetProjParams(fFOV, fAspect, fNear, fFar);
}
//-----------------------------------------------------------------------------
// 説明: 関節ノードのワールド座標の取得
// 引数:
// jid [in] 関節インデクス
// 返り値:
// 関節ノードのワールド座標ベクトル
// その他:
//-----------------------------------------------------------------------------
Vector3f CFigure::GetWorldPosition(size_t jid) const
{
//Vector3f vTmp(0, 0, 0), vWorldPos(0, 0, 0);
//Matrix4f mTrans = Matrix4f::Identity();
Vector3f vTmp(0, 0, 0), vWorldPos(0, 0, 0);
Matrix4f mTrans = Matrix4f::Identity();
mTrans = m_vecJoints[jid]->GetOffsetMatrix();
NEigenMath::Vector3fTransformCoord(&vWorldPos, &vTmp, &mTrans);
//D3DXVec3TransformCoord(&vWorldPos, &vTmp, &mTrans);
for (CJoint *pNode = m_vecJoints[jid]->GetParent();pNode != NULL;pNode = pNode->GetParent())
{
mTrans = pNode->GetTransformMatrix();
vTmp = vWorldPos;
NEigenMath::Vector3fTransformCoord(&vWorldPos, &vTmp, &mTrans);
//vWorldPos = vTmp.transpose() * mTrans;
}
return vWorldPos;
}
void simpleQPSolve(Matrix mH, Vector vG, Matrix mA, Vector vB, // in
Vector vP, Vector vLambda, int *info) // out
{
const double tolRelFeasibility=1e-6;
// int *info=NULL;
int dim=mA.nColumn(), nc=mA.nLine(), ncr, i,j,k, lastJ=-2, *ii;
MatrixTriangle M(dim);
Matrix mAR, mZ(dim,dim-1), mHZ(dim,dim-1), mZHZ(dim-1,dim-1);
Vector vTmp(mmax(nc,dim)), vYB(dim), vD(dim), vTmp2(dim), vTmp3(nc), vLast(dim),
vBR_QP, vLambdaScale(nc);
VectorChar vLB(nc);
double *lambda=vLambda, *br, *b=vB, violationMax, violationMax2,
*rlambda,ax,al,dviolation, **a=mA, **ar=mAR, mymin, mymax, maxb, *llambda,
**r,t, *scaleLambda=vLambdaScale;
char finished=0, feasible=0, *lb=vLB;
if (info) *info=0;
// remove lines which are null
k=0; vi_QP.setSize(nc); maxb=0.0;
for (i=0; i<nc; i++)
{
mymin=INF; mymax=-INF;
for (j=0; j<dim; j++)
{
mymin=mmin(mymin,a[i][j]);
mymax=mmax(mymax,a[i][j]);
if ((mymin!=mymax)||(mymin!=0.0)||(mymax!=0.0)) break;
}
if ((mymin!=mymax)||(mymin!=0.0)||(mymax!=0.0))
{
lambda[k]=lambda[i];
maxb=mmax(maxb,condorAbs(b[i]));
scaleLambda[k]=mA.euclidianNorm(i);
vi_QP[k]=i;
k++;
}
}
nc=k; vi_QP.setSize(nc); ii=vi_QP; maxb=(1.0+maxb)*tolRelFeasibility;
vLast.zero();
for (i=0; i<nc; i++) if (lambda[i]!=0.0) lb[i]=2; else lb[i]=1;
while (!finished)
{
finished=1;
mAR.setSize(dim,dim); mAR.zero(); ar=mAR;
vBR_QP.setSize(mmin(nc,dim)); br=vBR_QP;
ncr=0;
for (i=0; i<nc; i++)
if (lambda[i]!=0.0)
{
// mAR.setLines(ncr,mA,ii[i],1);
k=ii[i];
t=scaleLambda[ncr];
for (j=0; j<dim; j++) ar[ncr][j]=a[k][j]*t;
br[ncr]=b[ii[i]]*t;
ncr++;
}
mAR.setSize(ncr,dim);
vBR_QP.setSize(ncr);
vLastLambda_QP.copyFrom(vLambda); llambda=vLastLambda_QP;
if (ncr==0)
{
// compute step
vYB.copyFrom(vG);
vYB.multiply(-1.0);
if (mH.cholesky(M))
{
M.solveInPlace(vYB);
M.solveTransposInPlace(vYB);
} else
{
printf("warning: cholesky factorisation failed.\n");
if (info) *info=2;
}
vLambda.zero();
} else
{
Matrix mAR2=mAR.clone(), mQ2;
MatrixTriangle mR2;
mAR2.QR(mQ2,mR2);
mAR.QR(mQ_QP,mR_QP, viPermut_QP); // content of mAR is destroyed here !
bQRFailed_QP=0;
r=mR_QP;
for (i=0; i<ncr; i++)
if (r[i][i]==0.0)
{
// one constraint has been wrongly added.
bQRFailed_QP=1;
QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vLast); return;
}
for (i=0; i<ncr; i++)
if (viPermut_QP[i]!=i)
{
// printf("whoups.\n");
}
if (ncr<dim)
{
mQ_QP.getSubMatrix(mZ,0,ncr);
// Z^t H Z
mH.multiply(mHZ,mZ);
mZ.transposeAndMultiply(mZHZ,mHZ);
mQ_QP.setSize(dim,ncr);
}
// form Yb
vBR_QP.permutIn(vTmp,viPermut_QP);
mR_QP.solveInPlace(vTmp);
mQ_QP.multiply(vYB,vTmp);
if (ncr<dim)
{
// ( vG + H vYB )^t Z : result in vD
mH.multiply(vTmp,vYB);
vTmp+=vG;
vTmp.transposeAndMultiply(vD,mZ);
// calculate current delta (result in vD)
vD.multiply(-1.0);
if (mZHZ.cholesky(M))
{
M.solveInPlace(vD);
M.solveTransposInPlace(vD);
}
else
{
printf("warning: cholesky factorisation failed.\n");
if (info) *info=2;
};
// evaluate vX* (result in vYB):
mZ.multiply(vTmp, vD);
vYB+=vTmp;
}
// evaluate vG* (result in vTmp2)
mH.multiply(vTmp2,vYB);
vTmp2+=vG;
// evaluate lambda star (result in vTmp):
mQ2.transposeAndMultiply(vTmp,vTmp2);
mR2.solveTransposInPlace(vTmp);
// evaluate lambda star (result in vTmp):
mQ_QP.transposeAndMultiply(vTmp3,vTmp2);
mR_QP.solveTransposInPlace(vTmp3);
vTmp3.permutOut(vTmp,viPermut_QP);
rlambda=vTmp;
ncr=0;
for (i=0; i<nc; i++)
if (lambda[i]!=0.0)
{
lambda[i]=rlambda[ncr];
ncr++;
}
} // end of test on ncr==0
// find the most violated constraint j among non-active Linear constraints:
j=-1;
if (nc>0)
{
k=-1; violationMax=-INF; violationMax2=-INF;
for (i=0; i<nc; i++)
{
if (lambda[i]<=0.0) // test to see if this constraint is not active
{
ax=mA.scalarProduct(ii[i],vYB);
dviolation=b[ii[i]]-ax;
if (llambda[i]==0.0)
{
// the constraint was not active this round
// thus, it can enter the competition for the next
// active constraint
if (dviolation>maxb)
{
// the constraint should be activated
if (dviolation>violationMax2)
{ k=i; violationMax2=dviolation; }
al=mA.scalarProduct(ii[i],vLast)-ax;
if (al>0.0) // test to see if we are going closer
{
dviolation/=al;
if (dviolation>violationMax )
{ j=i; violationMax =dviolation; }
}
}
} else
{
lb[i]--;
if (feasible)
{
if (lb[i]==0)
{
vLambda.copyFrom(vLastLambda_QP);
if (lastJ>=0) lambda[lastJ]=0.0;
QPReconstructLambda(vLambda,vLambdaScale);
vP.copyFrom(vYB);
return;
}
} else
{
if (lb[i]==0)
{
if (info) *info=1;
QPReconstructLambda(vLambda,vLambdaScale);
vP.zero();
return;
}
}
finished=0; // this constraint was wrongly activated.
lambda[i]=0.0;
}
}
}
// !!! the order the tests is important here !!!
if ((j==-1)&&(!feasible))
{
feasible=1;
for (i=0; i<nc; i++) if (llambda[i]!=0.0) lb[i]=2; else lb[i]=1;
}
if (j==-1) { j=k; violationMax=violationMax2; } // change j to k after feasible is set
if (ncr==mmin(dim,nc)) {
if (feasible) { // feasible must have been checked before
QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vYB); return;
} else
{
if (info) *info=1;
QPReconstructLambda(vLambda,vLambdaScale); vP.zero(); return;
}
}
// activation of constraint only if ncr<mmin(dim,nc)
if (j>=0) { lambda[j]=1e-5; finished=0; } // we need to activate a new constraint
// else if (ncr==dim){
// QPReconstructLambda(vLambda); vP.copyFrom(vYB); return; }
}
// to prevent rounding error
if (j==lastJ) {
// if (0) {
QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vYB); return; }
lastJ=j;
vLast.copyFrom(vYB);
}
QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vYB); return;
}
