Anonymous

int  cg_filtered(lfVector *ldV, fmatrix3x3 *lA, lfVector *lB)
{
	// Solves for unknown X in equation AX=B
	unsigned int conjgrad_loopcount=0, conjgrad_looplimit=100;
	float conjgrad_epsilon=0.01f, conjgrad_lasterror=0;
	lfVector *q, *d, *tmp, *r; 
	q = create_lfvector(lA[0].vcount);
	d = create_lfvector(lA[0].vcount);
	tmp = create_lfvector(lA[0].vcount);
	r = create_lfvector(lA[0].vcount);
	
	// r = B - Mul(tmp,A,X);    // just use B if X known to be zero
	cp_lfvector(r, lB);
	
	filter(r,S);
	
	cp_lfvector(d, r);
	
	float s = dot_lfvector(r,r);
	float starget = s * sqrt(conjgrad_epsilon);
	
	while( s>starget && conjgrad_loopcount++ < conjgrad_looplimit)
	{	
		// Mul(q,A,d); // q = A*d;
		mul_bfmatrix_lfvector(q, lA, d);
		
		filter(q,S);
		
		float a = s/dot_lfvector(d,q);
		
		// X = X + d*a;
		add_lfvector_lfvectorS(ldV, ldV, d, a);
				
		// r = r - q*a;
		sub_lfvector_lfvectorS(r, r, q, a);
			
		float s_prev = s;
		s = dot_lfvector(r,r);
				
		//d = r+d*(s/s_prev);
		add_lfvector_lfvectorS(d, r, d, (s/s_prev));
		
		filter(d,S);
	}
	conjgrad_lasterror = s;
	
	del_lfvector(q);
	del_lfvector(d);
	del_lfvector(tmp);
	del_lfvector(r);
	
	return conjgrad_loopcount<conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
}