You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
266 lines
7.0 KiB
266 lines
7.0 KiB
10 months ago
|
#include "stdafx.h"
|
||
|
#include "Point.h"
|
||
|
#include "Geometry.h"
|
||
|
#include "Intersection.h"
|
||
|
#include<cmath>
|
||
|
#include<vector>
|
||
|
#include<queue>
|
||
|
#include<map>
|
||
|
#include<iostream>
|
||
|
#include<fstream>
|
||
|
#include<sstream>
|
||
|
|
||
|
using namespace std;
|
||
|
|
||
|
//���ļ�����һЩ����������ͨ�ú���
|
||
|
|
||
|
const int M=10010; //����������
|
||
|
const int N=5010; //��������
|
||
|
const int MaxSTL=210000; //STL�ļ�������Ƭ��
|
||
|
const double MAXDia=50; //���������ɵ�����ֱ��
|
||
|
const int maxCap=50; //���������ɵ�����������
|
||
|
const double pi=acos(-1.0); //��
|
||
|
const double minAngle=pi/144; //�ж�ƽ�кʹ�ֱ��ƫ����ֵ
|
||
|
const double minDis=30; //
|
||
|
const double R=2000; //�ɻ��������Ľ��ư뾶
|
||
|
const double MARGIN=1000; //���ߵĿռ䷶Χ����STLģ�����������ij���
|
||
|
//----------------------mark----------------------
|
||
|
const int MAXBranchPointNumOnSegment=2; //��֧�ϵ�������֧����
|
||
|
const int MAXPointNum=N+MAXBranchPointNumOnSegment*N; //������֧������������
|
||
|
double segmentLength=0.8; //����������֮���������
|
||
|
const double MinClipToBranchPointDistance=55; //��������֧�������̾���
|
||
|
const double MinBranchPointDistance=55; //��֧�㵽��֧�������̾���
|
||
|
|
||
|
P DX=P(1,0,0);
|
||
|
P DY=P(0,1,0);
|
||
|
P DZ=P(0,0,1);
|
||
|
//X��Y��Z��������
|
||
|
|
||
|
double Ycenter,Zcenter; //�ɻ���Y��Z����������
|
||
|
double MAXX=-1e9,MINX=1e9,MAXY=-1e9,MINY=1e9,MAXZ=-1e9,MINZ=1e9; //���������귶Χ
|
||
|
|
||
|
|
||
|
vector<Vec3f> vertices;//VS2008
|
||
|
vector<Vec3u> indices;//VS2008
|
||
|
Mesh mesh;
|
||
|
const double intersection_distance=1;
|
||
|
int intersection_model =0;
|
||
|
|
||
|
typedef P Point3;
|
||
|
typedef P Vector3;
|
||
|
|
||
|
|
||
|
//�ж�y�Ƿ���[x-margin,z+margin]�ķ�Χ��
|
||
|
inline bool inmid(double x,double y,double z,double margin=MARGIN){
|
||
|
return y>=x-margin&&y<=z+margin;
|
||
|
}
|
||
|
inline bool inbox(P& p){
|
||
|
return
|
||
|
inmid(MINX,p.x,MAXX)
|
||
|
&&inmid(MINY,p.y,MAXY)
|
||
|
&&inmid(MINZ,p.z,MAXZ);
|
||
|
}
|
||
|
|
||
|
//��d��0�Ƚϣ���������
|
||
|
inline int dcmp(double d){
|
||
|
if(d<-eps)return -1;
|
||
|
else if(d>eps)return 1;
|
||
|
return 0;
|
||
|
}
|
||
|
|
||
|
//�����ľ��뺯��
|
||
|
|
||
|
inline double distan1(P &p1,P &p2){
|
||
|
double x=p1.x-p2.x,y=p1.y-p2.y,z=p1.z-p2.z;
|
||
|
return sqrt(x*x+y*y+z*z);
|
||
|
}
|
||
|
/*
|
||
|
inline double distan2(P &p1,P &p2){
|
||
|
static double penalty_par=1;
|
||
|
if(intersection_model==1)
|
||
|
{
|
||
|
LineSegment lineSegment(Vec3f(p1.x, p1.y, p1.z),Vec3f(p2.x, p2.y, p2.z));
|
||
|
static BVH_intersection bvh(mesh);
|
||
|
bool hit = bvh.intersectWithLineSegment(lineSegment);
|
||
|
if(hit==0)
|
||
|
penalty_par=1;
|
||
|
else
|
||
|
penalty_par=100;
|
||
|
}
|
||
|
double x=p1.x-p2.x,y=p1.y-p2.y,z=p1.z-p2.z;
|
||
|
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 Length(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 DistanceToPlane(Point3 &p,Point3 &p0,Vector3 &n)
|
||
|
{
|
||
|
return fabs(Dot(p-p0,n));//������ȡ����ֵ���õ�������������
|
||
|
}
|
||
|
inline int ParallelorVertical(P& p1,P& p2){
|
||
|
double angel=Angle(p1,p2);
|
||
|
if(angel<=minAngle||angel>=pi-minAngle)return 1;
|
||
|
else if(angel>=pi/2-minAngle&&angel<=pi/2+minAngle)return 2;
|
||
|
return 0;
|
||
|
|
||
|
}
|
||
|
inline Point3 GetPlaneProjection(Point3 &p,Point3 &p0,Vector3 &n)
|
||
|
{
|
||
|
return p-n*Dot(p-p0,n);
|
||
|
}
|
||
|
inline Point3 LinePlaneIntersection(Point3 &p1,Point3 &p2,Point3 &p0,Vector3 &n)
|
||
|
{
|
||
|
Vector3 v=p2-p1;
|
||
|
double t=(Dot(n,p0-p1)/Dot(n,p2-p1));//�жϷ�ĸ�Ƿ�Ϊ 0
|
||
|
return p1+v*t;//�������߶Σ��ж� t �Dz����� 0 �� 1 ֮��
|
||
|
}
|
||
|
inline Vector3 Cross(Vector3 A,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);
|
||
|
}
|
||
|
inline double Area2(Point3 &A,Point3 &B,Point3 &C){return Length(Cross(B-A,C-A));}
|
||
|
|
||
|
|
||
|
/*���������߷��������������߳��ȵ��ۺ�Ȩֵ����
|
||
|
A����B type!=0ʱ�������Dz��ǿ�������Ӧ��inOutû������
|
||
|
inOut1=0��������A��dir��inOut1=1��������A��dir
|
||
|
inOut2=0��������B��dir��inOut2=1��������B��dir
|
||
|
*/
|
||
|
inline double distan(P A,P B,int inOut1,int inOut2){
|
||
|
//static int out1;
|
||
|
//cout<<"out:"<<out1++<<endl;
|
||
|
static double penalty_par_intersection=1;
|
||
|
static double penalty_par_distance=1;
|
||
|
|
||
|
//cout<<"out1"<<endl;
|
||
|
|
||
|
double angel=Angle(A-B,DX);
|
||
|
if(angel>pi/2)angel=pi-angel;
|
||
|
angel=min(angel,pi/2-angel);
|
||
|
double len=distan1(A,B);
|
||
|
|
||
|
if(intersection_model==1)
|
||
|
{
|
||
|
LineSegment lineSegment(Vec3f(A.x, A.y, A.z),Vec3f(B.x, B.y, B.z));
|
||
|
static BVH_intersection bvh(mesh);
|
||
|
bool hit = bvh.intersectWithLineSegment(lineSegment);
|
||
|
if(hit==0)
|
||
|
penalty_par_intersection=1;
|
||
|
else
|
||
|
penalty_par_intersection=400;
|
||
|
if(len>intersection_distance)
|
||
|
penalty_par_distance=8;
|
||
|
else
|
||
|
penalty_par_distance=1;
|
||
|
|
||
|
|
||
|
}
|
||
|
|
||
|
double len1=sqrt((A.y-Ycenter)*(A.y-Ycenter)+(A.z-Zcenter)*(A.z-Zcenter));
|
||
|
double len2=sqrt((B.y-Ycenter)*(B.y-Ycenter)+(B.z-Zcenter)*(B.z-Zcenter));
|
||
|
if(len1<R||len2<R){
|
||
|
double angel1=Angle(A-B,DX);
|
||
|
if(angel1>pi/2)angel1=pi-angel1;
|
||
|
|
||
|
double angel2=Angle(A-B,DY);
|
||
|
if(angel2>pi/2)angel2=pi-angel2;
|
||
|
|
||
|
double angel3=Angle(A-B,DZ);
|
||
|
if(angel3>pi/2)angel3=pi-angel3;
|
||
|
|
||
|
angel=min(angel1,min(angel2,angel3));
|
||
|
}
|
||
|
|
||
|
P C;
|
||
|
if(inOut1)A.reverse();
|
||
|
C.x=A.dx;C.y=A.dy;C.z=A.dz;
|
||
|
double angel2=Angle(B-A,C);
|
||
|
if(A.isend==1||A.type!=0)angel2=0;
|
||
|
|
||
|
if(inOut2)B.reverse();
|
||
|
C.x=B.dx;C.y=B.dy;C.z=B.dz;
|
||
|
double angel3=Angle(B-A,C);
|
||
|
if(B.isend==1||B.type!=0)angel3=0;
|
||
|
|
||
|
//return pow(len,angel*3+1);
|
||
|
double orign_distance=len*(angel*4+1)+300*600*(angel2+angel3)/len;
|
||
|
|
||
|
//cout<<"dir:"<<orign_distance<<" "<<"patch_par"<<patch_par<<endl;
|
||
|
//return len*(angel*4+1)+300*600*(angel2+angel3)/len;
|
||
|
return orign_distance*penalty_par_intersection*penalty_par_distance;
|
||
|
}
|
||
|
|
||
|
/*���������߷��������������߳��ȵ��ۺ�Ȩֵ����
|
||
|
A��B�Զ�ѡ�������ʵ�dir
|
||
|
*/
|
||
|
//----------------------mark----------------------
|
||
|
inline double distan(P A,P B){
|
||
|
//static int without1;
|
||
|
//cout<<"without:"<<without1++<<endl;
|
||
|
static double penalty_par_intersection=1;
|
||
|
static double penalty_par_distance=1;
|
||
|
double angel=Angle(A-B,DX);
|
||
|
if(angel>pi/2)angel=pi-angel;
|
||
|
angel=min(angel,pi/2-angel);
|
||
|
double len=distan1(A,B);
|
||
|
|
||
|
if(intersection_model==1)
|
||
|
{
|
||
|
LineSegment lineSegment(Vec3f(A.x, A.y, A.z),Vec3f(B.x, B.y, B.z));
|
||
|
static BVH_intersection bvh(mesh);
|
||
|
bool hit = bvh.intersectWithLineSegment(lineSegment);
|
||
|
if(hit==0)
|
||
|
penalty_par_intersection=1;
|
||
|
else
|
||
|
penalty_par_intersection=400;
|
||
|
if(len>intersection_distance)
|
||
|
penalty_par_distance=8;
|
||
|
else
|
||
|
penalty_par_distance=1;
|
||
|
}
|
||
|
|
||
|
double len1=sqrt((A.y-Ycenter)*(A.y-Ycenter)+(A.z-Zcenter)*(A.z-Zcenter));
|
||
|
double len2=sqrt((B.y-Ycenter)*(B.y-Ycenter)+(B.z-Zcenter)*(B.z-Zcenter));
|
||
|
if(len1<R||len2<R){
|
||
|
double angel1=Angle(A-B,DX);
|
||
|
if(angel1>pi/2)angel1=pi-angel1;
|
||
|
|
||
|
double angel2=Angle(A-B,DY);
|
||
|
if(angel2>pi/2)angel2=pi-angel2;
|
||
|
|
||
|
double angel3=Angle(A-B,DZ);
|
||
|
if(angel3>pi/2)angel3=pi-angel3;
|
||
|
|
||
|
angel=min(angel1,min(angel2,angel3));
|
||
|
}
|
||
|
|
||
|
|
||
|
P C;
|
||
|
C.x=A.dx;C.y=A.dy;C.z=A.dz;
|
||
|
double angel2=Angle(B-A,C);
|
||
|
angel2=min(angel2,pi-angel2);
|
||
|
if(A.isend==1||A.type!=0)angel2=0;
|
||
|
|
||
|
C.x=B.dx;C.y=B.dy;C.z=B.dz;
|
||
|
double angel3=Angle(B-A,C);
|
||
|
angel3=min(angel3,pi-angel3);
|
||
|
if(B.isend==1||B.type!=0)angel3=0;
|
||
|
|
||
|
//return pow(len,angel*3+1);
|
||
|
double orign_distance=len*(angel*4+1)+300*600*(angel2+angel3)/len;
|
||
|
return orign_distance*penalty_par_intersection;
|
||
|
|
||
|
}
|
||
|
|
||
|
|
||
|
//��ӡ·����Ϣ
|
||
|
void printPath(vector<P> vecp){
|
||
|
for(int j=0;j<vecp.size();j++){
|
||
|
P pp=vecp[j];
|
||
|
cout<<setprecision(10)<<"("<<pp.x<<","<<pp.y<<","<<pp.z<<")";
|
||
|
if(j!=vecp.size()-1)cout<<"->";
|
||
|
}
|
||
|
cout<<endl;
|
||
|
return;
|
||
|
}
|