00001 #include "state.h"
00002
00003 #include <cmath>
00004 #include <iostream>
00005
00006 #include "box.h"
00007 #include "container.h"
00008 #include "edge.h"
00009 #include "face.h"
00010 #include "gain_schedule.h"
00011 #include "intersecting_faces.h"
00012 #include "space.h"
00013 #include "vertex.h"
00014 #include "virtual_disp.h"
00015
00016 using std::cout;
00017 using std::endl;
00018
00019 State * State::only_one = NULL;
00020
00021 State & State::instance(void)
00022 {
00023
00024
00025 if (only_one == NULL)
00026 only_one = new State();
00027
00028 return *only_one;
00029 }
00030
00031 State::State (const State& rhs)
00032 :nb(),
00033 ae(),ea(),vd2(0)
00034 {
00035 cout << "Assignment operator prohibited on instances of State class.\n";
00036 cout << "State " << rhs.nb.size()<< endl;
00037 exit(0);
00038 }
00039
00040 State::State (void)
00041 :nb(),
00042 ae(),ea(),vd2(0)
00043 {
00044 ae.reserve(VECTOR_RESERVE);
00045 };
00046
00047 State& State::operator= (const State& rhs)
00048 {
00049 cout << "Copy assignment operator prohibited on instances of State class.\n";
00050 cout << "State " << rhs.nb.size() << endl;
00051 exit(0);
00052 }
00053
00054 double State::getVD2 (void)
00055 {
00056 return vd2;
00057 }
00058
00059 void State::setVD2 (double d)
00060 {
00061 vd2 = d;
00062 }
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074 void State::clearNewFaces (void)
00075 {
00076 nb.clear();
00077 }
00078
00079
00080
00081
00082
00083
00084 void State::addNewFace (Face * const f,Box * const b)
00085 {
00086 nb.insert(std::make_pair(f,b));
00087 }
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099 mmap_fp_bp State::getOrigBoxesOfVertAdjFaces (Vertex * const v)
00100 {
00101 vec_bp vbp;
00102
00103 mmap_fp_bp ob;
00104
00105 for(fp_cit i=v->begin();i!=v->end();i++)
00106 {
00107
00108 vbp.clear();
00109 Space::instance().getBoxesToCheck(*i,vbp);
00110
00111 for(bp_it j=vbp.begin();j!=vbp.end();j++)
00112 {
00113 ob.insert(std::make_pair(*i,*j));
00114 }
00115 }
00116 return ob;
00117 }
00118
00126 vec_vp State::getVertsForFullClosestPtSearch (Vertex const * const v)
00127 {
00128 double bb[6];
00129 v->getBoundingBox(bb);
00130 vec_d origin(3,0),end(3,0);
00131 origin[0] = bb[0]-(NUM_ADJACENT_BOXES*SPACE_LENGTH)*SCALE;
00132 origin[1] = bb[2]-(NUM_ADJACENT_BOXES*SPACE_LENGTH)*SCALE;
00133 origin[2] = bb[4]-(NUM_ADJACENT_BOXES*SPACE_LENGTH)*SCALE;
00134 end[0] = bb[1]+(NUM_ADJACENT_BOXES*SPACE_LENGTH)*SCALE;
00135 end[1] = bb[3]+(NUM_ADJACENT_BOXES*SPACE_LENGTH)*SCALE;
00136 end[2] = bb[5]+(NUM_ADJACENT_BOXES*SPACE_LENGTH)*SCALE;
00137 vec_bp bp;
00138 Space::instance().getBoxesFromPosition(origin,end,bp);
00139
00140 vec_vp fs;
00141 fs.reserve(VECTOR_RESERVE);
00142 vec_vp t;
00143 t.reserve(VECTOR_RESERVE);
00144
00145 for(bp_it i=bp.begin();i!=bp.end();i++)
00146 {
00147
00148 for(fp_cit j=(*i)->begin();j!=(*i)->end();j++)
00149 {
00150
00151 t.push_back((*j)->getVertex(0));
00152 t.push_back((*j)->getVertex(1));
00153 t.push_back((*j)->getVertex(2));
00154 }
00155 }
00156
00157 sort(t.begin(),t.end());
00158 t.assign(t.begin(),unique(t.begin(),t.end()));
00159
00160 for(vp_it j=t.begin();j!=t.end();j++)
00161 {
00162
00163
00164
00165 if( (Container::instance().vertexIsFrozen(*j)==false)
00166 && (*j)->getFace()!=NULL && binary_search(v->begin(),v->end(),(*j)->getFace()) )
00167 {
00168 fs.push_back(*j);
00169 }
00170 }
00171
00172
00173 t.clear();
00174
00175 for(fp_cit k=v->begin();k!=v->end();k++)
00176 {
00177
00178 t.push_back((*k)->getVertex(0));
00179 t.push_back((*k)->getVertex(1));
00180 t.push_back((*k)->getVertex(2));
00181 }
00182
00183 sort(t.begin(),t.end());
00184 t.assign(t.begin(),unique(t.begin(),t.end()));
00185
00186 for(vp_it j=t.begin();j!=t.end();j++)
00187 {
00188
00189
00190 if( (Container::instance().vertexIsFrozen(*j)==false) )
00191 {
00192 fs.push_back(*j);
00193 }
00194 }
00195 return fs;
00196 }
00197
00205 vec_vp State::getVertsForPartialClosestPtSearch (Vertex const * const v)
00206 {
00207 double bb[6];
00208 v->getBoundingBox(bb);
00209 vec_d origin(3,0),end(3,0);
00210 origin[0] = bb[0]-(NUM_ADJACENT_BOXES*SPACE_LENGTH)*SCALE;
00211 origin[1] = bb[2]-(NUM_ADJACENT_BOXES*SPACE_LENGTH)*SCALE;
00212 origin[2] = bb[4]-(NUM_ADJACENT_BOXES*SPACE_LENGTH)*SCALE;
00213 end[0] = bb[1]+(NUM_ADJACENT_BOXES*SPACE_LENGTH)*SCALE;
00214 end[1] = bb[3]+(NUM_ADJACENT_BOXES*SPACE_LENGTH)*SCALE;
00215 end[2] = bb[5]+(NUM_ADJACENT_BOXES*SPACE_LENGTH)*SCALE;
00216 vec_bp bp;
00217 Space::instance().getBoxesFromPosition(origin,end,bp);
00218
00219 vec_vp ps;
00220 ps.reserve(VECTOR_RESERVE);
00221
00222 for(bp_it i=bp.begin();i!=bp.end();i++)
00223 {
00224
00225 for(fp_cit j=(*i)->begin();j!=(*i)->end();j++)
00226 {
00227
00228 for(int k=0;k<3;k++)
00229 {
00230
00231 ps.push_back((*j)->getVertex(k));
00232 }
00233 }
00234 }
00235
00236 sort(ps.begin(),ps.end());
00237 ps.assign(ps.begin(),unique(ps.begin(),ps.end()));
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256 return ps;
00257 }
00258
00259 void State::collectEdgeAngles (Vertex const * const v)
00260 {
00261 ea.clear();
00263
00264 for(fp_cit i=v->begin();i!=v->end();i++)
00265 {
00266 ea[(*i)->getEdge(0)] = (*i)->getEdge(0)->getAngle();
00267 ea[(*i)->getEdge(1)] = (*i)->getEdge(1)->getAngle();
00268 ea[(*i)->getEdge(2)] = (*i)->getEdge(2)->getAngle();
00269 }
00270 }
00271
00280 void State::updateClosestFaceToVertices (Vertex * const v,vec_vp & fs,vec_vp & ps)
00281 {
00282 double dummy[3] = {0.0,0.0,0.0};
00283
00284 sort(fs.begin(),fs.end());
00285 fs.assign(fs.begin(),unique(fs.begin(),fs.end()));
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315 for(vp_it i=fs.begin();i!=fs.end();i++)
00316 {
00317
00318 if(Container::instance().vertexIsFrozen(*i)==false)
00319 {
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335 Face *cl=NULL;
00336 vec_d p;
00337 double sqd=0.0;
00338 bool flag = Container::instance().findClosestPtToVertex(*i,p,sqd,cl);
00339 if (flag==true)
00340 {
00341 (*i)->setFace(cl);
00342 }
00343 else
00344 {
00345 (*i)->setFace(NULL);
00346 }
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362 Virtual_Disp::instance().updateVirtualDisp(*i,flag);
00363
00364
00365 double e=(*i)->getSeparationForceEnergy(dummy,false);
00366
00367 Container::instance().updateEnergy(-(*i)->getEnergy());
00368 Container::instance().updateEnergy(e);
00369 (*i)->setEnergy(e);
00370 }
00371 }
00372
00373 for(vp_it i=ps.begin();i!=ps.end();i++)
00374 {
00375
00376 if((binary_search(fs.begin(),fs.end(),*i)==false) &&
00377 (Container::instance().vertexIsFrozen(*i)==false))
00378 {
00381 vec_d norm = (*i)->getNormal();
00382 bool gate=false;
00383 double squareD = 0.0;
00384
00385 if((*i)->getFace()!=NULL){ squareD = (*i)->getSqSepDist(); }
00386 vec_fp cf;
00387 cf.clear();
00388
00389 for(fp_cit j=v->begin();j!=v->end();j++)
00390 {
00391
00392 if ((*i)->faceIsAdjacent(*j)==false)
00393 {
00394 cf.push_back(*j);
00395 }
00396 }
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421 Face *cl=NULL;
00422
00423 vec_d p;
00424 if(Container::instance().findClosestPtToVertexAmongFaces(*i,cf,p,squareD,cl)==true)
00425 {
00426 gate=true;
00427 (*i)->setFace(cl);
00428 }
00429
00430 if(gate)
00431 {
00432
00433
00434
00435
00436
00437
00438
00439
00440 double e=(*i)->getSeparationForceEnergy(dummy,false);
00441
00442 Container::instance().updateEnergy(-(*i)->getEnergy());
00443 Container::instance().updateEnergy(e);
00444 (*i)->setEnergy(e);
00445
00446 Virtual_Disp::instance().updateSets(*i,(*i)->getSqVirtualDisp(Gain_Schedule::instance().getGain()));
00447 }
00448 else
00449 {
00450
00451
00452
00453
00454
00455
00456 }
00457 }
00458 }
00459 }
00460
00461 void State::updateAdjacentFaceBoxes (Vertex const * const v,mmap_fp_bp & ob)
00462 {
00463
00464 for(fp_cit k=v->begin();k!=v->end();k++)
00465 {
00466 (*k)->updateBoxes(ob,nb);
00467 }
00468 }
00469
00470 void State::updateAdjFaceBoundingBoxes (Vertex const * const v)
00471 {
00472
00473 for(fp_cit k=v->begin();k!=v->end();k++)
00474 {
00475 (*k)->setBoundingBox();
00476 }
00477 }
00478
00479 void State::updateMovedVertexEnergy (Vertex * const v)
00480 {
00481 double dummy[3] = {0.0,0.0,0.0};
00483
00484 double e=v->getSeparationForceEnergy(dummy,false);
00485
00486 Container::instance().updateEnergy(-v->getEnergy());
00487 Container::instance().updateEnergy(e);
00488 v->setEnergy(e);
00490
00491 for(ep_cit i=ae.begin();i!=ae.end();i++)
00492 {
00493 Vertex *v1=NULL,*v2=NULL,*o1=NULL,*o2=NULL;
00494 (*i)->getVertices(v1,v2,o1,o2);
00495
00496
00497 e=(*i)->getStretchForceEnergy(v1,dummy,false)
00498 +(*i)->getAngleForceEnergy(0,dummy,false);
00499
00500 Container::instance().updateEnergy(-(*i)->getEnergy());
00501 Container::instance().updateEnergy(e);
00502 (*i)->setEnergy(e);
00503 }
00504 }
00505
00514 void State::updateVertexVD (Vertex * const v)
00515 {
00516
00517 vec_ep e;
00518 v->getAdjacentEdges(e);
00519
00520 ae.clear();
00521 for(fp_cit i=v->begin();i!=v->end();i++)
00522 {
00523
00524 ae.push_back((*i)->getEdge(0));
00525 ae.push_back((*i)->getEdge(1));
00526 ae.push_back((*i)->getEdge(2));
00527 }
00528
00529 sort(ae.begin(),ae.end());
00530 ae.assign(ae.begin(),unique(ae.begin(),ae.end()));
00531
00532 v_set nearby;
00533
00534 for(ep_it i=ae.begin();i!=ae.end();i++)
00535 {
00536
00537 if( find(e.begin(),e.end(),*i)==e.end())
00538 {
00539 Vertex *v1,*v2,*o1,*o2;
00540 (*i)->getVertices(v1,v2,o1,o2);
00541
00542 nearby.insert(v1);
00543 nearby.insert(v2);
00544 nearby.insert(o1);
00545 nearby.insert(o2);
00546 }
00547 }
00548
00549
00550 for(vs_it i=nearby.begin();i!=nearby.end();i++)
00551 {
00552
00553 if((*i)->getFace()!=NULL)
00554 {
00555 Virtual_Disp::instance().updateSets(*i,(*i)->getSqVirtualDisp(Gain_Schedule::instance().getGain()));
00556 }
00557 }
00558 }
00559
00560 bool State::assignNewVertexCoords (Vertex * const v,vec_d const & p,bool &int_flag,bool &angle_flag)
00561 {
00562 Intersecting_Faces & i_f(Intersecting_Faces::instance());
00563
00564 mmap_fp_bp ob = getOrigBoxesOfVertAdjFaces(v);
00565
00566 clearNewFaces();
00567
00568 double pO[3]={v->getCoord(0),v->getCoord(1),v->getCoord(2)};
00569
00570
00571 vec_vp fs = getVertsForFullClosestPtSearch(v);
00572
00573 collectEdgeAngles(v);
00574
00575 v->setNewPos(p);
00576
00577
00578
00579 vec_vp ps = getVertsForPartialClosestPtSearch(v);
00580
00581 int_flag = (i_f.vertAdjFacesHaveNewInt(v) && INTERSECTION_WEIGHT!=100.0);
00582 angle_flag = State::instance().smallAnglesFound();
00583 if (int_flag==false && angle_flag==false)
00584 {
00585
00586 updateAdjacentFaceBoxes(v,ob);
00587
00588 updateAdjFaceBoundingBoxes(v);
00589
00590 if(i_f.getCountOfIntFaces(false)>0)
00591 {
00592
00593
00594
00595 hashset_v ni = i_f.getNiceCheckSet(v);
00596
00597 i_f.getNiceSet(fs,ni);
00598 }
00599
00600
00601 updateClosestFaceToVertices(v,fs,ps);
00602
00603 updateVertexVD(v);
00604
00605 updateMovedVertexEnergy(v);
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627 return true;
00628 }
00629 else
00630 {
00631
00632 v->setPos(pO[0],pO[1],pO[2]);
00633 return false;
00634 }
00635 }
00636
00637 bool State::smallAnglesFound (void) const
00638 {
00639
00640 for(edhm_cit j=ea.begin();j!=ea.end();j++)
00641 {
00642
00643
00644 double new_angle=(*j).first->getAngle();
00645
00646
00647 if (fabs(new_angle)<EDGE_ANGLE_THRESHOLD || fabs(2*PI-new_angle)<EDGE_ANGLE_THRESHOLD)
00648 {
00649
00650 if( !(fabs((*j).second)<EDGE_ANGLE_THRESHOLD ||
00651 fabs(2*PI-(*j).second)<EDGE_ANGLE_THRESHOLD) ||
00652 angleChangeIsWrong((*j).second,new_angle) )
00653 {
00654 return true;
00655 }
00656 }
00657 }
00658
00659 return false;
00660 }
00661
00662 bool State::angleChangeIsWrong (double old_angle,double new_angle) const
00663 {
00664 if (old_angle < PI)
00665 {
00666
00667
00668
00669
00670 if ( new_angle>old_angle ) {return false;}
00671 else {return true;}
00672 }
00673 else
00674 {
00675
00676
00677
00678
00679
00680 if ( new_angle<old_angle ) {return false;}
00681 else {return true;}
00682 }
00683 }
00684