00001 #include "edge.h"
00002
00003 #include <cmath>
00004 #include <iostream>
00005
00006 #include "face.h"
00007 #include "misc.h"
00008 #include "vertex.h"
00009
00010 using std::cout;
00011 using std::endl;
00012
00013 Edge::Edge (Face * const f,Vertex * const va,Vertex * const vb)
00014 :f1(f),f2(NULL),l(0),energy(0)
00015 {
00016
00017 l=sqrt((va->getCoord(0)-vb->getCoord(0))*(va->getCoord(0)-vb->getCoord(0))+
00018 (va->getCoord(1)-vb->getCoord(1))*(va->getCoord(1)-vb->getCoord(1))+
00019 (va->getCoord(2)-vb->getCoord(2))*(va->getCoord(2)-vb->getCoord(2)));
00020 }
00021
00022 void Edge::print (std::ostream &target) const
00023 {
00024 Vertex *v1=NULL,*v2=NULL,*o1=NULL,*o2=NULL;
00025 getVertices(v1,v2,o1,o2);
00026 cout.precision(12);
00027 if (v1==NULL || v2==NULL ||o1==NULL ||o2==NULL ||f1==NULL ||f2==NULL)
00028 {
00029 cout << "\nNULL POINTERS IN EDGE!\n";
00030 if(v1==NULL){cout << "v1 is NULL\n";}
00031 if(v2==NULL){cout << "v2 is NULL\n";}
00032 if(o1==NULL){cout << "o1 is NULL\n";}
00033 if(o2==NULL){cout << "o2 is NULL\n";}
00034 if(f1==NULL){cout << "f1 is NULL\n";}
00035 if(f2==NULL){cout << "f2 is NULL\n";}
00036 exit(0);
00037 }
00038 target << "\nprintEdge: v1:\n";
00039 v1->print(target);
00040 target << "printEdge: v2:\n";
00041 v2->print(target);
00042 target << "printEdge: o1:\n";
00043 o1->print(target);
00044 target << "printEdge: o2:\n";
00045 o2->print(target);
00046 target << "printEdge: f1:\n";
00047 f1->print(target);
00048 target << "printEdge: f2:\n";
00049 f2->print(target);
00050 target << "printEdge: orignal length " << l << endl;
00051 }
00052
00053 double Edge::getSqLength (void) const
00054 {
00055 Vertex *v1=NULL,*v2=NULL,*o1=NULL,*o2=NULL;
00056 getVertices(v1,v2,o1,o2);
00057 return (v1->getCoord(0)-v2->getCoord(0))*(v1->getCoord(0)-v2->getCoord(0))
00058 +(v1->getCoord(1)-v2->getCoord(1))*(v1->getCoord(1)-v2->getCoord(1))
00059 +(v1->getCoord(2)-v2->getCoord(2))*(v1->getCoord(2)-v2->getCoord(2));
00060 }
00061
00062 double Edge::getStretchForceEnergy (Vertex const * const v,double * const & force,bool flag) const
00063 {
00064 Vertex *v1=NULL,*v2=NULL,*o1=NULL,*o2=NULL;
00065 getVertices(v1,v2,o1,o2);
00066
00067 Vertex *av;
00068 if(v1==v){av=v2;}
00069 else {av=v1;}
00070
00071 double s[3];
00072 for(int i=0;i<3;i++)
00073 {
00074 s[i]=av->getCoord(i)-v->getCoord(i);
00075 }
00076
00077 double sd = sqrt(dot(s,s));
00078
00079 double ref = (f1->getEdge(0)->l+f1->getEdge(1)->l+f1->getEdge(2)->l)/3.0;
00080
00081 double se = (sd-ref)/ref;
00082 double force_magn = ASPECT_GAIN*STRETCH_GAIN*se;
00083 if(flag)
00084 {
00085
00086
00087
00088 double scaled_spring_force = EDGE_STRETCH_WEIGHT*force_magn/sd;
00089
00090
00091
00092 force[0]+=scaled_spring_force*s[0];
00093 force[1]+=scaled_spring_force*s[1];
00094 force[2]+=scaled_spring_force*s[2];
00095 }
00096
00097 return EDGE_STRETCH_WEIGHT/STRETCH_EXPONENT*force_magn*se;
00098 }
00099
00100 double Edge::getAngle (void) const
00101 {
00102 Vertex *v1=NULL,*v2=NULL,*o1=NULL,*o2=NULL;
00103 getVertices(v1,v2,o1,o2);
00104
00105
00106 vec_d n1 = f1->getNormal();
00107 vec_d n2 = f2->getNormal();
00108
00109 double normal_angle_cosine=dot(n1,n2)/sqrt(dot(n1,n1))/sqrt(dot(n2,n2));
00110
00111 if (normal_angle_cosine >= 1)
00112 {
00113 return PI;
00114 }
00115 else if (normal_angle_cosine <= -1)
00116 {
00117 return 0;
00118 }
00119 else
00120 {
00121
00122
00123 vec_d refvec(3,0);
00124 refvec[0] = v2->getCoord(0)-o2->getCoord(0);
00125 refvec[1] = v2->getCoord(1)-o2->getCoord(1);
00126 refvec[2] = v2->getCoord(2)-o2->getCoord(2);
00127
00128 double d = dot(refvec,n1);
00129 if(!d){ return PI;}
00130 else { return PI+d/fabs(d)*acos(normal_angle_cosine); }
00131
00132 }
00133 }
00134
00135 double Edge::getCurvatureLength (Vertex const * const o,
00136 Vertex const * const v1,
00137 Vertex const * const v2,
00138 double edge_length) const
00139 {
00140 double o_v1[3] = {
00141 o->getCoord(0)-v1->getCoord(0),
00142 o->getCoord(1)-v1->getCoord(1),
00143 o->getCoord(2)-v1->getCoord(2) };
00144 double v2_v1[3] = {
00145 v2->getCoord(0)-v1->getCoord(0),
00146 v2->getCoord(1)-v1->getCoord(1),
00147 v2->getCoord(2)-v1->getCoord(2) };
00148 double unum = dot(o_v1,v2_v1);
00149 double a = unum/(edge_length*edge_length);
00150 double I[3] = {
00151 o->getCoord(0)-v1->getCoord(0)+a*v2_v1[0],
00152 o->getCoord(1)-v1->getCoord(1)+a*v2_v1[1],
00153 o->getCoord(2)-v1->getCoord(2)+a*v2_v1[2] };
00154 return sqrt(dot(I,I));
00155 }
00156
00157 double Edge::getAngleForceEnergy (int i,double * const & force,bool flag) const
00158 {
00159
00160
00161 vec_d n;
00162
00163 if (i){n = f2->getNormal();}
00164
00165 else {n = f1->getNormal();}
00166
00167 double L = sqrt(dot(n,n));
00168
00169 double angle=getAngle();
00170
00171
00172
00173
00174
00175 double angle_error = angle-PI;
00176 int sign = 1;
00177 if(angle_error<0){sign = -1;}
00179 Vertex *v1=NULL,*v2=NULL,*o1=NULL,*o2=NULL;
00180 getVertices(v1,v2,o1,o2);
00181
00182 double edge_length = sqrt(getSqLength());
00183
00184 double L1 = getCurvatureLength(o1,v1,v2,edge_length);
00185 double L2 = getCurvatureLength(o2,v1,v2,edge_length);
00186 double ae = fabs(angle_error/PI);
00187 double force_metric = 1;
00188 for (int j=0;j<ANGLE_EXPONENT;j++) { force_metric *=ae; }
00189 force_metric = force_metric/(L1/2.0+L2/2.0);
00190 if(flag)
00191 {
00192
00193 double force_magn;
00194 if (i){ force_magn = ANGLE_GAIN*ANGLE_STRETCH_WEIGHT*force_metric/L2;}
00195 else { force_magn = ANGLE_GAIN*ANGLE_STRETCH_WEIGHT*force_metric/L1;}
00196 double force_norm = force_magn/L;
00197 force[0]+=sign*force_norm*n[0];
00198 force[1]+=sign*force_norm*n[1];
00199 force[2]+=sign*force_norm*n[2];
00200 }
00201
00202 return fabs((ANGLE_STRETCH_WEIGHT/100.0)/ANGLE_EXPONENT*force_metric*ae);
00203 }
00204
00205
00206 double Edge::getAngleReForceEnergy (double * const & force,bool flag) const
00207 {
00208
00209
00210 vec_d n1 = f1->getNormal();
00211 vec_d n2 = f2->getNormal();
00212
00213 double L1 = sqrt(dot(n1,n1));
00214 double L2 = sqrt(dot(n2,n2));
00215
00216 double angle=getAngle();
00217
00218
00219
00220
00221
00222 double angle_error = angle-PI;
00224 Vertex *v1=NULL,*v2=NULL,*o1=NULL,*o2=NULL;
00225 getVertices(v1,v2,o1,o2);
00226 double ae = angle_error/PI;
00227 double force_metric = 1;
00228 for (int j=0;j<ANGLE_EXPONENT;j++) { force_metric *=ae; }
00229 double a = 1.0/2.0;
00230 if(flag)
00231 {
00232
00233 double force_magn1 = ANGLE_GAIN*ANGLE_STRETCH_WEIGHT*force_metric/L1;
00234 double force_magn2 = ANGLE_GAIN*ANGLE_STRETCH_WEIGHT*force_metric/L2;
00235 double force1[3]={force_magn1*n1[0],force_magn1*n1[1],force_magn1*n1[2]};
00236 double force2[3]={force_magn2*n2[0],force_magn2*n2[1],force_magn2*n2[2]};
00237 double reaction[3]={-(force1[0]+force2[0]),-(force1[1]+force2[1]),-(force1[2]+force2[2])};
00238 force[0]+=reaction[0]*a;
00239 force[1]+=reaction[1]*a;
00240 force[2]+=reaction[2]*a;
00241 }
00242
00243 return a*ANGLE_STRETCH_WEIGHT*angle_error*angle_error;
00244 }
00245
00246 void Edge::getVertices (Vertex *&v1,Vertex *&v2,Vertex *&o1,Vertex *&o2) const
00247 {
00248
00249 Vertex *va=NULL,*vb=NULL;
00250 if(f1->getVertex(0)==f2->getVertex(0) || f1->getVertex(0)==f2->getVertex(1) || f1->getVertex(0)==f2->getVertex(2))
00251 {
00252 va=f1->getVertex(0);
00253 if(f1->getVertex(1)==f2->getVertex(0) || f1->getVertex(1)==f2->getVertex(1) || f1->getVertex(1)==f2->getVertex(2))
00254 {
00255 vb=f1->getVertex(1);
00256 }
00257 else
00258 {
00259 vb=f1->getVertex(2);
00260 }
00261 }
00262 else
00263 {
00264 va=f1->getVertex(1);
00265 vb=f1->getVertex(2);
00266 }
00267
00268 if(va==NULL || vb==NULL)
00269 {
00270 cout << "\n\nEdge::getVertices: "
00271 << "common face vertices were not identified.\n\n";
00272 if(f1!=NULL){f1->print(std::cout);}
00273 else{cout << "f1 is NULL.\n";}
00274 if(f2!=NULL){f2->print(std::cout);}
00275 else{cout << "f2 is NULL.\n";}
00276 if(va!=NULL){cout << "va index = " << va->getIndex() << endl;}
00277 else{cout << "va is NULL.\n";}
00278 if(vb!=NULL){cout << "vb index = " << vb->getIndex() << endl;}
00279 else{cout << "vb is NULL.\n";}
00280 exit(0);
00281 }
00282
00283 if( (f1->getVertex(0)==va && f1->getVertex(1)==vb) || (f1->getVertex(0)==vb && f1->getVertex(1)==va))
00284 {
00285 v1=f1->getVertex(0);
00286 v2=f1->getVertex(1);
00287 o1=f1->getVertex(2);
00288 }
00289 else if( (f1->getVertex(1)==va && f1->getVertex(2)==vb) || (f1->getVertex(1)==vb && f1->getVertex(2)==va))
00290 {
00291 v1=f1->getVertex(1);
00292 v2=f1->getVertex(2);
00293 o1=f1->getVertex(0);
00294 }
00295 else if( (f1->getVertex(2)==va && f1->getVertex(0)==vb) || (f1->getVertex(2)==vb && f1->getVertex(0)==va))
00296 {
00297 v1=f1->getVertex(2);
00298 v2=f1->getVertex(0);
00299 o1=f1->getVertex(1);
00300 }
00301 else
00302 {
00303 cout << "\n\nEdge::getVertices: "
00304 << "v1 and v2 were not successfully found.\n\n";
00305 exit(0);
00306 }
00307
00308 if( (f2->getVertex(0)==va && f2->getVertex(1)==vb) || (f2->getVertex(0)==vb && f2->getVertex(1)==va))
00309 {
00310 o2=f2->getVertex(2);
00311 }
00312 else if( (f2->getVertex(1)==va && f2->getVertex(2)==vb) || (f2->getVertex(1)==vb && f2->getVertex(2)==va))
00313 {
00314 o2=f2->getVertex(0);
00315 }
00316 else if( (f2->getVertex(2)==va && f2->getVertex(0)==vb) || (f2->getVertex(2)==vb && f2->getVertex(0)==va))
00317 {
00318 o2=f2->getVertex(1);
00319 }
00320 else
00321 {
00322 cout << "\n\nEdge::getVertices: "
00323 << "o2 was not successfully found.\n\n";
00324 exit(0);
00325 }
00326 }
00327
00328 void Edge::update (Face * const f)
00329 {
00330
00331 if(f1==NULL) {f1=f;}
00332 else if (f2==NULL) {f2=f;}
00333 else
00334 {
00335 cout << "Error. Tried to add third face to edge.\n";
00336 f->print(std::cout);
00337 exit(1);
00338 }
00339
00340 f->addEdge(this);
00341 }
00342
00343 Face * Edge::getFace1 (void) const
00344 {
00345 return f1;
00346 }
00347
00348 Face * Edge::getFace2 (void) const
00349 {
00350 return f2;
00351 }
00352
00353 void Edge::setEnergy (double ee)
00354 {
00355 energy = ee;
00356 }
00357
00358 double Edge::getEnergy (void) const
00359 {
00360 return energy;
00361 }
00362
00363 double Edge::getOriginalLength (void) const
00364 {
00365 return l;
00366 }
00367
00368 Face* Edge::getFaceNotAdjToVertex (Vertex const * const cv) const
00369 {
00370
00371 if(binary_search(cv->begin(),cv->end(),getFace1())==false)
00372 {
00373 return getFace1();
00374 }
00375
00376 if(binary_search(cv->begin(),cv->end(),getFace2())==false)
00377 {
00378 return getFace2();
00379 }
00380 cout << "Edge::getFaceNotAdjToVertex: Error:"
00381 << "no acceptable face was found!\n";
00382 exit(0);
00383 }
00384
00385 bool Edge::getNearPtToVertex (Vertex const * const v,vec_d & p) const
00386 {
00387
00388 Vertex *v1=NULL,*v2=NULL,*o1=NULL,*o2=NULL;
00389 getVertices(v1,v2,o1,o2);
00390
00391 vec_d const * const p1 = v1->getPos();
00392 vec_d const * const p2 = v2->getPos();
00393
00394 double e[3]={(*p2)[0]-(*p1)[0],(*p2)[1]-(*p1)[1],(*p2)[2]-(*p1)[2]};
00395
00396 double vv[3]={v->getCoord(0)-(*p1)[0],v->getCoord(1)-(*p1)[1],v->getCoord(2)-(*p1)[2]};
00397
00398 double num = dot(vv,e);
00399 double den = dot(e,e);
00400
00401
00402
00403 if ( ((num>0 && den>0)||(num<0 && den<0)) && (fabs(num)<fabs(den)))
00404 {
00405 double u = num/den;
00406 p.clear();
00407 p.push_back((*p1)[0]+u*(e[0]));
00408 p.push_back((*p1)[1]+u*(e[1]));
00409 p.push_back((*p1)[2]+u*(e[2]));
00410 return true;
00411 }
00412 return false;
00413 }
00414