1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54
|
int I1, I2, I3, I4, I5,I6,I7,I8,I9,I10,I11,I12; int i1,i2,i3,i4,i5,i6,i7,i8,i9,i10,i11,i12;
I1=70; I2=70; I3=70; I4=70; I5=456; I6=456; I7=456; I8=456; I9=456; I10=456; I11=456; I12=456;
map<double,int> p0obj, p1obj, p2obj, size_p; int count = 0;
int d[17]; std::ofstream s("filename");
boost::archive::text_oarchive oa(s);
for(int p0=0;p0<a.size() && ros::ok();p0+=5) {
for(int p1=0;p1<a.size() && ros::ok();p1++) {
int d1 = sqrt(pow(a.at(p1)-a.at(p0),2)+pow(b.at(p1)-b.at(p0),2)+pow(c.at(p1)-c.at(p0),2))*1000;
if(d1==20) {
for(int p2=0;p2<a.size() && ros::ok();p2++) {
int d2 = sqrt(pow(a.at(p2)-a.at(p0),2)+pow(b.at(p2)-b.at(p0),2)+pow(c.at(p2)-c.at(p0),2))*1000;
int d1d = sqrt(pow(a.at(p2)-a.at(p1),2)+pow(b.at(p2)-b.at(p1),2)+pow(c.at(p2)-c.at(p1),2))*1000;
if(d2==20 && d1d==20) {
float a1 = a.at(p1)-a.at(p0); float b1 = b.at(p1)-b.at(p0); float c1 = c.at(p1)-c.at(p0);
float a2 = a.at(p2)-a.at(p0); float b2 = b.at(p2)-b.at(p0); float c2 = c.at(p2)-c.at(p0);
float a3r = b1*c2-b2*c1; float b3r = a2*c1-a1*c2; float c3r = a1*b2-a2*b1;
float a3, b3, c3;
if(c3r>0) {
a3 = a3r/sqrt(a3r*a3r+b3r*b3r+c3r*c3r);
b3 = b3r/sqrt(a3r*a3r+b3r*b3r+c3r*c3r);
c3 = -c3r/sqrt(a3r*a3r+b3r*b3r+c3r*c3r);
}
else {
a3 = a3r/sqrt(a3r*a3r+b3r*b3r+c3r*c3r);
b3 = b3r/sqrt(a3r*a3r+b3r*b3r+c3r*c3r);
c3 = c3r/sqrt(a3r*a3r+b3r*b3r+c3r*c3r);
}
float x3 = (a.at(p0)+a.at(p1)+a.at(p2)/3)+0.02*a3;
float y3 = (b.at(p0)+b.at(p1)+b.at(p2)/3)+0.02*b3;
float z3 = (c.at(p0)+c.at(p2)+c.at(p2)/3)+0.02*c3;
for(int p4=0;p4<a.size() && ros::ok();p4++) {
int d0r = sqrt(pow(a.at(p4)-a.at(p0),2)+pow(b.at(p4)-b.at(p0),2)+pow(c.at(p4)-c.at(p0),2))*1000;
int d1r = sqrt(pow(a.at(p4)-a.at(p1),2)+pow(b.at(p4)-b.at(p1),2)+pow(c.at(p4)-c.at(p1),2))*1000;
int d2r = sqrt(pow(a.at(p4)-a.at(p2),2)+pow(b.at(p4)-b.at(p2),2)+pow(c.at(p4)-c.at(p2),2))*1000;
int d4r = sqrt(pow(x3-a.at(p1),2)+pow(y3-b.at(p1),2)+pow(z3-c.at(p1),2));
if(d0r>0 && d1r>0 && d2r>0 && d4r>0 && d0r<=70) {
i1=d0r; i2=d1r; i3=d2r; i4=d4r;
i5=ac.at(p0); i6=bc.at(p0);
i7=ac.at(p1); i8=bc.at(p1);
i9=ac.at(p2); i10=bc.at(p2);
i11=ac.at(p4); i12=bc.at(p4);
double key = i12+I12*(i11+I11*(i10+I10*(i9+I9*(i8+I8*(i7+I7*(i6+I6*(i5+I5*(i4+I4*(i3+I3*(i2+I2*i1))))))))));
p0obj[key] = p0; p1obj[key] = p1; p2obj[key] = p2; size_p[key]++;
oa << p0obj; oa << p1obj; oa << p2obj; oa << size_p;
cout<<++count<<"\t"<<key<<"\t"<<p0obj[key]<<"\t"<<p1obj[key]<<"\t"<<p2obj[key]<<"\t"<<size_p[key]<<endl;
}
}
}
}
}
}
}
cout<<"finish one loop"<<endl;
|