double Dist(const Span& sp, const Point& p , Point& pnear ) {
// returns distance of p from span, pnear is the nearpoint on the span (or endpoint)
if(!sp.dir) {
double d, t;
Point3d unused_pnear;
d = Dist(Line(sp), Point3d(p), unused_pnear, t);
if(t < -geoff_geometry::TOLERANCE) {
pnear = sp.p0; // nearpoint
d = pnear.Dist(p);
}
else if(t > sp.length + geoff_geometry::TOLERANCE) {
pnear = sp.p1;
d = pnear.Dist(p);
}
return d;
}
else {
// put pnear on the circle
double radiusp;
Vector2d v(sp.pc, p);
if((radiusp = v.magnitude()) < geoff_geometry::TOLERANCE) {
// point specified on circle centre - use first point as near point
pnear = sp.p0; // nearpoint
return sp.radius;
}
else {
pnear = v * (sp.radius / radiusp) + sp.pc;
// check if projected point is on the arc
if(sp.OnSpan(pnear)) return fabs(radiusp - sp.radius);
// double h1 = pnear.x - sp.p0.x ;
// double v1 = pnear.y - sp.p0.y ;
// double h2 = sp.p1.x - pnear.x ;
// double v2 = sp.p1.y - pnear.y ;
// if ( sp.dir * ( h1 * v2 - h2 * v1 ) >= 0 )return fabs(radiusp - sp.radius);
// point not on arc so calc nearest end-point
double ndist = p.Dist(sp.p0);
double dist = p.Dist(sp.p1);
if(ndist >= dist) {
// sp.p1 is near point
pnear = sp.p1;
return dist;
}
// sp.p0 is near point
pnear = sp.p0; // nearpoint
return ndist ;
}
}
}
// The vector i1-i2 in picture i should match the vector j1->j2 in j. Make this happen
void MakeMatch(vector<Picture> &vp, int i, Point i1, Point i2, int j, Point j1, Point j2)
{
printf("Make match (%f %f) to (%f %f) in %d, and (%f %f) to (%f %f) in %d\n",
i1.x, i1.y, i2.x, i2.y, i, j1.x, j1.y, j2.x, j2.y, j);
// We will adjust the transform of picture j. Transform the first vector from is space
// to global space.
vp[i].tr.Transform( i1 );
vp[i].tr.Transform( i2 );
printf("Now match (%f %f) to (%f %f) in global space, and (%f %f) to (%f %f) in %d\n",
i1.x, i1.y, i2.x, i2.y, j1.x, j1.y, j2.x, j2.y, j);
//first, what's the scale change:
double scale = i1.Dist( i2 ) / j1.Dist( j2 );
// and the angle
double ai = atan2(i2.y-i1.y, i2.x-i1.x);
double aj = atan2(j2.y-j1.y, j2.x-j1.x);
printf("Scale %f, angle %f radians, distance %f\n", scale, ai-aj, i1.Dist( i2 ) );
TAffine tf(
scale*cos(ai-aj),
scale*(-sin(ai-aj)),
0.0,
-tf.t[1],
tf.t[0],
0.0 );
// now transform the first point and make sure it ends up in the right place
Point p(j1.x,j1.y);
tf.Transform( p );
tf.AddXY( i1.x - p.x, i1.y - p.y );
// OK, test the new transform
Point p1(j1.x,j1.y);
Point p2(j2.x,j2.y);
tf.Transform( p1 );
tf.Transform( p2 );
printf("Point (%f %f), distance %f\n", p1.x, p1.y, p1.Dist( p2 ) );
printf(" Finally get (%f %f) to (%f %f) in global space, and (%f %f) to (%f %f) in %d\n",
i1.x, i1.y, i2.x, i2.y, p1.x, p1.y, p2.x, p2.y, j);
p1.x = p1.y = 0.0;
p2.x = vp[j].w-1; p2.y = vp[j].h-1;
tf.Transform( p1 );
tf.Transform( p2 );
printf(" Closure data: corners map to (%f %f) (%f %f)\n", p1.x, p1.y, p2.x, p2.y);
vp[j].tr.CopyIn( tf );
vp[j].Inverse.InverseOf( tf );
}
bool OnSpan(const Span& sp, const Point& p, bool nearPoints, Point& pNear, Point& pOnSpan) {
// function returns true if pNear == pOnSpan
// returns pNear & pOnSpan if nearPoints true
// pNear (nearest on unbound span)
// pOnSpan (nearest on finite span)
if(sp.dir) {
// arc
if(fabs(p.Dist(sp.pc) - sp.radius) > geoff_geometry::TOLERANCE) {
if(!nearPoints) return false;
}
pNear = On(Circle(sp.pc, sp.radius), p);
if(sp.OnSpan(pNear)) {
if(nearPoints) pOnSpan = pNear;
return true; // near point is on arc - already calculated
}
// point not on arc return the nearest end-point
if(nearPoints) pOnSpan = (p.Dist(sp.p0) >= p.Dist(sp.p1)) ?sp.p1 : sp.p0;
return false;
}
else {
// straight
if(fabs(CLine(sp.p0, sp.vs).Dist(p)) > geoff_geometry::TOLERANCE) {
if(!nearPoints) return false;
}
Vector2d v(sp.p0, p);
double t = v * sp.vs;
if(nearPoints) pNear = sp.vs * t + sp.p0;
bool onSpan = (t > - geoff_geometry::TOLERANCE && t < sp.length + geoff_geometry::TOLERANCE);
if(! onSpan) {
if(nearPoints) pOnSpan = (p.Dist(sp.p0) >= p.Dist(sp.p1))?sp.p1 : sp.p0;
}
else {
if(nearPoints) pOnSpan = pNear;
}
return onSpan;
}
}
Point On(const Circle& c, const Point& p) {
// returns point that is nearest to c from p
double r = p.Dist(c.pc);
if(r < TOLERANCE) FAILURE(getMessage(L",Point on Circle centre - On(Circle& c, Point& p)"));
return(Mid(p, c.pc, (r - c.radius) / r));
}
Circle::Circle( const Point& p, const Point& pc0){
if((ok = (p.ok && pc0.ok))) {
pc = pc0;
radius = p.Dist(pc0);
}
}
tOther = tFar;
} else {
t = tFar;
tOther = (nRoots == 2)?tNear : tFar;
}
otherInters = s.v * tOther + s.p;
return s.v * t + s.p;
}
return INVALID_POINT;
}
#else
// geometric solution - this is similar to the peps method, and it may offer better tolerancing than above??
Point intof;
CLine normal = Normal(s, c.pc);
intof = s.Intof(normal);
double d = intof.Dist(c.pc);
if(fabs(d - c.radius) < TOLERANCE) return intof; // tangent (near enough for non-large radius I suppose?)
if(d > c.radius + TOLERANCE) return INVALID_POINT; // no intersection
double q = (c.radius - d) * (c.radius + d);
if(q < 0) return intof; // line inside tolerance
return Along(s, -(double)NF * sqrt(q), intof); // 2 intersections (return near/far case)
}
#endif
Point Intof( int intMode, const Circle& c0, const Circle& c1) {
// inters of 2 circles eg. p1 = Intof(LEFTINT, c1, c2)
Point otherInters;
return Intof(intMode, c0, c1, otherInters);
double Dist(const Point &rhs){
if(cmp((rhs - a).Dot(b - a)) < 0) return (rhs - a).Abs();
if(cmp((rhs - b).Dot(a - b)) < 0) return (rhs - b).Abs();
return fabs((a - rhs).Cross(b - rhs) / a.Dist(b));
}
/*
* default not includes on the edge
*/
bool InCircle(const Point &rhs) const {
return cmp(O.Dist(rhs) - R) <= 0;
}
bool Span::OnSpan(const Point& p, double* t)const {
// FAST OnSpan test - assumes that p lies ON the unbounded span
#if _DEBUG
if(this->returnSpanProperties == false) {
FAILURE(L"OnSpan - properties no set, incorrect calling code");
}
#endif
#if 0
if(NullSpan) {
*t = 0.0;
return (p == p0);
}
if(p == p0) {
*t = 0.0;
return true;
}
if(p == p1) {
*t = 1.0;
return true;
}
#endif
bool ret;
// if(p == this->p0 || p == this->p1) return true;
if(dir == LINEAR) {
#if 1
#if _DEBUG
// check p is on line
CLine cl(*this);
double d = fabs(cl.Dist(p));
if( d > geoff_geometry::TOLERANCE) {
FAILURE(L"OnSpan - point not on linear span, incorrect calling code");
}
#endif
#endif
Vector2d v0(p0, p);
*t = vs * v0;
// ret = (*t > - geoff_geometry::TOLERANCE && *t < length + geoff_geometry::TOLERANCE);
*t = *t / length;
ret = (*t >= 0 && *t <= 1.0 );
}
else {
// true if p lies on arc span sp (p must be on circle of span)
#if 1
#if _DEBUG
// check that p lies on the arc
double d = p.Dist(pc);
if(FNE(d, radius, geoff_geometry::TOLERANCE)) {
FAILURE(L"OnSpan - point not on circular span, incorrect calling code");
}
#endif
#endif
#if 0 // alt method (faster, but doesn't provide t)
Vector2d v0(p0, p);
Vector2d v1(p0, p1);
// check angle to point from start
double cp;
ret = ((cp = (dir * (v0 ^ v1))) > 0);
*t = 0.0;// incorrect !!!
#else
Vector2d v = ~Vector2d(pc, p);
v.normalise();
if(dir == CW) v = -v;
double ang = IncludedAngle(vs, v, dir);
*t = ang / angle;
ret = (*t >= 0 && *t <= 1.0);
#endif
}
return ret;
}
Point On(const Circle& c, const Point& p) {
// returns point that is nearest to c from p
double r = p.Dist(c.pc);
if(r < TOLERANCE) FAILURE(getMessage(L",Point on Circle centre - On(Circle& c, Point& p)", GEOMETRY_ERROR_MESSAGES, MES_POINTONCENTRE));
return(Mid(p, c.pc, (r - c.radius) / r));
}