00001 #include "intersecting_faces.h"
00002
00003 #include <cassert>
00004 #include <cmath>
00005 #include <iostream>
00006
00007 #include "box.h"
00008 #include "container.h"
00009 #include "face.h"
00010 #include "misc.h"
00011 #include "nice.h"
00012 #include "object.h"
00013 #include "space.h"
00014 #include "state.h"
00015 #include "vertex.h"
00016
00017 using std::cout;
00018 using std::endl;
00019
00020 Intersecting_Faces * Intersecting_Faces::only_one = NULL;
00021
00022 Intersecting_Faces & Intersecting_Faces::instance(void)
00023 {
00024
00025
00026 if (only_one == NULL)
00027 only_one = new Intersecting_Faces();
00028
00029 return *only_one;
00030 }
00031
00032 Intersecting_Faces& Intersecting_Faces::operator= (const Intersecting_Faces& rhs)
00033 {
00034 cout << "Copy assignment operator prohibited on instances of Intersecting_Faces class.\n";
00035 cout << "Intersecting_Faces " << rhs.intf.size() << endl;
00036 exit(0);
00037 }
00038
00039 Intersecting_Faces::Intersecting_Faces (void)
00040 :intf()
00041 {
00042 }
00043
00044 Intersecting_Faces::Intersecting_Faces (const Intersecting_Faces & rhs)
00045 :intf()
00046 {
00047 cout << "Copy constructor prohibited on instances of Intersecting_Faces class.\n";
00048 cout << "Intersecting_Faces " << rhs.intf.size() << endl;
00049 exit(0);
00050 }
00051
00052 htff_cit Intersecting_Faces::begin (void)
00053 {
00054 return intf.begin();
00055 }
00056
00057 htff_cit Intersecting_Faces::end (void)
00058 {
00059 return intf.end();
00060 }
00061
00062 vec_fp * Intersecting_Faces::getIntersectingFacesRHS (Face const * const q)
00063 {
00064
00065 htff_it i = intf.find(const_cast<Face*>(q));
00066 if (i==intf.end())
00067 {
00068 cout << "Intersecting_Faces::getIntersectingFacesRHS: Error"
00069 << "Face was not intersected LHS.\n";
00070 exit(0);
00071 }
00072 else
00073 {
00074 return &((*i).second);
00075 }
00076 }
00077
00078 int Intersecting_Faces::getCountOfIntFaces (bool detect_self)
00079 {
00080
00081
00082
00083 int count = 0;
00084
00085 for (htff_it i = intf.begin();i!=intf.end();i++)
00086 {
00087
00088 assert((*i).second.empty()==false);
00089
00090 for(fp_it j=(*i).second.begin();j!=(*i).second.end();j++)
00091 {
00092
00093 assert(faceIsIntersectedLHS(*j)==true);
00094 assert(faceIntersectsFace(*j,(*i).first)==true);
00095 if (detect_self==true)
00096 {
00097 if ((*i).first->getVertex(0)->getObject()->getName() == (*j)->getVertex(0)->getObject()->getName())
00098 {
00099 count++;
00100 }
00101 }
00102 else
00103 {
00104 count++;
00105 }
00106 }
00107 }
00108 assert((count%2) == 0);
00109 return count/2;
00110 }
00111
00112 bool Intersecting_Faces::faceIntersectsFace (Face const * const haystack,Face const * const needle)
00113 {
00114
00115 vec_fp const * ifv=getIntersectingFacesRHS(haystack);
00116
00117 fp_cit i=find((*ifv).begin(),(*ifv).end(),needle);
00118
00119
00120 return (i!=(*ifv).end());
00121 }
00122
00123 bool Intersecting_Faces::faceIsIntersectedLHS (Face const * const i) const
00124 {
00125 return intf.find(const_cast<Face*>(i))!=intf.end();
00126 }
00127
00128 bool Intersecting_Faces::faceIsIntersectedRHS (Face const * const f) const
00129 {
00130
00131 htff_cit i = intf.find(const_cast<Face*>(f));
00132 if (i==intf.end())
00133 {
00134 cout << "Intersecting_Faces::faceIsIntersectedRHS: Error"
00135 << "Face was not intersected LHS.\n";
00136 exit(0);
00137 }
00138 else
00139 {
00140 return ((*i).second).empty()==false;
00141 }
00142 }
00143
00144 bool Intersecting_Faces::searchForNewFaceInts (Face * const f,bool flag)
00145 {
00146
00147 vec_bp bp;
00148 Space::instance().getBoxesToCheck(f,bp);
00149
00150 vec_fp of;
00151 of.reserve(VECTOR_RESERVE);
00152 for(bp_it i=bp.begin();i!=bp.end();i++)
00153 {
00154 if(flag==true)
00155 {
00156
00157
00158 State::instance().addNewFace(f,*i);
00159 }
00160
00161 of.insert(of.end(),(*i)->begin(),(*i)->end());
00162 }
00163
00164 sort(of.begin(),of.end());
00165 of.assign(of.begin(),unique(of.begin(),of.end()));
00166
00167 for(fp_it i=of.begin();i!=of.end();i++)
00168 {
00169
00170 if(*i!=f)
00171 {
00172
00173 if (checkFaceFaceInts(f,*i))
00174 {
00175 if(STRICT_FACE_INTERSECTION_PREVENTION==true)
00176 {
00177
00178 if(faceIsIntersectedLHS(f)==false){return true;}
00179
00180 else
00181 {
00182
00183 vec_fp *fv=getIntersectingFacesRHS(f);
00184 bool same=false;
00185
00186 for(fp_it p=(*fv).begin();p!=(*fv).end();p++)
00187 {
00188
00189
00190 if((*p)->getVertex(0)->getObject()==(*i)->getVertex(0)->getObject()){same=true;}
00191 }
00192
00193
00194 if(!same){return true;}
00195 }
00196 }
00197 else
00198 {
00199
00200 if(f->getVertex(0)->getObject()==(*i)->getVertex(0)->getObject()){return true;}
00201 }
00202 }
00203 }
00204 }
00205 return false;
00206 }
00207
00214 bool Intersecting_Faces::findAndRecordNewFaceInt (Face * const f)
00215 {
00216 bool flag = false;
00217
00218 vec_bp bp;
00219 Space::instance().getBoxesToCheck(f,bp);
00220
00221 vec_fp of;
00222 of.reserve(VECTOR_RESERVE);
00223 for(bp_it i=bp.begin();i!=bp.end();i++)
00224 {
00225
00226 of.insert(of.end(),(*i)->begin(),(*i)->end());
00227 }
00228
00229 sort(of.begin(),of.end());
00230 of.assign(of.begin(),unique(of.begin(),of.end()));
00231
00232 for(fp_it i=of.begin();i!=of.end();i++)
00233 {
00234
00235 if(*i!=f)
00236 {
00237
00238 if (checkFaceFaceInts(f,*i))
00239 {
00240
00241 addFaceToFace(f,*i);
00242
00243 flag = true;
00244 }
00245 }
00246 }
00247 return flag;
00248 }
00249
00256 bool Intersecting_Faces::vertAdjFacesHaveNewInt (Vertex const * const v)
00257 {
00258
00259 for(fp_cit i=v->begin();i!=v->end();i++)
00260 {
00261
00262 if(searchForNewFaceInts(*i,true)==true){return true;}
00263 }
00264
00265 return false;
00266 }
00267
00275 void Intersecting_Faces::addFaceToFace (Face * const lhs,Face const * const rhs)
00276 {
00277
00278 if(faceIsIntersectedLHS(lhs)==false)
00279 {
00280
00281 vec_fp nv;
00282 intf[lhs]=nv;
00283 }
00284
00285 if(faceIntersectsFace(lhs,rhs)==false)
00286 {
00287
00288 intf[lhs].push_back(const_cast<Face*>(rhs));
00289 }
00290 }
00291
00298 void Intersecting_Faces::setFaceNotIntersectedLHS (Face const * const f)
00299 {
00300
00301 if(faceIsIntersectedLHS(f)==true)
00302 {
00303
00304 vec_fp *ifv=getIntersectingFacesRHS(f);
00305
00306 if((*ifv).empty()==false)
00307 {
00308
00309 for(fp_it i=(*ifv).begin();i!=(*ifv).end();i++)
00310 {
00311
00312 removeFaceFromFaceInt(*i,f);
00313 }
00314 (*ifv).clear();
00315 }
00316
00317 intf.erase(const_cast<Face*>(f));
00318 }
00319 }
00320
00328 void Intersecting_Faces::removeFaceFromFaceInt (Face * const lhs,Face const * const rhs)
00329 {
00330
00331 if(faceIsIntersectedLHS(lhs)==true)
00332 {
00333
00334 vec_fp *ifv=getIntersectingFacesRHS(lhs);
00335
00336 fp_it i=find((*ifv).begin(),(*ifv).end(),rhs);
00337
00338 if (i!=(*ifv).end())
00339 {
00340
00341 (*ifv).erase(remove((*ifv).begin(),(*ifv).end(),rhs),(*ifv).end());
00342
00343 if (faceIsIntersectedRHS(lhs)==false)
00344 {
00345
00346 setFaceNotIntersectedLHS(lhs);
00347 }
00348 }
00349 }
00350 }
00351
00361 void Intersecting_Faces::removeOldIntersections (Vertex const * const v,hashset_v & ni)
00362 {
00363
00364
00365
00366
00367
00368 for(fp_cit k=v->begin();k!=v->end();k++)
00369 {
00370
00371 if(faceIsIntersectedLHS(*k)==true)
00372 {
00373
00374 vec_fp *fv=getIntersectingFacesRHS(*k);
00375 for(fp_it p=(*fv).begin();p!=(*fv).end();p++)
00376 {
00377
00378 ni.insert((*p)->getVertex(0));
00379 ni.insert((*p)->getVertex(1));
00380 ni.insert((*p)->getVertex(2));
00381
00382 removeFaceFromFaceInt(*p,*k);
00383 }
00384 }
00385
00386 setFaceNotIntersectedLHS(*k);
00387 }
00388 }
00389
00399 void Intersecting_Faces::updateNewIntersections (Vertex const * const v,hashset_v & ni)
00400 {
00401
00402 for(fp_cit k=v->begin();k!=v->end();k++)
00403 {
00404
00405 if (searchForNewFaceInts(*k,false))
00406 {
00407
00408 vec_fp *fv=getIntersectingFacesRHS(*k);
00409 for(fp_it p=(*fv).begin();p!=(*fv).end();p++)
00410 {
00411
00412 ni.insert((*p)->getVertex(0));
00413 ni.insert((*p)->getVertex(1));
00414 ni.insert((*p)->getVertex(2));
00415
00416 if(faceIsIntersectedLHS(*p)==false)
00417 {
00418
00419 vec_fp nv;
00420 intf[*p]=nv;
00421 }
00422
00423 addFaceToFace(*p,*k);
00424 }
00425 }
00426 }
00427 }
00428
00436 hashset_v Intersecting_Faces::getNiceCheckSet (Vertex const * const v)
00437 {
00438 hashset_v ni;
00439
00440 ni.insert(const_cast<Vertex*>(v));
00441 removeOldIntersections(v,ni);
00442 updateNewIntersections(v,ni);
00443 return ni;
00444 }
00445
00454 void Intersecting_Faces::getNiceSet (vec_vp & fs,hashset_v & ni)
00455 {
00456
00457 for(hv_it i=ni.begin();i!=ni.end();i++)
00458 {
00459
00460 if(Nice::instance().updateVertexNiceness(*i))
00461 {
00462
00463
00464
00465
00466
00467 fs.push_back(*i);
00468
00469 }
00470 }
00471 }
00472
00478 void Intersecting_Faces::findAllFaceIntersections (void)
00479 {
00480 cout << "Find all face intersections....................";
00481 cout.flush();
00482 int k=1;
00483 double goal = 0.2;
00484 cout << "0%..";
00485 cout.flush();
00486
00487 intf.clear();
00488
00489 double a = 1.0/Container::instance().o.size();
00490 for (o_it i=Container::instance().o.begin();i!=Container::instance().o.end();i++)
00491 {
00492
00493 for (f_it j=i->f.begin();j!=i->f.end();j++)
00494 {
00495 findAndRecordNewFaceInt(&(*j));
00496 }
00497
00498 double progress = static_cast<double>(k++)*a;
00499 if(progress>goal)
00500 {
00501 cout << static_cast<int>(goal*100) << "%..";
00502 cout.flush();
00503 goal+=0.2;
00504 }
00505 }
00506 cout << "100%..complete.\n";
00507 cout.flush();
00508 }
00509
00517 void Intersecting_Faces::getFaceIntersectionForce (Face * const f,double * const & total_force)
00518 {
00519 vec_d d(3,0);
00520
00521 assert(faceIsIntersectedLHS(f)==true && faceIsIntersectedRHS(f)==true );
00522
00523 vec_fp* ptr = getIntersectingFacesRHS(f);
00524
00525 for(fp_it i=ptr->begin();i!=ptr->end();i++)
00526 {
00527 getFaceFaceIntForce(f,*i,d);
00528 }
00529 for(int i=0;i<3;i++) {total_force[0]+=d[0];}
00530 }
00531
00540 void Intersecting_Faces::getFaceFaceIntForce (Face * const lhs,
00541 Face * const rhs,
00542 vec_d & total_force)
00543 {
00544 assert(faceIntersectsFace(lhs,rhs) && faceIntersectsFace(rhs,lhs));
00545
00546 vec_d n1 = lhs->getNormal();
00547 double m1=1.0/sqrt(n1[0]*n1[0]+n1[1]*n1[1]+n1[2]*n1[2]);
00548 n1[0]=n1[0]*m1;
00549 n1[1]=n1[1]*m1;
00550 n1[2]=n1[2]*m1;
00551
00552 vec_d n2 = rhs->getNormal();
00553 double m2=1.0/sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2]);
00554 n2[0]=n2[0]*m2;
00555 n2[1]=n2[1]*m2;
00556 n2[2]=n2[2]*m2;
00557
00558 double R[3];
00559 R[0]=n1[0]+n2[0];
00560 R[1]=n1[1]+n2[1];
00561 R[2]=n1[2]+n2[2];
00562
00563 double T[3];
00564 T[0] = n1[1]*n2[2]-n1[2]*n2[1];
00565 T[1] = n1[2]*n2[0]-n1[0]*n2[2];
00566 T[2] = n1[0]*n2[1]-n1[1]*n2[0];
00567
00568 double P[3];
00569 P[0] = T[1]*R[2]-T[2]*R[1];
00570 P[1] = T[2]*R[0]-T[0]*R[2];
00571 P[2] = T[0]*R[1]-T[1]*R[0];
00572
00573 double den = INTERSECTION_WEIGHT/sqrt(P[0]*P[0]+P[1]*P[1]+P[2]*P[2]);
00574
00575 total_force[0]+=P[0]*den;
00576 total_force[1]+=P[1]*den;
00577 total_force[2]+=P[2]*den;
00578 }
00579
00587 bool Intersecting_Faces::checkFaceFaceInts (Face const * const cf,
00588 Face const * const of) const
00589 {
00590
00591 vec_d const * cv0=NULL;
00592 vec_d const * cv1=NULL;
00593 vec_d const * cv2=NULL;
00594 vec_d const * ov0=NULL;
00595 vec_d const * ov1=NULL;
00596 vec_d const * ov2=NULL;
00597 cf->getVertexCoord(cv0,cv1,cv2);
00598 of->getVertexCoord(ov0,ov1,ov2);
00599
00600 int single_shared_vert[2]={-1,-1};
00601 int num_unique = numUniqueVertices(cf,of,single_shared_vert);
00602
00603 if (facesCoplanar(cf,of)==true)
00604 {
00605 if (num_unique == 3)
00606 {
00607
00608 return true;
00609 }
00610 else
00611 {
00612
00613 if (checkEdgeEdgeIntersection(cf,of,num_unique==4)==true) {return true;}
00614 else {return false;}
00615 }
00616 }
00617 else
00618 {
00619 if (num_unique==5)
00620 {
00621
00622 vec_d const * target1 = cv0;
00623 vec_d const * target2 = cv1;
00624
00625 if (single_shared_vert[0]==0){target1 = cv1;target2 = cv2;}
00626 else if (single_shared_vert[0]==1){target2 = cv2;}
00627 result r = checkLineFaceInt(of,*target1,*target2,false);
00628 if (r.line_flag && (r.poly_flag || r.poly_edge_flag)) {return true;}
00629
00630 target1 = ov0;
00631 target2 = ov1;
00632 if (single_shared_vert[1]==0){target1 = ov1;target2 = ov2;}
00633 else if (single_shared_vert[1]==1){target2 = ov2;}
00634 r = checkLineFaceInt(cf,*target1,*target2,false);
00635 if (r.line_flag && (r.poly_flag || r.poly_edge_flag)) {return true;}
00636
00637 return false;
00638 }
00639 else if (!(num_unique==4))
00640 {
00641
00642 if (checkFaceEdgeIntersection(cf,of)) {return true;}
00643 else {return false;}
00644 }
00645 else { return false;}
00646 }
00647 }
00648
00659 int Intersecting_Faces::numUniqueVertices (Face const * const cf,
00660 Face const * const of,
00661 int * const single_shared_vert) const
00662 {
00663 int num_unique = 0;
00664
00665 for (int j=0;j<3;j++)
00666 {
00667
00668 for (int k=0;k<3;k++)
00669 {
00670 if (cf->getVertex(j)!=of->getVertex(k))
00671 {
00672 num_unique++;
00673 }
00674 else
00675 {
00676 single_shared_vert[0]=j;
00677 single_shared_vert[1]=k;
00678 }
00679 }
00680 }
00681 return num_unique-3;
00682 }
00683
00691 bool Intersecting_Faces::facesParallel (Face const * const cf,
00692 Face const * const of) const
00693 {
00694
00695 vec_d cn = cf->getNormal();
00696 vec_d on = of->getNormal();
00697
00698
00699
00700 if (!distinguishable(dot(cn,on)*dot(cn,on),dot(cn,cn)*dot(on,on))) {return true;}
00701 else {return false;}
00702 }
00703
00711 bool Intersecting_Faces::facesCoplanar (Face const * const cf,
00712 Face const * const of) const
00713 {
00714
00715 if (facesParallel(cf,of)==false) return false;
00716
00717 vec_d const * cv0=NULL;
00718 vec_d const * cv1=NULL;
00719 vec_d const * cv2=NULL;
00720 vec_d const * ov0=NULL;
00721 vec_d const * ov1=NULL;
00722 vec_d const * ov2=NULL;
00723
00724 cf->getVertexCoord(cv0,cv1,cv2);
00725 of->getVertexCoord(ov0,ov1,ov2);
00726 vec_d const * o_target = ov2;
00727 vec_d const * c_target = cv2;
00728
00729 int cvi[3] = {cf->getVertex(0)->getIndex(),
00730 cf->getVertex(1)->getIndex(),
00731 cf->getVertex(2)->getIndex()};
00732 int ovi[3] = {of->getVertex(0)->getIndex(),
00733 of->getVertex(1)->getIndex(),
00734 of->getVertex(2)->getIndex()};
00735
00736 vec_d on = of->getNormal();
00737
00738
00739
00740 if ((ovi[0]!=cvi[0])&&(ovi[0]!=cvi[1])&&(ovi[0]!=cvi[2])) {o_target=ov0;}
00741 else if ((ovi[1]!=cvi[0])&&(ovi[1]!=cvi[1])&&(ovi[2]!=cvi[2])) {o_target=ov1;}
00742 if ((cvi[0]!=ovi[0])&&(cvi[0]!=ovi[1])&&(cvi[0]!=ovi[2])) {c_target=cv0;}
00743 else if ((cvi[1]!=ovi[0])&&(cvi[1]!=ovi[1])&&(cvi[1]!=ovi[2])) {c_target=cv1;}
00744
00745
00746 if (fabs(on[0]*((*o_target)[0]-(*c_target)[0])
00747 +on[1]*((*o_target)[1]-(*c_target)[1])
00748 +on[2]*((*o_target)[2]-(*c_target)[2]))<DOUBLE_EPSILON) {return true;}
00749 else {return false;}
00750 }
00751
00761 bool Intersecting_Faces::checkEdgeEdgeIntersection (Face const * const cf,
00762 Face const * const of,
00763 bool const share_edge_flag) const
00764 {
00765 int pairs[3][2] = {{0,1},{1,2},{2,0}};
00766
00767
00768
00769
00770 Vertex *cv[2],*ov[2];
00771
00772 for (int i=0;i<3;i++)
00773 {
00774
00775 for (int j=0;j<3;j++)
00776 {
00777 cv[0] = cf->getVertex(pairs[i][0]);
00778 cv[1] = cf->getVertex(pairs[i][1]);
00779 ov[0] = of->getVertex(pairs[j][0]);
00780 ov[1] = of->getVertex(pairs[j][0]);
00781
00782 if (cv[0]!=ov[0]&&cv[0]!=ov[1]&&cv[1]!=ov[0]&&cv[1]!=ov[1])
00783 {
00784
00785 double x[3],y[3];
00786 for (int k=0;k<3;k++)
00787 {
00788 x[k] = cv[1]->getCoord(k)-cv[0]->getCoord(k);
00789 y[k] = ov[1]->getCoord(k)-ov[0]->getCoord(k);
00790 }
00791 if (distinguishable(dot(x,y)*dot(x,y),dot(x,x)*dot(y,y))==true)
00792 {
00793
00794 double qDen = (cv[1]->getCoord(0)-cv[0]->getCoord(0))
00795 *(ov[1]->getCoord(1)-ov[0]->getCoord(1))
00796 -(cv[1]->getCoord(1)-cv[0]->getCoord(1))
00797 *(ov[1]->getCoord(0)-ov[0]->getCoord(0));
00798 double qNum = (cv[1]->getCoord(0)-cv[0]->getCoord(0))
00799 *(cv[0]->getCoord(1)-ov[0]->getCoord(1))
00800 -(cv[1]->getCoord(1)-cv[0]->getCoord(1))
00801 *(cv[0]->getCoord(0)-ov[0]->getCoord(0));
00802 double q = qNum/qDen;
00803 double uNum = ov[0]->getCoord(0)-cv[0]->getCoord(0)
00804 +q*(ov[1]->getCoord(0)-ov[0]->getCoord(0));
00805 double uDen = cv[1]->getCoord(0)-cv[0]->getCoord(0);
00806 if(fabs(qDen)>DOUBLE_EPSILON && fabs(uDen)>DOUBLE_EPSILON)
00807 {
00808 double u = uNum/uDen;
00809 if (((u > DOUBLE_EPSILON && u < (1-DOUBLE_EPSILON)) &&
00810 (q > DOUBLE_EPSILON && q < (1-DOUBLE_EPSILON)) ) ||
00811 ( share_edge_flag && ((u > DOUBLE_EPSILON && u < (1-DOUBLE_EPSILON)) ||
00812 (q > DOUBLE_EPSILON && q < (1-DOUBLE_EPSILON))) ))
00813 {
00814 return true;
00815 }
00816 }
00817 }
00818 }
00819 }
00820 }
00821 return false;
00822 }
00823
00831 bool Intersecting_Faces::checkFaceEdgeIntersection (Face const * const cf,Face const * const of) const
00832 {
00833 vec_d const * v0=NULL;
00834 vec_d const * v1=NULL;
00835 vec_d const * v2=NULL;
00836
00837 cf->getVertexCoord(v0,v1,v2);
00838
00839 result r = checkLineFaceInt(of,*v0,*v1,false);
00840 if(!r.line_flag || !r.poly_flag)
00841 {
00842 r = checkLineFaceInt(of,*v1,*v2,false);
00843 }
00844 if(!r.line_flag || !r.poly_flag)
00845 {
00846 checkLineFaceInt(of,*v2,*v0,false);
00847 }
00848 if (r.line_flag && (r.poly_flag || r.poly_edge_flag)) {return(1);}
00849
00850 of->getVertexCoord(v0,v1,v2);
00851
00852 r = checkLineFaceInt(cf,*v0,*v1,false);
00853 if(!r.line_flag || !r.poly_flag)
00854 {
00855 r = checkLineFaceInt(cf,*v1,*v2,false);
00856 }
00857 if(!r.line_flag || !r.poly_flag)
00858 {
00859 r = checkLineFaceInt(cf,*v2,*v0,false);
00860 }
00861
00862 if (r.line_flag && (r.poly_flag || r.poly_edge_flag)) {return true;}
00863 else {return false;}
00864 }
00865