1 // *************************************************************************************************************************************** 2 // Point, CLine & Circle classes part of geometry.lib 3 // g.j.hawkesford August 2006 for Camtek Gmbh 4 // 5 // This program is released under the BSD license. See the file COPYING for details. 6 // 7 // *************************************************************************************************************************************** 8 9 #include "geometry.h" 10 using namespace geoff_geometry; 11 12 namespace geoff_geometry { 13 int UNITS = MM; 14 double TOLERANCE = 1.0e-06; 15 double TOLERANCE_SQ = TOLERANCE * TOLERANCE; 16 double TIGHT_TOLERANCE = 1.0e-09; 17 double UNIT_VECTOR_TOLERANCE = 1.0e-10; 18 double RESOLUTION = 1.0e-06; 19 20 // dummy functions getMessage(const wchar_t * original)21 const wchar_t* getMessage(const wchar_t* original){return original;} FAILURE(const wchar_t * str)22 void FAILURE(const wchar_t* str){throw(str);} FAILURE(const std::wstring & str)23 void FAILURE(const std::wstring& str){throw(str);} 24 set_Tolerances(int mode)25 void set_Tolerances(int mode) { 26 UNIT_VECTOR_TOLERANCE = 1.0e-10; 27 switch (UNITS = mode) 28 { 29 case MM: 30 geoff_geometry::TOLERANCE = 1.0e-03; // Peps 31 RESOLUTION = 1.0e-03; 32 TIGHT_TOLERANCE = 1.0e-06; 33 break; 34 case INCHES: 35 TOLERANCE = 1.0e-04; // Peps 36 RESOLUTION = 1.0e-04; 37 TIGHT_TOLERANCE = 1.0e-7; 38 break; 39 case METRES: 40 TOLERANCE = 1.0e-06; // p4c...SW 41 RESOLUTION = 1.0e-06; 42 TIGHT_TOLERANCE = 1.0e-09; 43 break; 44 default: 45 FAILURE(L"INVALID UNITS"); 46 } 47 TOLERANCE_SQ = TOLERANCE * TOLERANCE; 48 } 49 mm(double value)50 double mm(double value) { 51 switch(UNITS) { 52 default: 53 return value; 54 case METRES: 55 return value * .001; 56 case INCHES: 57 return value / 25.4; 58 } 59 } 60 61 // ostream operators = non-member overload 62 // ********************************************************************************************************* operator <<(wostream & op,Point & p)63 wostream& operator << (wostream& op, Point& p){ 64 // for debug - print point to file 65 if(p.ok == false) 66 op << L" ok=\"false\""; 67 else 68 op << L" x=\"" << p.x << L"\" y=\"" << p.y << L"\""; 69 return op; 70 } 71 operator <<(wostream & op,CLine & cl)72 wostream& operator <<(wostream& op, CLine& cl){ 73 // for debug - print cline to file 74 if(cl.ok == false) 75 op << L"(CLine UNSET)"; 76 else 77 op << L"sp=" << cl.p << L" v=" << cl.v; 78 return op; 79 } 80 operator <<(wostream & op,Plane & pl)81 wostream& operator <<(wostream& op, Plane& pl){ 82 // for debug - print plane to file stream 83 if(pl.ok == false) 84 op << L"(Plane UNSET)"; 85 else 86 op << L"d=" << pl.d << L" normal=" << pl.normal; 87 return op; 88 } 89 operator <<(ostream & op,Point3d & p)90 ostream& operator << (ostream& op, Point3d& p){ 91 // for debug - print point to file 92 // if(p.ok == false) 93 // op << "ok=\"false\""; 94 // else 95 op << "x=\"" << p.x << "\" y=\"" << p.y << "\" z=" << p.z << "\""; 96 return op; 97 98 } 99 operator <<(wostream & op,Vector2d & v)100 wostream& operator <<(wostream& op, Vector2d& v){ 101 // for debug - print vector to file 102 op << L"(" << v.getx() << L", " << v.gety() << L")"; 103 return op; 104 } 105 operator <<(wostream & op,Vector3d & v)106 wostream& operator <<(wostream& op, Vector3d& v){ 107 // for debug - print vector to file 108 op << L"(" << v.getx() << L", " << v.gety() << L"," << v.getz() << L")"; 109 return op; 110 } 111 operator <<(wostream & op,Circle & c)112 wostream& operator <<(wostream& op, Circle& c){ 113 // for debug - print circle to file 114 if(c.ok == false) 115 op << L"ok=\"false\""; 116 else 117 op << L" x=\"" << c.pc.x << L"\" y=\"" << c.pc.y << L"\" radius=\"" << c.radius << L"\""; 118 return op; 119 } 120 operator <<(wostream & op,Span & sp)121 wostream& operator <<(wostream& op, Span& sp){ 122 // for debug - print span to file stream 123 op << L"p0 = " << sp.p0 << L" p1=" << sp.p1; 124 if(sp.dir) { 125 op << L" pc=" << sp.pc << L" dir=" << ((sp.dir == CW)?L"CW" : L"ACW") << L" radius=" << sp.radius; 126 } 127 return op; 128 } 129 130 131 132 // *************************************************************************************************************************************** 133 // point classes 134 // *************************************************************************************************************************************** Point(const Point3d & p)135 Point::Point( const Point3d& p ) { // copy constructor Point p1(p2); 136 x = p.x; 137 y = p.y; 138 // ok = p.ok; 139 ok = true; 140 } 141 Point(const Vector2d & v)142 Point::Point(const Vector2d& v) 143 { 144 x = v.getx(); y = v.gety(); 145 } 146 Point3d(const Vector3d & v)147 Point3d::Point3d(const Vector3d& v) { 148 x = v.getx(); y = v.gety(); z = v.getz();// ok = true; 149 } 150 operator ==(const Point3d & p) const151 bool Point3d::operator==(const Point3d &p)const{ 152 // p1 == p2 (uses TOLERANCE) 153 if(FNE(this->x, p.x, TOLERANCE) || FNE(this->y, p.y, TOLERANCE) || FNE(this->z, p.z, TOLERANCE)) return false; 154 return true; 155 } 156 Transform(const Matrix & m)157 Point Point::Transform(const Matrix& m) { 158 // transform Point 159 Point ret; 160 m.Transform2d(&x, &ret.x); 161 ret.ok = true; 162 return ret; 163 } Transform(const Matrix & m)164 Point3d Point3d::Transform(const Matrix& m) { 165 // transform Point 166 Point3d ret; 167 m.Transform(&x, &ret.x); 168 // ret.ok = true; 169 return ret; 170 } 171 operator +(const Vector2d & v) const172 Point Point::operator+(const Vector2d &v)const{ 173 return Point(x + v.getx(), y + v.gety()); 174 } 175 operator +(const Vector3d & v) const176 Point3d Point3d::operator+(const Vector3d &v)const{ 177 return Point3d(x + v.getx(), y + v.gety(), z + v.getz()); 178 } 179 operator ==(const Point & p) const180 bool Point::operator==(const Point &p) const{ 181 // p1 == p2 (uses TOLERANCE) 182 if(FNE(this->x, p.x, TOLERANCE) || FNE(this->y, p.y, TOLERANCE)) return false; 183 return true; 184 } 185 186 187 Dist(const Point & p) const188 double Point::Dist(const Point& p)const{ // distance between 2 points 189 return Vector2d(*this, p).magnitude(); 190 } 191 DistSq(const Point & p) const192 double Point::DistSq(const Point& p)const{ // distance squared between 2 points 193 return Vector2d(*this, p).magnitudesqd(); 194 } 195 Dist(const Point3d & p) const196 double Point3d::Dist(const Point3d& p)const { // distance between 2 points 197 return Vector3d(*this, p).magnitude(); 198 } 199 DistSq(const Point3d & p) const200 double Point3d::DistSq(const Point3d& p)const { // distance squared 201 return (this->x - p.x) * (this->x - p.x) + (this->y - p.y) * (this->y - p.y) + (this->z - p.z) * (this->z - p.z); 202 } 203 Mid(const Point & p1,double factor) const204 Point Point::Mid(const Point& p1, double factor)const{ 205 // Mid 206 return geoff_geometry::Mid(*this, p1, factor); 207 } 208 Mid(const Point3d & p,double factor) const209 Point3d Point3d::Mid(const Point3d& p, double factor)const{ 210 // Mid 211 return Vector3d(*this, p) * factor + *this; 212 } 213 Mid(const Point & p0,const Point & p1,double factor)214 Point Mid(const Point& p0, const Point& p1, double factor){ 215 // mid or partway between 2 points 216 return Vector2d(p0, p1) * factor + p0; 217 } Rel(const Point & p,double x0,double y0)218 Point Rel(const Point& p, double x0, double y0) { 219 // Relative point 220 return (p.ok)?Point(p.x + x0, p.y + y0) : INVALID_POINT; 221 } 222 Polar(const Point & p,double angle,double r)223 Point Polar(const Point& p, double angle, double r) { 224 // polar from this point 225 angle *= DegreesToRadians; 226 return (p.ok)?Point(p.x + r * cos(angle), p.y + r * sin(angle)) : INVALID_POINT; 227 } 228 229 // *************************************************************************************************************************************** 230 // clines 231 // *************************************************************************************************************************************** 232 //const CLine horiz(Point(0, 0), 1, 0); // define global horizontal line 233 c()234 double CLine::c() { 235 // returns c for ax + by + c = 0 format (peps format where needed) 236 return (v.getx() * p.y - v.gety() * p.x); 237 } Normalise()238 void CLine::Normalise() { 239 // normalise the cline vector 240 ok = v.normalise() >= TOLERANCE; 241 } 242 CLine(const Span & sp)243 CLine::CLine(const Span& sp){ 244 p = sp.p0; 245 v = sp.vs; 246 ok = sp.returnSpanProperties && !sp.NullSpan; 247 } 248 Normal(const CLine & s)249 CLine Normal(const CLine& s) { 250 // returns normal to this line 251 return CLine(s.p, ~s.v, false); 252 } operator ~(void)253 const CLine CLine::operator ~(void){ 254 return CLine(this->p, ~v, false); 255 } Normal(const CLine & s,const Point & p)256 CLine Normal(const CLine& s, const Point& p) { 257 // returns normal to this line thro' p 258 return CLine(p, ~s.v, false); 259 } 260 Transform(Matrix & m)261 CLine CLine::Transform(Matrix& m) { 262 263 Point p0 = this->p; 264 Point p1(p0.x + v.getx(), p0.y + v.gety()); 265 return CLine(p0.Transform(m), p1.Transform(m)); 266 } 267 268 Dist(const Point & p0) const269 double CLine::Dist(const Point& p0)const { 270 // distance between cline & point >0 cw about point <0 acw about point 271 return this->v ^ Vector2d(p0, this->p); 272 } 273 Dist(const CLine & cl) const274 double Point::Dist(const CLine& cl)const { 275 // distance between cline & point >0 cw about point <0 acw about point 276 return cl.v ^ Vector2d(*this, cl.p); 277 } 278 Intof(const CLine & s)279 Point CLine::Intof(const CLine& s) { 280 // Intof 2 Clines 281 return geoff_geometry::Intof(*this, s); 282 } 283 Intof(int NF,const Circle & c)284 Point CLine::Intof(int NF, const Circle& c) { 285 // Intof Cline & Circleconst 286 return geoff_geometry::Intof(NF, *this, c); 287 } Intof(int NF,const Circle & c,Point & otherInters)288 Point CLine::Intof(int NF, const Circle& c, Point& otherInters) { 289 // Intof Cline & Circle & other intersection 290 return geoff_geometry::Intof(NF, *this, c, otherInters); 291 } 292 Intof(const CLine & s0,const CLine & s1)293 Point Intof(const CLine& s0, const CLine& s1) { 294 // inters of 2 clines (parameterise lines x = x0 + t * dx) 295 double cp = s1.v ^ s0.v; 296 if(fabs (cp) > 1.0e-6) { 297 double t = (s1.v ^ Vector2d(s0.p, s1.p)) / cp; 298 return s0.v * t + s0.p; 299 } 300 return INVALID_POINT; 301 } XonCLine(CLine & s,double xval)302 Point XonCLine(CLine& s, double xval) { 303 // return point given X on a line 304 return Intof(s, CLine(Point(xval,0),0,1,false)); 305 } YonCLine(CLine & s,double yval)306 Point YonCLine(CLine& s, double yval) { 307 // return point given Y on a line 308 return Intof(s, CLine(Point(0,yval),1,0,false)); 309 } Along(const CLine & s,double d)310 Point Along(const CLine& s, double d) { 311 // distance along line 312 return Point(s.p.x + d * s.v.getx(), s.p.y + d * s.v.gety(), s.ok); 313 } 314 Along(const CLine & s,double d,Point & p)315 Point Along(const CLine& s, double d, Point& p) { 316 // distance along line from point 317 return Point(p.x + d * s.v.getx(), p.y + d * s.v.gety(), p.ok); 318 } Around(const Circle & c,double d,const Point & p)319 Point Around(const Circle& c, double d, const Point& p) { 320 // distance around circle from point 321 CLine radial(c.pc, p); 322 if(radial.ok) { 323 if(fabs(c.radius) > TOLERANCE ) { 324 double a = sin(- d / c.radius); 325 double b = cos(- d / c.radius); 326 return Point(c.pc.x - c.radius * (radial.v.gety() * a - radial.v.getx() * b), c.pc.y + c.radius * (radial.v.gety() * b + radial.v.getx() * a)); 327 } 328 } 329 return INVALID_POINT; 330 } AtAngle(double angle,const Point & p0,const CLine & s)331 CLine AtAngle(double angle, const Point& p0, const CLine& s) { 332 // cline at angle [to a cline] thro' a point 333 angle *= DegreesToRadians; 334 Vector2d v(cos(angle), sin(angle)); 335 return CLine(p0, v.getx() * s.v.getx() - v.gety() * s.v.gety(), v.gety() * s.v.getx() + v.getx() * s.v.gety()); 336 } Parallel(int side,const CLine & s0,double distance)337 CLine Parallel(int side, const CLine& s0, double distance) { 338 // parallel to line by distance 339 Vector2d v = ~s0.v; 340 return CLine(v * ((double)side * distance) + s0.p, s0.v.getx(), s0.v.gety()); 341 } 342 Parallel(const CLine & s0,Point & p)343 CLine Parallel(const CLine& s0, Point& p) { 344 // parallel to line through point 345 return CLine(p, s0.v.getx(), s0.v.gety()); 346 } 347 Bisector(const CLine & s)348 CLine CLine::Bisector(const CLine& s) { 349 // bisector of 2 clines 350 return CLine (this->Intof(s), this->v.getx() + s.v.getx(), this->v.gety() + s.v.gety()); 351 } 352 353 354 355 356 // *************************************************************************************************************************************** 357 // circle methods 358 // *************************************************************************************************************************************** 359 Circle(const Point & p,double rad)360 Circle::Circle(const Point& p, double rad){ 361 // Circle 362 pc = p; 363 radius = rad; 364 ok = pc.ok; 365 } 366 Circle(const Point & p,const Point & pc0)367 Circle::Circle( const Point& p, const Point& pc0){ 368 if((ok = (p.ok && pc0.ok))) { 369 pc = pc0; 370 radius = p.Dist(pc0); 371 } 372 } 373 Circle(const Span & sp)374 Circle::Circle( const Span& sp){ 375 pc = sp.pc; 376 radius = sp.radius; 377 ok = sp.returnSpanProperties; 378 } 379 operator ==(const Circle & c) const380 bool Circle::operator==(const Circle &c)const{ 381 // c1 == c2 (uses TOLERANCE) 382 return FEQ(this->radius, c.radius, TOLERANCE) && (this->pc == c.pc); 383 } 384 Transform(Matrix & m)385 Circle Circle::Transform(Matrix& m) { // transform 386 Point p0 = this->pc; 387 double scale; 388 if(m.GetScale(scale) == false) FAILURE(getMessage(L"Differential Scale not allowed for this method")); 389 return Circle(p0.Transform(m), radius * scale); 390 } 391 Intof(int LR,const Circle & c1)392 Point Circle::Intof(int LR, const Circle& c1) { 393 // intof 2 circles 394 return geoff_geometry::Intof(LR, *this, c1); 395 } Intof(int LR,const Circle & c1,Point & otherInters)396 Point Circle::Intof(int LR, const Circle& c1, Point& otherInters) { 397 // intof 2 circles, (returns the other intersection) 398 return geoff_geometry::Intof(LR, *this, c1, otherInters); 399 } Intof(const Circle & c1,Point & leftInters,Point & rightInters)400 int Circle::Intof(const Circle& c1, Point& leftInters, Point& rightInters) { 401 // intof 2 circles, (returns the other intersection) 402 return geoff_geometry::Intof(*this, c1, leftInters, rightInters); 403 } 404 Tanto(int AT,double angle,const CLine & s0) const405 CLine Circle::Tanto(int AT, double angle, const CLine& s0) const{ 406 // cline tanto circle at angle to optional cline 407 return geoff_geometry::Tanto(AT, *this, angle, s0); 408 } 409 Tanto(int AT,const Circle & c,const Point & p)410 CLine Tanto(int AT, const Circle& c, const Point& p) { 411 // CLine tangent to a circle through a point 412 Vector2d v(p, c.pc); 413 double d = v.magnitude(); 414 CLine s(p, ~v, false); // initialise cline 415 416 if ( d < TOLERANCE || d < fabs(c.radius) - TOLERANCE) // point inside circle ? 417 return INVALID_CLINE; 418 else { 419 if(d > fabs(c.radius) + TOLERANCE) { // point outside circle 420 v.Rotate(sqrt((d - c.radius) * (d + c.radius)), - AT * c.radius); 421 s.v = v; 422 } 423 } 424 s.Normalise(); 425 return s; 426 } 427 Tanto(int AT0,const Circle & c0,int AT1,const Circle & c)428 CLine Tanto(int AT0, const Circle& c0, int AT1, const Circle& c) { 429 // cline tanto 2 circles 430 CLine s; 431 Circle c1 = c; 432 c1.radius -= (double) (AT0 * AT1) * c0.radius; 433 s = Tanto(AT1, c1, c0.pc); 434 s.p.x += (double) AT0 * c0.radius * s.v.gety(); 435 s.p.y -= (double) AT0 * c0.radius * s.v.getx(); 436 return s; 437 } 438 Tanto(int AT,const Circle & c,double angle,const CLine & s0)439 CLine Tanto(int AT, const Circle& c, double angle, const CLine& s0) { 440 // cline at an angle [to a cline] tanto a circle 441 CLine s = AtAngle(angle, c.pc, s0); 442 s.p.x += (double) AT * c.radius * s.v.gety(); 443 s.p.y -= (double) AT * c.radius * s.v.getx(); 444 // s.p += ~s.v * (AT * c.radius); 445 s.ok = true; 446 return s; 447 } AtAngle(const Circle & c,double angle)448 Point AtAngle(const Circle& c, double angle) { 449 // Point at an angle on circle 450 angle *= DegreesToRadians; 451 return Point(c.pc.x + c.radius * cos(angle), c.pc.y + c.radius * sin(angle)); 452 } 453 On(const CLine & s,const Point & p)454 Point On(const CLine& s, const Point& p) { 455 // returns point that is nearest to s from p 456 double t = s.v * Vector2d(s.p, p); 457 return s.v * t + s.p; 458 } 459 On(const Circle & c,const Point & p)460 Point On(const Circle& c, const Point& p) { 461 // returns point that is nearest to c from p 462 double r = p.Dist(c.pc); 463 if(r < TOLERANCE) FAILURE(getMessage(L",Point on Circle centre - On(Circle& c, Point& p)")); 464 return(Mid(p, c.pc, (r - c.radius) / r)); 465 } 466 467 Intof(int NF,const CLine & s,const Circle & c)468 Point Intof( int NF, const CLine& s, const Circle& c) { 469 // inters of cline & circle eg. p1 = Intof(NEARINT, s1, c1); 470 Point otherInters; 471 return Intof(NF, s, c, otherInters); 472 } 473 Intof(int NF,const CLine & s,const Circle & c,Point & otherInters)474 Point Intof( int NF, const CLine& s, const Circle& c, Point& otherInters) { 475 // inters of cline & circle eg. p1 = Intof(NEARINT, s1, c1); 476 // otherInters returns the other intersection 477 #if 1 478 // solving x = x0 + dx * t x = y0 + dy * t 479 // x = xc + R * cos(a) y = yc + R * sin(a) for t 480 // gives :- t� (dx� + dy�) + 2t(dx*dx0 + dy*dy0) + (x0-xc)� + (y0-yc)� - R� = 0 481 int nRoots; 482 double t, tFar, tNear, tOther; 483 Vector2d v0(c.pc, s.p); 484 if((nRoots = quadratic(1, 2 * (v0 * s.v), v0.magnitudesqd() - c.radius * c.radius, tFar, tNear)) != 0) { 485 if(nRoots == 2 && NF == NEARINT) { 486 t = tNear; 487 tOther = tFar; 488 } else { 489 t = tFar; 490 tOther = (nRoots == 2)?tNear : tFar; 491 } 492 otherInters = s.v * tOther + s.p; 493 return s.v * t + s.p; 494 } 495 return INVALID_POINT; 496 } 497 #else 498 // geometric solution - this is similar to the peps method, and it may offer better tolerancing than above?? 499 Point intof; 500 CLine normal = Normal(s, c.pc); 501 intof = s.Intof(normal); 502 double d = intof.Dist(c.pc); 503 504 if(fabs(d - c.radius) < TOLERANCE) return intof; // tangent (near enough for non-large radius I suppose?) 505 506 if(d > c.radius + TOLERANCE) return INVALID_POINT; // no intersection 507 508 double q = (c.radius - d) * (c.radius + d); 509 if(q < 0) return intof; // line inside tolerance 510 511 return Along(s, -(double)NF * sqrt(q), intof); // 2 intersections (return near/far case) 512 } 513 #endif Intof(int intMode,const Circle & c0,const Circle & c1)514 Point Intof( int intMode, const Circle& c0, const Circle& c1) { 515 // inters of 2 circles eg. p1 = Intof(LEFTINT, c1, c2) 516 Point otherInters; 517 return Intof(intMode, c0, c1, otherInters); 518 } 519 Intof(int intMode,const Circle & c0,const Circle & c1,Point & otherInters)520 Point Intof( int intMode, const Circle& c0, const Circle& c1, Point& otherInters) { 521 // inters of 2 circles eg. p1 = Intof(LEFTINT, c1, c2);u 522 Point pLeft, pRight; 523 switch(Intof(c0, c1, pLeft, pRight)) { 524 default: 525 return INVALID_POINT; 526 case 1: 527 otherInters = pLeft; 528 return pLeft; 529 case 2: 530 if(intMode == LEFTINT) { 531 otherInters = pRight; 532 return pLeft; 533 }else { 534 otherInters = pLeft; 535 return pRight; 536 } 537 } 538 } 539 Intof(const Circle & c0,const Circle & c1,Point & pLeft,Point & pRight)540 int Intof(const Circle& c0, const Circle& c1, Point& pLeft, Point& pRight) { 541 // inters of 2 circles 542 // returns the number of intersctions 543 Vector2d v(c0.pc, c1.pc); 544 double d = v.normalise(); 545 if(d < TOLERANCE) return 0; // co-incident circles 546 547 double sum = fabs(c0.radius) + fabs(c1.radius); 548 double diff = fabs(fabs(c0.radius) - fabs(c1.radius)); 549 if(d > sum + TOLERANCE || d < diff - TOLERANCE) return 0; 550 551 // dist from centre of this circle to mid intersection 552 double d0 = 0.5 * (d + (c0.radius + c1.radius) * (c0.radius - c1.radius) / d); 553 if(d0 - c0.radius > TOLERANCE) return 0; // circles don't intersect 554 555 double h = (c0.radius - d0) * (c0.radius + d0); // half distance between intersects squared 556 if(h < 0) d0 = c0.radius; // tangent 557 pLeft = v * d0 + c0.pc; // mid-point of intersects 558 if(h < TOLERANCE_SQ) return 1; // tangent 559 h = sqrt(h); 560 561 v = ~v; // calculate 2 intersects 562 pRight = v * h + pLeft; 563 v = -v; 564 pLeft = v * h + pLeft; 565 return 2; 566 } 567 Tanto(int NF,CLine & s0,Point & p,double rad)568 Circle Tanto(int NF, CLine& s0, Point& p, double rad) { 569 // circle tanto a CLine thro' a point 570 double d = s0.Dist(p); 571 if(fabs(d) > rad + TOLERANCE) return INVALID_CIRCLE; // point too far from line 572 CLine s0offset = Parallel(RIGHTINT, s0, rad); 573 574 return Circle(Intof(NF, s0offset, Circle(p, rad)), rad); 575 } 576 Tanto(int AT1,CLine & s1,int AT2,CLine & s2,double rad)577 Circle Tanto(int AT1, CLine& s1, int AT2, CLine& s2, double rad) { 578 // circle tanto 2 clines with radius 579 CLine Offs1 = Parallel(AT1, s1, rad); 580 CLine Offs2 = Parallel(AT2, s2, rad); 581 Point pc = Intof(Offs1, Offs2); 582 return (pc.ok)? Circle(pc, rad) : INVALID_CIRCLE; 583 } Tanto(int AT1,CLine s1,int AT2,CLine s2,int AT3,CLine s3)584 Circle Tanto(int AT1, CLine s1, int AT2, CLine s2, int AT3, CLine s3) { 585 // circle tanto 3 CLines 586 double s1c = s1.c(), s2c = s2.c(), s3c = s3.c(); 587 double d = s1.v.gety() * (AT2 * s3.v.getx() - AT3 * s2.v.getx()) 588 + s2.v.gety() * (AT3 * s1.v.getx() - AT1 * s3.v.getx()) 589 + s3.v.gety() * (AT1 * s2.v.getx() - AT2 * s1.v.getx()); 590 if(fabs(d) < UNIT_VECTOR_TOLERANCE) return INVALID_CIRCLE; 591 double radius = (s1.v.gety() * (s2.v.getx() * s3c - s3.v.getx() * s2c) 592 + s2.v.gety() * (s3.v.getx() * s1c - s1.v.getx() * s3c) 593 + s3.v.gety() * (s1.v.getx() * s2c - s2.v.getx() * s1c)) / d ; 594 if(radius < TOLERANCE) return INVALID_CIRCLE; 595 596 CLine Offs1 = Parallel(AT1, s1, radius); 597 CLine Offs2 = Parallel(AT2, s2, radius); 598 599 Point p = Intof(Offs1, Offs2); 600 if(!p.ok) { 601 CLine Offs3 = Parallel(AT3, s3, radius); // s1 & s2 parallel 602 p = Intof(Offs1, Offs3); 603 if(!p.ok) return INVALID_CIRCLE; // 3 parallel lines 604 } 605 return Circle(p, radius); 606 } Thro(int LR,const Point & p0,const Point & p1,double rad)607 Circle Thro(int LR, const Point& p0, const Point& p1, double rad) { 608 // circle thro' 2 points, given radius and side 609 CLine thro(p0, p1); 610 if(thro.ok) { 611 double d = 0.5 * p0.Dist(p1); 612 Point pm = Mid(p0, p1); 613 614 if(d > rad + TOLERANCE) return INVALID_CIRCLE; 615 else if(d > rad - TOLERANCE) { 616 // within tolerance of centre of 2 points 617 return Circle(pm, d); 618 } 619 else { 620 // 2 solutions 621 return Circle(Along(Normal(thro, pm), (double)LR * sqrt((rad + d) * (rad - d)), pm), rad); 622 } 623 } 624 return INVALID_CIRCLE; 625 } 626 Thro(const Point & p0,const Point & p1)627 Circle Thro(const Point& p0, const Point& p1) { 628 // circle thro 2 points (diametric) 629 return Circle(p0.Mid(p1), .5*p0.Dist(p1)); 630 } Thro(const Point & p0,const Point & p1,const Point & p2)631 Circle Thro(const Point& p0, const Point& p1, const Point& p2) { 632 // circle thro 3 points 633 CLine s0(p0, p1); 634 if(!s0.ok) return Thro(p1,p2); // p0 & p1 coincident 635 636 CLine s1(p0, p2); 637 if(!s1.ok) return Thro(p0, p1); // p0 & p2 coincident 638 639 CLine s2(p2, p1); 640 if(!s2.ok) return Thro(p0, p2); // p1 & p2 coincident 641 642 Point p = Intof(Normal(s0, Mid(p0, p1)), Normal(s1, Mid(p0, p2))); 643 return (p.ok)? Circle(p, p0.Dist(p)) : INVALID_CIRCLE; 644 } Tanto(int NF,int AT0,const CLine & s0,int AT1,const Circle & c1,double rad)645 Circle Tanto(int NF, int AT0, const CLine& s0, int AT1, const Circle &c1, double rad) { 646 // circle tanto cline & circle with radius 647 CLine Offs0 = Parallel(AT0, s0, rad); 648 Circle c2 = c1; 649 c2.radius += AT1 * rad; 650 Point pc = Intof(NF, Offs0, c2); 651 return (pc.ok)? Circle(pc, rad) : INVALID_CIRCLE; 652 } 653 Tanto(int LR,int AT0,const Circle & c0,const Point & p,double rad)654 Circle Tanto( int LR, int AT0, const Circle& c0, const Point& p, double rad) { 655 // circle tanto circle & thro' a point 656 Circle c2 = c0; 657 c2.radius += AT0 * rad; 658 Circle c1(p, rad); 659 Point pc = Intof(LR, c2, c1); 660 return (pc.ok)? Circle(pc, rad) : INVALID_CIRCLE; 661 } Tanto(int LR,int AT0,const Circle & c0,int AT1,const Circle & c1,double rad)662 Circle Tanto(int LR, int AT0, const Circle& c0, int AT1, const Circle& c1, double rad) { 663 // circle tanto 2 circles 664 Circle c2 = c0; 665 Circle c3 = c1; 666 c2.radius += AT0 * rad; 667 c3.radius += AT1 * rad; 668 Point pc = Intof(LR, c2, c3); 669 return (pc.ok)? Circle(pc, rad) : INVALID_CIRCLE; 670 } 671 Parallel(int side,const Circle & c0,double distance)672 Circle Parallel(int side, const Circle& c0, double distance) { 673 // parallel to circle by distance 674 return Circle(c0.pc, c0.radius + (double) side * distance); 675 } 676 677 // distance atn360(double dy,double dx)678 double atn360(double dy, double dx) { 679 // angle 0 to 2pi 680 double ang = atan2(dy, dx); 681 return ((ang < 0)? 2 * PI + ang : ang); 682 } 683 Dist(const Point & p0,const Circle & c,const Point & p1)684 double Dist(const Point& p0, const Circle& c, const Point& p1) { 685 // clockwise distance around c from p0 to p1 686 double a0 = atn360(p0.y - c.pc.y, p0.x - c.pc.x); 687 double a1 = atn360(p1.y - c.pc.y ,p1.x - c.pc.x); 688 if ( a1 > a0 ) a1 -= 2 * PI ; 689 return (a0 - a1) * c.radius; 690 } Dist(const CLine & s,const Circle & c)691 double Dist(const CLine& s, const Circle& c) { 692 // distance between line and circle 693 return fabs(s.Dist(c.pc)) - c.radius; 694 } Dist(const Circle & c0,const Circle & c1)695 double Dist(const Circle& c0, const Circle& c1) { 696 // distance between 2 circles 697 return c0.pc.Dist(c1.pc) - c0.radius - c1.radius; 698 } Dist(const Circle & c,const Point & p)699 double Dist(const Circle& c, const Point& p) { 700 // distance between circle and point 701 return p.Dist(On(c, p)); 702 } 703 IncludedAngle(const Vector2d & v0,const Vector2d & v1,int dir)704 double IncludedAngle(const Vector2d& v0, const Vector2d& v1, int dir) { 705 // returns the absolute included angle between 2 vectors in the direction of dir ( 1=acw -1=cw) 706 double inc_ang = v0 * v1; 707 if(inc_ang > 1. - UNIT_VECTOR_TOLERANCE) return 0; 708 if(inc_ang < -1. + UNIT_VECTOR_TOLERANCE) 709 inc_ang = PI; 710 else { // dot product, v1 . v2 = cos ang 711 if(inc_ang > 1.0) inc_ang = 1.0; 712 inc_ang = acos(inc_ang); // 0 to pi radians 713 714 if(dir * (v0 ^ v1) < 0) inc_ang = 2 * PI - inc_ang ; // cp 715 } 716 return dir * inc_ang; 717 } 718 IncludedAngle(const Vector3d & v0,const Vector3d & v1,const Vector3d & normal,int dir)719 double IncludedAngle(const Vector3d& v0, const Vector3d& v1, const Vector3d& normal, int dir) { 720 // returns the absolute included angle between 2 vectors in the direction of dir ( 1=acw -1=cw) about normal 721 double inc_ang = v0 * v1; 722 723 if(inc_ang >= -NEARLY_ONE) { // dot product, v1 . v2 = cos ang 724 inc_ang = acos(inc_ang); // 0 to pi radians 725 726 if(dir * (normal * (v0 ^ v1)) < 0) inc_ang = 2 * PI - inc_ang ; // cp 727 } 728 else 729 inc_ang = PI; // semi-cicle 730 731 return dir * inc_ang; 732 } 733 corner(const Vector2d & v0,const Vector2d & v1,double cpTol)734 int corner(const Vector2d& v0, const Vector2d& v1, double cpTol) { 735 // returns corner 736 // 0 (TANGENT) = tangent 737 // 1 (LEFT) = left turn 738 // -1 (RIGHT) = right turn 739 double cp = v0 ^ v1; 740 if(fabs(cp) < cpTol) return TANGENT; 741 742 return (cp > 0)?GEOFF_LEFT : GEOFF_RIGHT; 743 } 744 quadratic(double a,double b,double c,double & x0,double & x1)745 int quadratic(double a, double b, double c, double& x0, double& x1) { 746 // solves quadratic equation ax² + bx + c = 0 747 // returns number of real roots 748 // double epsilon = 1.0e-6; 749 double epsilon = (geoff_geometry::UNITS == METRES)?1.0e-09 : 1.0e-06; 750 double epsilonsq = epsilon * epsilon; 751 if(fabs(a) < epsilon) { 752 if(fabs(b) < epsilon) return 0; // invalid 753 x0 = - c / b; 754 return 1; 755 } 756 b /= a; 757 c /= a; 758 double s = b * b - 4 * c; 759 if(s < -epsilon) return 0; // imaginary roots 760 x0 = - 0.5 * b; 761 if(s > epsilonsq) { 762 s = 0.5 * sqrt(s); 763 x1 = x0 - s; 764 x0 += s; 765 return 2; 766 } 767 return 1; 768 } 769 Plane(const Point3d & p0,const Point3d & p1,const Point3d & p2)770 Plane::Plane(const Point3d& p0, const Point3d& p1, const Point3d& p2) { 771 // constructor plane from 3 points 772 normal = Vector3d(p0, p1) ^ Vector3d(p0, p2); 773 normal.normalise(); 774 ok = (normal != NULL_VECTOR); 775 d = -(normal * Vector3d(p0)); 776 } 777 Plane(const Point3d & p0,const Vector3d & v,bool normalise)778 Plane::Plane(const Point3d& p0, const Vector3d& v, bool normalise) { 779 // constructor plane from point & vector 780 normal = v; 781 if(normalise == true) normal.normalise(); 782 d = -(normal * Vector3d(p0)); 783 } 784 Plane(double dist,const Vector3d & n)785 Plane::Plane(double dist, const Vector3d& n) { 786 normal = n; 787 double mag = normal.normalise(); 788 ok = (normal != NULL_VECTOR); 789 if(ok) d = dist / mag; 790 } 791 Dist(const Point3d & p) const792 double Plane::Dist(const Point3d& p)const{ 793 // returns signed distance to plane from point p 794 return (normal * Vector3d(p)) + d; 795 } 796 Near(const Point3d & p) const797 Point3d Plane::Near(const Point3d& p)const { 798 // returns near point to p on the plane 799 return - normal * Dist(p) + p; 800 } 801 Intof(const Line & l,Point3d & intof,double & t) const802 bool Plane::Intof(const Line& l, Point3d& intof, double& t) const{ 803 // intersection between plane and line 804 // input this plane, line 805 // output intof 806 // method returns true for valid intersection 807 double den = l.v * this->normal; 808 if(fabs(den) < UNIT_VECTOR_TOLERANCE) return false; // line is parallel to the plane, return false, even if the line lies on the plane 809 810 t = -(normal * Vector3d(l.p0) + d) / den; 811 intof = l.v * t + l.p0; 812 return true; 813 } 814 Intof(const Plane & pl,Line & intof) const815 bool Plane::Intof(const Plane& pl, Line& intof)const { 816 // intersection of 2 planes 817 Vector3d d = this->normal ^ pl.normal; 818 d.normalise(); 819 intof.ok = false; 820 if(d == NULL_VECTOR) return false; // parallel planes 821 822 intof.v = d; 823 intof.length = 1; 824 825 double dot = this->normal * pl.normal; 826 827 double den = dot * dot - 1.; 828 double a = (this->d - pl.d * dot) / den; 829 double b = (pl.d - this->d * dot) / den; 830 intof.p0 = a * this->normal + b * pl.normal; 831 intof.ok = true; 832 return true; 833 } 834 Intof(const Plane & pl0,const Plane & pl1,Point3d & intof) const835 bool Plane::Intof(const Plane& pl0, const Plane& pl1, Point3d& intof) const{ 836 // intersection of 3 planes 837 Line tmp; 838 if(Intof(pl0, tmp)) { 839 double t; 840 return pl1.Intof(tmp, intof, t); 841 } 842 return false; 843 } 844 } 845