📄 mymath.cpp
字号:
Vector operator-(Vector u, Vector v)
{
return Vector(u.x - v.x, u.y - v.y, u.z - v.z);
}
// Vector cross product (u cross v)
Vector operator^(Vector u, Vector v)
{
#ifdef USE_FLOAT
return Vector( u.y*v.z - u.z*v.y,
-u.x*v.z + u.z*v.x,
u.x*v.y - u.y*v.x);
#else
PhType tX=0,tY=0,tZ=0;
#ifdef DEBUG_OVERFLOW
if(abs(u.y)>=DANGER_VALUE&&abs(v.z)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.y>=CRIT_VALUE)&&abs(v.z)>=CRIT_VALUE)
tX += (u.y>>PHT_EXT)*v.z;
else
tX += u.y*v.z>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(u.z)>=DANGER_VALUE&&abs(v.y)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.z>=CRIT_VALUE)&&abs(v.y)>=CRIT_VALUE)
tX -= (u.z>>PHT_EXT)*v.y;
else
tX -= u.z*v.y>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(u.x)>=DANGER_VALUE&&abs(v.z)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(-u.x>=CRIT_VALUE)&&abs(v.z)>=CRIT_VALUE)
tY += (-u.x>>PHT_EXT)*v.z;
else
tY += -u.x*v.z>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(u.z)>=DANGER_VALUE&&abs(v.x)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.z>=CRIT_VALUE)&&abs(v.x)>=CRIT_VALUE)
tY += (u.z>>PHT_EXT)*v.x;
else
tY += u.z*v.x>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(u.x)>=DANGER_VALUE&&abs(v.y)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.x>=CRIT_VALUE)&&abs(v.y)>=CRIT_VALUE)
tZ += (u.x>>PHT_EXT)*v.y;
else
tZ += u.x*v.y>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(u.y)>=DANGER_VALUE&&abs(v.x)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.y>=CRIT_VALUE)&&abs(v.x)>=CRIT_VALUE)
tZ -= (u.y>>PHT_EXT)*v.x;
else
tZ -= u.y*v.x>>PHT_EXT;
return Vector(tX,tY,tZ);
#endif
}
// Vector dot product
PhType operator*(Vector u, Vector v)
{
#ifdef USE_FLOAT
return (u.x*v.x + u.y*v.y + u.z*v.z);
#else
PhType r=0;
#ifdef DEBUG_OVERFLOW
if(abs(u.x)>=DANGER_VALUE&&abs(v.x)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.x)>=CRIT_VALUE&&abs(v.x)>=CRIT_VALUE)
r += (u.x>>PHT_EXT)*v.x;
else
r += u.x*v.x>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(u.y)>=DANGER_VALUE&&abs(v.y)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.y)>=CRIT_VALUE&&abs(v.y)>=CRIT_VALUE)
r += (u.y>>PHT_EXT)*v.y;
else
r += u.y*v.y>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(u.z)>=DANGER_VALUE&&abs(v.z)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.z)>=CRIT_VALUE&&abs(v.z)>=CRIT_VALUE)
r += (u.z>>PHT_EXT)*v.z;
else
r += u.z*v.z>>PHT_EXT;
return r;
#endif
}
Vector operator*(PhType s, Vector u)
{
#ifdef USE_FLOAT
return Vector(u.x*s, u.y*s, u.z*s);
#else
PhType tX,tY,tZ;
#ifdef DEBUG_OVERFLOW
if(abs(u.x)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.x)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
tX = (u.x>>PHT_EXT)*s;
else
tX= u.x*s>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(u.y)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.y)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
tY = (u.y>>PHT_EXT)*s;
else
tY= u.y*s>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(u.z)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.z)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
tZ = (u.z>>PHT_EXT)*s;
else
tZ= u.z*s>>PHT_EXT;
return Vector(tX,tY,tZ);
#endif
}
Vector operator*(Vector u, PhType s)
{
#ifdef USE_FLOAT
return Vector(u.x*s, u.y*s, u.z*s);
#else
PhType tX,tY,tZ;
#ifdef DEBUG_OVERFLOW
if(abs(u.x)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.x)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
tX = (u.x>>PHT_EXT)*s;
else
tX= u.x*s>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(u.y)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.y)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
tY = (u.y>>PHT_EXT)*s;
else
tY= u.y*s>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(u.z)>=DANGER_VALUE&&abs(s)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(u.z)>=CRIT_VALUE&&abs(s)>=CRIT_VALUE)
tZ = (u.z>>PHT_EXT)*s;
else
tZ= u.z*s>>PHT_EXT;
return Vector(tX,tY,tZ);
#endif
}
Vector operator/(Vector u, PhType s)
{
return Vector((u.x<<PHT_EXT)/s, (u.y<<PHT_EXT)/s, (u.z<<PHT_EXT)/s);
}
// triple scalar product (u dot (v cross w))
//PhType TripleScalarProduct(Vector u, Vector v, Vector w)
//{
// return ( (u.x * (v.y*w.z/PHT_EXTENDED - v.z*w.y/PHT_EXTENDED)/PHT_EXTENDED) +
// (u.y * (-v.x*w.z/PHT_EXTENDED + v.z*w.x/PHT_EXTENDED)/PHT_EXTENDED) +
// (u.z * (v.x*w.y/PHT_EXTENDED - v.y*w.x/PHT_EXTENDED)/PHT_EXTENDED) );
//u*(v^w);
//}
bool isParallel(Vector l1, Vector l2)
{
Vector l = l1^l2;
if(l.x!=0 || l.y!=0 || l.z!=0)
return false;
return true;
}
int calcPlumbAmong2L(Vector p1, Vector l1, Vector p2, Vector l2, Vector *p)
{
Vector p12;
PhType len1,len2,l12,lp12,lp21;
PhType temp=0,tempp=0;
if(isParallel(l1,l2))
return 0;
p12 = p1-p2;
len1 = l1*l1;
len2 = l2*l2;
l12 = l1*l2;
lp12 = l1*p12;
lp21 = -l2*p12;
#ifdef DEBUG_OVERFLOW
if(abs(l12)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(l12)>=CRIT_VALUE)
temp += (l12>>PHT_EXT)*l12;
else
temp += l12*l12>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(len1)>=DANGER_VALUE&&abs(len2)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(len1)>=CRIT_VALUE&&abs(len2)>=CRIT_VALUE)
temp -= (len1>>PHT_EXT)*len2;
else
temp -= len1*len2>>PHT_EXT;
//temp = (l12*l12>>PHT_EXT)-(len1*len2>>PHT_EXT);
if(temp==0)
return 0;
#ifdef DEBUG_OVERFLOW
if(abs(lp12)>=DANGER_VALUE&&abs(len2)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(lp12)>=CRIT_VALUE&&abs(len2)>=CRIT_VALUE)
tempp += (lp12>>PHT_EXT)*len2;
else
tempp += lp12*len2>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(lp21)>=DANGER_VALUE&&abs(l12)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(lp21)>=CRIT_VALUE&&abs(l12)>=CRIT_VALUE)
tempp += (lp21>>PHT_EXT)*l12;
else
tempp += lp21*l12>>PHT_EXT;
p[0] = tempp*l1;
p[0] /= temp;
p[0] += p1;
tempp=0;
#ifdef DEBUG_OVERFLOW
if(abs(lp12)>=DANGER_VALUE&&abs(l12)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(lp12)>=CRIT_VALUE&&abs(l12)>=CRIT_VALUE)
tempp += (lp12>>PHT_EXT)*l12;
else
tempp += lp12*l12>>PHT_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(lp21)>=DANGER_VALUE&&abs(len1)>=DANGER_VALUE)
myPrintError("ERROR");
#endif
if(abs(lp21)>=CRIT_VALUE&&abs(len1)>=CRIT_VALUE)
tempp += (lp21>>PHT_EXT)*len1;
else
tempp += lp21*len1>>PHT_EXT;
p[1] = tempp*l2;
p[1] /= temp;
p[1] += p2;
return 1;
}
//------------------------------------------------------------------------//
// Matrix Class and matrix functions
//------------------------------------------------------------------------//
Matrix3x3::Matrix3x3(void)
{
e11 = 0;
e12 = 0;
e13 = 0;
e21 = 0;
e22 = 0;
e23 = 0;
e31 = 0;
e32 = 0;
e33 = 0;
}
Matrix3x3::Matrix3x3( TriValue r1c1, TriValue r1c2, TriValue r1c3,
TriValue r2c1, TriValue r2c2, TriValue r2c3,
TriValue r3c1, TriValue r3c2, TriValue r3c3 )
{
e11 = r1c1;
e12 = r1c2;
e13 = r1c3;
e21 = r2c1;
e22 = r2c2;
e23 = r2c3;
e31 = r3c1;
e32 = r3c2;
e33 = r3c3;
}
PhType Matrix3x3::det(void)
{
#ifdef USE_FLOAT
return e11*(e22*e33 - e32*e23) +
e21*(e32*e13 - e12*e33) +
e31*(e12*e23 - e22*e13);
#else
TriValue temp1=0,temp2=0,temp3=0;
PhType r=0;
#ifdef DEBUG_OVERFLOW
if(abs(e22)>=DANGER_VALUET&&abs(e33)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e22)>=CRIT_VALUE&&abs(e33)>=CRIT_VALUE)
temp1 += (e22>>TRI_EXT)*e33;
else
temp1 += e22*e33>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(e32)>=DANGER_VALUET&&abs(e23)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e32)>=CRIT_VALUE&&abs(e23)>=CRIT_VALUE)
temp1 -= (e32>>TRI_EXT)*e23;
else
temp1 -= e32*e23>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(e32)>=DANGER_VALUET&&abs(e13)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e32)>=CRIT_VALUE&&abs(e13)>=CRIT_VALUE)
temp2 += (e32>>TRI_EXT)*e13;
else
temp2 += e32*e13>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(e12)>=DANGER_VALUET&&abs(e33)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e12)>=CRIT_VALUE&&abs(e33)>=CRIT_VALUE)
temp2 -= (e12>>TRI_EXT)*e33;
else
temp2 -= e12*e33>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(e12)>=DANGER_VALUET&&abs(e23)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e12)>=CRIT_VALUE&&abs(e23)>=CRIT_VALUE)
temp3 += (e12>>TRI_EXT)*e23;
else
temp3 += e12*e23>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(e22)>=DANGER_VALUET&&abs(e13)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e22)>=CRIT_VALUE&&abs(e13)>=CRIT_VALUE)
temp3 -= (e22>>TRI_EXT)*e13;
else
temp3 -= e22*e13>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(e11)>=DANGER_VALUET&&abs(temp1)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e11)>=CRIT_VALUE&&abs(temp1)>=CRIT_VALUE)
r += (e11>>TRI_EXT)*temp1;
else
r += e11*temp1>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(e21)>=DANGER_VALUET&&abs(temp2)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e21)>=CRIT_VALUE&&abs(temp2)>=CRIT_VALUE)
r += (e21>>TRI_EXT)*temp2;
else
r += e21*temp2>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(e31)>=DANGER_VALUET&&abs(temp3)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e31)>=CRIT_VALUE&&abs(temp3)>=CRIT_VALUE)
r += (e31>>TRI_EXT)*temp3;
else
r += e31*temp3>>TRI_EXT;
return r>>PHT_EXT;
#endif
}
Matrix3x3 Matrix3x3::Transpose(void)
{
return Matrix3x3(e11,e21,e31,e12,e22,e32,e13,e23,e33);
}
Matrix3x3 Matrix3x3::Inverse(void)
{
PhType d = det();
#ifdef USE_FLOAT
if (d == 0) d = 1;
return Matrix3x3( (e22*e33-e23*e32)/d,
-(e12*e33-e13*e32)/d,
(e12*e23-e13*e22)/d,
-(e21*e33-e23*e31)/d,
(e11*e33-e13*e31)/d,
-(e11*e23-e13*e21)/d,
(e21*e32-e22*e31)/d,
-(e11*e32-e12*e31)/d,
(e11*e22-e12*e21)/d );
#else
TriValue te11=0,te12=0,te13=0,te21=0,te22=0,te23=0,te31=0,te32=0,te33=0;
if(d==0)
d = 1<<PHT_EXT;
//(e22*e33-e23*e32)
#ifdef DEBUG_OVERFLOW
if(abs(e22)>=DANGER_VALUET&&abs(e33)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e22)>=CRIT_VALUE&&abs(e33)>=CRIT_VALUE)
te11 += (e22>>TRI_EXT)*e33;
else
te11 += e22*e33>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(e23)>=DANGER_VALUET&&abs(e32)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e23)>=CRIT_VALUE&&abs(e32)>=CRIT_VALUE)
te11 -= (e23>>TRI_EXT)*e32;
else
te11 -= e23*e32>>TRI_EXT;
//-(e12*e33-e13*e32)
#ifdef DEBUG_OVERFLOW
if(abs(e12)>=DANGER_VALUET&&abs(e33)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e12)>=CRIT_VALUE&&abs(e33)>=CRIT_VALUE)
te12 += (e12>>TRI_EXT)*e33;
else
te12 += e12*e33>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(e13)>=DANGER_VALUET&&abs(e32)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e13)>=CRIT_VALUE&&abs(e32)>=CRIT_VALUE)
te12 -= (e13>>TRI_EXT)*e32;
else
te12 -= e13*e32>>TRI_EXT;
te12 = -te12;
//(e12*e23-e13*e22)
#ifdef DEBUG_OVERFLOW
if(abs(e12)>=DANGER_VALUET&&abs(e23)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e12)>=CRIT_VALUE&&abs(e23)>=CRIT_VALUE)
te13 += (e12>>TRI_EXT)*e23;
else
te13 += e12*e23>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(e13)>=DANGER_VALUET&&abs(e22)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e13)>=CRIT_VALUE&&abs(e22)>=CRIT_VALUE)
te13 -= (e13>>TRI_EXT)*e22;
else
te13 -= e13*e22>>TRI_EXT;
//-(e21*e33-e23*e31)
#ifdef DEBUG_OVERFLOW
if(abs(e21)>=DANGER_VALUET&&abs(e33)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e21)>=CRIT_VALUE&&abs(e33)>=CRIT_VALUE)
te21 += (e21>>TRI_EXT)*e33;
else
te21 += e21*e33>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(e23)>=DANGER_VALUET&&abs(e31)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e23)>=CRIT_VALUE&&abs(e31)>=CRIT_VALUE)
te21 -= (e23>>TRI_EXT)*e31;
else
te21 -= e23*e31>>TRI_EXT;
te21 = -te21;
//(e11*e33-e13*e31)
#ifdef DEBUG_OVERFLOW
if(abs(e11)>=DANGER_VALUET&&abs(e33)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e11)>=CRIT_VALUE&&abs(e33)>=CRIT_VALUE)
te22 += (e11>>TRI_EXT)*e33;
else
te22 += e11*e33>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
if(abs(e13)>=DANGER_VALUET&&abs(e31)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e13)>=CRIT_VALUE&&abs(e31)>=CRIT_VALUE)
te22 -= (e13>>TRI_EXT)*e31;
else
te22 -= e13*e31>>TRI_EXT;
//-(e11*e23-e13*e21)
#ifdef DEBUG_OVERFLOW
if(abs(e11)>=DANGER_VALUET&&abs(e23)>=DANGER_VALUET)
myPrintError("ERROR_T");
#endif
if(abs(e11)>=CRIT_VALUE&&abs(e23)>=CRIT_VALUE)
te23 += (e11>>TRI_EXT)*e23;
else
te23 += e11*e23>>TRI_EXT;
#ifdef DEBUG_OVERFLOW
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -