bullet3/examples/Experiments/ImplicitCloth/stan/vec3n.cpp

//
// Big Vector and Sparse Matrix Classes
//

#include <float.h>

#include "vec3n.h"

float conjgrad_lasterror;
float conjgrad_epsilon = 0.1f;
int conjgrad_loopcount;
int conjgrad_looplimit = 100;

/*EXPORTVAR(conjgrad_lasterror);
EXPORTVAR(conjgrad_epsilon  );
EXPORTVAR(conjgrad_loopcount);
EXPORTVAR(conjgrad_looplimit);
*/

int ConjGradient(float3N &X, float3Nx3N &A, float3N &B)
{
	// Solves for unknown X in equation AX=B
	conjgrad_loopcount = 0;
	int n = B.count;
	float3N q(n), d(n), tmp(n), r(n);
	r = B - Mul(tmp, A, X);  // just use B if X known to be zero
	d = r;
	float s = dot(r, r);
	float starget = s * squared(conjgrad_epsilon);
	while (s > starget && conjgrad_loopcount++ < conjgrad_looplimit)
	{
		Mul(q, A, d);  // q = A*d;
		float a = s / dot(d, q);
		X = X + d * a;
		if (conjgrad_loopcount % 50 == 0)
		{
			r = B - Mul(tmp, A, X);
		}
		else
		{
			r = r - q * a;
		}
		float s_prev = s;
		s = dot(r, r);
		d = r + d * (s / s_prev);
	}
	conjgrad_lasterror = s;
	return conjgrad_loopcount < conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
}

int ConjGradientMod(float3N &X, float3Nx3N &A, float3N &B, int3 hack)
{
	// obsolete!!!
	// Solves for unknown X in equation AX=B
	conjgrad_loopcount = 0;
	int n = B.count;
	float3N q(n), d(n), tmp(n), r(n);
	r = B - Mul(tmp, A, X);  // just use B if X known to be zero
	r[hack[0]] = r[hack[1]] = r[hack[2]] = float3(0, 0, 0);
	d = r;
	float s = dot(r, r);
	float starget = s * squared(conjgrad_epsilon);
	while (s > starget && conjgrad_loopcount++ < conjgrad_looplimit)
	{
		Mul(q, A, d);  // q = A*d;
		q[hack[0]] = q[hack[1]] = q[hack[2]] = float3(0, 0, 0);
		float a = s / dot(d, q);
		X = X + d * a;
		if (conjgrad_loopcount % 50 == 0)
		{
			r = B - Mul(tmp, A, X);
			r[hack[0]] = r[hack[1]] = r[hack[2]] = float3(0, 0, 0);
		}
		else
		{
			r = r - q * a;
		}
		float s_prev = s;
		s = dot(r, r);
		d = r + d * (s / s_prev);
		d[hack[0]] = d[hack[1]] = d[hack[2]] = float3(0, 0, 0);
	}
	conjgrad_lasterror = s;
	return conjgrad_loopcount < conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
}

static inline void filter(float3N &V, const float3Nx3N &S)
{
	for (int i = 0; i < S.blocks.count; i++)
	{
		V[S.blocks[i].r] = V[S.blocks[i].r] * S.blocks[i].m;
	}
}

int ConjGradientFiltered(float3N &X, const float3Nx3N &A, const float3N &B, const float3Nx3N &S)
{
	// Solves for unknown X in equation AX=B
	conjgrad_loopcount = 0;
	int n = B.count;
	float3N q(n), d(n), tmp(n), r(n);
	r = B - Mul(tmp, A, X);  // just use B if X known to be zero
	filter(r, S);
	d = r;
	float s = dot(r, r);
	float starget = s * squared(conjgrad_epsilon);
	while (s > starget && conjgrad_loopcount++ < conjgrad_looplimit)
	{
		Mul(q, A, d);  // q = A*d;
		filter(q, S);
		float a = s / dot(d, q);
		X = X + d * a;
		if (conjgrad_loopcount % 50 == 0)
		{
			r = B - Mul(tmp, A, X);
			filter(r, S);
		}
		else
		{
			r = r - q * a;
		}
		float s_prev = s;
		s = dot(r, r);
		d = r + d * (s / s_prev);
		filter(d, S);
	}
	conjgrad_lasterror = s;
	return conjgrad_loopcount < conjgrad_looplimit;  // true means we reached desired accuracy in given time - ie stable
}

// test big vector math library:
static void testfloat3N()
{
	float3N a(2), b(2), c(2);
	a[0] = float3(1, 2, 3);
	a[1] = float3(4, 5, 6);
	b[0] = float3(10, 20, 30);
	b[1] = float3(40, 50, 60);
	//      c =  a+b+b * 10.0f;
	//      float d = dot(a+b,-b);
	int k;
	k = 0;
}
class dotest
{
public:
	dotest() { testfloat3N(); }
} do_test_at_program_startup;