//todo put convergence into one function
structmatrix conjugategradient(structmatrix *A,structmatrix *b){
const unsigned int MAXITER=100;
const MATDOUBLE TOL=1e-6;
MATDOUBLE *xp=calloc(A->nrows,sizeof(MATDOUBLE));//init guess 0s
MATDOUBLE *x= malloc(A->nrows*sizeof(MATDOUBLE));//
structmatrix XP=creatematrix(xp
,A->nrows,1//solns in col vector
,endt(MATDOUBLE),rowmjr);
structmatrix X=creatematrix(x
,A->nrows,1//solns in col vector
,endt(MATDOUBLE),rowmjr);
structmatrix rP=copymatrix(b); //previous error
structmatrix pP=copymatrix(b); //CG vec
structmatrix r=creatematrix(malloc(sizeof(MATDOUBLE)*rP.nrows)
,rP.nrows,1
,endt(MATDOUBLE),rowmjr);
structmatrix p=creatematrix(malloc(sizeof(MATDOUBLE)*pP.nrows)
,pP.nrows,1
,endt(MATDOUBLE),rowmjr);
fpidx idxA= getidxingfunc( A);
fpidx idxb= getidxingfunc( b);
fpidx idxX= getidxingfunc(&X);
fpidx idxXP=getidxingfunc(&XP);
fpidx idxrP=getidxingfunc(&rP);
fpidx idxpP=getidxingfunc(&pP);
fpidx idxr =getidxingfunc(&r );
fpidx idxp =getidxingfunc(&p );
#define Xv(ri,ci) *(double*) idxX( &ri,&ci,&X)
#define XPv(ri,ci) *(double*) idxXP(&ri,&ci,&XP)
#define Av(ri,ci) *(double*) idxA( &ri,&ci, A)
#define bv(ri,ci) *(double*) idxb( &ri,&ci, b)
#define rPv(ri,ci) *(double*) idxrP( &ri,&ci, &rP)
#define pPv(ri,ci) *(double*) idxb( &ri,&ci, &pP)
#define rv(ri,ci) *(double*) idxA( &ri,&ci, &r)
#define pv(ri,ci) *(double*) idxb( &ri,&ci, &p)
//result definition
#define ApPv(ri,ci) *(double*) idxApP(&ri,&ci,&ApP)
unsigned int zero=0;
unsigned int k=0;
while(k<MAXITER){
double alpha, beta;//scalars to compute in each interatin
structmatrix rPT=(vecT(&rP)),pPT=vecT(&pP);
structmatrix rPTrP=matrixmatrixmuldbl(&rPT,&rP);//1x1
structmatrix ApP=matrixmatrixmuldbl(A,&pP);// vec nrows
fpidx idxApP =getidxingfunc(&ApP);
double alphan=*(double*) rPTrP.data;
double alphad=*(double*) (matrixmatrixmuldbl(&pPT,&ApP)).data;
alpha=alphan/alphad;//alpha new
unsigned int ri;
for(ri=0;ri<XP.nrows;ri++){
Xv(ri,zero)=XPv(ri,zero)+alpha*pPv(ri,zero); //new x approx soln
rv(ri,zero)=rPv(ri,zero)-alpha*ApPv(ri,zero);//new r error
}
structmatrix rT=vecT(&r);
structmatrix rTr=matrixmatrixmuldbl(&rT,&r);
beta=(*(double*) rTr.data)/(*(double*) rPTrP.data); //scalar/scalar
for(ri=0;ri<p.nrows;ri++){
pv(ri,zero)=rv(ri,zero)+beta*pPv(ri,zero);
}
double normofr=0;
for(ri=0;ri<X.nrows;ri++){normofr+=pow(rv(ri,zero),2);}
normofr=pow(normofr,.5);
if(normofr<TOL){printf("converged\n in %d iterations\n",k+1);return X;}
//previous=current
for(ri=0;ri<X.nrows;ri++){
XPv(ri,zero)=Xv(ri,zero);
pPv(ri,zero)=pv(ri,zero);
rPv(ri,zero)=rv(ri,zero);
}
/* todo does not work if i add these free statements
does the memory get freed with each loop? i thought it would not!*/
/*
free(rPT.data);
free(rPTrP.data);
free(ApP.data);
free(rT.data);
free(rTr.data);
*/
k++;}
free(XP.data);
free(rP.data);
free(pP.data);
free(r.data);
free(p.data);
return X;
#undef Xv
#undef XPv
#undef Av
#undef bv
#undef rPv
#undef pPv
#undef rv
#undef pv
#undef ApPv
}
//-----------------------------------------------------------------------------
// Name: FrameMove
// Desc:
//-----------------------------------------------------------------------------
HRESULT CMyD3DApplication::FrameMove()
{
HRESULT hr;
//
// Process keyboard input
//
D3DXVECTOR3 vecT(0.0f, 0.0f, 0.0f);
D3DXVECTOR3 vecR(0.0f, 0.0f, 0.0f);
if(m_bKey[VK_NUMPAD1] || m_bKey[VK_LEFT]) vecT.x -= 1.0f; // Slide Left
if(m_bKey[VK_NUMPAD3] || m_bKey[VK_RIGHT]) vecT.x += 1.0f; // Slide Right
if(m_bKey[VK_DOWN]) vecT.y -= 1.0f; // Slide Down
if(m_bKey[VK_UP]) vecT.y += 1.0f; // Slide Up
if(m_bKey['W']) vecT.z -= 2.0f; // Move Forward
if(m_bKey['S']) vecT.z += 2.0f; // Move Backward
if(m_bKey['A'] || m_bKey[VK_NUMPAD8]) vecR.x -= 1.0f; // Pitch Down
if(m_bKey['Z'] || m_bKey[VK_NUMPAD2]) vecR.x += 1.0f; // Pitch Up
if(m_bKey['E'] || m_bKey[VK_NUMPAD6]) vecR.y -= 1.0f; // Turn Right
if(m_bKey['Q'] || m_bKey[VK_NUMPAD4]) vecR.y += 1.0f; // Turn Left
if(m_bKey[VK_NUMPAD9]) vecR.z -= 2.0f; // Roll CW
if(m_bKey[VK_NUMPAD7]) vecR.z += 2.0f; // Roll CCW
m_vecVelocity = m_vecVelocity * 0.9f + vecT * 0.1f;
m_vecAngularVelocity = m_vecAngularVelocity * 0.9f + vecR * 0.1f;
//
// Update position and view matricies
//
D3DXMATRIX matT, matR;
D3DXQUATERNION qR;
vecT = m_vecVelocity * m_fElapsedTime * m_fSpeed;
vecR = m_vecAngularVelocity * m_fElapsedTime * m_fAngularSpeed;
D3DXMatrixTranslation(&matT, vecT.x, vecT.y, vecT.z);
D3DXMatrixMultiply(&m_matPosition, &matT, &m_matPosition);
D3DXQuaternionRotationYawPitchRoll(&qR, vecR.y, vecR.x, vecR.z);
D3DXMatrixRotationQuaternion(&matR, &qR);
D3DXMatrixMultiply(&m_matPosition, &matR, &m_matPosition);
D3DXMatrixInverse(&m_matView, NULL, &m_matPosition);
//
// Update simulation
//
if(!m_bPause && m_bDrawWater)
{
BOOL bCaustics = m_bDrawCaustics && m_pEffect->IsParameterUsed("tCAU");
D3DXVECTOR3 vecPos(m_matPosition._41, m_matPosition._42, m_matPosition._43);
D3DXVECTOR3 vecLight(0.0f, 1.0f, 0.0f);
m_Water.Update(vecPos, vecLight, bCaustics);
m_fTime += m_fSecsPerFrame;
if(bCaustics)
{
if(SUCCEEDED(m_pRenderToSurface->BeginScene(m_pCausticSurf, NULL)))
{
D3DXMATRIX matProj;
D3DXMATRIX matView;
D3DXMatrixOrthoRH(&matProj, 63.0f, 63.0f, 1.0f, 100.0f);
D3DXMatrixRotationX(&matView, 0.5f * D3DX_PI);
matView._43 = -50.0f;
m_pd3dDevice->SetTransform(D3DTS_PROJECTION, &matProj);
m_pd3dDevice->SetTransform(D3DTS_VIEW, &matView);
m_pd3dDevice->Clear(0, NULL, D3DCLEAR_TARGET, 0xff000000, 0.0f, 0);
m_pd3dDevice->SetRenderState(D3DRS_ZENABLE, FALSE);
m_pd3dDevice->SetRenderState(D3DRS_CULLMODE, D3DCULL_NONE);
m_pd3dDevice->SetRenderState(D3DRS_ALPHABLENDENABLE, TRUE);
m_Water.DrawCaustics();
m_pd3dDevice->SetRenderState(D3DRS_ALPHABLENDENABLE, FALSE);
m_pd3dDevice->SetRenderState(D3DRS_CULLMODE, D3DCULL_CW);
m_pd3dDevice->SetRenderState(D3DRS_ZENABLE, TRUE);
m_pRenderToSurface->EndScene();
}
else
{
m_bDrawCaustics = FALSE;
m_pEffect->SetTexture("tCAU", NULL);
if(FAILED(hr = GetNextTechnique(0, FALSE)))
return hr;
}
}
}
return S_OK;
}
