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; } }