⭐ 欢迎来到虫虫下载站！ | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站
📤 上传资源

📄 mymath.cpp

📁 liu7788414
💻 CPP
📖 第 1 页 / 共 5 页
字号:
🔍
12345 
	if(abs(e13)>=DANGER_VALUET&&abs(e21)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(e13)>=CRIT_VALUE&&abs(e21)>=CRIT_VALUE)
		te23 -= (e13>>TRI_EXT)*e21;
	else
		te23 -= e13*e21>>TRI_EXT;
	te23 = -te23;

	//(e21*e32-e22*e31)
#ifdef DEBUG_OVERFLOW
	if(abs(e21)>=DANGER_VALUET&&abs(e32)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(e21)>=CRIT_VALUE&&abs(e32)>=CRIT_VALUE)
		te31 += (e21>>TRI_EXT)*e32;
	else
		te31 += e21*e32>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e22)>=DANGER_VALUET&&abs(e31)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(e22)>=CRIT_VALUE&&abs(e31)>=CRIT_VALUE)
		te31 -= (e22>>TRI_EXT)*e31;
	else
		te31 -= e22*e31>>TRI_EXT;

	//-(e11*e32-e12*e31)
#ifdef DEBUG_OVERFLOW
	if(abs(e11)>=DANGER_VALUET&&abs(e32)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(e11)>=CRIT_VALUE&&abs(e32)>=CRIT_VALUE)
		te32 += (e11>>TRI_EXT)*e32;
	else
		te32 += e11*e32>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e12)>=DANGER_VALUET&&abs(e31)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(e12)>=CRIT_VALUE&&abs(e31)>=CRIT_VALUE)
		te32 -= (e12>>TRI_EXT)*e31;
	else
		te32 -= e12*e31>>TRI_EXT;
	te32 = -te32;

	//(e11*e22-e12*e21)
#ifdef DEBUG_OVERFLOW
	if(abs(e11)>=DANGER_VALUET&&abs(e22)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(e11)>=CRIT_VALUE&&abs(e22)>=CRIT_VALUE)
		te33 += (e11>>TRI_EXT)*e22;
	else
		te33 += e11*e22>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e12)>=DANGER_VALUET&&abs(e21)>=DANGER_VALUET)
		myPrintError("ERROR_T");
#endif
	if(abs(e12)>=CRIT_VALUE&&abs(e21)>=CRIT_VALUE)
		te33 -= (e12>>TRI_EXT)*e21;
	else
		te33 -= e12*e21>>TRI_EXT;
	return Matrix3x3((te11<<PHT_EXT)/d, (te12<<PHT_EXT)/d, (te13<<PHT_EXT)/d,
					 (te21<<PHT_EXT)/d, (te22<<PHT_EXT)/d, (te23<<PHT_EXT)/d,
					 (te31<<PHT_EXT)/d, (te32<<PHT_EXT)/d, (te33<<PHT_EXT)/d);
#endif
}
Matrix3x3 Matrix3x3::InverseR(void)
{
	PhType	d;
	PhType te11=0,te12=0,te13=0,te21=0,te22=0,te23=0,te31=0,te32=0,te33=0;
	Matrix3x3 tm(e11>>PHT_EXT,e12>>PHT_EXT,e13>>PHT_EXT,
				 e21>>PHT_EXT,e22>>PHT_EXT,e23>>PHT_EXT,
				 e31>>PHT_EXT,e32>>PHT_EXT,e33>>PHT_EXT);

	d=tm.det();
	d <<= PHT_EXT*2;
	

	//(e22*e33-e23*e32)
#ifdef DEBUG_OVERFLOW
	if(abs(e22)>=DANGER_VALUE&&abs(e33)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e22)>=CRIT_VALUE&&abs(e33)>=CRIT_VALUE)
		te11 += (e22>>PHT_EXT)*e33;
	else
		te11 += e22*e33>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e23)>=DANGER_VALUE&&abs(e32)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e23)>=CRIT_VALUE&&abs(e32)>=CRIT_VALUE)
		te11 -= (e23>>PHT_EXT)*e32;
	else
		te11 -= e23*e32>>PHT_EXT;

	//-(e12*e33-e13*e32)
#ifdef DEBUG_OVERFLOW
	if(abs(e12)>=DANGER_VALUE&&abs(e33)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e12)>=CRIT_VALUE&&abs(e33)>=CRIT_VALUE)
		te12 += (e12>>PHT_EXT)*e33;
	else
		te12 += e12*e33>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e13)>=DANGER_VALUE&&abs(e32)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e13)>=CRIT_VALUE&&abs(e32)>=CRIT_VALUE)
		te12 -= (e13>>PHT_EXT)*e32;
	else
		te12 -= e13*e32>>PHT_EXT;
	te12 = -te12;

	//(e12*e23-e13*e22)
#ifdef DEBUG_OVERFLOW
	if(abs(e12)>=DANGER_VALUE&&abs(e23)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e12)>=CRIT_VALUE&&abs(e23)>=CRIT_VALUE)
		te13 += (e12>>PHT_EXT)*e23;
	else
		te13 += e12*e23>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e13)>=DANGER_VALUE&&abs(e22)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e13)>=CRIT_VALUE&&abs(e22)>=CRIT_VALUE)
		te13 -= (e13>>PHT_EXT)*e22;
	else
		te13 -= e13*e22>>PHT_EXT;

	//-(e21*e33-e23*e31)
#ifdef DEBUG_OVERFLOW
	if(abs(e21)>=DANGER_VALUE&&abs(e33)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e21)>=CRIT_VALUE&&abs(e33)>=CRIT_VALUE)
		te21 += (e21>>PHT_EXT)*e33;
	else
		te21 += e21*e33>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e23)>=DANGER_VALUE&&abs(e31)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e23)>=CRIT_VALUE&&abs(e31)>=CRIT_VALUE)
		te21 -= (e23>>PHT_EXT)*e31;
	else
		te21 -= e23*e31>>PHT_EXT;
	te21 = -te21;

	//(e11*e33-e13*e31)
#ifdef DEBUG_OVERFLOW
	if(abs(e11)>=DANGER_VALUE&&abs(e33)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e11)>=CRIT_VALUE&&abs(e33)>=CRIT_VALUE)
		te22 += (e11>>PHT_EXT)*e33;
	else
		te22 += e11*e33>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e13)>=DANGER_VALUE&&abs(e31)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e13)>=CRIT_VALUE&&abs(e31)>=CRIT_VALUE)
		te22 -= (e13>>PHT_EXT)*e31;
	else
		te22 -= e13*e31>>PHT_EXT;

	//-(e11*e23-e13*e21)
#ifdef DEBUG_OVERFLOW
	if(abs(e11)>=DANGER_VALUE&&abs(e23)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e11)>=CRIT_VALUE&&abs(e23)>=CRIT_VALUE)
		te23 += (e11>>PHT_EXT)*e23;
	else
		te23 += e11*e23>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e13)>=DANGER_VALUE&&abs(e21)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e13)>=CRIT_VALUE&&abs(e21)>=CRIT_VALUE)
		te23 -= (e13>>PHT_EXT)*e21;
	else
		te23 -= e13*e21>>PHT_EXT;
	te23 = -te23;

	//(e21*e32-e22*e31)
#ifdef DEBUG_OVERFLOW
	if(abs(e21)>=DANGER_VALUE&&abs(e32)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e21)>=CRIT_VALUE&&abs(e32)>=CRIT_VALUE)
		te31 += (e21>>PHT_EXT)*e32;
	else
		te31 += e21*e32>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e22)>=DANGER_VALUE&&abs(e31)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e22)>=CRIT_VALUE&&abs(e31)>=CRIT_VALUE)
		te31 -= (e22>>PHT_EXT)*e31;
	else
		te31 -= e22*e31>>PHT_EXT;

	//-(e11*e32-e12*e31)
#ifdef DEBUG_OVERFLOW
	if(abs(e11)>=DANGER_VALUE&&abs(e32)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e11)>=CRIT_VALUE&&abs(e32)>=CRIT_VALUE)
		te32 += (e11>>PHT_EXT)*e32;
	else
		te32 += e11*e32>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e12)>=DANGER_VALUE&&abs(e31)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e12)>=CRIT_VALUE&&abs(e31)>=CRIT_VALUE)
		te32 -= (e12>>PHT_EXT)*e31;
	else
		te32 -= e12*e31>>PHT_EXT;
	te32 = -te32;

	//(e11*e22-e12*e21)
#ifdef DEBUG_OVERFLOW
	if(abs(e11)>=DANGER_VALUE&&abs(e22)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e11)>=CRIT_VALUE&&abs(e22)>=CRIT_VALUE)
		te33 += (e11>>PHT_EXT)*e22;
	else
		te33 += e11*e22>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e12)>=DANGER_VALUE&&abs(e21)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e12)>=CRIT_VALUE&&abs(e21)>=CRIT_VALUE)
		te33 -= (e12>>PHT_EXT)*e21;
	else
		te33 -= e12*e21>>PHT_EXT;

	if(d==0) d = 1<<PHT_EXT;
	
	if(te11==0)
		te11 = 0x7fffffff;
	else if(abs(d)<=abs(te11))
	{
		if(abs(d)>=CRIT_VALUE)
			te11 = (d<<PHT_EXT)/te11<<PHT_EXT;
		else
			te11 = (d<<(2*PHT_EXT))/te11;
	}
	else{
		if(abs(d)>=CRIT_VALUE)
			te11 = d/te11<<(2*PHT_EXT);
		else
			te11 = (d<<(2*PHT_EXT))/te11;
	}
	if(te12==0)
		te12 = 0x7fffffff;
	else if(abs(d)<=abs(te12))
	{
		if(abs(d)>=CRIT_VALUE)
			te12 = (d<<PHT_EXT)/te12<<PHT_EXT;
		else
			te12 = (d<<(2*PHT_EXT))/te12;
	}
	else{
		if(abs(d)>=CRIT_VALUE)
			te12 = d/te12<<(2*PHT_EXT);
		else
			te12 = (d<<(2*PHT_EXT))/te12;
	}
	if(te13==0)
		te13 = 0x7fffffff;
	else if(abs(d)<=abs(te13))
	{
		if(abs(d)>=CRIT_VALUE)
			te13 = (d<<PHT_EXT)/te13<<PHT_EXT;
		else
			te13 = (d<<(2*PHT_EXT))/te13;
	}
	else{
		if(abs(d)>=CRIT_VALUE)
			te13 = d/te13<<(2*PHT_EXT);
		else
			te13 = (d<<(2*PHT_EXT))/te13;
	}
	if(te21==0)
		te21 = 0x7fffffff;
	else if(abs(d)<=abs(te21))
	{
		if(abs(d)>=CRIT_VALUE)
			te21 = (d<<PHT_EXT)/te21<<PHT_EXT;
		else
			te21 = (d<<(2*PHT_EXT))/te21;
	}
	else{
		if(abs(d)>=CRIT_VALUE)
			te21 = d/te21<<(2*PHT_EXT);
		else
			te21 = (d<<(2*PHT_EXT))/te21;
	}
	if(te22==0)
		te22 = 0x7fffffff;
	else if(abs(d)<=abs(te22))
	{
		if(abs(d)>=CRIT_VALUE)
			te22 = (d<<PHT_EXT)/te22<<PHT_EXT;
		else
			te22 = (d<<(2*PHT_EXT))/te22;
	}
	else{
		if(abs(d)>=CRIT_VALUE)
			te22 = d/te22<<(2*PHT_EXT);
		else
			te22 = (d<<(2*PHT_EXT))/te22;
	}
	if(te23==0)
		te23 = 0x7fffffff;
	else if(abs(d)<=abs(te23))
	{
		if(abs(d)>=CRIT_VALUE)
			te23 = (d<<PHT_EXT)/te23<<PHT_EXT;
		else
			te23 = (d<<(2*PHT_EXT))/te23;
	}
	else{
		if(abs(d)>=CRIT_VALUE)
			te23 = d/te23<<(2*PHT_EXT);
		else
			te23 = (d<<(2*PHT_EXT))/te23;
	}
	if(te31==0)
		te31 = 0x7fffffff;
	else if(abs(d)<=abs(te31))
	{
		if(abs(d)>=CRIT_VALUE)
			te31 = (d<<PHT_EXT)/te31<<PHT_EXT;
		else
			te31 = (d<<(2*PHT_EXT))/te31;
	}
	else{
		if(abs(d)>=CRIT_VALUE)
			te31 = d/te31<<(2*PHT_EXT);
		else
			te31 = (d<<(2*PHT_EXT))/te31;
	}
	if(te32==0)
		te32 = 0x7fffffff;
	else if(abs(d)<=abs(te32))
	{
		if(abs(d)>=CRIT_VALUE)
			te32 = (d<<PHT_EXT)/te32<<PHT_EXT;
		else
			te32 = (d<<(2*PHT_EXT))/te32;
	}
	else{
		if(abs(d)>=CRIT_VALUE)
			te32 = d/te32<<(2*PHT_EXT);
		else
			te32 = (d<<(2*PHT_EXT))/te32;
	}
	if(te33==0)
		te33 = 0x7fffffff;
	else if(abs(d)<=abs(te33))
	{
		if(abs(d)>=CRIT_VALUE)
			te33 = (d<<PHT_EXT)/te33<<PHT_EXT;
		else
			te33 = (d<<(2*PHT_EXT))/te33;
	}
	else{
		if(abs(d)>=CRIT_VALUE)
			te33 = d/te33<<(2*PHT_EXT);
		else
			te33 = (d<<(2*PHT_EXT))/te33;
	}
/*
	te11= (te11==0) ? 0x7fffffff : ( d>=CRIT_VALUE ? d/te11<<(2*PHT_EXT) : (d<<(2*PHT_EXT))/te11 );
	te12= (te12==0) ? 0x7fffffff : ( d>=CRIT_VALUE ? d/te12<<(2*PHT_EXT) : (d<<(2*PHT_EXT))/te12 );
	te13= (te13==0) ? 0x7fffffff : ( d>=CRIT_VALUE ? d/te13<<(2*PHT_EXT) : (d<<(2*PHT_EXT))/te13 );
	te21= (te21==0) ? 0x7fffffff : ( d>=CRIT_VALUE ? d/te21<<(2*PHT_EXT) : (d<<(2*PHT_EXT))/te21 );
	te22= (te22==0) ? 0x7fffffff : ( d>=CRIT_VALUE ? d/te22<<(2*PHT_EXT) : (d<<(2*PHT_EXT))/te22 );
	te23= (te23==0) ? 0x7fffffff : ( d>=CRIT_VALUE ? d/te23<<(2*PHT_EXT) : (d<<(2*PHT_EXT))/te23 );
	te31= (te31==0) ? 0x7fffffff : ( d>=CRIT_VALUE ? d/te31<<(2*PHT_EXT) : (d<<(2*PHT_EXT))/te31 );
	te32= (te32==0) ? 0x7fffffff : ( d>=CRIT_VALUE ? d/te32<<(2*PHT_EXT) : (d<<(2*PHT_EXT))/te32 );
	te33= (te33==0) ? 0x7fffffff : ( d>=CRIT_VALUE ? d/te33<<(2*PHT_EXT) : (d<<(2*PHT_EXT))/te33 );
*/
	return Matrix3x3( te11,te12,te13,te21,te22,te23,te31,te32,te33 );
}

Vector MulRMV(Matrix3x3 m, Vector u)
{
	PhType x=0,y=0,z=0;

	if(m.e11!=0x7fffffff)
		x += (u.x<<PHT_EXT)/m.e11;
	if(m.e12!=0x7fffffff)
		x += (u.y<<PHT_EXT)/m.e12;
	if(m.e13!=0x7fffffff)
		x += (u.z<<PHT_EXT)/m.e13;

	if(m.e21!=0x7fffffff)
		y += (u.x<<PHT_EXT)/m.e21;
	if(m.e22!=0x7fffffff)
		y += (u.y<<PHT_EXT)/m.e22;
	if(m.e23!=0x7fffffff)
		y += (u.z<<PHT_EXT)/m.e23;

	if(m.e31!=0x7fffffff)
		z += (u.x<<PHT_EXT)/m.e31;
	if(m.e32!=0x7fffffff)
		z += (u.y<<PHT_EXT)/m.e32;
	if(m.e33!=0x7fffffff)
		z += (u.z<<PHT_EXT)/m.e33;

	return Vector(x,y,z);
}

Vector MulVRM(Vector u, Matrix3x3 m)
{
	PhType x=0,y=0,z=0;

	if(m.e11!=0x7fffffff)
		x += (u.x<<PHT_EXT)/m.e11;
	if(m.e21!=0x7fffffff)
		x += (u.y<<PHT_EXT)/m.e21;
	if(m.e31!=0x7fffffff)
		x += (u.z<<PHT_EXT)/m.e31;

	if(m.e12!=0x7fffffff)
		y += (u.x<<PHT_EXT)/m.e12;
	if(m.e22!=0x7fffffff)
		y += (u.y<<PHT_EXT)/m.e22;
	if(m.e32!=0x7fffffff)
		y += (u.z<<PHT_EXT)/m.e32;

	if(m.e13!=0x7fffffff)
		z += (u.x<<PHT_EXT)/m.e13;
	if(m.e23!=0x7fffffff)
		z += (u.y<<PHT_EXT)/m.e23;
	if(m.e33!=0x7fffffff)
		z += (u.z<<PHT_EXT)/m.e33;

	return Vector(x,y,z);	
}


Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m)
{
	e11 += m.e11;
	e12 += m.e12;
	e13 += m.e13;
	e21 += m.e21;
	e22 += m.e22;
	e23 += m.e23;
	e31 += m.e31;
	e32 += m.e32;
	e33 += m.e33;
	return *this;
}

Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m)
{
	e11 -= m.e11;
	e12 -= m.e12;
	e13 -= m.e13;
	e21 -= m.e21;
	e22 -= m.e22;
	e23 -= m.e23;
	e31 -= m.e31;
	e32 -= m.e32;
	e33 -= m.e33;
	return *this;
}

Matrix3x3& Matrix3x3::operator*=(PhType s)
{
#ifdef DEBUG_OVERFLOW
	if(abs(e11)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e11)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
		e11 = (e11>>PHT_EXT)*s;
	else
		e11 = e11*s>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e12)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e12)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
		e12 = (e12>>PHT_EXT)*s;
	else
		e12 = e12*s>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e13)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
		myPrintError("ERROR");
#endif
	if(abs(e13)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
		e13 = (e13>>PHT_EXT)*s;
	else
		e13 = e13*s>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
	if(abs(e21)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
		myPrintError("ERROR");
12345

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -