使用Graham扫描法求二维凸包的一个程序

1 #include <iostream>
2 #include <cstring>
3 #include <cstdlib>
4 #include <cmath>
5 #include <cstdio>
6 using namespace std;

includeall.h

  1 #include "includeall.h"
  2 typedef struct node{//一维链表使用的
  3     double x, y, z;
  4     struct node * next;
  5 }Node;
  6 
  7 class Link{//有头节点的链表类
  8 private:
  9 
 10 public:
 11     Node * data=NULL;
 12 private:
 13     void init()//初始化链表
 14     {
 15         this->data=NULL;
 16         this->data=new Node;
 17         this->data->x=this->data->y=this->data->z=0.0;
 18         this->data->next=NULL;
 19     }
 20     
 21     void destroy()//清除链表占用的空间，包括头节点，所以之后链表不能再继续使用
 22     {
 23         this->clear();
 24         delete this->data;
 25         this->data=NULL;
 26     }
 27     
 28     void check()//检验链表是否有效
 29     {
 30         if(this->data==NULL)
 31         {
 32             cout<<"链表未正确初始化或已经被销毁"<<endl;
 33             exit(0);
 34         }
 35     }
 36     
 37 public:
 38 
 39     void add(double x,double y,double z)//增加一个节点
 40     {
 41         this->check();
 42         Node * tmp=new Node;
 43         tmp->x=x;tmp->y=y;tmp->z=z;
 44         tmp->next=this->data->next;
 45         this->data->next=tmp;
 46     }
 47     
 48     void del(Node * prev)//删除一个节点
 49     {
 50         this->check();
 51         Node * tmp=prev->next;
 52         prev->next=prev->next->next;
 53         delete tmp;
 54     }
 55     
 56     void insert(Node * prev,double x,double y,double z)//在某个元素的后面插入一个节点
 57     {
 58         this->check();
 59         Node * tmp=new Node;
 60         tmp->x=x;tmp->y=y;tmp->z=z;
 61         tmp->next=prev->next;
 62         prev->next=tmp;
 63     }
 64     
 65     int count()//统计链表中节点的数目
 66     {
 67         this->check();
 68         int i=0;
 69         Node * tmp=this->data->next;;
 70         while(tmp!=NULL)
 71         {
 72             tmp=tmp->next;
 73             ++i;
 74         }
 75         return i;
 76     }
 77     
 78     Node * pointOfSpecificElement(int n)//获取指向特定第几个元素的指针
 79     {
 80         Node * tmp=this->data;
 81         while(tmp!=NULL)
 82         {
 83             if(n<=0) break;
 84             --n;
 85             tmp=tmp->next;
 86         }
 87         return tmp;
 88     }
 89     
 90     void clear()//清空链表中的所有元素，不包括头节点
 91     {
 92         this->check();
 93         while(!this->empty())
 94         {
 95             this->del(this->data);
 96         }
 97     }
 98     
 99     void copy(Link * link,int n=1)//将参数链表中从第n个开始的元素拷贝增加到this链表，头节点作为第0个
100     {
101         Node * tmp=link->pointOfSpecificElement(n);
102         while(tmp!=NULL)
103         {
104             this->add(tmp->x,tmp->y,tmp->z);
105             tmp=tmp->next;
106         }
107     }
108     
109     
110     bool empty()//链表是否为空
111     {
112         this->check();
113         return (this->data->next==NULL);
114     }
115     
116     Link()
117     {
118         this->init();
119     }
120     virtual ~Link()
121     {
122         this->destroy();
123     }
124 };

Link.class.h

  1 int RightOrLeft(double x1,double y1,double x2,double y2,double x3,double y3)//判断第三个点在前两个点连成的直线的哪个位置，-1 左边，0，直线上，1 右边
  2 {
  3     int result;
  4     if(x1-x2<0.000001 && x1-x2>-0.000001 && y1-y2<0.000001 && y1-y2>-0.000001)
  5     {
  6         return 1;
  7     }
  8     double X=(y3-y1)*(x2-x1)/(y2-y1)+x1;
  9 
 10     if(X-x3<-0.000001)
 11     {
 12         result=1;
 13     }
 14     else if(X-x3>0.000001)
 15     {
 16         result=-1;
 17     }
 18     else{
 19         result=0;
 20     }
 21     if(y2-y1<-0.000001)
 22     {
 23         result=-result;
 24     }
 25     return result;
 26 }
 27 
 28 
 29 /*
 30 *该函数对link的所有点进行二维凸包运算
 31 *link中的每个节点保存了一个点的三维坐标，二维凸包运输只会选取其中的两个坐标进行运算
 32 *具体选取哪两个坐标由该函数的type参数决定
 33 *type的合法取值为 3 5 6，取三代表选取yz坐标（3的二进制为011），取5代表选取xz坐标，取6代表选取xy坐标
 34 *执行完凸包运算之后，link中的节点将被修改，所以如有必要，应该手动在调用该函数之前备份参数链表
 35 *运算执行完之后，link的节点的z坐标将被清零，xy坐标将依据type的取值，对应于原始链表的某两个坐标
 36 *
 37 */
 38 void convelHull(Link * link,int type=5)//对link中的元素进行二维凸包运算，执行后link中只有x,y值有效
 39 {
 40     if(link->count()<1)
 41     {
 42         printf("Warning: no node in link\n");
 43         return;
 44         //exit(-1);
 45     }
 46     Link * link_tmp=new Link();
 47     {//对利用link的某两个坐标构造link_tmp
 48         Node * tmp=link->pointOfSpecificElement(1);
 49         if(type==3)
 50         {
 51             while(tmp!=NULL)
 52             {
 53                 //此处将link的三维坐标系的某两个映射到函数内使用的link_tmp中，之后的操作将对link_tmp的x,y成员进行操作
 54                 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 55                 //可以在此处修改需要对原始的link链表中的哪两个坐标进行凸包运算
 56                 link_tmp->add(tmp->y,tmp->z,0.0);
 57                 //进行凸包运算之后，link的z坐标将被清零，例如此处，link的xy坐标分别对应原始的xz坐标
 58                 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 59                 tmp=tmp->next;
 60             }
 61         }
 62         else if(type==5)
 63         {
 64             while(tmp!=NULL)
 65             {
 66                 //此处将link的三维坐标系的某两个映射到函数内使用的link_tmp中，之后的操作将对link_tmp的x,y成员进行操作
 67                 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 68                 //可以在此处修改需要对原始的link链表中的哪两个坐标进行凸包运算
 69                 link_tmp->add(tmp->x,tmp->z,0.0);
 70                 //进行凸包运算之后，link的z坐标将被清零，例如此处，link的xy坐标分别对应原始的xz坐标
 71                 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 72                 tmp=tmp->next;
 73             }
 74         }
 75         else if(type==6)
 76         {
 77             while(tmp!=NULL)
 78             {
 79                 //此处将link的三维坐标系的某两个映射到函数内使用的link_tmp中，之后的操作将对link_tmp的x,y成员进行操作
 80                 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 81                 //可以在此处修改需要对原始的link链表中的哪两个坐标进行凸包运算
 82                 link_tmp->add(tmp->x,tmp->y,0.0);
 83                 //进行凸包运算之后，link的z坐标将被清零，例如此处，link的xy坐标分别对应原始的xz坐标
 84                 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 85                 tmp=tmp->next;
 86             }
 87         }
 88         else{
 89             printf("参数不符合规范\n");
 90             exit(-1);
 91         }
 92     }//link_tmp已经构造完成
 93     {//将link_tmp中y坐标最小的点放到链表的第一个位置
 94         Node * tmp=link_tmp->pointOfSpecificElement(0);
 95         Node * tmp2=tmp;
 96         while(tmp!=NULL && tmp->next!=NULL)
 97         {
 98             if(tmp->next->y<tmp2->next->y)
 99             {
100                 tmp2=tmp;
101             }
102             tmp=tmp->next;
103         }//tmp2指向y坐标最小的节点的前一个节点
104         link_tmp->add(tmp2->next->x,tmp2->next->y,tmp2->next->z);
105         link_tmp->del(tmp2);
106     }//y坐标最小的元素已经是链表的第一个节点
107     Node trans;//保存此时平移的距离，以便于最后把它们平移回来
108     {//所有点平移，使link_tmp第一个节点的坐标是坐标原点
109         trans.x=link_tmp->pointOfSpecificElement(1)->x;
110         trans.y=link_tmp->pointOfSpecificElement(1)->y;
111         //trans.z=link_tmp->pointOfSpecificElement(1)->z;
112         Node * tmp=link_tmp->pointOfSpecificElement(1);
113         while(tmp!=NULL)
114         {
115             tmp->x-=trans.x;
116             tmp->y-=trans.y;
117             //tmp->z-=trans.z;
118             tmp=tmp->next;
119         }
120     }//所有点的平移完成
121     {//从第二个点开始按与第一个点的距离从小到大排序
122         Node * tmp1=link_tmp->pointOfSpecificElement(0);
123         Node * tmp2=tmp1->next;
124         while(tmp2!=NULL)
125         {
126             //使用z成员保存其到原点的距离
127             tmp2->z=sqrt((tmp2->x* tmp2->x)+(tmp2->y*tmp2->y));
128             tmp2=tmp2->next;
129         }
130         tmp2=tmp1->next;
131         //int count=0;
132         while(tmp2!=NULL && tmp2->next!=NULL)
133         {//保证只有第一个点在坐标原点
134             if(tmp2->next->z==0.0)
135             {
136                 link_tmp->del(tmp2);
137                 //++count;
138                 continue;
139             }
140             tmp2=tmp2->next;
141         }
142         //printf("去重次数：%d\n",count);
143         /*if(link_tmp->count()<3)
144         {
145             printf("对链表进行去重操作之后导致链表中剩余元素不足三个，无法进行后续运算\n");
146             exit(-1);
147         }*/
148         tmp2=tmp1->next->next;
149         while(tmp2!=NULL)
150         {
151             Node * minDis=tmp2;
152             Node * tmp3=tmp2;
153             while(tmp3!=NULL)
154             {
155                 if(tmp3->z-minDis->z<-0.000001)
156                 {
157                     minDis=tmp3;
158                 }
159                 tmp3=tmp3->next;
160             }
161             if(minDis!=tmp2)
162             {
163                 Node tmp;
164                 tmp.x=minDis->x;
165                 tmp.y=minDis->y;
166                 tmp.z=minDis->z;
167                 minDis->x=tmp2->x;
168                 minDis->y=tmp2->y;
169                 minDis->z=tmp2->z;
170                 tmp2->x=tmp.x;
171                 tmp2->y=tmp.y;
172                 tmp2->z=tmp.z;
173             }
174             tmp2=tmp2->next;
175         }
176     }//按照距离排序完成
177     /*{//输出排序完的点的顺序
178         Node * tmp=link_tmp->pointOfSpecificElement(1);
179         printf("距离排序后的点（%d）：\n",link_tmp->count());
180         while(tmp!=NULL)
181         {
182             printf("%.10f %.10f %.10f\n",tmp->x+trans.x,tmp->y+trans.y,tmp->z);
183             tmp=tmp->next;
184         }
185     }*/
186     {//从第二个点开始按与第一个点的幅角从小到大排序
187         Node * tmp1=link_tmp->pointOfSpecificElement(1);
188         Node * tmp2=tmp1->next;
189         while(tmp2!=NULL)
190         {
191             //使用z成员保存其幅角
192             tmp2->z=acos((tmp2->x/tmp2->z));
193             tmp2=tmp2->next;
194         }
195         tmp2=tmp1->next;
196         while(tmp2!=NULL)
197         {
198             Node * tmp3=tmp2->next;
199             Node * tmp4=tmp1->next;
200             while(tmp3!=NULL)
201             {
202                 if(tmp4->next->z-tmp4->z<-0.000001)
203                 {
204                     Node tmp;
205                     tmp.x=tmp4->x;
206                     tmp.y=tmp4->y;
207                     tmp.z=tmp4->z;
208                     tmp4->x=tmp4->next->x;
209                     tmp4->y=tmp4->next->y;
210                     tmp4->z=tmp4->next->z;
211                     tmp4->next->x=tmp.x;
212                     tmp4->next->y=tmp.y;
213                     tmp4->next->z=tmp.z;
214                 }
215                 tmp4=tmp4->next;
216                 tmp3=tmp3->next;
217             }
218             tmp2=tmp2->next;
219         }
220     }//按照幅角排序完成
221     /*{//输出排序完的点的顺序
222         Node * tmp=link_tmp->pointOfSpecificElement(1);
223         printf("幅角排序后的点（%d）：\n",link_tmp->count());
224         while(tmp!=NULL)
225         {
226             printf("%.10f %.10f %.10f\n",tmp->x+trans.x,tmp->y+trans.y,tmp->z);
227             tmp=tmp->next;
228         }
229     }*/
230     {//对其进行求凸包运算
231         Link * stk_tmp=new Link();
232         Node * tmp=link_tmp->pointOfSpecificElement(1);
233         stk_tmp->add(tmp->x,tmp->y,0.0);
234         tmp=tmp->next;
235         if(tmp!=NULL) stk_tmp->add(tmp->x,tmp->y,0.0);
236         if(link_tmp->count()>=3)
237         {
238             Node * stkTop=stk_tmp->pointOfSpecificElement(1);//指向栈顶元素
239             Node * stkNext=stkTop->next;//指向栈顶的下一个元素
240             Node * current=tmp->next;//指向当前点
241             //int count1=0,count2=0;
242             while(1)
243             {
244                 if(RightOrLeft(stkNext->x,stkNext->y,stkTop->x,stkTop->y,current->x,current->y)==1)
245                 {
246                     //++count1;
247                     stk_tmp->del(stk_tmp->pointOfSpecificElement(0));
248                     stkTop=stk_tmp->pointOfSpecificElement(1);
249                     stkNext=stkTop->next;
250                 }
251                 else{
252                     //++count2;
253                     stk_tmp->add(current->x,current->y,0.0);
254                     //cout<<"入栈"<<current->x+trans.x<<" "<<current->y+trans.y<<endl;
255                     stkTop=stk_tmp->pointOfSpecificElement(1);
256                     stkNext=stkTop->next;
257                     if(current->next==NULL) break;
258                     else{
259                         current=current->next;
260                     }
261                 }
262             }//end of while
263         }
264         //printf("入栈次数：%d\n",count2);
265         //printf("出栈次数：%d\n",count1);
266         //现在栈 stk_tmp中保存有凸包上的点
267         {//对凸包上的点平移到原位置
268             Node * tmp=stk_tmp->pointOfSpecificElement(1);
269             while(tmp!=NULL)
270             {
271                 tmp->x+=trans.x;
272                 tmp->y+=trans.y;
273                 //tmp->z+=trans.z;
274                 tmp=tmp->next;
275             }
276         }//平移到原位置完成
277         delete link_tmp;
278         link->clear();
279         link->copy(stk_tmp);
280         delete stk_tmp;
281     }//凸包运算完成
282 }// end of function convelHull

convelHull.func.h

#include "includeall.h"
#include "Link.class.h"
#include "convelHull.func.h"

char * filename1=(char *)"./data/凸包问题输入.asc";
char * filename2 = (char *)"./data/tubao_out.asc";
void constructBody(Link * link,char * filename)//由点云文件构造点云模型
{
    FILE * fp;
    if (fp = fopen(filename, "r"), fp == NULL)
    {
        cout << "文件打开失败，文件路径：" << filename << endl;
        exit(0);
    }
    fseek(fp,0,SEEK_SET);
    float x,y,z;
    int count=0;
    while(!feof(fp))
    {
        fscanf(fp,"%f %f %f\n",&x,&y,&z);
        
        ++count;
        if(count>=1)
        {
            link->add(x*1,y*1,z*1);
            count=0;
        }
    }
    fclose(fp);
}
void saveToFile(Link * link,char * filename)//将一个链表中的内容保存到指定的文件中
{
    FILE * fp=NULL;
    if(fp=fopen(filename,"w"),fp==NULL) exit(0);
    Node * tmp=link->data->next;
    while(tmp!=NULL)
    {
        fprintf(fp,"%.5f %.5f %.5f\n",tmp->x,tmp->y,tmp->z);
        tmp=tmp->next;
    }
    fclose(fp);
}

int main()
{
    Link * link=new Link();

    /*  link->add(0,0,-1);
     link->add(0,0,0);
     link->add(0,0,1);
     link->add(0,0,2);
     
     link->add(-1,0,-1);
     link->add(-1,0,0);
     link->add(-1,0,1);
     link->add(-1,0,2);
     
     link->add(-2,0,-1);
     link->add(-2,0,0);
     link->add(-2,0,1);
     link->add(-2,0,2);
     
     link->add(1,0,-1);
     link->add(1,0,0);
     link->add(1,0,1);
     link->add(1,0,2);
     
     link->add(2,0,-1);
     link->add(2,0,0);
     link->add(2,0,1);
     link->add(2,0,2);

     link->add(-2,0,-2);
     link->add(-1,0,-2);
     link->add(0,0,-2);
     link->add(1,0,-2);
     link->add(2,0,-2);
     link->add(3,0,-2);
     link->add(5,0,0);
     link->add(-5,0,0); */
     
    /*  link->add(0,0,0);
     link->add(1,0,1);
     link->add(2,0,2);
     link->add(3,0,3);
     link->add(4,0,4);
     link->add(5,0,5);
     link->add(6,0,6);
     */
    //constructBody(link,filename1);
    link->add(1,1,0);
    link->add(2,2,0);
    link->add(3,3,0);
    Node * tmp=link->pointOfSpecificElement(1);
    printf("未进行凸包运算的点（%d）：\n",link->count());
    while(tmp!=NULL)
    {
        printf("%f %f %f\n",tmp->x,tmp->y,tmp->z);
        tmp=tmp->next;
    }
    printf("\n");
    convelHull(link,6);
    saveToFile(link,filename2);
    tmp=link->pointOfSpecificElement(1);
    printf("进行过凸包运算的点（%d）：\n",link->count());
    while(tmp!=NULL)
    {
        printf("%f %f %f\n",tmp->x,tmp->y,tmp->z);
        tmp=tmp->next;
    }
    printf("\n");
    printf("test: %d\n",RightOrLeft(0,0,1,1,-1,0));
    printf("test: %d\n",RightOrLeft(0,0,-1,1,1,0));
    printf("test: %d\n",RightOrLeft(0,0,-1,-1,-1,0));
    printf("test: %d\n",RightOrLeft(0,0,1,-1,0,-1));
    return 0;
}

main.cpp

就当是记录一下，程序写得不完美

posted @ 2015-11-08 12:25 张不正阅读(270) 评论(0) 编辑收藏举报

会员力量，点亮园子希望

刷新页面返回顶部

zhang

站在站在巨人的肩膀上的人的肩膀上

使用Graham扫描法求二维凸包的一个程序

公告