float _AngleBetween(VECTOR3D v1,VECTOR3D v2)
{
if(_IsParallel(v1,v2)) return 0;
CVector3D cv1(v1),cv2(v2);
CVector3D flag = cv2*cv1;
//TRACE( "\nflag %f", flag.dz );
if( flag.dz >0 )
return acos((cv1|cv2.GetNormal())/cv1.GetLength());
else
return -acos((cv1|cv2.GetNormal())/cv1.GetLength());
}
BOOL _Intersection(CPoint3D pt1, CPoint3D pt2, CPoint3D pt3, CPoint3D pt4, CPoint3D* intersection/* = NULL*/)
{
CPoint3D tmp;
CVector3D v1 = pt1 - pt2;
CVector3D v2 = pt3 - pt4;
double dx1 = pt2.x - pt1.x;
double dy1 = pt2.y - pt1.y;
double dz1 = pt2.z - pt1.z;
double dx3 = pt4.x - pt3.x;
double dy3 = pt4.y - pt3.y;
double dz3 = pt4.z - pt3.z;
if ( _IsParallel(v1, v2) )
return FALSE;
if ( !_IsInTheSameFace(pt1, pt2, pt3, pt4) )
return FALSE;
//in a plane and not parallel
//calculation x
if ( IS_ZERO(dx1) )
tmp.x = pt1.x;
else
{
if ( IS_ZERO(dx3) )
{
tmp.x = pt3.x;
}
else
{
if ( !IS_ZERO(dx3*dy1 - dx1*dy3) )
{
tmp.x = ( dx1*dx3*( pt3.y - pt1.y ) + dx3*dy1*pt1.x - dx1*dy3*pt3.x ) / ( dx3*dy1 - dx1*dy3 );
}
else
{
tmp.x = ( dx1*dx3*( pt3.z - pt1.z ) + dx3*dz1*pt1.x - dx1*dz3*pt3.x ) / ( dx3*dz1 - dx1*dz3 );
}
}
}
//cal y
if ( IS_ZERO(dy1) )
{
tmp.y = pt1.y;
}
else
{
if ( IS_ZERO(dy3) )
{
tmp.y = pt3.y;
}
else
{
if ( !IS_ZERO(dy3*dz1 - dy1*dz3) )
{
tmp.y = ( dy1*dy3*( pt3.z - pt1.z ) + dy3*dz1*pt1.y - dy1*dz3*pt3.y ) / ( dy3*dz1 - dy1*dz3 );
}
else
{
tmp.y = ( dy1*dy3*( pt3.x - pt1.x ) + dy3*dx1*pt1.y - dy1*dx3*pt3.y ) / ( dy3*dx1 - dy1*dx3 );
}
}
}
//cal z
if ( IS_ZERO(dz1) )
{
tmp.z = pt1.z;
}
else
{
if ( IS_ZERO(dz3) )
{
tmp.z = pt3.z;
}
else
{
if ( !IS_ZERO(dz3*dx1 - dz1*dx3) )
{
tmp.z = ( dz1*dz3*( pt3.x - pt1.x ) + dz3*dx1*pt1.z - dz1*dx3*pt3.z ) / ( dz3*dx1 - dz1*dx3 );
}
else
{
tmp.z = ( dz1*dz3*( pt3.y - pt1.y ) + dz3*dy1*pt1.z - dz1*dy3*pt3.z ) / ( dz3*dy1 - dz1*dy3 );
}
}
}
if ( intersection != NULL )
*intersection = tmp;
if ( tmp == pt1 || tmp == pt2 || tmp == pt3 || tmp == pt4 )
return FALSE;
if ( ( ( tmp.x <= max(pt1.x, pt2.x) ) && ( tmp.x >= min(pt1.x, pt2.x) ) &&
( tmp.y <= max(pt1.y, pt2.y) ) && ( tmp.y >= min(pt1.y, pt2.y) ) &&
( tmp.z <= max(pt1.z, pt2.z) ) && ( tmp.z >= min(pt1.z, pt2.z) ) ) &&
( ( tmp.x <= max(pt3.x, pt4.x) ) && ( tmp.x >= min(pt3.x, pt4.x) ) &&
( tmp.y <= max(pt3.y, pt4.y) ) && ( tmp.y >= min(pt3.y, pt4.y) ) &&
( tmp.z <= max(pt3.z, pt4.z) ) && ( tmp.z >= min(pt3.z, pt4.z) ) ) )
{
return TRUE;
}
else
{
return FALSE;
}
}
