//SGOBBCalculator seems weird, because it wont store points, but use the points twice outside
//the usuage: Reset -> Step1_AddPoint -> Step1_CalculatePricipalComponents -> check m_bSucc1, if it's false, exit
// ->Step2_AddPoint -> Step2_CalculateOBB_2 -> GetResult
class SGOBBCalculator
{
public:
	SGOBBCalculator();
	~SGOBBCalculator();
	void Reset();
	void Step1_AddPoint(const gp_Pnt& pnt);
	void Step1_CalculatePricipalComponents();
	void Step2_AddPoint(const gp_Pnt& pnt);
	void Step2_CalculateOBB_2();
	bool IsFirstStepSucc() { return m_bSucc1;}
	bool GetResult(gp_Ax2& obbAx2, double& dx, double& dy, double& dz);
private:
	double m_xMin;
	double m_yMin;
	double m_zMin;
	double m_xMax;
	double m_yMax;
	double m_zMax;
	//we don't store points, because the model maybe million or larger
	bool m_bSucc1;
	GProp_PGProps m_Calculator;
	gp_Pnt m_center;
	gp_Vec m_PrincipalVec1;
	gp_Vec m_PrincipalVec2;
	gp_Vec m_PrincipalVec3;
	bool m_bSucc2;
	gp_Ax2 m_OBBAx2;
	double m_dx;
	double m_dy;
	double m_dz;
};
/**************************************** OBB calculator******************************************/
SGOBBCalculator::SGOBBCalculator()
{
	m_bSucc1=m_bSucc2=false;
	m_xMin=DZ_MAX;
	m_yMin=DZ_MAX;
	m_zMin=DZ_MAX;
	m_xMax=-DZ_MAX;
	m_yMax=-DZ_MAX;
	m_zMax=-DZ_MAX;
}

SGOBBCalculator::~SGOBBCalculator(){;}

void SGOBBCalculator::Reset()
{
	m_bSucc1=false;
	m_bSucc2=false;
	m_Calculator = GProp_PGProps();
	m_xMin=DZ_MAX;
	m_yMin=DZ_MAX;
	m_zMin=DZ_MAX;
	m_xMax=-DZ_MAX;
	m_yMax=-DZ_MAX;
	m_zMax=-DZ_MAX;
	m_dx = m_dy = m_dz = 0.0;
	m_OBBAx2 = gp_Ax2();
}
void SGOBBCalculator::Step1_AddPoint(const gp_Pnt& pnt)
{
	m_Calculator.AddPoint(pnt);
}
void SGOBBCalculator::Step1_CalculatePricipalComponents()
{
	double eigen[3];
	double radii[3];

	GProp_PrincipalProps p= m_Calculator.PrincipalProperties();

	m_center = m_Calculator.CentreOfMass();
	if(p.HasSymmetryPoint ())
	{
		m_PrincipalVec1 = gp_Vec(1,0,0);
		m_PrincipalVec2 = gp_Vec(0,1,0);
		m_PrincipalVec3 = gp_Vec(0,0,1);
		m_bSucc1 = true;
		return;
	}
	else
	{
		p.RadiusOfGyration(radii[0], radii[1], radii[2]);
		p.Moments (eigen[0],eigen[1],eigen[2]);
		m_PrincipalVec1 = p.FirstAxisOfInertia();
		m_PrincipalVec2 = p.SecondAxisOfInertia();
		m_PrincipalVec3 = p.ThirdAxisOfInertia();

		double t = m_PrincipalVec2.Crossed(m_PrincipalVec3).Dot(m_PrincipalVec1);
		if(t<0.0)
		{
			std::swap(m_PrincipalVec2, m_PrincipalVec3);
			std::swap(eigen[1], eigen[2]);
			std::swap(radii[1], radii[2]);
		}
		m_bSucc1 = true;
	}
}
void SGOBBCalculator::Step2_AddPoint(const gp_Pnt& pnt)
{
	gp_Vec vec;
	double x, y, z;
	vec = pnt.XYZ()-m_center.XYZ();
	x = vec.Dot(m_PrincipalVec1);
	y = vec.Dot(m_PrincipalVec2);
	z = vec.Dot(m_PrincipalVec3);
	if(m_xMin>x)
		m_xMin = x;
	if(m_xMax<x)
		m_xMax = x;
	if(m_yMin>y)
		m_yMin = y;
	if(m_yMax<y)
		m_yMax = y;
	if(m_zMin>z)
		m_zMin = z;
	if(m_zMax<z)
		m_zMax = z;
}
void SGOBBCalculator::Step2_CalculateOBB_2()
{
	double sizes[3];
	sizes[0] = m_xMax-m_xMin;
	sizes[1] = m_yMax-m_yMin;
	sizes[2] = m_zMax-m_zMin;
	double maxSize = 0.0;
	LS_INTEGER id, i;
	for(i=0; i<3; ++i)
	{
		if(sizes[i]>maxSize)
		{
			maxSize = sizes[i];
			id = i;
		}
	}
	m_center=m_center.XYZ()+m_xMin*m_PrincipalVec1.XYZ()+m_yMin*m_PrincipalVec2.XYZ()+m_zMin*m_PrincipalVec3.XYZ();
	m_dx = (m_xMax-m_xMin);
	m_dy = (m_yMax-m_yMin);
	m_dz = (m_zMax-m_zMin);
	if(id==0)
		m_OBBAx2 = gp_Ax2(m_center, m_PrincipalVec1, m_PrincipalVec2);
	else if(id==1)
		m_OBBAx2 = gp_Ax2(m_center, m_PrincipalVec2, m_PrincipalVec3);
	else
		m_OBBAx2 = gp_Ax2(m_center, m_PrincipalVec3, m_PrincipalVec1);
	if(id==0)
	{
		std::swap(m_dx, m_dz);
		std::swap(m_dz, m_dy);
	}
	else if(id==1)
	{
		std::swap(m_dx, m_dz);
		std::swap(m_dz, m_dy);
	}
	m_bSucc2 = true;
	return;
//	m_AABBTrsf.SetDisplacement(m_OBBAx2, gp_Ax2());
//	m_AABB.Add(gp_Pnt(0,0,0));
//	m_AABB.Add(gp_Pnt(m_OBBDX,m_OBBDY,m_OBBDZ));
}
bool SGOBBCalculator::GetResult(gp_Ax2& obbAx2, double& dx, double& dy, double& dz)
{
	if(!m_bSucc2)
		return false;
	obbAx2 = m_OBBAx2;
	dx = m_dx;
	dy = m_dy;
	dz = m_dz;
	return true;
}