00001
00002 #include <iostream>
00003 #include <math.h>
00004 #include <vector>
00005 #include <list>
00006
00007 #include "Macrodefs.h"
00008
00009 #include "SketchSpace.h"
00010 #include "Sketch.h"
00011 #include "Region.h"
00012 #include "visops.h"
00013
00014 #include "ShapeSpace.h"
00015 #include "ShapeRoot.h"
00016
00017 #include "PointData.h"
00018 #include "LineData.h"
00019 #include "ShapeLine.h"
00020
00021 using namespace std;
00022
00023 namespace DualCoding {
00024
00025 DATASTUFF_CC(LineData);
00026
00027 const Point LineData::origin_pt = Point(0,0);
00028
00029 LineData::LineData(ShapeSpace& _space, const Point &p1, orientation_t orient)
00030 : BaseData(_space,getStaticType()), end1_pt(p1), end2_pt(),
00031 rho_norm(0), theta_norm(0), orientation(0), length(0) {
00032 int const width = space->getDualSpace().getWidth();
00033 int const height = space->getDualSpace().getHeight();
00034
00035
00036
00037
00038 float p2x=0, p2y=0;
00039 if ( fabs(orient-M_PI/2) < 0.001 ) {
00040 p2x = p1.coordX();
00041 p2y = p1.coordY() > height/2 ? 0 : height-1;
00042 } else {
00043 float slope = tan(orient);
00044 float intcpt = p1.coordY() - p1.coordX()*slope;
00045 p2x = p1.coordX() >= width/2 ? 0.0 : width-1;
00046 p2y = p2x * slope + intcpt;
00047 if ( p2y > height-1 ) {
00048 p2x = (height - intcpt) / slope;
00049 p2y = height;
00050 } else if ( p2y < 0 ) {
00051 p2x = -intcpt / slope;
00052 p2y = 0;
00053 }
00054 }
00055 end2_pt = Point(p2x,p2y);
00056 end1_pt.setValid(false);
00057 end1_pt.setActive(false);
00058 end2_pt.setValid(false);
00059 end2_pt.setActive(false);
00060 update_derived_properties();
00061 }
00062
00063 Point LineData::getCentroid() const { return (end1Pt()+end2Pt())*0.5; }
00064
00065 void LineData::setInfinite(bool value) {
00066 end1_pt.setActive(!value);
00067 end2_pt.setActive(!value);
00068 }
00069
00070 #define NORMPOINT_MATCH_DISTSQ 3500
00071 #define LINE_MATCH_OVERLAP -10
00072 #define LINE_ORIENTATION_MATCH M_PI/6
00073
00074 bool LineData::isMatchFor(const ShapeRoot& other) const {
00075 if (!(isSameTypeAs(other) && isSameColorAs(other)))
00076 return false;
00077 else {
00078 const Shape<LineData>& other_ln = ShapeRootTypeConst(other,LineData);
00079 return isMatchFor(other_ln.getData());
00080 }
00081 }
00082
00083 bool LineData::isMatchFor(const LineData& other_line) const {
00084 float const px = rho_norm*cos(theta_norm),
00085 py = rho_norm*sin(theta_norm);
00086 float const qx = other_line.rho_norm*cos(other_line.theta_norm),
00087 qy = other_line.rho_norm*sin(other_line.theta_norm);
00088 AngPi theta_diff = float(theta_norm) - float(other_line.theta_norm);
00089 if ( theta_diff > M_PI/2 )
00090 theta_diff = M_PI - theta_diff;
00091 float normpointdistsq = (px-qx)*(px-qx) + (py-qy)*(py-qy);
00092
00093
00094
00095
00096 return normpointdistsq < NORMPOINT_MATCH_DISTSQ
00097 && theta_diff < LINE_ORIENTATION_MATCH
00098 && perpendicularDistanceFrom(other_line.getCentroid()) < 100
00099 && isOverlappedWith(other_line,LINE_MATCH_OVERLAP);
00100 }
00101
00102 bool LineData::isOverlappedWith(const LineData& otherline, int amount) const {
00103 if ( firstPtCoord() <= otherline.firstPtCoord() )
00104 return secondPtCoord()-amount >= otherline.firstPtCoord();
00105 else
00106 return firstPtCoord()+amount <= otherline.secondPtCoord();
00107 }
00108
00109
00110 void LineData::mergeWith(const ShapeRoot& other) {
00111 const Shape<LineData>& other_line = ShapeRootTypeConst(other,LineData);
00112 if (other_line->confidence <= 0)
00113 return;
00114 const int other_conf = other_line->confidence;
00115 confidence += other_conf;
00116 end1_pt = (end1_pt*confidence + other_line->end1Pt()*other_conf) / (confidence+other_conf);
00117 end2_pt = (end2_pt*confidence + other_line->end2Pt()*other_conf) / (confidence+other_conf);
00118 update_derived_properties();
00119 }
00120
00121
00122 bool LineData::isValidUpdate(coordinate_t c1_cur, coordinate_t c2_cur, coordinate_t c1_new, coordinate_t c2_new) {
00123 const float c1_noise = 10.0 + fabs(c1_cur+c1_new) / 20.0;
00124 const float c2_noise = 10.0 + fabs(c2_cur+c2_new) / 20.0;
00125 return (c1_new-c1_noise < c1_cur && c2_cur < c2_new+c2_noise);
00126 }
00127
00128
00129
00130 bool LineData::updateParams(const ShapeRoot& ground_root, bool force) {
00131 const Shape<LineData>& ground_line = ShapeRootTypeConst(ground_root,LineData);
00132
00133
00134
00135
00136
00137 const coordinate_t c1_cur = firstPtCoord();
00138 const coordinate_t c2_cur = secondPtCoord();
00139
00140 Point _end1_pt = firstPt();
00141 Point _end2_pt = secondPt();
00142
00143 updateLinePt(firstPt(), firstPtCoord(), firstPt(ground_line), firstPtCoord(ground_line), -1);
00144 updateLinePt(secondPt(), secondPtCoord(), secondPt(ground_line), secondPtCoord(ground_line), +1);
00145
00146
00147
00148 const coordinate_t c1_new = firstPtCoord();
00149 const coordinate_t c2_new = secondPtCoord();
00150
00151 if (isValidUpdate(c1_cur, c2_cur, c1_new, c2_new) || force){
00152
00153
00154 update_derived_properties();
00155 return true;
00156 }
00157
00158
00159 setEndPts(_end1_pt, _end2_pt);
00160 return false;
00161 }
00162
00163 void LineData::updateLinePt(EndPoint& localPt, coordinate_t local_coord,
00164 const EndPoint& groundPt, coordinate_t ground_coord,
00165 int sign) {
00166 if ( groundPt.isValid() ) {
00167 if ( localPt.isValid() )
00168 localPt.updateParams(groundPt);
00169 else
00170 localPt = groundPt;
00171 }
00172 else if ( (ground_coord - local_coord)*sign > 0 )
00173 localPt = groundPt;
00174 }
00175
00176 bool LineData::isAdmissible() const {
00177 if (end1Pt().isValid() && end2Pt().isValid())
00178 return length >= 70.0;
00179 else
00180 return length >= 40.0;
00181 }
00182
00183
00184 void LineData::printParams() const {
00185 cout << "Type = " << getTypeName() << " ID=" << getId() << " ParentID=" << getParentId() << endl;
00186 cout << " end1{" << end1Pt().coordX() << ", " << end1Pt().coordY() << "}"
00187 << " active=" << end1Pt().isActive()
00188 << " valid=" << end1Pt().isValid() << endl;
00189
00190 cout << " end2{" << end2Pt().coordX() << ", " << end2Pt().coordY() << "}"
00191 << " active=" << end2Pt().isActive()
00192 << " valid=" << end2Pt().isValid() << std::endl;
00193
00194 cout << " rho_norm=" << rho_norm
00195 << ", theta_norm=" << theta_norm
00196 << ", orientation=" << getOrientation()
00197 << ", length=" << getLength() << endl;
00198
00199 printf(" color = %d %d %d\n",getColor().red,getColor().green,getColor().blue);
00200
00201 cout << " mobile=" << getMobile()
00202 << ", viewable=" << isViewable() << endl;
00203
00204 vector<float> abc = lineEquation_abc();
00205 printf(" equ = %f %f %f\n",abc[0],abc[1],abc[2]);
00206 }
00207
00208 void LineData::printEnds() const {
00209 cout << " end1{" << end1Pt().coordX() << ", " << end1Pt().coordY() << "}";
00210 cout << " active=" << end1Pt().isActive() << ", valid=" << end1Pt().isValid() << endl;
00211 cout << " end2{" << end2Pt().coordX() << ", " << end2Pt().coordY() << "}";
00212 cout << " active=" << end2Pt().isActive() << ", valid=" << end2Pt().isValid() << endl;
00213
00214 }
00215
00216
00217
00218
00219
00220
00221 void LineData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) {
00222 end1Pt().applyTransform(Tmat,newref);
00223 end2Pt().applyTransform(Tmat,newref);
00224 update_derived_properties();
00225 }
00226
00227 void LineData::projectToGround(const NEWMAT::Matrix& camToBase,
00228 const NEWMAT::ColumnVector& groundplane) {
00229 end1Pt().projectToGround(camToBase,groundplane);
00230 end2Pt().projectToGround(camToBase,groundplane);
00231 update_derived_properties();
00232 }
00233
00234
00235
00236
00237 EndPoint& LineData::leftPt() { return end1Pt().isLeftOf(end2Pt()) ? end1_pt : end2_pt; }
00238 EndPoint& LineData::rightPt() { return end1Pt().isLeftOf(end2Pt()) ? end2_pt : end1_pt; }
00239 EndPoint& LineData::topPt() { return end1Pt().isAbove(end2Pt()) ? end1_pt : end2_pt; }
00240 EndPoint& LineData::bottomPt() { return end1Pt().isAbove(end2Pt()) ? end2_pt : end1_pt; }
00241
00242 Shape<PointData> LineData::leftPtShape() {
00243 Shape<PointData> result(new PointData(*space, leftPt()));
00244 result->setName("leftPt");
00245 result->inheritFrom(*this);
00246 result->setViewable(false);
00247 return result;
00248 }
00249
00250 Shape<PointData> LineData::rightPtShape() {
00251 Shape<PointData> result(new PointData(*space, leftPt()));
00252 result->setName("rightPt");
00253 result->inheritFrom(*this);
00254 result->setViewable(false);
00255 return result;
00256 }
00257
00258 Shape<PointData> LineData::topPtShape() {
00259 Shape<PointData> result(new PointData(*space, leftPt()));
00260 result->setName("topPt");
00261 result->inheritFrom(*this);
00262 result->setViewable(false);
00263 return result;
00264 }
00265
00266 Shape<PointData> LineData::bottomPtShape() {
00267 Shape<PointData> result(new PointData(*space, leftPt()));
00268 result->setName("bottomPt");
00269 result->inheritFrom(*this);
00270 result->setViewable(false);
00271 return result;
00272 }
00273
00274 EndPoint& LineData::firstPt() {
00275 if ( isNotVertical() )
00276 if ( end1Pt().coordX() < end2Pt().coordX() )
00277 return end1Pt();
00278 else return end2Pt();
00279 else
00280 if ( end1Pt().coordY() < end2Pt().coordY() )
00281 return end1Pt();
00282 else return end2Pt();
00283 }
00284
00285 EndPoint& LineData::firstPt(const Shape<LineData> &otherline) const {
00286 if ( isNotVertical() )
00287 if ( otherline->end1Pt().coordX() < otherline->end2Pt().coordX() )
00288 return otherline->end1Pt();
00289 else return otherline->end2Pt();
00290 else
00291 if ( otherline->end1Pt().coordY() < otherline->end2Pt().coordY() )
00292 return otherline->end1Pt();
00293 else return otherline->end2Pt();
00294 }
00295
00296 EndPoint& LineData::secondPt() {
00297 if ( isNotVertical() )
00298 if ( end1Pt().coordX() > end2Pt().coordX() )
00299 return end1Pt();
00300 else return end2Pt();
00301 else
00302 if ( end1Pt().coordY() > end2Pt().coordY() )
00303 return end1Pt();
00304 else return end2Pt();
00305 }
00306
00307 EndPoint& LineData::secondPt(const Shape<LineData> &otherline) const {
00308 if ( isNotVertical() )
00309 if ( otherline->end1Pt().coordX() > otherline->end2Pt().coordX() )
00310 return otherline->end1Pt();
00311 else return otherline->end2Pt();
00312 else
00313 if ( otherline->end1Pt().coordY() > otherline->end2Pt().coordY() )
00314 return otherline->end1Pt();
00315 else return otherline->end2Pt();
00316 }
00317
00318 Shape<PointData> LineData::firstPtShape() {
00319 Shape<PointData> result(new PointData(*space, firstPt()));
00320 result->setName("firstPt");
00321 result->inheritFrom(*this);
00322 result->setViewable(false);
00323 return result;
00324 }
00325
00326 Shape<PointData> LineData::secondPtShape() {
00327 Shape<PointData> result(new PointData(*space, secondPt()));
00328 result->setName("secondPt");
00329 result->inheritFrom(*this);
00330 result->setViewable(false);
00331 return result;
00332 }
00333
00334 coordinate_t LineData::firstPtCoord() const {
00335 return isNotVertical() ?
00336 const_cast<LineData*>(this)->firstPt().coordX() :
00337 const_cast<LineData*>(this)->firstPt().coordY();
00338 }
00339
00340 coordinate_t LineData::firstPtCoord(const Shape<LineData> &otherline) const {
00341 return isNotVertical() ?
00342 firstPt(otherline).coordX() :
00343 firstPt(otherline).coordY();
00344 }
00345
00346 coordinate_t LineData::secondPtCoord() const {
00347 return isNotVertical() ?
00348 const_cast<LineData*>(this)->secondPt().coordX() :
00349 const_cast<LineData*>(this)->secondPt().coordY();
00350 }
00351
00352 coordinate_t LineData::secondPtCoord(const Shape<LineData> &otherline) const {
00353 return isNotVertical() ?
00354 secondPt(otherline).coordX() :
00355 secondPt(otherline).coordY();
00356 }
00357
00358
00359
00360
00361 void LineData::setEndPts(const EndPoint& _end1_pt, const EndPoint& _end2_pt) {
00362 end1_pt.setCoords(_end1_pt);
00363 end1_pt.setActive(_end1_pt.isActive());
00364 end1_pt.setValid(_end1_pt.isValid());
00365 end1_pt.setNumUpdates(_end1_pt.numUpdates());
00366
00367 end2_pt.setCoords(_end2_pt);
00368 end2_pt.setActive(_end2_pt.isActive());
00369 end2_pt.setValid(_end2_pt.isValid());
00370 end2_pt.setNumUpdates(_end2_pt.numUpdates());
00371
00372 update_derived_properties();
00373 }
00374
00375
00376
00377
00378 std::pair<float,float> LineData::lineEquation_mb() const {
00379 float m;
00380 if ((fabs(end2Pt().coordX() - end1Pt().coordX()) * BIG_SLOPE)
00381 <= fabs(end2Pt().coordY() - end2Pt().coordY()))
00382 m = BIG_SLOPE;
00383 else
00384 m = (end2Pt().coordY() - end1Pt().coordY())/(end2Pt().coordX() - end1Pt().coordX());
00385 float b = end1Pt().coordY() - m * end1Pt().coordX();
00386 return pair<float,float>(m,b);
00387 }
00388
00389
00390
00391 std::vector<float> LineData::lineEquation_abc_xz() const {
00392 float dx = end2Pt().coordX() - end1Pt().coordX();
00393 float dz = end2Pt().coordZ() - end1Pt().coordZ();
00394
00395 std::vector<float> abc;
00396 abc.resize(3, 1.0);
00397 float& a = abc[0];
00398 float& b = abc[1];
00399 float& c = abc[2];
00400
00401
00402 if((dx == 0.0)
00403 || (dz/dx > BIG_SLOPE)) {
00404 a = 1.0;
00405 b = 0.0;
00406 c = end1Pt().coordX();
00407 }
00408
00409
00410 else if((dz == 0.0)
00411 || (dx/dz > BIG_SLOPE)) {
00412 a = 0.0;
00413 b = 1.0;
00414 c = end1Pt().coordZ();
00415 }
00416
00417
00418 else {
00419 a = 1.0;
00420 b = (end1Pt().coordX() - end2Pt().coordX())
00421 / (end2Pt().coordZ() - end1Pt().coordZ());
00422 c = end1Pt().coordX() + b*end1Pt().coordZ();
00423 }
00424
00425 return(abc);
00426
00427 }
00428
00429
00430 std::vector<float> LineData::lineEquation_abc() const {
00431 float dx = end2Pt().coordX() - end1Pt().coordX();
00432 float dy = end2Pt().coordY() - end1Pt().coordY();
00433
00434 std::vector<float> abc;
00435 abc.resize(3, 1.0);
00436 float& a = abc[0];
00437 float& b = abc[1];
00438 float& c = abc[2];
00439
00440
00441 if( fabs(dx) < 1.0e-6 || dy/dx > BIG_SLOPE) {
00442 a = 1.0;
00443 b = 0.0;
00444 c = end1Pt().coordX();
00445 }
00446
00447
00448 else if ( fabs(dy) < 1.0e-6 || dx/dy > BIG_SLOPE ) {
00449 a = 0.0;
00450 b = 1.0;
00451 c = end1Pt().coordY();
00452 }
00453
00454
00455 else {
00456 a = 1.0;
00457
00458
00459 b = -dx / dy;
00460 c = end1Pt().coordX() + b*end1Pt().coordY();
00461 }
00462
00463 return(abc);
00464 }
00465
00466
00467
00468
00469 void LineData::update_derived_properties() {
00470 rho_norm = perpendicularDistanceFrom(origin_pt);
00471 const vector<float> abc = lineEquation_abc();
00472 const float& a1 = abc[0];
00473 const float& b1 = abc[1];
00474 const float& c1 = abc[2];
00475 const float c1sign = (c1 >= 0) ? 1.0 : -1.0;
00476 theta_norm = atan2(b1*c1sign, a1*c1sign);
00477 orientation = theta_norm + AngPi(M_PI/2);
00478 length = end1Pt().distanceFrom(end2Pt());
00479 const ReferenceFrameType_t ref = getRefFrameType();
00480 end1_pt.setRefFrameType(ref);
00481 end2_pt.setRefFrameType(ref);
00482 deleteRendering();
00483 }
00484
00485 bool LineData::isNotVertical() const {
00486 const AngPi threshold = M_PI / 3;
00487 const AngPi orient = getOrientation();
00488 return (orient <= threshold) || (orient >= M_PI - threshold);
00489 }
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511 bool LineData::isLongerThan(const Shape<LineData>& other) const {
00512 return length > other->length; }
00513
00514 bool LineData::isLongerThan(float ref_length) const {
00515 return length > ref_length; }
00516
00517 bool LineData::isShorterThan(const Shape<LineData>& other) const {
00518 return length < other->length; }
00519
00520 bool LineData::isShorterThan(float ref_length) const {
00521 return length < ref_length; }
00522
00523 bool LineData::isBetween(const Point &p, const LineData &other) const {
00524 if (getOrientation() == other.getOrientation()) {
00525 float dl = perpendicularDistanceFrom(other.end1Pt());
00526 return (perpendicularDistanceFrom(p) <= dl && other.perpendicularDistanceFrom(p) <= dl);
00527 }
00528 else {
00529 bool b;
00530 const LineData p_line (*space, p,
00531 intersectionWithLine(other, b, b));
00532 const AngPi theta_pline = p_line.getOrientation();
00533 const AngPi theta_greater =
00534 (getOrientation() > other.getOrientation()) ? getOrientation() : other.getOrientation();
00535 const AngPi theta_smaller =
00536 (getOrientation() < other.getOrientation()) ? getOrientation() : other.getOrientation();
00537 if (theta_greater - theta_smaller > M_PI/2)
00538 return (theta_pline >= theta_greater || theta_pline <= theta_smaller);
00539 else
00540 return (theta_pline <= theta_greater && theta_pline >= theta_smaller);
00541 }
00542 }
00543
00544
00545
00546
00547 bool
00548 LineData::intersectsLine(const Shape<LineData>& other) const {
00549 return intersectsLine(other.getData());
00550 }
00551
00552 bool
00553 LineData::intersectsLine(const LineData& other) const {
00554
00555 pair<float,float> F = lineEquation_mb();
00556
00557
00558 pair<float,float> G = other.lineEquation_mb();
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578 float r3 = F.first * other.end1Pt().coordX() + F.second - other.end1Pt().coordY();
00579
00580
00581 float r4 = F.first * other.end2Pt().coordX() + F.second - other.end2Pt().coordY();
00582
00583
00584
00585
00586
00587
00588 if((r3 != 0)
00589 && (r4 != 0)
00590 && (SGN(r3) == SGN(r4))
00591 )
00592 return false;
00593
00594
00595
00596
00597
00598 float r1 = G.first * end1Pt().coordX() + G.second - end1Pt().coordY();
00599
00600
00601 float r2 = G.first * end2Pt().coordX() + G.second - end2Pt().coordY();
00602
00603
00604
00605
00606
00607
00608 if((r1 != 0)
00609 && (r2 != 0)
00610 && (SGN(r1) == SGN(r2))
00611 )
00612 return false;
00613
00614
00615 return true;
00616 }
00617
00618
00619 Point LineData::intersectionWithLine(const Shape<LineData>& other,
00620 bool& intersection_on_this,
00621 bool& intersection_on_other) const {
00622 return intersectionWithLine(other.getData(), intersection_on_this,intersection_on_other);
00623 }
00624
00625 Point
00626 LineData::intersectionWithLine(const LineData& other,
00627 bool& intersection_on_this, bool& intersection_on_other) const {
00628
00629
00630
00631
00632
00633
00634
00635 float x1, x2, x3, x4, y1, y2, y3, y4;
00636 x1 = end1Pt().coordX();
00637 x2 = end2Pt().coordX();
00638 x3 = other.end1Pt().coordX();
00639 x4 = other.end2Pt().coordX();
00640
00641
00642 y1 = end1Pt().coordY();
00643 y2 = end2Pt().coordY();
00644 y3 = other.end1Pt().coordY();
00645 y4 = other.end2Pt().coordY();
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660 float denom = ((y4-y3)*(x2-x1) - (x4-x3)*(y2-y1));
00661 float u_a_numerator = ((x4-x3)*(y1-y3) - (y4-y3)*(x1-x3));
00662 float u_b_numerator = ((x2-x1)*(y1-y3) - (y2-y1)*(x1-x3));
00663
00664
00665 if(denom == 0.0) {
00666 if (u_a_numerator == 0.0 && u_b_numerator == 0.0) {
00667 PRINTF("intersectionWithLine: lines are coincident!\n");
00668 return(end1Pt());
00669 }
00670 else {
00671 cout << x1 << " " << x2 << " " << x3 << " " << x4 << " "
00672 << y1 << " " << y2 << " " << y3 << " " << y4 << endl;
00673 cout << "this theta; " << getOrientation() << ", other theta: " << other.getOrientation() << endl;
00674 PRINTF("ERROR in intersectionWithLine: lines are parallel!\n");
00675 return(Point(-9999.0,-99999.0));
00676 }
00677 }
00678
00679 else {
00680 float u_a = u_a_numerator / denom;
00681 float u_b = u_b_numerator / denom;
00682
00683
00684 if(0.0 <= u_a <=1.0)
00685 intersection_on_this = true;
00686 else
00687 intersection_on_this = false;
00688
00689
00690 if(0.0 <= u_b <=1.0)
00691 intersection_on_other = true;
00692 else
00693 intersection_on_other = false;
00694
00695 return(Point((x1+u_a*(x2-x1)),
00696 (y1+u_a*(y2-y1))));
00697 }
00698 }
00699
00700
00701
00702
00703 float LineData::perpendicularDistanceFrom(const Point& otherPt) const {
00704
00705
00706
00707
00708 vector<float> abc = lineEquation_abc();
00709 float& a = abc[0];
00710 float& b = abc[1];
00711 float& c = abc[2];
00712
00713
00714
00715
00716
00717 float d = fabs((a * otherPt.coordX() + b * otherPt.coordY() - c)/sqrt(a*a + b*b));
00718
00719 return(d);
00720 }
00721
00722
00723
00724
00725
00726
00727
00728 #define BEG_DIST_THRESH 2
00729 #define END_DIST_THRESH 2
00730
00731 Shape<LineData> LineData::extractLine(Sketch<bool>& sketch) {
00732 NEW_SKETCH_N(fatmask,bool,visops::fillin(sketch,1,2,8));
00733 NEW_SKETCH_N(skel,bool,visops::skel(fatmask));
00734 return extractLine(skel, sketch);
00735 }
00736
00737 Shape<LineData> LineData::extractLine(Sketch<bool>& skeleton,
00738 const Sketch<bool>& occlusions) {
00739 const int width = skeleton->getWidth(), height = skeleton->getHeight();
00740 SketchSpace &SkS = skeleton->getSpace();
00741 ShapeSpace &ShS = SkS.getDualSpace();
00742
00743
00744 NEW_SKETCH_N(labels,uint,visops::oldlabelcc(skeleton,visops::EightWayConnect));
00745 list<Region> regions = Region::extractRegions(labels,EXTRACT_LINE_MIN_AREA);
00746 if ( regions.empty() ) {
00747 ShapeRoot invalid;
00748 return ShapeRootType(invalid,LineData);
00749 }
00750 Region skelchunk = regions.front();
00751 AngPi orientation(skelchunk.findPrincipalAxisOrientation());
00752 Point centroid(skelchunk.findCentroid());
00753
00754 if (! skelchunk.isContained(centroid, 3) ) {
00755 Shape<LineData> result(splitLine(ShS, skelchunk, skeleton, occlusions));
00756 if ( result.isValid() )
00757 return result;
00758 }
00759
00760
00761 Shape<LineData> extracted_line(ShS, centroid, orientation);
00762 extracted_line->setColor(skeleton->getColor());
00763 extracted_line->setParentId(skeleton->getViewableId());
00764
00765
00766
00767
00768 float x_normpoint = extracted_line->rho_norm*cos(extracted_line->theta_norm);
00769 float y_normpoint = extracted_line->rho_norm*sin(extracted_line->theta_norm);
00770
00771
00772
00773 float m = max(min(tan(extracted_line->theta_norm+AngPi(M_PI/2)), (float)BIG_SLOPE), (float)(-BIG_SLOPE));
00774 float b = y_normpoint - m*x_normpoint;
00775
00776
00777
00778 NEW_SKETCH_N(skelDist,uint,visops::edist(skeleton));
00779 if ( abs(m) <= 1 )
00780 extracted_line->scanHorizForEndPts(skelDist,occlusions,m,b);
00781 else
00782 extracted_line->scanVertForEndPts(skelDist,occlusions,m,b);
00783
00784
00785
00786 extracted_line->end1_pt.checkValidity(width,height,BEG_DIST_THRESH);
00787 extracted_line->end2_pt.checkValidity(width,height,BEG_DIST_THRESH);
00788
00789
00790 SkS.applyTmatinv(extracted_line->end1_pt.getCoords());
00791 SkS.applyTmatinv(extracted_line->end2_pt.getCoords());
00792 extracted_line->update_derived_properties();
00793 return extracted_line;
00794 }
00795
00796
00797
00798 Shape<LineData> LineData::splitLine(ShapeSpace &ShS, Region &skelchunk,
00799 Sketch<bool> &skeleton, const Sketch<bool> &occlusions) {
00800
00801 Point bounds[4] = {skelchunk.findTopBoundPoint(), skelchunk.findLeftBoundPoint(),
00802 skelchunk.findRightBoundPoint(), skelchunk.findBotBoundPoint()};
00803
00804
00805
00806
00807 for (int i = 0; i < 4; i++) {
00808 for (int j = i+1; j < 4; j++) {
00809 if (bounds[i].distanceFrom(bounds[j]) > 20 && ! skelchunk.isContained((bounds[i]+bounds[j])/2.0, 3)) {
00810
00811 LineData ln(ShS,bounds[i],bounds[j]);
00812 PointData most_distant_pt(ShS, skelchunk.mostDistantPtFrom(ln));
00813
00814 const Sketch<bool>& point_rendering = most_distant_pt.getRendering();
00815 uint const clear_dist = 10;
00816 NEW_SKETCH_N(not_too_close, bool, visops::edist(point_rendering) >= clear_dist);
00817 skeleton &= not_too_close;
00818 return extractLine(skeleton, occlusions);
00819 }
00820 }
00821 }
00822 ShapeRoot invalid;
00823 return ShapeRootType(invalid,LineData);
00824 }
00825
00826 void LineData::scanHorizForEndPts(const Sketch<uint>& skelDist,
00827 const Sketch<bool>& occlusions,
00828 float m, float b) {
00829
00830 bool on_line = false;
00831 int beg_dist_thresh = BEG_DIST_THRESH;
00832 int end_dist_thresh = END_DIST_THRESH;
00833 int curxstart = -1;
00834 int possxstop = -1;
00835 int bestxstart = -1;
00836 int bestxstop = -1;
00837 int bestlength = -1;
00838 for (int x = 0, y, dist=0; x < skelDist.width; x++) {
00839 y = int(m*x+b);
00840
00841
00842 if (y >= 0 && y < skelDist.height) {
00843 dist = skelDist(x,y);
00844
00845 if (on_line == false) {
00846 if (dist <= beg_dist_thresh) {
00847
00848 on_line = true;
00849 curxstart = x - dist;
00850
00851 int curystart;
00852 bool backupflag = false;
00853 while ( curxstart >= 0 && (curystart=int(m*curxstart+b)) >= 0 && curystart < skelDist.height )
00854 if ( occlusions(curxstart,curystart) || skelDist(curxstart,curystart) == 0 ) {
00855 --curxstart;
00856 backupflag = true;
00857 } else {
00858 curxstart += backupflag;
00859 break;
00860 }
00861 if ( curxstart < 0)
00862 curxstart = 0;
00863 }
00864 }
00865 else {
00866 const bool occ = occlusions(x,y);
00867
00868 if ( dist <= end_dist_thresh || occ ) {
00869 if ( occ )
00870 possxstop = x;
00871 else
00872 possxstop = x - dist;
00873
00874 if ( possxstop-curxstart > bestlength ) {
00875 bestxstart = curxstart;
00876 bestxstop = possxstop;
00877 bestlength = bestxstop-bestxstart;
00878
00879 }
00880 }
00881 else if ( dist > extractorGapTolerance ) {
00882
00883 on_line = false;
00884
00885 };
00886
00887 }
00888 }
00889 }
00890
00891
00892 float y1 = m*bestxstart + b;
00893 float y2 = m*bestxstop + b;
00894 setEndPts(Point(bestxstart,y1), Point(bestxstop,y2));
00895
00896 balanceEndPointHoriz(end1_pt,occlusions,m,b);
00897 balanceEndPointHoriz(end2_pt,occlusions,m,b);
00898 }
00899
00900 void LineData::scanVertForEndPts(const Sketch<uint>& skelDist,
00901 const Sketch<bool>& occlusions,
00902 float m, float b) {
00903
00904 bool on_line = false;
00905 int beg_dist_thresh = BEG_DIST_THRESH;
00906 int end_dist_thresh = END_DIST_THRESH;
00907 int curystart = -1;
00908 int possystop = -1;
00909 int bestystart = -1;
00910 int bestystop = -1;
00911 int bestlength = -1;
00912 for (int x, y = 0, dist=0; y < skelDist.height; y++) {
00913 x = int((y-b)/m);
00914
00915
00916 if (x >= 0 && x < skelDist.width) {
00917 dist = int(skelDist(x,y));
00918
00919 if (on_line == false) {
00920 if (dist <= beg_dist_thresh) {
00921
00922 on_line = true;
00923 curystart = y - dist;
00924
00925 int curxstart;
00926 bool backupflag = false;
00927 while ( curystart >= 0 && (curxstart=int((curystart-b)/m)) >= 0 && curxstart < skelDist.width )
00928 if ( occlusions(curxstart,curystart) || skelDist(curxstart,curystart) == 0 ){
00929 --curystart;
00930 backupflag = true;
00931 } else {
00932 curystart += backupflag;
00933 break;
00934 }
00935 if ( curystart < 0)
00936 curystart = 0;
00937 }
00938 }
00939 else {
00940 const bool occ = occlusions(x,y);
00941
00942 if ( dist <= end_dist_thresh || occ ) {
00943 if ( occ )
00944 possystop = y;
00945 else
00946 possystop = y - dist;
00947
00948 if ( possystop-curystart > bestlength ) {
00949 bestystart = curystart;
00950 bestystop = possystop;
00951 bestlength = bestystop-bestystart;
00952
00953 }
00954 }
00955 else if ( dist > extractorGapTolerance ) {
00956
00957 on_line = false;
00958
00959 };
00960
00961 }
00962 }
00963 }
00964
00965 float x1 = (bestystart-b)/m;
00966 float x2 = (bestystop-b)/m;
00967
00968 setEndPts(Point(x1,bestystart), Point(x2,bestystop));
00969
00970 balanceEndPointVert(end1_pt,occlusions,m,b);
00971 balanceEndPointVert(end2_pt,occlusions,m,b);
00972 }
00973
00974 void LineData::balanceEndPointHoriz(EndPoint &, Sketch<bool> const &, float, float) {
00975
00976
00977
00978
00979
00980
00981 return;
00982 }
00983
00984 void LineData::balanceEndPointVert(EndPoint &pt, Sketch<bool> const &occluders, float m, float b) {
00985 int ystep = ( pt.coordY() < max(end1_pt.coordY(),end2_pt.coordY()) ? +1 : -1 );
00986 int leftpix = 0, rightpix = 0;
00987 int const balance_rows = 8;
00988 int const row_samples = 10;
00989
00990 for ( int y = (int)pt.coordY(), ycnt=balance_rows ;
00991 y>= 0 && y<occluders.height && ycnt-- > 0 ; y+=ystep ) {
00992 int const xstart = (int) ((y-b)/m);
00993 for ( int x = xstart-row_samples; x < xstart; x++ )
00994 if ( x >= 0 )
00995 leftpix += occluders(x,y);
00996 for ( int x = xstart+row_samples; x > xstart; x-- )
00997 if ( x < occluders.width )
00998 rightpix += occluders(x,y);
00999
01000 }
01001 float const new_x = pt.coordX() + (rightpix-leftpix)/(2*balance_rows);
01002
01003 pt.setCoords(new_x, pt.coordY());
01004 }
01005
01006 vector<Shape<LineData> > LineData::extractLines(Sketch<bool> const& sketch,
01007 const int num_lines) {
01008 NEW_SKETCH_N(fatmask,bool,visops::fillin(sketch,1,2,8));
01009 NEW_SKETCH_N(skel,bool,visops::skel(fatmask));
01010 return extractLines(skel, sketch, num_lines);
01011 }
01012
01013 vector<Shape<LineData> > LineData::extractLines(Sketch<bool> const& skel,
01014 Sketch<bool> const& occluders,
01015 const int num_lines) {
01016 vector<Shape<LineData> > lines_vec;
01017 NEW_SKETCH_N(temp,bool,visops::copy(skel))
01018 for (int gloop = 0; gloop<num_lines; gloop++) {
01019 Shape<LineData> newline(extractLine(temp,occluders));
01020 if ( !newline.isValid() ) break;
01021 newline->clearLine(temp);
01022 if (newline->isLongerThan(DEFAULT_MIN_LENGTH)) {
01023 lines_vec.push_back(newline);
01024 }
01025 }
01026
01027
01028 return lines_vec;
01029 }
01030
01031
01032 void LineData::clearLine(Sketch<bool>& sketch) {
01033 const Sketch<bool>& line_rendering = getRendering();
01034 uint const clear_dist = 5;
01035 Sketch<bool> not_too_close = (visops::edist(line_rendering) >= clear_dist);
01036 sketch &= not_too_close;
01037 }
01038
01039
01040
01041
01042
01043
01044
01045 Sketch<bool>& LineData::getRendering() {
01046 if ( ! end1Pt().rendering_valid || ! end2Pt().rendering_valid )
01047 deleteRendering();
01048 if ( rendering_sketch == NULL )
01049 rendering_sketch = render();
01050 return *rendering_sketch;
01051 }
01052
01053
01054
01055 Sketch<bool>* LineData::render() const {
01056 SketchSpace &renderspace = space->getDualSpace();
01057 int const width = renderspace.getWidth();
01058 int const height = renderspace.getHeight();
01059 float x1,y1,x2,y2;
01060 setDrawCoords(x1, y1, x2, y2, width, height);
01061 Sketch<bool>* draw_result =
01062 new Sketch<bool>(renderspace, "render("+getName()+")");
01063 (*draw_result)->setParentId(getViewableId());
01064 (*draw_result)->setColor(getColor());
01065 *draw_result = 0;
01066 drawline2d(*draw_result, (int)x1, (int)y1, (int)x2, (int)y2);
01067 const_cast<LineData*>(this)->end1Pt().rendering_valid = true;
01068 const_cast<LineData*>(this)->end2Pt().rendering_valid = true;
01069 return draw_result;
01070 }
01071
01072
01073
01074 void LineData::setDrawCoords(float& x1,float& y1, float& x2, float& y2,
01075 const int width, const int height) const {
01076 EndPoint e1(end1Pt());
01077 EndPoint e2(end2Pt());
01078 space->getDualSpace().applyTmat(e1.getCoords());
01079 space->getDualSpace().applyTmat(e2.getCoords());
01080 const EndPoint &left_point = e1.coordX() <= e2.coordX() ? e1 : e2;
01081 const EndPoint &right_point = e1.coordX() <= e2.coordX() ? e2 : e1;
01082
01083
01084 if((int)left_point.coordY() == (int)right_point.coordY()) {
01085 if (!left_point.isActive())
01086 x1 = 0;
01087 else x1 = max(0,(int)left_point.coordX());
01088
01089 if (!right_point.isActive())
01090 x2 = width-1;
01091 else x2 = min(width-1, (int)right_point.coordX());
01092
01093 y1 = (int)left_point.coordY();
01094 y2 = y1;
01095 }
01096 else
01097 if ((int)left_point.coordX() == (int)right_point.coordX()) {
01098
01099 x1 = (int)left_point.coordX();
01100 x2 = x1;
01101
01102 const EndPoint &top_point = end1Pt().coordY() <= end2Pt().coordY() ? end1Pt() : end2Pt();
01103 const EndPoint &bottom_point = end1Pt().coordY() <= end2Pt().coordY() ? end2Pt() : end1Pt();
01104
01105 if(!top_point.isActive())
01106 y1 = 0;
01107 else y1 = max(0,(int)top_point.coordY());
01108
01109 if (!bottom_point.isActive())
01110 y2 = height-1;
01111 else y2 = min(height-1,(int)bottom_point.coordY());
01112 }
01113
01114 else {
01115 float m = (right_point.coordY()-left_point.coordY())/(right_point.coordX()-left_point.coordX());
01116 float b = left_point.coordY() - m*left_point.coordX();
01117
01118
01119 int i0x = (int)((0-b)/m);
01120 int ihx = (int)(((height-1)-b)/m);
01121 int i0y = (int)(m*0+b);
01122 int iwy = (int)(m*(width-1)+b);
01123
01124
01125 if(left_point.isActive()) {
01126 x1 = (int)left_point.coordX();
01127 y1 = (int)left_point.coordY();
01128 }
01129
01130
01131 else {
01132
01133
01134 if(i0y >= 0 && i0y < height) {
01135 x1 = 0;
01136 y1 = i0y;
01137 }
01138
01139
01140 else {
01141
01142
01143 if (i0x < ihx) {
01144 x1 = i0x;
01145 y1 = 0;
01146 }
01147
01148
01149 else {
01150 x1 = ihx;
01151 y1 = height-1;
01152 }
01153 }
01154 }
01155
01156
01157 if(right_point.isActive()) {
01158 x2 = (int)right_point.coordX();
01159 y2 = (int)right_point.coordY();
01160 }
01161 else {
01162 if(iwy >= 0 && iwy < height) {
01163 x2 = width-1;
01164 y2 = iwy;
01165 }
01166 else {
01167 if (i0x > ihx) {
01168 x2 = i0x;
01169 y2 = 0;
01170 }
01171 else {
01172 x2 = ihx;
01173 y2 = height-1;
01174 }
01175 }
01176 }
01177 }
01178 }
01179
01180 void LineData::drawline2d(Sketch<bool>& canvas, int x0, int y0, int x1, int y1) {
01181 int width = canvas->getWidth();
01182 int height = canvas->getHeight();
01183
01184
01185 int d, x, y, ax, ay, sx, sy, dx, dy;
01186 dx = x1-x0; ax = abs(dx)<<1; sx = SGN(dx);
01187 dy = y1-y0; ay = abs(dy)<<1; sy = SGN(dy);
01188
01189 x = x0;
01190 y = y0;
01191 if ( ax > ay ) {
01192 d = ay-(ax>>1);
01193 for (;;) {
01194 if (x >= 0 && y >= 0 && x < width && y < height)
01195 canvas(x,y) = true;
01196
01197 if (x==x1)
01198 return;
01199
01200 if ( d >= 0 ) {
01201 y += sy;
01202 d -= ax;
01203 }
01204
01205 x += sx;
01206 d += ay;
01207 }
01208 }
01209 else {
01210 d = ax-(ay>>1);
01211 for (;;) {
01212 if (x >= 0 && y >= 0 && x < width && y < height)
01213 canvas(x,y) = true;
01214
01215 if ( y == y1 )
01216 return;
01217
01218 if ( d >= 0 ) {
01219 x += sx;
01220 d -= ay;
01221 }
01222
01223 y += sy;
01224 d += ax;
01225 }
01226 }
01227 }
01228
01229
01230
01231 std::vector<Shape<LineData> > LineData::houghTransform(const Sketch<bool>& fat,
01232 const Sketch<bool>& skinny,
01233 const Sketch<bool>& occlusions,
01234 const size_t num_lines, int minLength)
01235 {
01236 std::vector<Shape<LineData> > result;
01237 ShapeSpace& ShS = fat->getSpace().getDualSpace();
01238
01239 const int width = fat->getWidth(), height = fat->getHeight();
01240 const int numR = 120, numTheta = 120;
01241 const int numRf = 40, numThetaf = 40;
01242 int hsize = numR*numTheta, hsizef = numRf * numThetaf;
01243 int htab[hsize], hfine[hsizef];
01244 float minTheta = 0.0, maxTheta = 2*M_PI;
01245 float minR = 0.000, maxR = sqrt((float)width*width+(float)height*height);
01246 float thetaSpread = maxTheta - minTheta;
01247 float rSpread = maxR - minR;
01248
01249
01250 for (int i = 0; i < hsize; i++)
01251 htab[i] = 0;
01252
01253
01254 float theta, r;
01255 int ridx;
01256 for (int x = 0; x < width; x++) {
01257 for (int y = 0; y < height; y++) {
01258 if (fat(x,y) == true) {
01259 for (int tidx = 0; tidx < numTheta ; tidx++) {
01260 theta = minTheta + tidx*thetaSpread/numTheta;
01261 r = (float)x*cos(theta)+(float)y*sin(theta);
01262 ridx = (int)((r-minR)*(float)numR/rSpread);
01263 if (ridx >= 0 && ridx < numR)
01264 htab[ridx+tidx*numR]++;
01265 }
01266 }
01267 }
01268 }
01269
01270
01271
01272
01273
01274
01275
01276
01277
01278
01279
01280
01281
01282
01283
01284
01285
01286
01287
01288
01289
01290 int max_val = -1, max_i = 0;
01291 while(result.size() < num_lines) {
01292
01293
01294 max_val = -1;
01295 max_i = 0;
01296 for (int i = 0; i < numR*numTheta; i++) {
01297 if (max_val < htab[i]) {
01298 max_val = htab[i];
01299 max_i = i;
01300 }
01301 }
01302
01303
01304 if (max_val < minLength)
01305 break;
01306
01307
01308 float bestR = (max_i%numR)*rSpread/numR + minR;
01309 float bestTheta = (max_i/numR)*thetaSpread/numTheta + minTheta;
01310
01311
01312 const float fthetaSpread = M_PI/40.0;
01313 const float frSpread = maxR/40.0;
01314 float fminTheta = bestTheta-fthetaSpread/2.0;
01315 float fminR = bestR-frSpread/2.0;
01316
01317 for (int i = 0; i < hsizef; i++)
01318 hfine[i] = 0;
01319
01320
01321 float thetaf, rf;
01322 int ridxf;
01323 for (int x = 0; x < width; x++) {
01324 for (int y = 0; y < height; y++) {
01325 if (skinny(x,y) == true) {
01326 for (int tidx = 0; tidx < numThetaf ; tidx++) {
01327 thetaf = fminTheta + tidx*fthetaSpread/numThetaf;
01328 rf = (float)x*cos(thetaf)+(float)y*sin(thetaf);
01329 ridxf = (int)((rf-fminR)*(float)numRf/frSpread);
01330 if (ridxf >= 0 && ridxf < numRf)
01331 hfine[ridxf+tidx*numRf]++;
01332 }
01333 }
01334 }
01335 }
01336
01337
01338
01339 int max_val_f = -1, max_i_f = 0;
01340 for (int i = 0; i < numRf*numThetaf; i++) {
01341 if (max_val_f < hfine[i]) {
01342 max_val_f = hfine[i];
01343 max_i_f = i;
01344 }
01345 }
01346 float bestRf = (max_i_f%numRf)*frSpread/numRf + fminR;
01347 float bestThetaf = (max_i_f/numRf)*fthetaSpread/numThetaf + fminTheta;
01348
01349
01350
01351 float lineLen = 500;
01352
01353 const float x1 = bestRf*cos(bestThetaf), y1 = bestRf*sin(bestThetaf);
01354 const float x2 = x1+lineLen*cos(bestThetaf+M_PI/2), y2 = y1+lineLen*sin(bestThetaf+M_PI/2);
01355 const float x3 = x1-lineLen*cos(bestThetaf+M_PI/2), y3 = y1-lineLen*sin(bestThetaf+M_PI/2);
01356
01357 Point e1(x3,y3), e2(x2,y2);
01358 Shape<LineData> line(ShS,e1,e2);
01359
01360
01361 line->setColor(skinny->getColor());
01362 line->setParentId(skinny->getViewableId());
01363
01364
01365 float m = (y1 != 0) ? -(x1/y1) : BIG_SLOPE;
01366 float b = y1 - m*x1;
01367
01368 NEW_SKETCH_N(skelDist,uint,visops::edist(skinny));
01369 if ( abs(m) <= 1 )
01370 line->scanHorizForEndPts(skelDist,occlusions,m,b);
01371 else
01372 line->scanVertForEndPts(skelDist,occlusions,m,b);
01373
01374
01375
01376 line->end1_pt.checkValidity(width,height,BEG_DIST_THRESH);
01377 line->end2_pt.checkValidity(width,height,BEG_DIST_THRESH);
01378
01379
01380
01381
01382
01383 float len = line->getLength();
01384 EndPoint start = line->end1_pt;
01385 EndPoint dx = (line->end2_pt - start)/len;
01386 float onCount = 0;
01387 for (float i=0; i<len; i++) {
01388 EndPoint cur = start+dx*i;
01389 if (skinny((int)(cur.coordX()), (int)(cur.coordY()))) {
01390 onCount++;
01391 }
01392 }
01393
01394
01395 std::cout<<"Line with "<<onCount<<" / "<<len<<" pixels";
01396 if (line->isLongerThan(minLength) && (onCount / len) > .5 )
01397 {
01398 std::cout<<" accepted";
01399 result.push_back(line);
01400 }
01401 std::cout<<std::endl;
01402
01403
01404
01405
01406 for(int row = -5; row <= 5; row++)
01407 {
01408 for (int col = -(5-abs(row)); col <=5-abs(row); col++)
01409 {
01410
01411
01412 htab[(max_i - (max_i%numR) + ((max_i+col)%numR)+row*numR + numR*numTheta)%(numR*numTheta)] = 0;
01413 }
01414
01415 }
01416
01417 }
01418
01419 return result;
01420 }
01421
01422 bool LineData::linesParallel(Shape<LineData> l1, Shape<LineData>l2)
01423 {
01424 const float maxDTheta = .15, minDThetaR = 10.0;
01425 float dTheta = l1->getOrientation() - l2->getOrientation();
01426 if (dTheta > M_PI_2)
01427 dTheta = dTheta - M_PI;
01428 if (abs(dTheta) < maxDTheta)
01429 {
01430 if (abs (100*dTheta + (l1->rho_norm - l2->rho_norm)) > minDThetaR)
01431 return true;
01432 }
01433 return false;
01434 }
01435
01436 LineData& LineData::operator=(const LineData& other) {
01437 if (&other == this)
01438 return *this;
01439 BaseData::operator=(other);
01440 end1_pt = other.end1_pt;
01441 end2_pt = other.end2_pt;
01442 rho_norm = other.rho_norm;
01443 theta_norm = other.theta_norm;
01444 orientation = other.orientation;
01445 length = other.length;
01446 return *this;
01447 }
01448
01449
01450
01451 bool LineData::pointsOnSameSide(const Point& p1, const Point& p2)
01452 {
01453 float dx = end2_pt.coordX() - end1_pt.coordX();
01454 float dy = end2_pt.coordY() - end1_pt.coordY();
01455
01456 float p1val = (p1.coordY() - end1_pt.coordY())*dx - (p1.coordX() - end1_pt.coordX())*dy;
01457 float p2val = (p2.coordY() - end1_pt.coordY())*dx - (p2.coordX() - end1_pt.coordX())*dy;
01458
01459 return (p1val>0) == (p2val>0);
01460 }
01461
01462
01463
01464 bool LineData::pointOnLine(const Point& p)
01465 {
01466 const float BOUNDS_EXTEND = 1.0;
01467 float dx = end2_pt.coordX() - end1_pt.coordX();
01468 float dy = end2_pt.coordY() - end1_pt.coordY();
01469 float val = (p.coordY() - end1_pt.coordY())*dx - (p.coordX() - end1_pt.coordX())*dy;
01470 val /= length;
01471 bool inBounds = (p.coordX() >= (leftPt().coordX() - BOUNDS_EXTEND)) &&
01472 (p.coordX() <= (rightPt().coordX() + BOUNDS_EXTEND)) &&
01473 (p.coordY() >= (topPt().coordY() - BOUNDS_EXTEND)) &&
01474 (p.coordY() <= (bottomPt().coordY() + BOUNDS_EXTEND));
01475 return (val > -1) && (val < 1) && inBounds;
01476 }
01477
01478
01479
01480
01481 bool LineData::LengthLessThan::operator() (const Shape<LineData> &line1, const Shape<LineData> &line2) const {
01482 return line1->getLength() < line2->getLength();
01483 }
01484
01485 bool LineData::ParallelTest::operator() (const Shape<LineData> &line1, const Shape<LineData> &line2) const {
01486 return angdist(line1->getOrientation(),line2->getOrientation()) <= tolerance;
01487 }
01488
01489 bool LineData::PerpendicularTest::operator() (const Shape<LineData> &line1, const Shape<LineData> &line2) const {
01490 return angdist(angdist(line1->getOrientation(),line2->getOrientation()), M_PI/2) <= tolerance;
01491 }
01492
01493 bool LineData::ColinearTest::operator() (const Shape<LineData> &line1, const Shape<LineData> &line2) const {
01494 return ParallelTest(ang_tol)(line1,line2) &&
01495 abs( line1->getRhoNorm() - line2->getRhoNorm() ) <= dist_tol;
01496 }
01497
01498 bool LineData::IsHorizontal::operator() (const Shape<LineData> &line) const {
01499 const AngPi orient = line->getOrientation();
01500 return (orient <= threshold) || (orient >= M_PI - threshold);
01501 }
01502
01503 bool LineData::IsVertical::operator() (const Shape<LineData> &line) const {
01504 const AngPi orient = line->getOrientation();
01505 return (orient >= threshold) && (orient <= M_PI - threshold);
01506 }
01507
01508 }