////////////////////////////////////////////////////////////////////////////////////////////////
// kurve
////////////////////////////////////////////////////////////////////////////////////////////////

/*==============================
Copyright (c) 2006 g.j.hawkesford 

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
1. Redistributions of source code must retain the above copyright
   notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright
   notice, this list of conditions and the following disclaimer in the
   documentation and/or other materials provided with the distribution.
3. The name of the author may not be used to endorse or promote products
   derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
==============================*/

#include "geometry.h"
using namespace geoff_geometry;

#ifdef PEPSPOST
	#include "postoutput.h"
#endif

namespace geoff_geometry {

	SpanVertex::SpanVertex() {
		for(int i = 0; i < SPANSTORAGE; i++) index[i] = NULL;
	}

	SpanVertex::~SpanVertex() {
#ifndef PEPSDLL 
		// don't know what peps did about this?
		for(int i = 0; i < SPANSTORAGE; i++) {
			if(index[i] != NULL) {
				delete index[i];
			}
		}
#endif
	}

	const SpanVertex& SpanVertex::operator= (const SpanVertex& spv ){
		/// 

		memcpy(x, spv.x, SPANSTORAGE * sizeof(double));
		memcpy(y, spv.y, SPANSTORAGE * sizeof(double));
		memcpy(xc, spv.xc, SPANSTORAGE * sizeof(double));
		memcpy(yc, spv.yc, SPANSTORAGE * sizeof(double));

		for(unsigned int i = 0; i < SPANSTORAGE; i++) {
			type[i] = spv.type[i];
			spanid[i] = spv.spanid[i];
			index[i] = spv.index[i];
#ifndef PEPSDLL
			if(index[i] != NULL) {
				SpanDataObject* obj = new SpanDataObject(index[i]);
				index[i] = obj;
			}
#endif
		}
		return *this;
	}


	void SpanVertex::Add(int offset, int spantype, const Point& p, const Point& pc, int ID)
	{
		type[offset] = spantype;
//		index[offset] = NULL;
		x[offset] = p.x;
		y[offset] = p.y;
		xc[offset] = pc.x;
		yc[offset] = pc.y;
		spanid[offset] = ID;
	}
	void SpanVertex::AddSpanID(int offset, int ID)
	{
		spanid[offset] = ID;
	}

#if PEPSDLL
	void SpanVertex::Add(int offset, WireExtraData* Index )
	{
		index[offset] = Index;
	}
	WireExtraData* SpanVertex::Get(int offset)
	{
		return index[offset];
	}
#else
	void SpanVertex::Add(int offset, const SpanDataObject* Index ) {
		index[offset] = Index;
	}
		
	const SpanDataObject* SpanVertex::GetIndex(int offset) const{
		return index[offset];
	}
#endif

	int SpanVertex::Get(int offset, Point& pe, Point& pc)
	{
		pe = Point(x[offset], y[offset]);
		pc = Point(xc[offset], yc[offset]);

		return type[offset];
	}
	int SpanVertex::GetSpanID(int offset)
	{
		return spanid[offset];
	}

	Span Span::Offset(double offset)
	{
		Span Offsp = *this;
		if(FNEZ(offset) && !NullSpan) {
			if ( !dir ) {
				// straight
				Offsp.p0.x -= offset * vs.gety();
				Offsp.p0.y += offset * vs.getx();

				Offsp.p1.x -= offset * vs.gety();
				Offsp.p1.y += offset * vs.getx();
			}
			else {
				//	 circular span
				//	double coffset = (double) dir * offset;
				Offsp.p0.x -= vs.gety() * offset;
				Offsp.p0.y += vs.getx() * offset;

				Offsp.p1.x -= ve.gety() * offset;
				Offsp.p1.y += ve.getx() * offset;

//				Offsp.radius -= dir * offset;
			}
			Offsp.SetProperties(true);
		}
		return Offsp;
	}

	bool Span::JoinSeparateSpans(Span& sp) {
		// this method joins this span to sp where they are separated normally by an offset from original
		//
		// parameters:-
		//	Input sp	near span
		//	Output	this->p1 and sp.p0 assigned to the spans intersection
		Point inters;
		int turnLeft = ((this->ve ^ sp.vs) > 0)? 1 : -1;
		if(!this->dir) {
			CLine one(*this);
			if(!sp.dir) {
				// line line
				CLine two(sp);
				inters = one.Intof(two);
			}
			else {
				// line arc
				Circle two(sp);
				inters = one.Intof(-turnLeft * sp.dir, two);
			}
		}
		else {
			Circle one(*this);
			if(!sp.dir) {
				// arc line
				CLine two(sp);
				inters = two.Intof(turnLeft * this->dir, one);
			}
			else {
				// arc arc
				Circle two(sp);
				inters = one.Intof(-turnLeft * this->dir * sp.dir, two);
			}
		}
		if(inters.ok) {
			this->p1 = sp.p0 = inters;
			this->SetProperties(true);
			sp.SetProperties(true);
		}
		return inters.ok;
	}
	static int Split(double tolerance, double angle, double radius, int dir);
	int Span::Split(double tolerance) {
		// returns the number of divisions required to keep in tolerance
		if(returnSpanProperties == false) this->SetProperties(true);
		return geoff_geometry::Split(tolerance, angle, radius, dir);
	}
#if 0
	int Span3d::Split(double tolerance) {
		// returns the number of divisions required to keep in tolerance
		if(returnSpanProperties == false) this->SetProperties(true);
		return geoff_geometry::Split(tolerance, angle, radius, dir);
	}
#endif
	static int Split(double tolerance, double angle, double radius, int dir) {
		if(dir == LINEAR) return 0;	// straight span
		double cosa = 1 - tolerance / radius;
		if(cosa > NEARLY_ONE) cosa = NEARLY_ONE;
		cosa = 2 * cosa * cosa - 1 ;              /* double angle */
		double sina = sqrt(1 - cosa * cosa) * dir;
		double tempang = atan2(sina, cosa);
		int num_vectors = (int)(fabs (angle / tempang )) + 1 ;
		return num_vectors;
	}

	void Span::SplitMatrix(int num_vectors, Matrix* matrix) {
		// returns the incremental matrix
		matrix->Unit();
		if(dir) {
			// arc span
			double incang = angle / (double) num_vectors ;

			matrix->Translate(-pc.x, -pc.y, 0);
			matrix->Rotate(incang, 3);
			matrix->Translate(pc.x, pc.y, 0);
		} else {
			// linear span
			matrix->Translate(length / num_vectors * vs.getx(), length / num_vectors * vs.gety(), 0);
		}
	}

#if 0
	void Span3d::SplitMatrix(int num_vectors, Matrix* matrix) {
		// returns the incremental matrix
		matrix->Unit();
		if(dir) {
			// arc span
			if(normal.getz() <= NEARLY_ONE) FAILURE(getMessage(L"Unfinished coding - contact the Company", GENERAL_MESSAGES, MES_UNFINISHEDCODING));
			double incang = angle / (double) num_vectors ;

			matrix->Translate(-pc.x, -pc.y, -pc.z);
			matrix->Rotate(incang, 3);
			matrix->Translate(pc.x, pc.y, pc.z);
		} else {
			// linear span
			double d = length / num_vectors;
			matrix->Translate(d * vs.getx(), d * vs.gety(), d * vs.getz());
		}
	}
#endif

	void Span::minmax(Box& box, bool start) {
		minmax(box.min, box.max, start);
	}
#if 0
	void Span3d::minmax(Box3d& box, bool start) {
		minmax(box.min, box.max, start);
	}
#endif
	void Span::minmax(Point& min, Point& max, bool start) {
		// box a span (min/max)
		if(start) {
			MinMax(p0, min, max);
		}
		MinMax(p1, min, max);

		if(dir) {
			// check the quadrant points
			double dx1 = p1.x - p0.x;
			double dy1 = p1.y - p0.y;

			double dx = pc.x - p0.x;
			double dy = pc.y - p0.y;

			double dx0 = dx + radius;		// 0deg

			if( dir * (dx0 * dy1 - dx1 * dy) > 0) {
				if(pc.x + radius > max.x) max.x = pc.x + radius;	
			}
			dx0 = dx - radius;				// 180deg
			if( dir * (dx0 * dy1 - dx1 * dy) > 0) {
				if(pc.x - radius < min.x) min.x = pc.x - radius;	
			}
			double dy0 = dy + radius;		// 90deg
			if( dir * (dx * dy1 - dx1 * dy0) > 0) {
				if(pc.y + radius > max.y) max.y = pc.y + radius;	
			}

			dy0 = dy - radius;				// 270deg
			if( dir * (dx * dy1 - dx1 * dy0) > 0) {
				if(pc.y - radius < min.y) min.y = pc.y - radius;	
			}
		}
	}

#if 0
	void Span3d::minmax(Point3d& min, Point3d& max, bool start) {
		// box a span (min/max)
		if(start) {
			MinMax(p0, min, max);
		}
		MinMax(p1, min, max);

		if(dir) {
			// check the quadrant points ... MUST RECODE THIS FOR 3D sometime
			double dx1 = p1.x - p0.x;
			double dy1 = p1.y - p0.y;

			double dx = pc.x - p0.x;
			double dy = pc.y - p0.y;

			double dx0 = dx + radius;		// 0deg

			if( dir * (dx0 * dy1 - dx1 * dy) > 0) {
				if(pc.x + radius > max.x) max.x = pc.x + radius;	
			}
			dx0 = dx - radius;				// 180deg
			if( dir * (dx0 * dy1 - dx1 * dy) > 0) {
				if(pc.x - radius < min.x) min.x = pc.x - radius;	
			}
			double dy0 = dy + radius;		// 90deg
			if( dir * (dx * dy1 - dx1 * dy0) > 0) {
				if(pc.y + radius > max.y) max.y = pc.y + radius;	
			}

			dy0 = dy - radius;				// 270deg
			if( dir * (dx * dy1 - dx1 * dy0) > 0) {
				if(pc.y - radius < min.y) min.y = pc.y - radius;	
			}
		}
	}
#endif

	int Span::Intof(const Span& sp, Point& pInt1, Point& pInt2, double t[4])const {
		// Intof 2 spans
		return geoff_geometry::Intof(*this, sp, pInt1, pInt2, t);
	}

	Point Span::Near(const Point& p)const{
		// returns the near point to span from p 
		if(this->dir == LINEAR) {
			double t;
			t = (Vector2d(this->p0, p) * this->vs);		// t parametised 0 - line length
			return this->vs * t + this->p0;
		} else {
			double r = p.Dist(this->pc);
			if(r < geoff_geometry::TOLERANCE) return (p.Dist(this->p0) < p.Dist(this->p1))?this->p0 : this->p1;
			return(p.Mid(this->pc, (r - this->radius) / r));
		}
	}
	Point Span::NearOn(const Point& p)const{
		// returns the near point to span from p - returned point is always on the span
		Point pn;
		pn = Near(p);
		if(this->OnSpan(pn) == true) return pn;

		// return nearest endpoint
		return (pn.Dist(p0) < pn.Dist(p1))?p0 : p1;
	}

	void Span::Transform(const Matrix& m, bool setprops) {
		p0 = p0.Transform(m);
		p1 = p1.Transform(m);
		if(dir != LINEAR) {
			pc = pc.Transform(m);
			if(m.m_mirrored == -1) FAILURE(L"Don't know mirror - use IsMirrored method on object");
			if(m.m_mirrored) dir = -dir;
		}
		if(setprops == true) SetProperties(true);
	}

#if 0
	void Span3d::Transform(const Matrix& m, bool setprops) {
		p0 = p0.Transform(m);
		p1 = p1.Transform(m);
		if(dir != LINEAR) {
			pc = pc.Transform(m);
			normal.Transform(m);
			if(m.m_mirrored == -1) FAILURE(L"Don't know mirror - use IsMirrored method on object");
			if(m.m_mirrored) dir = -dir;
		}
		if(setprops == true) SetProperties(true);
	}
#endif

	Point Span::Mid()const {
		// midpoint of a span

		return geoff_geometry::Mid(*this);

	}


	Point Span::MidPerim(double d)const {
		/// returns a point which is 0-d along span
		Point p;
		if(this->dir == LINEAR) {
			p = this->vs * d + this->p0;
		}
		else {
			Vector2d v(pc, p0);
			v.Rotate(d * dir / this->radius);
			p = v + pc;
		}
		return p;
	}

	Point Span::MidParam(double param)const {
		/// returns a point which is 0-1 along span
		if(fabs(param) < 0.00000000000001)return p0;
		if(fabs(param - 1.0) < 0.00000000000001)return p1;
		return MidPerim(param * this->length);
	}

	Vector2d Span::GetVector(double fraction)const {
		/// returns the direction vector at point which is 0-1 along span
		if(dir == 0){
			Vector2d v(p0, p1);
			v.normalise();
			return v;
		}

		Point p= MidParam(fraction);
		Vector2d v(pc, p);
		v.normalise();
		if(dir == ACW)
		{
			return Vector2d(-v.gety(), v.getx());
		}
		else
		{
			return Vector2d(v.gety(), -v.getx());
		}
	}

	Kurve::Kurve(const Kurve& k) :Matrix(){
		/// copy constructor
		this->m_nVertices = k.m_nVertices;
		memcpy(this->e, k.e, 16 * sizeof(double));
		this->m_unit = k.m_unit;
		this->m_mirrored = k.m_mirrored;
		this->m_isReversed = k.m_isReversed;
		this->m_started = k.m_started;
		for(unsigned int i = 0; i < k.m_spans.size(); i++) {
			SpanVertex* spv = new SpanVertex;
			*spv = *k.m_spans[i];
			this->m_spans.push_back(spv);
		}
	}

	const Kurve& Kurve::operator=( const Kurve &k) {
		memcpy(e, k.e, 16 * sizeof(double));
		m_unit = k.m_unit;
		m_mirrored = k.m_mirrored;
		m_isReversed = k.m_isReversed;

		this->Clear();

		if(k.m_nVertices) m_started = true;
//		m_nVertices = 0;

//		spVertex spv;
//		for(int i = 0; i < k.m_nVertices; i++) {
//			k.Get(i, spv);
//			Add(spv);
//		}
		for(unsigned int i = 0; i < k.m_spans.size(); i++) {
			SpanVertex* spv = new SpanVertex;
			*spv = *k.m_spans[i];
			this->m_spans.push_back(spv);
		}
		m_nVertices = k.m_nVertices;
		return *this;
	}

#if 0

	 Kurve::Kurve(Kurve& k) :Matrix(){
*this = k;
return;
		*this = Matrix(k);

		Point p, pc;
		m_nVertices = 0;

		for(int i = 0; i < k.m_nVertices; i++) {
			int spantype = k.Get(i, p, pc);
			int spanid = k.GetSpanID(i);
			if(Add(spantype, p, pc)) this->AddSpanID(spanid);
		}
		if(k.m_nVertices) m_started = true;
	}

	const Kurve& Kurve::operator=( Kurve &k)
	{
		*this = Matrix(k);

		Point p, pc;
		this->Clear();
		m_isReversed = k.m_isReversed;

		for(int i = 0; i < k.m_nVertices; i++) {
			int spantype = k.Get(i, p, pc);
			int spanid = k.GetSpanID(i);
			if(Add(spantype, p, pc)) this->AddSpanID(spanid);
		}
		if(k.m_nVertices) m_started = true;
		return *this;
	}
#endif

	const Kurve& Kurve::operator=(const Matrix &m)
	{
//		*this = Matrix(m);
//		return *this;

		for(int i = 0; i < 16; i++) e[i] = m.e[i];
		m_unit = m.m_unit;
		m_mirrored = m.m_mirrored;
		return *this;
	}

	Kurve::~Kurve()
	{
		this->Clear();
	}



	bool Kurve::Closed()const
	{
		// returns true if kurve closed
		if(m_nVertices > 1) {
			Point ps, pe, pc;
			Get(0, ps, pc);
			Get(m_nVertices - 1, pe, pc);
			return (ps == pe);
		}
		else
			return false;
	}

	void Kurve::FullCircle(int dir, const Point& c, double radius) {
		/// make a full circle Kurve (2 spans)
		/// mark the first span for later
		this->Clear();
		Point ps = c;
		ps.x = c.x + radius;
		this->Start(ps);
		this->AddSpanID(FULL_CIRCLE_KURVE);
		ps.x = c.x - radius;
		this->Add(dir, ps, c, true);
		ps.x = c.x + radius;
		this->Add(dir, ps, c, true);
	}

	void Kurve::Start()
	{
		if(m_started) this->Clear();
		m_started = true;
	}


	void Kurve::Start(const Point& p)
	{
		Start();
		Add(0, p, Point(0,0));
	}

	bool Kurve::Add(const Span& sp, bool AddNullSpans) {
		// add a span, including ID
		if(this->m_started == false) this->Start(sp.p0);
		if(this->Add(sp.dir, sp.p1, sp.pc, AddNullSpans)) {
			this->AddSpanID(sp.ID);
			return true;
		}
		return false;
	}

	bool Kurve::Add(const spVertex& spv, bool AddNullSpans) {
		if(Add(spv.type, spv.p, spv.pc, AddNullSpans)) {
			AddSpanID(spv.spanid);
			return true;
		}
		return false;
	}

	bool Kurve::Add(int span_type, const Point& p0, const Point& pc, bool AddNullSpans)
	{
		// add a span (cw = -1 (T)   acw = 1 (A) )
#ifdef _DEBUG
		if(this == NULL) FAILURE(L"Kurve::Add - No Kurve Object");
#endif

		if(!m_started) {
			Start(p0);
			return true;
		}

		if(m_nVertices) {
			// see if a null span would result by the addition of this span
			//		double xl, yl, cxl, cyl;
			Point pv, pcc;
			Get(m_nVertices - 1, pv, pcc);
			if(pv.Dist(p0) < geoff_geometry::TOLERANCE) {
				if(!AddNullSpans)return false;
				span_type = LINEAR;				// linear span
			}
		}

		SpanVertex* p;
		if(m_nVertices % SPANSTORAGE == 0) {
			p = new SpanVertex;
			m_spans.push_back(p);
		}
		else 
			p = (SpanVertex*) m_spans[m_nVertices / SPANSTORAGE];

		p->Add(m_nVertices % SPANSTORAGE, span_type, p0, pc);
		m_nVertices++;
		return true;
	}
	void Kurve::AddSpanID(int ID)
	{
		// add a extra data - must be called after Add
		int vertex = this->m_nVertices - 1;
		SpanVertex* p = (SpanVertex*) m_spans[vertex / SPANSTORAGE];
		p->AddSpanID(vertex % SPANSTORAGE, ID);
	}

	void Kurve::Add() {
		// null span
		if(m_nVertices == 0) FAILURE(L"Invalid attempt to add null span - no start");
		Point p, pc;
		Get(m_nVertices - 1, p, pc);
		Add(p, true);
	}

	bool Kurve::Add(const Point& p0, bool AddNullSpans) {
		return Add(0, p0, Point(0,0), AddNullSpans);
	}


	void Kurve::Add(const Kurve* k, bool AddNullSpans) {
		Span sp;
		Matrix m;
		if(this->m_unit == false) {
			m = *k;
			Matrix im = this->Inverse();
			m.Multiply(im);
			m.IsUnit();
		}
		for(int i = 1; i <= k->nSpans(); i++) {
			k->Get(i, sp, false, this->m_unit);
			#ifndef PEPSDLL
			const SpanDataObject* obj = k->GetIndex(i-1);
			#endif
			if(this->m_unit == false) sp.Transform(m);

			if(i == 1) {
				// check if this is the same as last point in kurve
				bool AddFirstVertex = true;
				if(nSpans()) {
					Span spLast;
					Get(nSpans(), spLast, false, false);
					if(spLast.p1.Dist(sp.p0) <= geoff_geometry::TOLERANCE) AddFirstVertex = false;
				}
				if(AddFirstVertex) { 
					Add(sp.p0, AddNullSpans);
					#ifndef PEPSDLL
					if(obj != NULL) {
						SpanDataObject* objnew = new SpanDataObject(obj);
						AddIndex(nSpans() - 1, objnew);
					}
					#endif
				}
			}
			
			Add(sp.dir, sp.p1, sp.pc, AddNullSpans);
			#ifndef PEPSDLL
				if(obj != NULL) {
					SpanDataObject* objnew = new SpanDataObject(obj);
					AddIndex(nSpans() - 1, objnew);
			}
			#endif
		}
	}

	void Kurve::Replace(int vertexnumber, const spVertex& spv) {
		// replace a span
		Replace(vertexnumber, spv.type, spv.p, spv.pc, spv.spanid);
	}


	void Kurve::Replace(int vertexnumber, int type, const Point& p0, const Point& pc, int ID) {
		// replace a span
#ifdef _DEBUG
		if(this == NULL || vertexnumber > m_nVertices) FAILURE(getMessage(L"Kurve::Replace - vertexNumber out of range", GEOMETRY_ERROR_MESSAGES, MES_BAD_VERTEX_NUMBER));
#endif
		SpanVertex* p = (SpanVertex*) m_spans[vertexnumber / SPANSTORAGE];
		p->Add(vertexnumber % SPANSTORAGE, type, p0, pc, ID);
	}

#ifdef PEPSDLL
	void Kurve::ModifyIndex(int vertexnumber, WireExtraData* i) {
		// replace an index
#ifdef _DEBUG
		if(this == NULL || vertexnumber > m_nVertices) FAILURE(getMessage(L"Kurve::ModifyIndex - vertexNumber out of range", GEOMETRY_ERROR_MESSAGES, MES_BAD_VERTEX_NUMBER));
#endif
		SpanVertex* p = (SpanVertex*) m_spans[vertexnumber / SPANSTORAGE];
		p->Add(vertexnumber % SPANSTORAGE, i);
	}
#else
	void Kurve::AddIndex(int vertexNumber, const SpanDataObject* data) {
		if(this == NULL || vertexNumber > m_nVertices - 1) FAILURE(L"Kurve::AddIndex - vertexNumber out of range");
		SpanVertex* p = (SpanVertex*) m_spans[vertexNumber / SPANSTORAGE];
		p->Add(vertexNumber % SPANSTORAGE, data);
	}

	const SpanDataObject* Kurve::GetIndex(int vertexNumber)const {
		if(this == NULL || vertexNumber > m_nVertices - 1) FAILURE(L"Kurve::GetIndex - vertexNumber out of range");
		SpanVertex* p = (SpanVertex*) m_spans[vertexNumber / SPANSTORAGE];
		return p->GetIndex(vertexNumber % SPANSTORAGE);
	}


#endif
	void Kurve::Get(std::vector<Span> *all, bool igNoreNullSpans)const {
		/// put all spans to vector
		for(int i = 1; i <= nSpans(); i++) {
			Span sp;
			Get(i, sp, true);
			if(igNoreNullSpans == true && sp.NullSpan == true) continue;
			all->push_back(sp);
		}
	}

	void Kurve::Get(int vertexnumber, spVertex& spv) const {
		spv.type = Get(vertexnumber, spv.p, spv.pc);
		spv.spanid = GetSpanID(vertexnumber);
	}

	int	Kurve::Get(int vertexnumber, Point& pe, Point& pc) const {
		// returns spantype with end / centre by reference
		if(vertexnumber < 0 || vertexnumber >= m_nVertices) FAILURE(getMessage(L"Kurve::Get - vertexNumber out of range", GEOMETRY_ERROR_MESSAGES, MES_BAD_VERTEX_NUMBER));
		if(m_isReversed == true) {
			int revVertexnumber = m_nVertices - 1 - vertexnumber;
			SpanVertex* p = (SpanVertex*)m_spans[revVertexnumber / SPANSTORAGE];
			int offset = revVertexnumber % SPANSTORAGE;
			pe = Point(p->x[offset], p->y[offset]);
			if(vertexnumber > 0) {
				revVertexnumber++;
				offset = revVertexnumber % SPANSTORAGE;
				p = (SpanVertex*)m_spans[revVertexnumber / SPANSTORAGE];
				pc = Point(p->xc[offset], p->yc[offset]);
				return -p->type[offset];
			}
			else return LINEAR;
		}
		else {
			SpanVertex* p = (SpanVertex*)m_spans[vertexnumber / SPANSTORAGE];
			return p->Get(vertexnumber % SPANSTORAGE, pe, pc);
		}
	}
	int	Kurve::GetSpanID(int vertexnumber) const {
		// for spanID (wire offset)
		if(vertexnumber < 0 || vertexnumber >= m_nVertices) FAILURE(getMessage(L"Kurve::Get - vertexNumber out of range", GEOMETRY_ERROR_MESSAGES, MES_BAD_VERTEX_NUMBER));
		if(m_isReversed == true) vertexnumber = m_nVertices - 1 - vertexnumber;
		SpanVertex* p = (SpanVertex*)m_spans[vertexnumber / SPANSTORAGE];
		return p->GetSpanID(vertexnumber % SPANSTORAGE);
	}
	int Kurve::Get(int spannumber, Span& sp, bool returnSpanProperties, bool transform) const {
		// returns span data and optional properties - the function returns as the span type
		if(spannumber < 1 || spannumber > m_nVertices) FAILURE(getMessage(L"Kurve::Get - vertexNumber out of range", GEOMETRY_ERROR_MESSAGES, MES_BAD_VERTEX_NUMBER));
		if(m_nVertices < 2) return -99;

		int spanVertexNumber = spannumber - 1;
		if(m_isReversed) spanVertexNumber = m_nVertices - 1 - spanVertexNumber;
		SpanVertex* p = (SpanVertex*)m_spans[spanVertexNumber / SPANSTORAGE];
		sp.p0.x = p->x[spanVertexNumber % SPANSTORAGE];
		sp.p0.y = p->y[spanVertexNumber % SPANSTORAGE];
		sp.p0.ok = 1;

		sp.dir = Get(spannumber, sp.p1, sp.pc);
		sp.ID = GetSpanID(spannumber);

		if(transform && !m_unit) {
			const Matrix *m = this;
			sp.Transform(*m, false);
		}

		sp.SetProperties(returnSpanProperties);

		return sp.dir;
	}

#if 0
	int Kurve::Get(int spannumber, Span3d& sp, bool returnSpanProperties, bool transform) const {
		// returns span data and optional properties - the function returns as the span type
		if(spannumber < 1 || spannumber > m_nVertices) FAILURE(getMessage(L"Kurve::Get - vertexNumber out of range", GEOMETRY_ERROR_MESSAGES, MES_BAD_VERTEX_NUMBER));
		if(m_nVertices < 2) return -99;

		int spanVertexNumber = spannumber - 1;
		SpanVertex* p = (SpanVertex*)m_spans[spanVertexNumber / SPANSTORAGE];
		sp.p0.x = p->x[spanVertexNumber % SPANSTORAGE];
		sp.p0.y = p->y[spanVertexNumber % SPANSTORAGE];
		sp.p0.z = 0;
//		sp.p0.ok = 1;

		sp.dir = Get(spannumber, sp.p1, sp.pc);

		if(transform && !m_unit) {
			const Matrix *m = this;
			sp.Transform(*m, false);
		}

		sp.SetProperties(returnSpanProperties);

		return sp.dir;
	}
#endif

	void Kurve::Get(Point &ps,Point &pe) const
	{
		// returns the start- and endpoint of the kurve
		Span sp;
		Get(1,sp,true,true);
		ps = sp.p0;
		Get(m_nVertices-1,sp,true,true);
		pe = sp.p1;
	}

	void Span::SetProperties(bool returnProperties) {
		if((returnSpanProperties = returnProperties)) {
			// return span properties
			if(dir) {
				// arc properties
				vs = ~Vector2d(pc, p0);		// tangent at start ( perp to radial vector)
				ve = ~Vector2d(pc, p1);		// tangent at end   ( perp to radial vector)
				if(dir == CW) {
					vs = -vs;				// reverse directions for CW arc
					ve = -ve;
				}

				radius = vs.normalise();
				double radCheck = ve.normalise();
//				if(FNE(radius, radCheck, geoff_geometry::TOLERANCE * 0.5)){
				if(FNE(radius, radCheck, geoff_geometry::TOLERANCE)){
					FAILURE(getMessage(L"Invalid Geometry - Radii mismatch - SetProperties", GEOMETRY_ERROR_MESSAGES, MES_INVALIDARC));
				}
				
				length = 0.0;
				angle = 0.0;
				if(radius > geoff_geometry::TOLERANCE) {
					if((NullSpan = (p0.Dist(p1)) <= geoff_geometry::TOLERANCE)) {
						dir = LINEAR;
					}
					else {
						// arc length & included angle
						length = fabs(angle = IncludedAngle(vs, ve, dir)) * radius;
					}
				}
				else
					NullSpan = true;
			}
			else {
				// straight properties
				vs = Vector2d(p0, p1);

				length = vs.normalise();
				NullSpan = (length <= geoff_geometry::TOLERANCE);
				ve = vs;
			}
			minmax(box, true);
		}
	}

#if 0
	void Span3d::SetProperties(bool returnProperties) {
		if(returnSpanProperties = returnProperties) {
			// return span properties
			if(dir) {
				// arc properties
				vs = normal ^ Vector3d(pc, p0);// tangent at start ( perp to radial vector)
				ve = normal ^ Vector3d(pc, p1);// tangent at end   ( perp to radial vector)
				if(dir == CW) {
					vs = -vs;				// reverse directions for CW arc
					ve = -ve;
				}

				radius = vs.normalise();
				double radCheck = ve.normalise();
				if(FNE(radius, radCheck, geoff_geometry::TOLERANCE * 0.5)) FAILURE(getMessage(L"Invalid Geometry - Radii mismatch - SetProperties", GEOMETRY_ERROR_MESSAGES, MES_INVALIDARC));
				if(radius > geoff_geometry::TOLERANCE) {
					if(NullSpan = (p0.Dist(p1) <= geoff_geometry::TOLERANCE)) {
						length = 0.0;
						angle = 0.0;
						dir = LINEAR;
					}
					else {
						// arc length & included angle
						length = fabs(angle = IncludedAngle(vs, ve, normal, dir)) * radius;
					}
				}
				else
					NullSpan = true;
			}
			else {
				// straight properties
				vs = Vector3d(p0, p1);

				length = vs.normalise();
				NullSpan = (length <= geoff_geometry::TOLERANCE);
				ve = vs;
			}
			minmax(box, true);
		}
	}
#endif


	Point Mid(const Span& span) {
		// mid point of a span
		if(span.dir) {
			CLine chord(span.p0, span.p1);
			if(chord.ok) {
				CLine bisector(Mid(span.p0, span.p1), ~chord.v, false);
				return Intof((span.dir == CW) ?FARINT : NEARINT, bisector, Circle(span));
			}
			else
				return span.p0;
		}
		else
			return Mid(span.p0, span.p1);
	}

	Point Kurve::Near(const Point& p, int& nearSpanNumber)const {
		// finds the nearest span on kurve to the the given point, nearSpanNumber is the spannumber
		double minDist = 1.0e100;
		Point pNear, pn;

		nearSpanNumber = 0;
		for(int i = 1; i <= nSpans(); i++) {
			Span sp;
			Get(i, sp, true, true);
			pNear = sp.NearOn(p);
			double d = pNear.Dist(p);
			if(minDist > d) {
				nearSpanNumber = i;
				pn = pNear;
				minDist = d;
				if(minDist < geoff_geometry::TOLERANCE) break;		// p must be on the span
			}
		}
		return pn;
	}


	Point Kurve::NearToVertex(const Point& p, int& nearSpanNumber)const {
		// finds the nearest span endpoint on kurve to the the given point, nearSpanNumber is the spannumber
		double minDistSquared = 1.0e100;
		Point pn;

		Matrix inv_mat = *this;
		inv_mat.Inverse();
		
		Point tp = p;
		if(!m_unit)	tp = tp.Transform(inv_mat); // Inverse transform point (rather than transform each vertex!)

		nearSpanNumber = 0;

		for(int i = 0; i < m_nVertices; i++) {
			Point ps, pc;
			Get(i, ps, pc);
			double DistSquared = Vector2d(ps, tp).magnitudesqd();
			if(DistSquared < minDistSquared) {
				minDistSquared = DistSquared;
				nearSpanNumber = i;
				pn = ps;
			}
		}
		return pn.Transform(*this);
	}

	void Kurve::ChangeStart(const Point *pNewStart, int startSpanno) {
		// changes the start position of the Kurve
		if(startSpanno == 1) {
			Span spFirst;
			this->Get(1, spFirst, false, true);
			if(spFirst.p0 == *pNewStart) return;
		}
		else if(startSpanno == this->nSpans()) {
			Span spLast;
			this->Get(this->nSpans(), spLast, false, true);
			if(spLast.p1 == *pNewStart) return;
		}
		Kurve temp;
		
		bool wrapped = false;
		int nSpans = 0;
		int spanno = startSpanno;
		Span sp;
		while(1) {
			this->Get(spanno, sp, false, true);
			if(spanno == startSpanno && wrapped == false) {
				temp.Start(*pNewStart);
				temp.Add(sp.dir, sp.p1, sp.pc, true);
			}
			else {
				if(nSpans == this->nSpans() && this->Closed() == true) {
					sp.p1 = *pNewStart;
				}
				temp.Add(sp, true);
			}

			spanno++;
			nSpans++;
			if(nSpans > this->nSpans()) break;
			if(spanno > this->nSpans()) {
				if(this->Closed() == false) break;
				spanno = 1;
				wrapped = true;
			}
		}

		*this = temp;
	}


	void Kurve::ChangeEnd(const Point *pNewEnd, int endSpanno) {
		// changes the end position of the Kurve, doesn't keep closed kurves closed
		if(endSpanno == 1) {
			Span spFirst;
			this->Get(1, spFirst, false, true);
			if(spFirst.p0 == *pNewEnd) return;
		}
		else if(endSpanno == this->nSpans()) {
			Span spLast;
			this->Get(this->nSpans(), spLast, false, true);
			if(spLast.p1 == *pNewEnd) return;
		}
		Kurve temp;
		
		int spanno = 1;
		Span sp;
		while(1) {
			this->Get(spanno, sp, false, true);
			if(spanno == 1) {
				temp.Start(sp.p0);
			}

			if(spanno == endSpanno)sp.p1 = *pNewEnd;

			temp.Add(sp.dir, sp.p1, sp.pc, true);
			if(spanno == endSpanno)break;

			spanno++;
		}

		*this = temp;
	}

	void Kurve::minmax(Point& min, Point& max) {
		// boxes kurve
		double xscale = 1.0;
		min = Point(1.0e61, 1.0e61);
		max = Point(-1.0e61, -1.0e61);

		if(!GetScale(xscale)) FAILURE(getMessage(L"Differential Scale not allowed for this method", GEOMETRY_ERROR_MESSAGES, MES_DIFFSCALE));	// differential scale
		Span sp;
		for(int i = 1; i < m_nVertices; i++) {
			Get(i, sp, true, true);
			if(i == 1) MinMax(sp.p0, min, max);
			sp.minmax(min, max, false);
		}
	}

	void	Kurve::minmax(Box& b) {
		minmax(b.min, b.max);
	}

	void Kurve::StoreAllSpans(std::vector<Span>& kSpans)const {	// store all kurve spans in array, normally when fast access is reqd
		Span span;
		for(int i = 1; i <= this->nSpans(); i++) {
			this->Get(i, span, true, false);					
			kSpans.push_back(span);
		}
	}

	void Kurve::Clear()
	{
		for(vector<SpanVertex*>::iterator It = m_spans.begin(); It != m_spans.end(); It++)
		{
			SpanVertex* spv = *It;
			delete spv;
		}
		m_spans.clear();
		m_started = false;
		m_nVertices = 0;
		m_isReversed = false;
	}

	bool Kurve::operator==(const Kurve &k)const{
		// k = kk (vertex check)
		if(nSpans() != k.nSpans()) return false;
		spVertex thisvertex, vertex;
		for(int i = 0; i <= nSpans(); i++) {
			this->Get(i, thisvertex);
			k.Get(i, vertex);
			if(thisvertex != vertex) return false;
		}
		return true;
	}

	double Kurve::Perim() const{
		// returns perimeter of kurve
		double perim = 0;
		Span sp;
		double xscale = 1.0;
		if(!GetScale(xscale)) FAILURE(getMessage(L"Differential Scale not allowed for this method", GEOMETRY_ERROR_MESSAGES, MES_DIFFSCALE));	// differential scale

		if(m_nVertices > 1) {
			for(int i = 1; i < m_nVertices; i++)
				perim += (Get(i, sp, true))? fabs(sp.angle) * sp.radius : sp.length;
		}
		return perim * xscale;
	}
	double Kurve::Area() const{
		// returns Area of kurve (+ve clockwise , -ve anti-clockwise sense)
		double xscale = 1.0;
		double area = 0;
		Span sp;

		if(Closed()) {
			if(!GetScale(xscale)) FAILURE(getMessage(L"Differential Scale not allowed for this method", GEOMETRY_ERROR_MESSAGES, MES_DIFFSCALE));	// differential scale
			for(int i = 1; i < m_nVertices; i++) {			
				if(Get(i, sp, true))
					area += ( 0.5 * ((sp.pc.x - sp.p0.x) * (sp.pc.y + sp.p0.y) - (sp.pc.x - sp.p1.x) * (sp.pc.y + sp.p1.y) - sp.angle * sp.radius * sp.radius));
				else
					area += 0.5 * (sp.p1.x - sp.p0.x) * (sp.p0.y + sp.p1.y);
			}
		}
		return area * xscale * xscale;
	}

	static void bubblesort(vector<Point>&p, vector<double>& d);

	int Kurve::Intof(const Span& spin, vector<Point>& p)const {
		// returns a vector (array) of intersection points
		int totalPoints = 0;
		for(int i = 1; i <= nSpans(); i++) {
			Span sp;
			Get(i, sp, true, true);

			Point pInt1, pInt2;
			double t[4];
			int numint = sp.Intof(spin, pInt1, pInt2, t);
			if(numint)		p.push_back(pInt1);
			if(numint == 2)	p.push_back(pInt2);
			totalPoints += numint;
		}
		if(totalPoints) {
			// sort intersects along span
			vector<double> d;
			Span temp(spin);

			for(int i = 0; i < (int)p.size(); i++) {
				temp.p1 = p[i];
				temp.SetProperties(true);

				d.push_back(temp.length);
			}
			bubblesort(p, d);
		}
		return totalPoints;
	}

	static void bubblesort(vector<Point>&p, vector<double>& d) {

		for(int pass = 1; pass < (int)p.size() ; pass++) {
			for(int j = 0; j < (int)p.size() - 1; j++) {
				if(d[j] > d[j+1] ) {
					// swap
					Point temp = p[j];
					p[j] = p[j+1];
					p[j+1] = temp;
					double dtemp = d[j];
					d[j] = d[j+1];
					d[j+1] = dtemp;
				}
			}
		}
	}

	int Kurve::Intof(const Kurve&k, vector<Point>& p)const {
		vector<Point> all;

		int totalPoints = 0;
		for(int i = 1; i <= nSpans(); i++) {
			Span sp;
			Get(i, sp, true, true);
			vector<Point> p0;
			totalPoints += k.Intof(sp, p0);

			for(int j = 0; j < (int)p0.size(); j++) all.push_back(p0[j]);
		}
		//FILE* d;
		//d = fopen("\\temp\\test.txt", "w");
		//		for(int l = 0; l < all.size(); l++) all[l].print(d, "all","\n");


		for(int i = 0; i < (int)all.size(); i++) {
			if(i == 0) 
				p.push_back(all[0]);
			else
				if(all[i-1].Dist(all[i]) > geoff_geometry::TOLERANCE) p.push_back(all[i]);
		}

		//fclose(d);
		return (int)p.size();
	}

	bool Kurve::Split(double MaximumRadius, double resolution) {

		Span sp;
		bool changed = false;
		Kurve ko;

		Get(0, sp.p0, sp.pc);
		ko.Start(sp.p0);

		for(int i = 1 ; i < m_nVertices; i++) {
			sp.dir = Get(i, sp.p1, sp.pc);
			if(sp.dir) {
				sp.SetProperties(true);
				if(sp.radius >= MaximumRadius) {
					// split this arc
					int nSplits = sp.Split(resolution);
					if(nSplits > 1) {
						Matrix m;
						sp.SplitMatrix(nSplits, &m);
						for(int j = 1; j < nSplits; j++) {
							sp.p0 = sp.p0.Transform(m);
							ko.Add(sp.p0);	
						}

						sp.dir = LINEAR;
						changed = true;
					}
				}
			}

			ko.Add(sp.dir, sp.p1, sp.pc);

			sp.p0 = sp.p1;

		}		
		// copy kurve
		if(changed) *this = ko;
		return changed;
	}

	void Kurve::Reverse() {
		// reverse the direction of a kurve
		int nSwaps = (m_nVertices - 1) / 2;
		if(nSwaps == 0) return;
		Point p0, pc0;			// near
		Point pend, pcend;	// far

		int i = 0, j = m_nVertices - 1;
		int dir0 = Get(i, p0, pc0);
		int spanID0 = GetSpanID(i);
		int dirend = Get(j, pend, pcend);
		int spanIDend = GetSpanID(j);

		while(i <= nSwaps) {
			Point p1, pc1;
			int dir1 = Get(i+1, p1, pc1);
			int spanID1 = GetSpanID(i+1);
			Point pendp, pcendp;	// far previous
			int direndp = Get(j-1, pendp, pcendp);
			int spanIDendp = GetSpanID(j-1);
			// end point
			Replace(i, dir0, pend, pc0, spanID0);
			Replace(j, dirend, p0, pcend, spanIDend);

			dir0 = dir1;
			p0 = p1;
			pc0 = pc1;
			dirend = direndp;
			pend = pendp;
			pcend = pcendp;
			spanID0 = spanID1;
			spanIDend = spanIDendp;

			i++;
			j--;
		}

		// now circle data - but it should be easy to modify centre data in the loop above (for another day)
		i = 0;
		j = m_nVertices - 1;
		dir0 = Get(i, p0, pc0);
		dirend = Get(j, pend, pcend);

		while(i < nSwaps) {
			Point p1, pc1;
			Point pendp, pcendp;	// far previous

			int dir1 = Get(i+1, p1, pc1);
			int direndp = Get(j-1, pendp, pcendp);

			Replace(i+1, -dirend, p1, pcend);
			Replace(j, -dir1, pend, pc1);
			dir0 = dir1;
			p0 = p1;
			pc0 = pc1;
			dirend = direndp;
			pend = pendp;
			pcend = pcendp;
			i++;
			j--;
		}
	}

	int Kurve::Reduce(double tolerance) {
		// remove spans that lie within tolerance
		// returns the number of spans removed
		if(nSpans() <= 2) return 0;								// too few spans for this method
		Kurve kReduced;
		kReduced = Matrix(*this);

#if 0
		for(int i = 1; i <= this->nSpans(); i++) {
			Span sp;
			this->Get(i, sp, true);

			for(int j = i+1; j <= this->nSpans(); j++) {
				Span spnext;
				this->Get(j, spnext, true);

			}
		}
		return m_nVertices - kReduced.m_nVertices;

#else 
		int dir1 = 0, dir2 = 0;
		Point p0, p1, p2, pc0, pc1, pc2;
		int vertex = 0;
		int dir0 = Get(vertex++, p0, pc0);		// first vertex
		kReduced.Start(p0);
		int lvertex = vertex++;

		while(vertex < m_nVertices) {
			while(vertex < m_nVertices) {
				int savelvertex = lvertex;
				int addvertex = vertex - 1;
				dir2 = Get(vertex++, p2, pc2);
				CLine cl(p0, p2);
				if(cl.ok) {
					bool outoftol = false;
					while(lvertex < vertex - 1) {									// interior loop, p1 after p0 up to vertex before p2
						dir1 = Get(lvertex++, p1, pc1);

						if(dir1 || fabs(cl.Dist(p1)) > tolerance) {
							outoftol = true;
							break;
						}
					}
					if(outoftol) {
						dir0 = Get(addvertex, p0, pc0);
						kReduced.Add(dir0, p0, pc0);
						lvertex = addvertex + 1;
					}
					else {
						lvertex = savelvertex;
					}
				}
			}
		}
		kReduced.Add(dir2, p2, pc2);

		if(m_nVertices != kReduced.m_nVertices) *this = kReduced;
		return m_nVertices - kReduced.m_nVertices;
#endif
	}

void Kurve::Part(int startVertex, int EndVertex, Kurve *part) {
	// make a part kurve
	spVertex spv;
	for(int i = startVertex; i <= EndVertex; i++) {
		Get(i, spv);
		part->Add(spv, true);
	}
	return;
}


Kurve Kurve::Part(int fromSpanno, const Point& fromPt, int toSpanno, const Point& toPt) {
            // make a Part Kurve
            // if spanno are known containing from/to Points then this is used, otherwise set = 0
            Kurve kPart;
            Span span;
            Point ps,pe;
            int iStartSpanNr,iEndSpanNr,i;
            
            // get start point and start spannumber
            if(fromSpanno == 0)
                  ps = Near(fromPt,iStartSpanNr);
            else
            {
                  Get(fromSpanno,span,true,true);
                  ps = span.p0;
                  iStartSpanNr = fromSpanno;
            }
            // get end point and end spannumber
            if(toSpanno == 0)
                  pe = Near(toPt,iEndSpanNr);
            else
            {
                  Get(toSpanno,span,true,true);
                  pe = span.p1;
                  iEndSpanNr = toSpanno;
            }
            
            kPart.Start(ps);
            Get(iStartSpanNr,span,true,true);

            if(iStartSpanNr == iEndSpanNr)
            {
                  kPart.Add(span.dir,pe,span.pc);
                  return kPart;
            }

            if(iStartSpanNr < iEndSpanNr)
            {
                  for(i=iStartSpanNr;i<iEndSpanNr;i++)
                  {
                        Get(i,span,true,true);
                        kPart.Add(span.dir,span.p1,span.pc);
                  }
                  Get(iEndSpanNr,span,true,true);
                  kPart.Add(span.dir,pe,span.pc);
            }
            if(iStartSpanNr > iEndSpanNr)
            {
                  for(i=iStartSpanNr;i<=nSpans();i++)
                  {
                        Get(i,span,true,true);
                        kPart.Add(span.dir,span.p1,span.pc);
                  }
                  if(Closed() == false)
                  {
                        Get(1,span,true,true);
                        kPart.Add(0,span.p0,Point(0.0,0.0)); // Add new span from kend to kstart
                  }
                  for(i=1;i<iEndSpanNr;i++)
                  {
                        Get(i,span,true,true);
                        kPart.Add(span.dir,span.p1,span.pc);
                  }
                  Get(iEndSpanNr,span,true,true);
                  kPart.Add(span.dir,pe,span.pc);
            }
            return kPart;
      }

	Kurve Kurve::Part(double fromParam, double toParam) {
		/// return a part Kurve - perimeter parameterisation
		/// fromParam & toParam 0 - 1 perimeter parameter
		Kurve k;

		double perimTotal = this->Perim();
		double fromPerim = fromParam * perimTotal;
		double toPerim = toParam * perimTotal;
		double perim = 0.;
		double perimLast = 0.;
		for(int i = 1; i <= this->nSpans(); i++) {
			Span sp;
			this->Get(i, sp, true, true);
			perim += sp.length;
			if(fromPerim <= perim && k.m_started == false) {
				// start
				if(FEQ(fromPerim, perim) == true)
					k.Start(sp.p0);
				else {
					double d = fromPerim - perimLast;
					k.Start(sp.MidPerim(d));
				}
			}

			if(perim >= toPerim) {
				// end
				if(FEQ(toPerim, perim) == true)
					k.Add(sp);
				else {
					double d = toPerim - perimLast;
					sp.p1 = sp.MidPerim(d);
					k.Add(sp);
				}				
				break;
			}
			if(k.m_started == true) k.Add(sp);
			perimLast = perim;
		}
		return k;
	}

	void tangential_arc(const Point &p0, const Point &p1, const Vector2d &v0, Point &c, int &dir)
	{
		// sets dir to 0, if a line is needed, else to 1 or -1 for acw or cw arc and sets c
		dir = 0;

		if(p0.Dist(p1) > 0.0000000001 && v0.magnitude() > 0.0000000001){
			Vector2d v1(p0, p1);
			Point halfway(p0 + Point(v1 * 0.5));
			Plane pl1(halfway, v1);
			Plane pl2(p0, v0);
			Line plane_line;
			if(pl1.Intof(pl2, plane_line))
			{
				Line l1(halfway, v1);
				double t1, t2;
				Line lshort;
				plane_line.Shortest(l1, lshort, t1, t2);
				c = lshort.p0;
				Vector3d cross = Vector3d(v0) ^ Vector3d(v1);
				dir = (cross.getz() > 0) ? 1:-1;
			}
		}
	}

}
