/********************Public*Routine****************************************\
* CPlane
*
\**************************************************************************/
CHall::CPlane::CPlane(
IDirect3DDevice9* pDevice,
TCHAR *achName,
LPCTSTR lpResourceName,
float fU,
float fV,
D3DXMATRIX& M,
float a,
HRESULT *phr)
: m_du( 0.f)
, m_fU( fU )
, m_fV( fV )
, m_fSpeed( 0.f )
, m_pTexture( NULL )
{
HRESULT hr = S_OK;
TCHAR achTexturePath[MAX_PATH];
D3DXVECTOR3 vLB( M(0,0), M(1,0), M(2,0));
D3DXVECTOR3 vLT( M(0,1), M(1,1), M(2,1));
D3DXVECTOR3 vRT( M(0,2), M(1,2), M(2,2));
D3DXVECTOR3 vRB( M(0,3), M(1,3), M(2,3));
D3DXVECTOR3 V;
try
{
if( !achName )
throw E_POINTER;
hr = FindMediaFile( achTexturePath,
sizeof(TCHAR)*MAX_PATH,
achName,
lpResourceName,
RT_BITMAP);
if( FAILED(hr))
{
TCHAR achMsg[MAX_PATH];
_stprintf( achMsg, TEXT("Cannot find media file '%s'. ")
TEXT("Make sure you have valid installation of DirectX SDK"),
achName);
::MessageBox( NULL, achMsg, TEXT("Error"), MB_OK);
if( phr )
{
*phr = hr;
return;
}
}
if( !pDevice )
throw E_POINTER;
if( a<0.01f || a>0.5f )
throw E_INVALIDARG;
D3DXVECTOR3 vecDiff = vLB - vLT;
if( D3DXVec3LengthSq( &vecDiff )< 0.001f )
throw E_INVALIDARG;
if( m_fU < 0.001f || m_fV < 0.001f )
throw E_INVALIDARG;
CHECK_HR(
hr = DXUtil_FindMediaFileCb( achTexturePath, sizeof(TCHAR)*MAX_PATH, achName ),
DbgMsg("CPlane::CPlane: cannot find bitmap file %s in SDK media folder", achTexturePath));
// load texture
ASSERT( g_pEnvFormat );
CHECK_HR(
hr = D3DUtil_CreateTexture( pDevice, achTexturePath, &m_pTexture, *g_pEnvFormat ),
DbgMsg("CPlane::CPlane: failed to create the texture, hr = 0x%08x", hr));
// initialize geometry
// POSITION
// corners
memcpy( &(m_V[0].Pos), &vLB, sizeof(D3DVECTOR));
memcpy( &(m_V[1].Pos), &vLT, sizeof(D3DVECTOR));
memcpy( &(m_V[2].Pos), &vRB, sizeof(D3DVECTOR));
memcpy( &(m_V[3].Pos), &vRT, sizeof(D3DVECTOR));
// TEXTURE COORD
m_V[0].tu = 0.f; m_V[0].tv = fV;
m_V[1].tu = 0.f; m_V[1].tv = 0.f;
m_V[2].tu = fU; m_V[2].tv = fV;
m_V[3].tu = fU; m_V[3].tv = 0.f;
// corners are transparent, middle vertices are opaque
m_V[0].color = D3DCOLOR_RGBA( 0xFF, 0xFF, 0xFF, 0xFF);
m_V[1].color = D3DCOLOR_RGBA( 0xFF, 0xFF, 0xFF, 0xFF);
m_V[2].color = D3DCOLOR_RGBA( 0xFF, 0xFF, 0xFF, 0xFF);
m_V[3].color = D3DCOLOR_RGBA( 0xFF, 0xFF, 0xFF, 0xFF);
}
catch( HRESULT hr1 )
{
RELEASE( m_pTexture );
hr = hr1;
}
if( phr )
{
*phr = hr;
}
}
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;
}
