|
|
@ -50,17 +50,17 @@ typedef P Point3; |
|
|
typedef P Vector3; |
|
|
typedef P Vector3; |
|
|
|
|
|
|
|
|
// 判断y是否在[x-margin,z+margin]的范围内
|
|
|
// 判断y是否在[x-margin,z+margin]的范围内
|
|
|
inline bool inmid(double x, double y, double z, double margin = MARGIN) |
|
|
inline bool inmid(const double x, const double y, const double z, const double margin = MARGIN) |
|
|
{ |
|
|
{ |
|
|
return y >= x - margin && y <= z + margin; |
|
|
return y >= x - margin && y <= z + margin; |
|
|
} |
|
|
} |
|
|
inline bool inbox(P &p) |
|
|
inline bool inbox(const P &p) |
|
|
{ |
|
|
{ |
|
|
return inmid(MINX, p.x, MAXX) && inmid(MINY, p.y, MAXY) && inmid(MINZ, p.z, MAXZ); |
|
|
return inmid(MINX, p.x, MAXX) && inmid(MINY, p.y, MAXY) && inmid(MINZ, p.z, MAXZ); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
// 将d与0比较，返回正负
|
|
|
// 将d与0比较，返回正负
|
|
|
inline int dcmp(double d) |
|
|
inline int dcmp(const double d) |
|
|
{ |
|
|
{ |
|
|
if (d < -eps) |
|
|
if (d < -eps) |
|
|
return -1; |
|
|
return -1; |
|
|
@ -71,7 +71,7 @@ inline int dcmp(double d) |
|
|
|
|
|
|
|
|
// 真正的距离函数
|
|
|
// 真正的距离函数
|
|
|
|
|
|
|
|
|
inline double distan1(P &p1, P &p2) |
|
|
inline double distan1(const P &p1, const P &p2) |
|
|
{ |
|
|
{ |
|
|
double x = p1.x - p2.x, y = p1.y - p2.y, z = p1.z - p2.z; |
|
|
double x = p1.x - p2.x, y = p1.y - p2.y, z = p1.z - p2.z; |
|
|
return sqrt(x * x + y * y + z * z); |
|
|
return sqrt(x * x + y * y + z * z); |
|
|
@ -93,14 +93,14 @@ inline double distan2(P &p1,P &p2){ |
|
|
return sqrt(x*x+y*y+z*z)*penalty_par; |
|
|
return sqrt(x*x+y*y+z*z)*penalty_par; |
|
|
} |
|
|
} |
|
|
*/ |
|
|
*/ |
|
|
inline double Dot(Vector3 &A, Vector3 &B) { return A.x * B.x + A.y * B.y + A.z * B.z; } |
|
|
inline double Dot(const Vector3 &A, const Vector3 &B) { return A.x * B.x + A.y * B.y + A.z * B.z; } |
|
|
inline double Length(Vector3 &A) { return sqrt(Dot(A, A)); } |
|
|
inline double Length(const Vector3 &A) { return sqrt(Dot(A, A)); } |
|
|
inline double Angle(Vector3 &A, Vector3 &B) { return acos(Dot(A, B) / Length(A) / Length(B)); } |
|
|
inline double Angle(const Vector3 &A, const Vector3 &B) { return acos(Dot(A, B) / Length(A) / Length(B)); } |
|
|
inline double DistanceToPlane(Point3 &p, Point3 &p0, Vector3 &n) |
|
|
inline double DistanceToPlane(const Point3 &p, const Point3 &p0, const Vector3 &n) |
|
|
{ |
|
|
{ |
|
|
return fabs(Dot(p - p0, n)); // 如果不取绝对值，得到的是有向距离
|
|
|
return fabs(Dot(p - p0, n)); // 如果不取绝对值，得到的是有向距离
|
|
|
} |
|
|
} |
|
|
inline int ParallelorVertical(P &p1, P &p2) |
|
|
inline int ParallelorVertical(const P &p1, const P &p2) |
|
|
{ |
|
|
{ |
|
|
double angel = Angle(p1, p2); |
|
|
double angel = Angle(p1, p2); |
|
|
if (angel <= minAngle || angel >= pi - minAngle) |
|
|
if (angel <= minAngle || angel >= pi - minAngle) |
|
|
@ -109,23 +109,23 @@ inline int ParallelorVertical(P &p1, P &p2) |
|
|
return 2; |
|
|
return 2; |
|
|
return 0; |
|
|
return 0; |
|
|
} |
|
|
} |
|
|
inline Point3 GetPlaneProjection(Point3 &p, Point3 &p0, Vector3 &n) |
|
|
inline Point3 GetPlaneProjection(const Point3 &p, const Point3 &p0, const Vector3 &n) |
|
|
{ |
|
|
{ |
|
|
return p - n * Dot(p - p0, n); |
|
|
return p - n * Dot(p - p0, n); |
|
|
} |
|
|
} |
|
|
inline Point3 LinePlaneIntersection(Point3 &p1, Point3 &p2, Point3 &p0, Vector3 &n) |
|
|
inline Point3 LinePlaneIntersection(const Point3 &p1, const Point3 &p2, const Point3 &p0, const Vector3 &n) |
|
|
{ |
|
|
{ |
|
|
Vector3 v = p2 - p1; |
|
|
Vector3 v = p2 - p1; |
|
|
double t = (Dot(n, p0 - p1) / Dot(n, p2 - p1)); // 判断分母是否为 0
|
|
|
double t = (Dot(n, p0 - p1) / Dot(n, p2 - p1)); // 判断分母是否为 0
|
|
|
return p1 + v * t; // 如果是线段，判断 t 是不是在 0 和 1 之间
|
|
|
return p1 + v * t; // 如果是线段，判断 t 是不是在 0 和 1 之间
|
|
|
} |
|
|
} |
|
|
inline Vector3 Cross(Vector3 A, Vector3 B) |
|
|
inline Vector3 Cross(const Vector3 A, const Vector3 B) |
|
|
{ |
|
|
{ |
|
|
return Vector3(A.y * B.z - A.z * B.y, A.z * B.x - A.x * B.z, A.x * B.y - A.y * B.x); |
|
|
return Vector3(A.y * B.z - A.z * B.y, A.z * B.x - A.x * B.z, A.x * B.y - A.y * B.x); |
|
|
} |
|
|
} |
|
|
inline double Area2(Point3 &A, Point3 &B, Point3 &C) { return Length(Cross(B - A, C - A)); } |
|
|
inline double Area2(const Point3 &A, const Point3 &B, const Point3 &C) { return Length(Cross(B - A, C - A)); } |
|
|
|
|
|
|
|
|
inline double get_penalty_par_distance(double len) |
|
|
inline double get_penalty_par_distance(const double len) |
|
|
{ |
|
|
{ |
|
|
static const double intersection_distance = 180; |
|
|
static const double intersection_distance = 180; |
|
|
if (len <= intersection_distance) |
|
|
if (len <= intersection_distance) |
|
|
@ -312,7 +312,7 @@ inline double distan(P A, P B) |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
// 打印路径信息
|
|
|
// 打印路径信息
|
|
|
void printPath(vector<P> vecp) |
|
|
void printPath(const vector<P> vecp) |
|
|
{ |
|
|
{ |
|
|
for (int j = 0; j < vecp.size(); j++) |
|
|
for (int j = 0; j < vecp.size(); j++) |
|
|
{ |
|
|
{ |
|
|
|
