mymath.cpp

liu7788414

#endif
	if(abs(m.e33)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
		e33 = (m.e33>>PHT_EXT)*s;
	else
		e33 = m.e33*s>>PHT_EXT;
	return	Matrix3x3( e11,e12,e13,
					   e21,e22,e23,
					   e31,e32,e33 );
}

Vector operator*(Matrix3x3 m, Vector u)
{
#ifdef USE_FLOAT
	return Vector(	m.e11*u.x + m.e12*u.y + m.e13*u.z,
					m.e21*u.x + m.e22*u.y + m.e23*u.z,
					m.e31*u.x + m.e32*u.y + m.e33*u.z);
#else
	PhType x=0,y=0,z=0;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e11)>=DANGER_VALUE&&abs(u.x)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e11)>=CRIT_VALUE&&abs(u.x)>=CRIT_VALUE)
		x += (m.e11>>PHT_EXT)*u.x;
	else
		x += m.e11*u.x>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e12)>=DANGER_VALUE&&abs(u.y)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e12)>=CRIT_VALUE&&abs(u.y)>=CRIT_VALUE)
		x += (m.e12>>PHT_EXT)*u.y;
	else
		x += m.e12*u.y>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e13)>=DANGER_VALUE&&abs(u.z)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e13)>=CRIT_VALUE&&abs(u.z)>=CRIT_VALUE)
		x += (m.e13>>PHT_EXT)*u.z;
	else
		x += m.e13*u.z>>PHT_EXT;
	x >>= MULT_BTW_TRI_PHT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e21)>=DANGER_VALUE&&abs(u.x)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e21)>=CRIT_VALUE&&abs(u.x)>=CRIT_VALUE)
		y += (m.e21>>PHT_EXT)*u.x;
	else
		y += m.e21*u.x>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e22)>=DANGER_VALUE&&abs(u.y)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e22)>=CRIT_VALUE&&abs(u.y)>=CRIT_VALUE)
		y += (m.e22>>PHT_EXT)*u.y;
	else
		y += m.e22*u.y>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e23)>=DANGER_VALUE&&abs(u.z)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e23)>=CRIT_VALUE&&abs(u.z)>=CRIT_VALUE)
		y += (m.e23>>PHT_EXT)*u.z;
	else
		y += m.e23*u.z>>PHT_EXT;
	y >>= MULT_BTW_TRI_PHT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e31)>=DANGER_VALUE&&abs(u.x)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e31)>=CRIT_VALUE&&abs(u.x)>=CRIT_VALUE)
		z += (m.e31>>PHT_EXT)*u.x;
	else
		z += m.e31*u.x>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e32)>=DANGER_VALUE&&abs(u.y)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e32)>=CRIT_VALUE&&abs(u.y)>=CRIT_VALUE)
		z += (m.e32>>PHT_EXT)*u.y;
	else
		z += m.e32*u.y>>PHT_EXT;
	if(abs(m.e33)>=CRIT_VALUE&&abs(u.z)>=CRIT_VALUE)
		z += (m.e33>>PHT_EXT)*u.z;
	else
		z += m.e33*u.z>>PHT_EXT;
	z >>= MULT_BTW_TRI_PHT;
	return Vector(x,y,z);
#endif
}

Vector operator*(Vector u, Matrix3x3 m)
{
#ifdef USE_FLOAT
	return Vector(	u.x*m.e11 + u.y*m.e21 + u.z*m.e31,
					u.x*m.e12 + u.y*m.e22 + u.z*m.e32,
					u.x*m.e13 + u.y*m.e23 + u.z*m.e33);
#else
	PhType x=0,y=0,z=0;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e11)>=DANGER_VALUE&&abs(u.x)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e11)>=CRIT_VALUE&&abs(u.x)>=CRIT_VALUE)
		x += (m.e11>>PHT_EXT)*u.x;
	else
		x += m.e11*u.x>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e21)>=DANGER_VALUE&&abs(u.y)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e21)>=CRIT_VALUE&&abs(u.y)>=CRIT_VALUE)
		x += (m.e21>>PHT_EXT)*u.y;
	else
		x += m.e21*u.y>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e31)>=DANGER_VALUE&&abs(u.z)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e31)>=CRIT_VALUE&&abs(u.z)>=CRIT_VALUE)
		x += (m.e31>>PHT_EXT)*u.z;
	else
		x += m.e31*u.z>>PHT_EXT;
	x >>= MULT_BTW_TRI_PHT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e12)>=DANGER_VALUE&&abs(u.x)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e12)>=CRIT_VALUE&&abs(u.x)>=CRIT_VALUE)
		y += (m.e12>>PHT_EXT)*u.x;
	else
		y += m.e12*u.x>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e22)>=DANGER_VALUE&&abs(u.y)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e22)>=CRIT_VALUE&&abs(u.y)>=CRIT_VALUE)
		y += (m.e22>>PHT_EXT)*u.y;
	else
		y += m.e22*u.y>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e32)>=DANGER_VALUE&&abs(u.z)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e32)>=CRIT_VALUE&&abs(u.z)>=CRIT_VALUE)
		y += (m.e32>>PHT_EXT)*u.z;
	else
		y += m.e32*u.z>>PHT_EXT;
	y >>= MULT_BTW_TRI_PHT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e13)>=DANGER_VALUE&&abs(u.x)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e13)>=CRIT_VALUE&&abs(u.x)>=CRIT_VALUE)
		z += (m.e13>>PHT_EXT)*u.x;
	else
		z += m.e13*u.x>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e23)>=DANGER_VALUE&&abs(u.y)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e23)>=CRIT_VALUE&&abs(u.y)>=CRIT_VALUE)
		z += (m.e23>>PHT_EXT)*u.y;
	else
		z += m.e23*u.y>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(m.e33)>=DANGER_VALUE&&abs(u.z)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(m.e33)>=CRIT_VALUE&&abs(u.z)>=CRIT_VALUE)
		z += (m.e33>>PHT_EXT)*u.z;
	else
		z += m.e33*u.z>>PHT_EXT;
	z >>= MULT_BTW_TRI_PHT;
	return Vector(x,y,z);
#endif
}

//------------------------------------------------------------------------//
// Quaternion Class and Quaternion functions
//------------------------------------------------------------------------//


Quaternion::Quaternion(void)
{
	n = 0;
	x = 0;
	y =  0;
	z = 0;
}

Quaternion::Quaternion(TriValue e0, TriValue e1, TriValue e2, TriValue e3)
{
	n = e0;
	x = e1;
	y = e2;
	z = e3;
}

PhType	Quaternion::Magnitude(void)
{
#ifdef USE_FLOAT
	return sqrt(n*n + x*x + y*y + z*z);
#else
	PhType m[4];
	m[0]=(PhType)n; m[1]=(PhType)x; m[2]=(PhType)y; m[3]=(PhType)z;
	return calLength(m,4)>>MULT_BTW_TRI_PHT;
#endif
}


void Quaternion::Normalize(void){
	
#ifdef USE_FLOAT
	
	PhType l = sqrt(n*n+x*x+y*y+z*z);
	n /= l;
	x /= l;
	y /= l;
	z /= l;
	
#else
	
	PhType m;
	PhType mm[4];
	mm[0]=(PhType)n; mm[1]=(PhType)x; mm[2]=(PhType)y; mm[3]=(PhType)z;
	m = calLength(mm,4);//>>MULT_BTW_TRI_PHT;
	
	
	if(m==0)   m=1<<TRI_EXT;
	n = (n<<(TRI_EXT-4))/(m>>4);
	x = (x<<(TRI_EXT-4))/(m>>4);
	y = (y<<(TRI_EXT-4))/(m>>4);
	z = (z<<(TRI_EXT-4))/(m>>4);
	
#endif
	
	
}




Vector	Quaternion::GetVector(void)
{
	return Vector((PhType)x>>MULT_BTW_TRI_PHT, (PhType)y>>MULT_BTW_TRI_PHT, (PhType)z>>MULT_BTW_TRI_PHT);
}
/*
PhType	Quaternion::GetScalar(void)
{
	return n>>MULT_BTW_TRI_PHT;
}
*/
Quaternion	Quaternion::operator+=(Quaternion q)
{
	n += q.n;
	x += q.x;
	y += q.y;
	z += q.z;
	return *this;
}

Quaternion	Quaternion::operator-=(Quaternion q)
{
	n -= q.n;
	x -= q.x;
	y -= q.y;
	z -= q.z;
	return *this;
}

Quaternion Quaternion::operator*=(PhType s)
{
#ifdef DEBUG_OVERFLOW
	if(abs(n)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(n)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
		n = (n>>PHT_EXT)*s;
	else
		n = n*s>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(x)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(x)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
		x = (x>>PHT_EXT)*s;
	else
		x = x*s>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(y)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(y)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
		y = (y>>PHT_EXT)*s;
	else
		y = y*s>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(z)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(z)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
		z = (z>>PHT_EXT)*s;
	else
		z = z*s>>PHT_EXT;	
	return *this;
}

Quaternion Quaternion::operator/=(PhType s)
{
	n = n<<PHT_EXT;
	n /= s;
	x = x<<PHT_EXT;
	x /= s;
	y = y<<PHT_EXT;
	y /= s;
	z = z<<PHT_EXT;
	z /= s;
	return *this;
}

Quaternion operator+(Quaternion q1, Quaternion q2)
{
	return	Quaternion(	q1.n + q2.n,
							q1.x + q2.x,
							q1.y + q2.y,
							q1.z + q2.z);
}

Quaternion operator-(Quaternion q1, Quaternion q2)
{
	return	Quaternion(	q1.n - q2.n,
							q1.x - q2.x,
							q1.y - q2.y,
							q1.z - q2.z);
}

Quaternion operator*(Quaternion q1, Quaternion q2)
{
#ifdef USE_FLOAT
	return	Quaternion(	q1.n*q2.n - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
						q1.n*q2.x + q1.x*q2.n + q1.y*q2.z - q1.z*q2.y,
						q1.n*q2.y + q1.y*q2.n + q1.z*q2.x - q1.x*q2.z,
						q1.n*q2.z + q1.z*q2.n + q1.x*q2.y - q1.y*q2.x);
#else
	TriValue n=0,x=0,y=0,z=0;
	//q1.n*q2.n - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z
#ifdef DEBUG_OVERFLOW
	if(abs(q1.n)>=DANGER_VALUET&&abs(q2.n)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.n)>=CRIT_VALUE&&abs(q2.n)>=CRIT_VALUE)
		n += (q1.n>>TRI_EXT)*q2.n;
	else
		n += q1.n*q2.n>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(q1.x)>=DANGER_VALUET&&abs(q2.x)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.x)>=CRIT_VALUE&&abs(q2.x)>=CRIT_VALUE)
		n -= (q1.x>>TRI_EXT)*q2.x;
	else
		n -= q1.x*q2.x>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(q1.y)>=DANGER_VALUET&&abs(q2.y)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.y)>=CRIT_VALUE&&abs(q2.y)>=CRIT_VALUE)
		n -= (q1.y>>TRI_EXT)*q2.y;
	else
		n -= q1.y*q2.y>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(q1.z)>=DANGER_VALUET&&abs(q2.z)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.z)>=CRIT_VALUE&&abs(q2.z)>=CRIT_VALUE)
		n -= (q1.z>>TRI_EXT)*q2.z;
	else
		n -= q1.z*q2.z>>TRI_EXT;
	
	//q1.n*q2.x + q1.x*q2.n + q1.y*q2.z - q1.z*q2.y
#ifdef DEBUG_OVERFLOW
	if(abs(q1.n)>=DANGER_VALUET&&abs(q2.x)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.n)>=CRIT_VALUE&&abs(q2.x)>=CRIT_VALUE)
		x += (q1.n>>TRI_EXT)*q2.x;
	else
		x += q1.n*q2.x>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(q1.x)>=DANGER_VALUET&&abs(q2.n)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.x)>=CRIT_VALUE&&abs(q2.n)>=CRIT_VALUE)
		x += (q1.x>>TRI_EXT)*q2.n;
	else
		x += q1.x*q2.n>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(q1.y)>=DANGER_VALUET&&abs(q2.z)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.y)>=CRIT_VALUE&&abs(q2.z)>=CRIT_VALUE)
		x += (q1.y>>TRI_EXT)*q2.z;
	else
		x += q1.y*q2.z>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(q1.z)>=DANGER_VALUET&&abs(q2.y)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.z)>=CRIT_VALUE&&abs(q2.y)>=CRIT_VALUE)
		x -= (q1.z>>TRI_EXT)*q2.y;
	else
		x -= q1.z*q2.y>>TRI_EXT;

	//q1.n*q2.y + q1.y*q2.n + q1.z*q2.x - q1.x*q2.z
#ifdef DEBUG_OVERFLOW
	if(abs(q1.n)>=DANGER_VALUET&&abs(q2.y)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.n)>=CRIT_VALUE&&abs(q2.y)>=CRIT_VALUE)
		y += (q1.n>>TRI_EXT)*q2.y;
	else
		y += q1.n*q2.y>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(q1.y)>=DANGER_VALUET&&abs(q2.n)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.y)>=CRIT_VALUE&&abs(q2.n)>=CRIT_VALUE)
		y += (q1.y>>TRI_EXT)*q2.n;
	else
		y += q1.y*q2.n>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(q1.z)>=DANGER_VALUET&&abs(q2.x)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.z)>=CRIT_VALUE&&abs(q2.x)>=CRIT_VALUE)
		y += (q1.z>>TRI_EXT)*q2.x;
	else
		y += q1.z*q2.x>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(q1.x)>=DANGER_VALUET&&abs(q2.z)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.x)>=CRIT_VALUE&&abs(q2.z)>=CRIT_VALUE)
		y -= (q1.x>>TRI_EXT)*q2.z;
	else
		y -= q1.x*q2.z>>TRI_EXT;

	//q1.n*q2.z + q1.z*q2.n + q1.x*q2.y - q1.y*q2.x
#ifdef DEBUG_OVERFLOW
	if(abs(q1.n)>=DANGER_VALUET&&abs(q2.z)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.n)>=CRIT_VALUE&&abs(q2.z)>=CRIT_VALUE)
		z += (q1.n>>TRI_EXT)*q2.z;
	else
		z += q1.n*q2.z>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(q1.z)>=DANGER_VALUET&&abs(q2.n)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.z)>=CRIT_VALUE&&abs(q2.n)>=CRIT_VALUE)
		z += (q1.z>>TRI_EXT)*q2.n;
	else
		z += q1.z*q2.n>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(q1.x)>=DANGER_VALUET&&abs(q2.y)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.x)>=CRIT_VALUE&&abs(q2.y)>=CRIT_VALUE)
		z += (q1.x>>TRI_EXT)*q2.y;
	else
		z += q1.x*q2.y>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(q1.y)>=DANGER_VALUET&&abs(q2.x)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(q1.y)>=CRIT_VALUE&&abs(q2.x)>=CRIT_VALUE)
		z -= (q1.y>>TRI_EXT)*q2.x;
	else
		z -= q1.y*q2.x>>TRI_EXT;
	return	Quaternion(n,x,y,z);
#endif
}

Quaternion operator*(Quaternion q, PhType s)
{
#ifdef USE_FLOAT
	return	Quaternion(q.n*s, q.x*s, q.y*s, q.z*s);
#else
	TriValue n,x,y,z;
#ifdef DEBUG_OVERFLOW
	if(abs(q.n)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(q.n)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
		n = (q.n>>PHT_EXT)*s;
	else
		n = q.n*s>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(q.x)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(q.x)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
		x = (q.x>>PHT_EXT)*s;
	else
		x = q.x*s>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(