// https://github.com/orocos/orocos_kinematics_dynamics/blob/master/orocos_kdl/src/frames.hpp // Vector2 defination #include <iostream> #include <cstring> #include<string> #include <math.h> #include <vector> #include <stdlib.h> #include <frames.hpp> using namespace std; using namespace KDL; #define PI 3.1415926 class Point //二维点class { public: Vector2 point;//二维点 string name; Point(Vector2 v,string s){point=v;name=s;}; Point(Vector2 v){point=v;}; Point(double x,double y){point=Vector2(x,y);} Point(){}; void move_p(double x,double y);//平移算子 void rota_p(double an);//旋转算子 }; void Point::move_p(double x,double y){point+=Vector2(x,y);} void Point::rota_p(double an){ Rotation2 rot(-an/180*PI); point=rot.Inverse(point); } //class defination finished; ostream& operator << (ostream& os, const Point& X) { return os << '(' << X.point.x()<< ',' << X.point.y()<< ')'; } //重载运算符 class Shape //由点构成的二维图形class { public: string name;//名称 int n; //顶点数 vector<Point> points;//顶点容器 Shape(string s){name=s;} Shape(){} void getpoint(Point p){points.push_back(p);} void move_s(double x,double y){for(int i=0;i<points.size();i++)points[i].move_p(x,y);} void rota_s(double an){for(int j=0;j<points.size();j++)points[j].rota_p(an);} void show(void){cout<<"图形"<<name<<"里面有"<<points.size()<<"个点"<<endl;} void showpoints(void){for(int i=0;i<points.size();i++)cout<<points[i]<<' ';cout<<endl;} }; Vector2 get(string str){ double x,y;int i,j; i=str.find(',',1);j=str.find(')',1); string X=str.substr(1,i-1),Y=str.substr(i+1,j-i); x=atof(X.c_str());y=atof(Y.c_str());//str2char2double return Vector2(x,y); } //从字符串(x,y)中获得点坐标 ,返回一个矢量 void test_class(void){ Point p(3,2),p1(2,3),p2(4,5); Shape s1("mine"); s1.getpoint(p); s1.getpoint(p1); s1.getpoint(p2); string s="(0.2,1)"; cout<<get(s).x()<<' '<<get(s).y()<<endl; cout<<"图形"<<s1.name<<"里面有"<<s1.points.size()<<"个点"<<endl; vector<Shape> volume(0);//图形容器 volume.push_back(s1); cout<<"当前有"<<volume.size()<<"个图形"<<endl; s1.move_s(1,2); for(int i=0;i<s1.points.size();i++)cout<<s1.points[i]<<endl; } //测试基本类的定义及对其操作的有效性 int main(){ // test_class(); vector<Shape> volume;//图形容器 //开始任务循环 while(1){ string com; vector<string> info(0);//存放keywords(create/operate/status),name,data getline(cin,com);//输入命令行 int start=0,end=0; while(start!=string::npos&&start!=com.size()){ start=com.find_first_not_of(' ',start); end=com.find_first_of(' ',start); if(end==string::npos){end=com.size();} info.push_back(com.substr(start,end-start)); start=end; } if(info[0]=="create") { Shape s(info[1]); for(int i=0;i<info.size()-2;i++)s.getpoint(Point(get(info[i+2]))); volume.push_back(s); // s.show(); } else{ if(info[0]=="operate"){ if(info[1]=="move"){ int j=0;for(int i=0;i<volume.size();i++){if(volume[i].name==info[2])j=i;} if(j>=volume.size())cout<<"不存在该图形,请查询volume状态!"<<endl;else{ volume[j].move_s(get(info[3]).x(),get(info[3]).y()); cout<<"对"<<volume[j].name<<"做了平移操作" <<" ";cout<<"新的顶点:"<<" "; volume[j].showpoints(); } } else{ if(info[1]=="rotate"){ int j=0;for(int i=0;i<volume.size();i++){if(volume[i].name==info[2])j=i;} if(j>=volume.size())cout<<"不存在该图形,请查询volume状态!"<<endl;else{ volume[j].rota_s(atof(info[3].c_str()));} cout<<"对"<<volume[j].name<<"做了旋转操作" <<" ";cout<<"新的顶点:"<<" ";volume[j].showpoints(); } else cout<<"操作指令错误!"<<endl; } } else{ if(info[0]=="status"){cout<<"当前volume里面有"<<volume.size()<<"个图形"<<" "; for(int i=0;i<volume.size();i++){cout<<volume[i].name<<' ';}cout<<endl; } else cout<<"command error!!!"<<endl; } } }//任务循环结束 return 0; }
有一点小bug还需要修改。。。。基本功能已经修改好了