Obb fitting

void Thysics::Obb::calculateObb( const float *vertexData, unsigned vDataCount ){	//Calc box center	m_center = Vector3f(vertexData[0],vertexData[1],vertexData[2]);	for (unsigned i = 3; i < vDataCount - 2; i += 3)	{		m_center += Vector3f(vertexData[i+0],vertexData[i+1],vertexData[i+2]);	}	float invNumPoints = 1.0f / (vDataCount / 3);	m_center *= invNumPoints;		//Calc eigen vectors/values		Thysics::Matrix4f m, v;	covarianceMatrixOfPts(m, vertexData, vDataCount);	jacobi(m, v);	for (unsigned i = 0; i < 3; ++i)	{		m_extents = m;		for (unsigned j = 0; j < 3; ++j)		{			m_axes[j] = v[j];		}	}	// Let C be the box center and let U0, U1, and U2 be the box axes.  Each	// input point is of the form X = C + y0*U0 + y1*U1 + y2*U2.  The	// following code computes min(y0), max(y0), min(y1), max(y1), min(y2),	// and max(y2).  The box center is then adjusted to be	//   C' = C + 0.5*(min(y0)+max(y0))*U0 + 0.5*(min(y1)+max(y1))*U1 +	//        0.5*(min(y2)+max(y2))*U2	Vector3f diff = Vector3f(vertexData[0],vertexData[1],vertexData[2]) - m_center;	Vector3f pmin = Vector3f(diff.Dot(m_axes[0]), diff.Dot(m_axes[1]), diff.Dot(m_axes[2]));	Vector3f pmax = pmin;	for (unsigned i = 3; i < vDataCount - 2; i += 3)	{		diff = Vector3f(vertexData[i+0],vertexData[i+1],vertexData[i+2]) - m_center;		for (unsigned j = 0; j < 3; ++j)		{			float dot = diff.Dot(m_axes[j]);			if(dot < pmin[j])			{				pmin[j] = dot;			}			else if (dot > pmax[j])			{				pmax[j] = dot;			}		}	}	m_center += (0.5f * (pmin[0] + pmax[0])) * m_axes[0] +		(0.5f * (pmin[1] + pmax[1])) * m_axes[1] +		(0.5f * (pmin[2] + pmax[2])) * m_axes[2];	m_extents[0] = 0.5f * (pmax[0] - pmin[0]);	m_extents[1] = 0.5f * (pmax[1] - pmin[1]);	m_extents[2] = 0.5f * (pmax[2] - pmin[2]);}

void Thysics::covarianceMatrixOfPts(Thysics::Matrix4f &cov, const float *vertexData, unsigned vDataCount){	cov.Identity();	int numPts = vDataCount / 3;	float oon = 1.0f / numPts;	Thysics::Vector3f c = Thysics::Vector3f(0.0f, 0.0f, 0.0f);	float e00, e11, e22, e01, e02, e12;	//Compute center of mass	for (unsigned i = 0; i < vDataCount;)	{		c.x += vertexData[i++];		c.y += vertexData[i++];		c.z += vertexData[i++];	}	c *= oon;	//Compute covariance elements	e00 = e11 = e22 = e01 = e02 = e12 = 0.0f;	Thysics::Vector3f curr;	for (unsigned i = 0; i < vDataCount;)	{		curr.x = vertexData[i++];		curr.y = vertexData[i++];		curr.z = vertexData[i++];		//translate points so center of mass is at origin		Thysics::Vector3f p = curr - c;		e00 += p.x * p.x;		e11 += p.y * p.y;		e22 += p.z * p.z;		e01 += p.x * p.y;		e02 += p.x * p.z;		e12 += p.y * p.z;	}	//fill in covariance matrix	cov[0][0] = e00 * oon;	cov[1][1] = e11 * oon;	cov[2][2] = e22 * oon;	cov[0][1] = cov[1][0] = e01 * oon;	cov[0][2] = cov[2][0] = e02 * oon;	cov[1][2] = cov[2][1] = e12 * oon;}

void Thysics::symSchur2(Thysics::Matrix4f &a, int p, int q, float &c, float &s){	if (abs(a[q]) > MATHS_EPSILON3)	{		float r = (a[q][q] - a) / (2.0f * a[q]);		float t;		if (r >= 0.0f)			t = 1.0f / (r + sqrt(1.0f + r*r));		else			t = -1.0f / (-r + sqrt(1.0f + r*r));		c = 1.0f / sqrt(1.0f + t*t);		s = t * c;	}	else	{		c = 1.0f;		s = 0.0f;	}}void Thysics::jacobi(Thysics::Matrix4f &a, Thysics::Matrix4f &v){	int i,j,n,p,q;	float prevoff, c, s;	Thysics::Matrix4f J;	v.Identity();	const int MAX_ITERATIONS = 50;	for (n = 0; n < MAX_ITERATIONS; ++n)	{		p = 0; q = 1;		for (i = 0; i < 3; ++i)		{			for (j = 0; j < 3; ++j)			{				if(i == j) continue;				if (abs(a[j]) > abs(a[q]))				{					p = i;					q = j;				}			}		}		symSchur2(a, p ,q, c, s);		J.Identity();		J = c; J[q] = s;		J[q] = -s; J[q][q] = c;		v = v*J;		a = (J.Transpose() * a) * J;		float off = 0.0f;		for (i = 0; i < 3; ++i)		{			for (j = 0; j < 3; ++j)			{				if (i == j) continue;				off += a[j] * a[j];			}		}		if (n > 2 && off >= prevoff)			return;		prevoff = off;	}}