655 lines
19 KiB
C++
655 lines
19 KiB
C++
// written by g.j.hawkesford 2006 for Camtek Gmbh
|
|
//
|
|
// This program is released under the BSD license. See the file COPYING for details.
|
|
//
|
|
|
|
#include "geometry.h"
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// finite intersections
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
#ifndef WIN32
|
|
#define __min(a,b) ((a<b)?a:b)
|
|
#define __max(a,b) ((a>b)?a:b)
|
|
#endif
|
|
|
|
namespace geoff_geometry {
|
|
int Intof(const Span& sp0, const Span& sp1, Point& p0, Point& p1, double t[4])
|
|
{
|
|
// returns the number of intersects (lying within spans sp0, sp1)
|
|
if(sp0.box.outside(sp1.box) == true) return 0;
|
|
if(!sp0.dir) {
|
|
if(!sp1.dir) {
|
|
// line line
|
|
return LineLineIntof(sp0, sp1, p0, t);
|
|
}
|
|
else {
|
|
// line arc
|
|
return LineArcIntof(sp0, sp1, p0, p1, t);
|
|
}
|
|
}
|
|
else {
|
|
if(!sp1.dir) {
|
|
// arc line
|
|
return LineArcIntof(sp1, sp0, p0, p1, t);
|
|
}
|
|
else {
|
|
// arc arc
|
|
return ArcArcIntof(sp0, sp1, p0, p1);
|
|
}
|
|
}
|
|
}
|
|
|
|
int LineLineIntof(const Span& sp0 , const Span& sp1, Point& p, double t[2]) {
|
|
// intersection between 2 Line2d
|
|
// returns 0 for no intersection in range of either span
|
|
// returns 1 for intersction in range of both spans
|
|
// t[0] is parameter on sp0,
|
|
// t[1] is parameter on sp1
|
|
Vector2d v0(sp0.p0, sp0.p1);
|
|
Vector2d v1(sp1.p0, sp1.p1);
|
|
Vector2d v2(sp0.p0, sp1.p0);
|
|
|
|
double cp = v1 ^ v0;
|
|
|
|
if(fabs(cp) < UNIT_VECTOR_TOLERANCE ) {
|
|
p = INVALID_POINT;
|
|
return 0; // parallel or degenerate lines
|
|
}
|
|
|
|
t[0] = (v1 ^ v2) / cp;
|
|
p = v0 * t[0] + sp0.p0;
|
|
p.ok = true;
|
|
double toler = geoff_geometry::TOLERANCE / sp0.length; // calc a parametric tolerance
|
|
|
|
t[1] = (v0 ^ v2) / cp;
|
|
if(t[0] < -toler || t[0] > 1 + toler) return 0; // intersection on first?
|
|
toler = geoff_geometry::TOLERANCE / sp1.length; // calc a parametric tolerance
|
|
if(t[1] < -toler || t[1] > 1 + toler) return 0; // intersection on second?
|
|
return 1;
|
|
}
|
|
|
|
int LineArcIntof(const Span& line, const Span& arc, Point& p0, Point& p1, double t[4]) {
|
|
// inters of line arc
|
|
// solving x = x0 + dx * t x = y0 + dy * t
|
|
// x = xc + R * cos(a) y = yc + R * sin(a) for t
|
|
// gives :- t² (dx² + dy²) + 2t(dx*dx0 + dy*dy0) + (x0-xc)² + (y0-yc)² - R² = 0
|
|
int nRoots;
|
|
Vector2d v0(arc.pc, line.p0);
|
|
Vector2d v1(line.p0, line.p1);
|
|
double s = v1.magnitudesqd();
|
|
|
|
p0.ok = p1.ok = false;
|
|
if((nRoots = quadratic(s, 2 * (v0 * v1), v0.magnitudesqd() - arc.radius * arc.radius, t[0], t[1])) != 0) {
|
|
double toler = geoff_geometry::TOLERANCE / sqrt(s); // calc a parametric tolerance
|
|
if(t[0] > -toler && t[0] < 1 + toler) {
|
|
p0 = v1 * t[0] + line.p0;
|
|
p0.ok = arc.OnSpan(p0, &t[2]);
|
|
}
|
|
if(nRoots == 2) {
|
|
if(t[1] > -toler && t[1] < 1 + toler) {
|
|
p1 = v1 * t[1] + line.p0;
|
|
p1.ok = arc.OnSpan(p1, &t[3]);
|
|
}
|
|
}
|
|
if(!p0.ok && p1.ok) {
|
|
p0 = p1;
|
|
p1.ok = false;
|
|
}
|
|
nRoots = (int)p0.ok + (int)p1.ok;
|
|
}
|
|
return nRoots;
|
|
}
|
|
|
|
int ArcArcIntof(const Span& arc0, const Span& arc1, Point& pLeft, Point& pRight) {
|
|
// Intof 2 arcs
|
|
int numInts = Intof(Circle(arc0.pc, arc0.radius), Circle(arc1.pc, arc1.radius), pLeft, pRight);
|
|
|
|
if(numInts == 0) {
|
|
pLeft = arc0.p1;
|
|
pLeft.ok = false;
|
|
return 0;
|
|
}
|
|
int nLeft = arc0.OnSpan(pLeft) && arc1.OnSpan(pLeft);
|
|
int nRight = (numInts == 2)?arc0.OnSpan(pRight) && arc1.OnSpan(pRight) : 0;
|
|
if(nLeft == 0 && nRight) pLeft = pRight;
|
|
return nLeft + nRight;
|
|
}
|
|
|
|
bool Span::OnSpan(const Point& p)const {
|
|
double t;
|
|
return OnSpan(p, &t);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
Line::Line(const Point3d& p, const Vector3d& v0, bool boxed){
|
|
// constructor from point & vector
|
|
p0 = p;
|
|
v = v0;
|
|
length = v.magnitude();
|
|
if(boxed) minmax();
|
|
ok = (length > geoff_geometry::TOLERANCE);
|
|
}
|
|
|
|
Line::Line(const Point3d& p, const Point3d& p1){
|
|
// constructor from 2 points
|
|
p0 = p;
|
|
v = Vector3d(p, p1);
|
|
length = v.magnitude();
|
|
minmax();
|
|
ok = (length > geoff_geometry::TOLERANCE);
|
|
}
|
|
|
|
Line::Line(const Span& sp){
|
|
// contructor from linear span
|
|
p0 = sp.p0;
|
|
v = sp.vs * sp.length;
|
|
length = sp.length;
|
|
// box = sp.box;
|
|
box.min = Point3d(sp.box.min);
|
|
box.max = Point3d(sp.box.max);
|
|
ok = !sp.NullSpan;
|
|
}
|
|
|
|
void Line::minmax() {
|
|
MinMax(this->p0, box.min, box.max);
|
|
MinMax(this->v + this->p0, box.min, box.max);
|
|
}
|
|
|
|
bool Line::atZ(double z, Point3d& p)const {
|
|
// returns p at z on line
|
|
if(FEQZ(this->v.getz())) return false;
|
|
double t = (z - this->p0.z) / this->v.getz();
|
|
p = Point3d(this->p0.x + t * this->v.getx(), this->p0.y + t * this->v.gety(), z);
|
|
return true;
|
|
}
|
|
|
|
|
|
bool Line::Shortest(const Line& l2, Line& lshort, double& t1, double& t2)const {
|
|
/*
|
|
Calculate the line segment PaPb that is the shortest route between
|
|
two lines P1P2 and P3P4. Calculate also the values of mua and mub where
|
|
Pa = P1 + t1 (P2 - P1)
|
|
Pb = P3 + t2 (P4 - P3)
|
|
Return FALSE if no solution exists. P Bourke method.
|
|
Input this 1st line
|
|
Input l2 2nd line
|
|
Output lshort shortest line between lines (if lshort.ok == false, the line intersect at a point lshort.p0)
|
|
Output t1 parameter at intersection on 1st Line
|
|
Output t2 parameter at intersection on 2nd Line
|
|
|
|
*/
|
|
Vector3d v13(l2.p0, this->p0);
|
|
if(this->ok == false || l2.ok == false) return false;
|
|
|
|
double d1343 = v13 * l2.v; // dot products
|
|
double d4321 = l2.v * this->v;
|
|
double d1321 = v13 * this->v;
|
|
double d4343 = l2.v * l2.v;
|
|
double d2121 = this->v * this->v;
|
|
|
|
double denom = d2121 * d4343 - d4321 * d4321;
|
|
if(fabs(denom) < 1.0e-09) return false;
|
|
double numer = d1343 * d4321 - d1321 * d4343;
|
|
|
|
t1 = numer / denom;
|
|
t2 = (d1343 + d4321 * t1) / d4343;
|
|
|
|
lshort = Line(t1* this->v + this->p0, t2 * l2.v + l2.p0);
|
|
t1 *= this->length;
|
|
t2 *= l2.length; // parameter in line length for tolerance checking
|
|
return true;
|
|
}
|
|
|
|
int Intof(const Line& l0, const Line& l1, Point3d& intof)
|
|
{
|
|
/* intersection of 2 vectors
|
|
returns 0 for intercept but not within either vector
|
|
returns 1 for intercept on both vectors
|
|
|
|
note that this routine always returns 0 for parallel vectors
|
|
method:
|
|
x = x0 + dx0 * t0 for l0
|
|
...
|
|
...
|
|
x = x1 + dx1 * t1 for l1
|
|
...
|
|
...
|
|
|
|
x0 + dx0 * t0 = x1 + dx1 * t1
|
|
dx0 * t0 - dx1 * t1 + x0 - x1 = 0
|
|
|
|
setup 3 x 3 determinent for
|
|
a0 t0 + b0 t1 + c0 = 0
|
|
a1 t0 + b1 t1 + c1 = 0
|
|
a2 t0 + b2 t1 + c2 = 0
|
|
|
|
from above a = l0.v
|
|
b = -l1.v
|
|
c = Vector3d(l1, l0)
|
|
*/
|
|
// Vector3d a = l0.v;
|
|
if(l0.box.outside(l1.box) == true) return 0;
|
|
Vector3d b = -l1.v;
|
|
Vector3d c = Vector3d(l1.p0, l0.p0);
|
|
Vector3d det = l0.v ^ b;
|
|
Vector3d t = b ^ c;
|
|
|
|
// choose largest determinant & corresponding parameter for accuracy
|
|
double t0 = t.getx();
|
|
double d = det.getx();
|
|
|
|
if(fabs(det.getz()) > fabs(det.gety())) {
|
|
if(fabs(det.getz()) > fabs(det.getx())) {
|
|
t0 = t.getz();
|
|
d = det.getz();
|
|
}
|
|
}
|
|
else {
|
|
if(fabs(det.gety()) > fabs(det.getx())) {
|
|
t0 = t.gety();
|
|
d = det.gety();
|
|
}
|
|
}
|
|
|
|
if(fabs(d) < 1.0e-06) return 0;
|
|
|
|
t0 /= d;
|
|
intof = l0.v * t0 + l0.p0;
|
|
|
|
Point3d other;
|
|
double t1;
|
|
if(Dist(l1, intof, other, t1) > geoff_geometry::TOLERANCE) return 0;
|
|
|
|
t0 *= l0.length;
|
|
if( t0 < -geoff_geometry::TOLERANCE || t0 > l0.length + geoff_geometry::TOLERANCE || t1 < -geoff_geometry::TOLERANCE || t1 > l1.length + geoff_geometry::TOLERANCE ) return 0;
|
|
return 1;
|
|
}
|
|
|
|
|
|
double Dist(const Line& l, const Point3d& p, Point3d& pnear, double& t){
|
|
// returns the distance of a point from a line and the near point on the extended line and the parameter of the near point (0-length) in range
|
|
pnear = Near(l, p, t );
|
|
return p.Dist(pnear);
|
|
}
|
|
|
|
Point3d Near(const Line& l, const Point3d& p, double& t){
|
|
// returns the near point from a line on the extended line and the parameter of the near point (0-length) in range
|
|
t = (Vector3d(l.p0, p) * l.v) / l.length; // t parametised 0 - line length
|
|
return l.v * (t / l.length) + l.p0;
|
|
}
|
|
|
|
Point3d Line::Near(const Point3d& p, double& t)const{
|
|
// returns the near point from a line on the extended line and the parameter of the near point (0-length) in range
|
|
t = (Vector3d(this->p0, p) * this->v) / this->length; // t parametised 0 - line length
|
|
return this->v * (t / this->length) + this->p0;
|
|
}
|
|
|
|
double DistSq(const Point3d *p, const Vector3d *vl, const Point3d *pf) {
|
|
/// returns the distance squared of pf from the line given by p,vl
|
|
/// vl must be normalised
|
|
Vector3d v(*p, *pf);
|
|
Vector3d vcp = *vl ^ v;
|
|
double d = vcp.magnitudeSq(); // l * sina
|
|
return d;
|
|
}
|
|
|
|
double Dist(const Point3d *p, const Vector3d *vl, const Point3d *pf) {
|
|
/// returns the distance of pf from the line given by p,vl
|
|
/// vl must be normalised
|
|
Vector3d v(*p, *pf);
|
|
Vector3d vcp = *vl ^ v;
|
|
double d = vcp.magnitude(); // l * sina
|
|
return d;
|
|
#if 0
|
|
// slower method requires 2 sqrts
|
|
Vector3d v(*p, *pf);
|
|
double magv = v.normalise();
|
|
Vector3d cp = *vl ^ v;
|
|
double d = magv * cp.magnitude();
|
|
return d; // l * sina
|
|
#endif
|
|
}
|
|
|
|
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 ;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool OnSpan(const Span& sp, const Point& p) {
|
|
Point nullPoint;
|
|
return OnSpan(sp, p, false, nullPoint, nullPoint);
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
// Triangle3d Constructors
|
|
Triangle3d::Triangle3d(const Point3d& p1, const Point3d& p2, const Point3d& p3) {
|
|
vert1 = p1;
|
|
vert2 = p2;
|
|
vert3 = p3;
|
|
v0 = Vector3d(vert1, vert2);
|
|
v1 = Vector3d(vert1, vert3);
|
|
ok = true;
|
|
|
|
// set box
|
|
box.min.x = __min(__min(vert1.x, vert2.x), vert3.x);
|
|
box.min.y = __min(__min(vert1.y, vert2.y), vert3.y);
|
|
box.min.z = __min(__min(vert1.z, vert2.z), vert3.z);
|
|
|
|
box.max.x = __max(__max(vert1.x, vert2.x), vert3.x);
|
|
box.max.y = __max(__max(vert1.y, vert2.y), vert3.y);
|
|
box.max.z = __max(__max(vert1.z, vert2.z), vert3.z);
|
|
}
|
|
|
|
// Triangle3d methods
|
|
bool Triangle3d::Intof(const Line& l, Point3d& intof)const {
|
|
// returns intersection triangle to line in intof
|
|
// funtion returns true for intersection, false for no intersection
|
|
// method based on Möller & Trumbore(1997) (Barycentric coordinates)
|
|
// based on incorrect Pseudo code from "Geometric Tools for Computer Graphics" p.487
|
|
if(box.outside(l.box) == true) return false;
|
|
|
|
Vector3d line(l.v);
|
|
line.normalise();
|
|
|
|
Vector3d p = line ^ v1; // cross product
|
|
double tmp = p * v0; // dot product
|
|
|
|
if(FEQZ(tmp)) return false;
|
|
|
|
tmp = 1 / tmp;
|
|
Vector3d s(vert1, l.p0);
|
|
|
|
double u = tmp * (s * p); // barycentric coordinate
|
|
if(u < 0 || u > 1) return false; // not inside triangle
|
|
|
|
Vector3d q = s ^ v0;
|
|
double v = tmp * (line * q); // barycentric coordinate
|
|
if(v < 0 || v > 1) return false; // not inside triangle
|
|
|
|
if( u + v > 1) return false; // not inside triangle
|
|
|
|
double t = tmp * (v1 * q);
|
|
intof = line * t + l.p0;
|
|
return true;
|
|
}
|
|
|
|
|
|
// box class
|
|
bool Box::outside(const Box& b)const {
|
|
// returns true if this box is outside b
|
|
if(b.ok == false || this->ok == false) return false; // no box set
|
|
if(this->max.x < b.min.x) return true;
|
|
if(this->max.y < b.min.y) return true;
|
|
if(this->min.x > b.max.x) return true;
|
|
if(this->min.y > b.max.y) return true;
|
|
return false;
|
|
}
|
|
|
|
void Box::combine(const Box& b) {
|
|
if(b.max.x > this->max.x) this->max.x = b.max.x;
|
|
if(b.max.y > this->max.y) this->max.y = b.max.y;
|
|
if(b.min.x < this->min.x) this->min.x = b.min.x;
|
|
if(b.min.y < this->min.y) this->min.y = b.min.y;
|
|
}
|
|
|
|
void Box3d::combine(const Box3d& b) {
|
|
if(b.max.x > this->max.x) this->max.x = b.max.x;
|
|
if(b.max.y > this->max.y) this->max.y = b.max.y;
|
|
if(b.max.z > this->max.z) this->max.z = b.max.z;
|
|
if(b.min.x < this->min.x) this->min.x = b.min.x;
|
|
if(b.min.y < this->min.y) this->min.y = b.min.y;
|
|
if(b.min.z < this->min.z) this->min.z = b.min.z;
|
|
}
|
|
|
|
bool Box3d::outside(const Box3d& b) const{
|
|
// returns true if this box is outside b
|
|
if(b.ok == false || this->ok == false) return false; // no box set
|
|
if(this->max.x < b.min.x) return true;
|
|
if(this->max.y < b.min.y) return true;
|
|
if(this->max.z < b.min.z) return true;
|
|
if(this->min.x > b.max.x) return true;
|
|
if(this->min.y > b.max.y) return true;
|
|
if(this->min.z > b.max.z) return true;
|
|
return false;
|
|
}
|
|
#if 0
|
|
Span3d IsPtsSpan3d(const double* a, int n, double tolerance, double* deviation) {
|
|
// returns a span3d if all points are within tolerance
|
|
int np = n / 3; // number of points
|
|
if(np < 2) return Span3d(); // Invalid span3d
|
|
Point3d sp = Point3d(&a[0]);
|
|
Point3d ep = Point3d(&a[n-3]);
|
|
Line line = IsPtsLine(a, n, tolerance, deviation);
|
|
if(line.ok) return Span3d(sp, ep); // it's a line
|
|
|
|
*deviation = 0; // cumulative deviation
|
|
Point3d mp = Point3d(&a[np / 2 * 3]); // mid point
|
|
Plane plane(sp, mp, ep);
|
|
if(plane.ok) {
|
|
// plane of the arc is ok
|
|
// calculate centre point
|
|
Vector3d vs(mp, sp);
|
|
vs.normalise();
|
|
Vector3d ve(mp, ep);
|
|
ve.normalise();
|
|
Vector3d rs = vs ^ plane.normal;
|
|
Vector3d re = ve ^ plane.normal;
|
|
|
|
Line rsl(sp.Mid(mp), rs, false);
|
|
Line rel(ep.Mid(mp), re, false);
|
|
|
|
Point3d pc;
|
|
Intof(rsl, rel, pc);
|
|
double radius = pc.Dist(sp);
|
|
|
|
// check other points on circle
|
|
for(int i = 2; i < np - 1; i++) {
|
|
Point3d p(&a[i*3]);
|
|
double dp = fabs(plane.Dist(p));
|
|
double dr = fabs(p.Dist(pc) - radius);
|
|
double tolerance = 10.0 * 1.0e-6;
|
|
if(dp > tolerance || dr > tolerance) {
|
|
return Span3d();
|
|
}
|
|
|
|
}
|
|
return Span3d(CW, plane.normal, sp, ep, pc);
|
|
|
|
}
|
|
return Span3d();
|
|
}
|
|
#endif
|
|
|
|
Line IsPtsLine(const double* a, int n, double tolerance, double* deviation) {
|
|
// returns a Line if all points are within tolerance
|
|
// deviation is returned as the sum of all deviations of interior points to line(sp,ep)
|
|
int np = n / 3; // number of points
|
|
*deviation = 0; // cumulative deviation
|
|
if(np < 2) return Line(); // Invalid line
|
|
|
|
Point3d sp(&a[0]);
|
|
Point3d ep(&a[n-3]);
|
|
Line line(sp, ep); // line start - end
|
|
|
|
if(line.ok) {
|
|
for(int j = 1; j < np - 1; j++) {
|
|
Point3d mp(&a[j * 3]);
|
|
double t, d=0;
|
|
if((d = mp.Dist(line.Near(mp, t))) > tolerance) {
|
|
line.ok = false;
|
|
return line;
|
|
}
|
|
*deviation = *deviation + d;
|
|
}
|
|
}
|
|
return line;
|
|
}
|
|
}
|