public PolynomialSplineFunction calculateSpline(){
		
		successfull= false;
		/*
		 * 1.Calculate the minimum bounding rectangle
		 */
		ArrayList<Point2D.Double> points = new ArrayList<Point2D.Double>();
		for(int i = 0; i < t.size(); i++){
			Point2D.Double p = new Point2D.Double();
			p.setLocation(t.get(i).x, t.get(i).y);
			points.add(p);
		}
		
		/*
		 * 1.1 Rotate that the major axis is parallel with the xaxis
		 */
		
		Array2DRowRealMatrix gyr = RadiusGyrationTensor2D.getRadiusOfGyrationTensor(t);
		EigenDecomposition eigdec = new EigenDecomposition(gyr);
		
		double inRad = -1*Math.atan2(eigdec.getEigenvector(0).getEntry(1), eigdec.getEigenvector(0).getEntry(0));
		boolean doTransform = (Math.abs(Math.abs(inRad)-Math.PI)>0.001);
		
		if(doTransform)
		{
			angleRotated = inRad;
			for(int i = 0; i < t.size(); i++){
				double x = t.get(i).x;
				double y = t.get(i).y;
				double newX = x*Math.cos(inRad)-y*Math.sin(inRad);
				double newY = x*Math.sin(inRad)+y*Math.cos(inRad);
				rotatedTrajectory.add(newX, newY, 0);
				points.get(i).setLocation(newX, newY);
			}
			//for(int i = 0; i < rect.length; i++){
			//	rect[i].setLocation(rect[i].x*Math.cos(inRad)-rect[i].y*Math.sin(inRad), rect[i].x*Math.sin(inRad)+rect[i].y*Math.cos(inRad));
			//}
		}
		else{
			angleRotated = 0;
			rotatedTrajectory = t;
		}
		
		
		/*
		 * 2. Divide the rectangle in n equal segments
		 * 2.1 Calculate line in main direction
		 * 2.2 Project the points in onto this line
		 * 2.3 Calculate the distance between the start of the line and the projected point
		 * 2.4 Assign point to segment according to distance of (2.3)
		 */
		List<List<Point2D.Double>> pointsInSegments =null;
		boolean allSegmentsContainingAtLeastTwoPoints = true;
		int indexSmallestX = 0;
		
		double segmentWidth = 0;
		do{
			double smallestX = Double.MAX_VALUE;
			double largestX = Double.MIN_VALUE;
			
			for(int i = 0; i < points.size(); i++){
				if(points.get(i).x< smallestX){
					smallestX = points.get(i).x;
					indexSmallestX = i;
				}
				if(points.get(i).x>largestX){
					largestX = points.get(i).x;
				}
			}
			
			allSegmentsContainingAtLeastTwoPoints = true;
			segmentWidth = (largestX-smallestX)/nSegments;
			pointsInSegments = new ArrayList<List<Point2D.Double>>(nSegments);
			for(int i = 0; i < nSegments; i++){
				pointsInSegments.add(new ArrayList<Point2D.Double>());
			}
			for(int i = 0; i < points.size(); i++){
	
				int index = (int)Math.abs((points.get(i).x/segmentWidth));
				
				if(index>(nSegments-1)){
					index = (nSegments-1);
				}
				pointsInSegments.get(index).add(points.get(i));
			}
			
			for(int i = 0; i < pointsInSegments.size(); i++){
				if(pointsInSegments.get(i).size()<2){
					if(nSegments>2){
						nSegments--;
						i = pointsInSegments.size();
						allSegmentsContainingAtLeastTwoPoints = false;
				
					}
				}
			}
			
		}while(allSegmentsContainingAtLeastTwoPoints==false);
		
		
		/*
		 * 3. Calculate the mean standard deviation over each segment: <s>
		 */
		//Point2D.Double eMajorP1 = new Point2D.Double(p1.x - (p3.x-p1.x)/2.0,p1.y - (p3.y-p1.y)/2.0); 
	//	Point2D.Double eMajorP2 = new Point2D.Double(p2.x - (p3.x-p1.x)/2.0,p2.y - (p3.y-p1.y)/2.0); 
		double sumSDOrthogonal=0;
		int Nsum = 0;
		CenterOfGravityFeature cogf = new CenterOfGravityFeature(rotatedTrajectory);
		Point2D.Double cog = new  Point2D.Double(cogf.evaluate()[0], cogf.evaluate()[1]) ;
		Point2D.Double eMajorP1 = cog;
		Point2D.Double eMajorP2 = new Point2D.Double(cog.getX()+1, cog.getY());
		for(int i = 0; i < nSegments; i++){
			StandardDeviation sd = new StandardDeviation();
			double[] distances = new double[pointsInSegments.get(i).size()];
			for(int j = 0; j < pointsInSegments.get(i).size(); j++){
				int factor = 1;
				if(isLeft(eMajorP1, eMajorP2, pointsInSegments.get(i).get(j))){
					factor = -1;
				}
				distances[j] = factor*distancePointLine(eMajorP1, eMajorP2, pointsInSegments.get(i).get(j));
			}
			if(distances.length >0){
				sd.setData(distances);
	
				sumSDOrthogonal += sd.evaluate();
				Nsum++;
			}
		}
		double s = sumSDOrthogonal/Nsum;
		if(segmentWidth<Math.pow(10, -15)){
			return null;
		}
		if(s<Math.pow(10, -15)){
			//If standard deviation is zero, replace it with the half of the segment width
		
			s=segmentWidth/2;
		}
		//rotatedTrajectory.showTrajectory("rot");
		/*
		 * 4. Build a kd-tree
		 */
		KDTree<Point2D.Double> kdtree = new KDTree<Point2D.Double>(2);
		
		for(int i = 0; i< points.size(); i++){
			try {
				//To ensure that all points have a different key, add small random number

				kdtree.insert(new double[]{points.get(i).x,points.get(i).y}, points.get(i));
			} catch (KeySizeException e) {
				e.printStackTrace();
			} catch (KeyDuplicateException e) {
				//Do nothing! It is not important
			}
			
		}
		
		/*
		 * 5. Using the first point f in trajectory and calculate the center of mass
		 * of all points around f (radius: 3*<s>))
		 */
		List<Point2D.Double> near = null;
		
		Point2D.Double first = points.get(indexSmallestX);//minDistancePointToLine(p1, p3, points);
		double r1 = 3*s;
		try {
			
			near = kdtree.nearestEuclidean(new double[]{first.x ,first.y}, r1);
			
		} catch (KeySizeException e) {
			e.printStackTrace();
		} 

		double cx = 0;
		double cy = 0;
		for(int i = 0; i < near.size(); i++){
			cx += near.get(i).x;
			cy += near.get(i).y;
		}
		cx /= near.size();
		cy /= near.size();

		splineSupportPoints = new ArrayList<Point2D.Double>();
		splineSupportPoints.add(new Point2D.Double(cx, cy));
		
		/* 
		 * 6. The second point is determined by finding the center-of-mass of particles in the p/2 radian 
		 * section of an annulus, r1 < r < 2r1, that is directed toward the angle with the highest number 
		 * of particles within p/2 radians.
		 * 7. This second point is then used as the center of the annulus for choosing the third point, and the process is repeated (6. & 7.).
		 */
		
		/*
		 * 6.1 Find all points in the annolous
		 */
		
		
		/*
		 * 6.2 Write each point in a coordinate system centered at the center of the sphere, calculate direction and
		 * check if it in the allowed bounds
		 */
		int nCircleSegments = 100;
		double deltaRad = 2*Math.PI/nCircleSegments;
		boolean stop = false;
		int minN = 7;
		double tempr1 = r1;
		double allowedDeltaDirection = 0.5*Math.PI;
				
		while(stop==false){
			List<Point2D.Double> nearestr1 = null;
			List<Point2D.Double> nearest2xr1 = null;
			try {
				nearestr1 = kdtree.nearestEuclidean(new double[]{splineSupportPoints.get(splineSupportPoints.size()-1).x, splineSupportPoints.get(splineSupportPoints.size()-1).y},tempr1);
				nearest2xr1 = kdtree.nearestEuclidean(new double[]{splineSupportPoints.get(splineSupportPoints.size()-1).x, splineSupportPoints.get(splineSupportPoints.size()-1).y},2*tempr1);
			} catch (KeySizeException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			nearest2xr1.removeAll(nearestr1);
			
			double lThreshRad = 0;
			double hThreshRad = Math.PI/2;
			double stopThresh = 2*Math.PI;
			if(splineSupportPoints.size()>1){
				double directionInRad = Math.atan2(splineSupportPoints.get(splineSupportPoints.size()-1).y-splineSupportPoints.get(splineSupportPoints.size()-2).y, 
						splineSupportPoints.get(splineSupportPoints.size()-1).x-splineSupportPoints.get(splineSupportPoints.size()-2).x)+Math.PI;
				lThreshRad = directionInRad - allowedDeltaDirection/2 - Math.PI/4; 
				if(lThreshRad<0){
					lThreshRad = 2*Math.PI+lThreshRad;
				}
				if(lThreshRad>2*Math.PI){
					lThreshRad = lThreshRad - 2*Math.PI;
				}
				hThreshRad = directionInRad + allowedDeltaDirection/2 + Math.PI/4; 
				if(hThreshRad<0){
					hThreshRad = 2*Math.PI+hThreshRad;
				}
				if(hThreshRad>2*Math.PI){
					hThreshRad = hThreshRad - 2*Math.PI;
				}
				stopThresh = directionInRad + allowedDeltaDirection/2 - Math.PI/4; 
				if(stopThresh>2*Math.PI){
					stopThresh = stopThresh - 2*Math.PI;
				}
				
			}
			
			double newCx=0;
			double newCy=0;
			int newCN = 0;
			int candN = 0;
			
			//Find center with highest density of points
			double lastDist = 0;
			double newDist = 0;
			do{ 
				lastDist=Math.min(Math.abs(lThreshRad-stopThresh), 2*Math.PI-Math.abs(lThreshRad-stopThresh));

				candN=0;
				double candCx =0;
				double candCy =0;
			
				for(int i = 0; i < nearest2xr1.size(); i++){
					Point2D.Double centerOfCircle = splineSupportPoints.get(splineSupportPoints.size()-1);
					Vector2d relativeToCircle = new Vector2d(nearest2xr1.get(i).x-centerOfCircle.x,nearest2xr1.get(i).y-centerOfCircle.y);
					relativeToCircle.normalize();
					double angleInRadians = Math.atan2(relativeToCircle.y, relativeToCircle.x)+Math.PI;

					if(lThreshRad<hThreshRad){
						if(angleInRadians>lThreshRad && angleInRadians < hThreshRad){
							candCx+=nearest2xr1.get(i).x;
							candCy+=nearest2xr1.get(i).y;
							candN++;
						}	
					}
					else{
						if(angleInRadians>lThreshRad || angleInRadians < hThreshRad){
							candCx+=nearest2xr1.get(i).x;
							candCy+=nearest2xr1.get(i).y;
							candN++;
						}
					}
					
								
				}
				
				if(candN>0 && candN > newCN ){
					candCx /= candN;
					candCy /= candN;
					newCx = candCx;
					newCy = candCy;
					newCN = candN;
				}
				lThreshRad += deltaRad;
				hThreshRad += deltaRad;
				if(lThreshRad>2*Math.PI){
					lThreshRad = lThreshRad - 2*Math.PI;
				}
				if(hThreshRad>2*Math.PI){
					hThreshRad = hThreshRad - 2*Math.PI;
				}
				newDist=Math.min(Math.abs(lThreshRad-stopThresh), 2*Math.PI-Math.abs(lThreshRad-stopThresh));
			
			}while((newDist-lastDist)>0);
			
			
			//Check if the new center is valid
			if(splineSupportPoints.size()>1){
				double currentDirectionInRad = Math.atan2(splineSupportPoints.get(splineSupportPoints.size()-1).y-splineSupportPoints.get(splineSupportPoints.size()-2).y, 
						splineSupportPoints.get(splineSupportPoints.size()-1).x-splineSupportPoints.get(splineSupportPoints.size()-2).x)+Math.PI;
				double candDirectionInRad = Math.atan2(newCy-splineSupportPoints.get(splineSupportPoints.size()-1).y,
						newCx-splineSupportPoints.get(splineSupportPoints.size()-1).x)+Math.PI;
				double dDir = Math.max(currentDirectionInRad, candDirectionInRad)-Math.min(currentDirectionInRad, candDirectionInRad);
				if(dDir>2*Math.PI){
					dDir = 2*Math.PI-dDir;
				}
				if(dDir>allowedDeltaDirection){

					stop = true;
				}
			}
			boolean enoughPoints = (newCN<minN);
			boolean isNormalRadius = Math.abs(tempr1-r1)<Math.pow(10, -18);
			boolean isExtendedRadius = Math.abs(tempr1-3*r1)<Math.pow(10, -18);
			
			if(enoughPoints&& isNormalRadius){
				//Not enough points, extend search radius
				tempr1 = 3*r1;
			}
			else if(enoughPoints && isExtendedRadius){
				//Despite radius extension: Not enough points!
				stop = true;
			}
			else if(stop==false){
				splineSupportPoints.add(new Point2D.Double(newCx,newCy));
				tempr1 = r1;
			}
			
		}

		//Sort
				Collections.sort(splineSupportPoints, new Comparator<Point2D.Double>() {

					public int compare(Point2D.Double o1, Point2D.Double o2) {
						if(o1.x<o2.x){
							return -1;
						}
						if(o1.x>o2.x){
							return 1;
						}
						return 0;
					}
				});
				
	
		
		//Add endpoints
		if(splineSupportPoints.size()>1){
			Vector2d start = new Vector2d(splineSupportPoints.get(0).x-splineSupportPoints.get(1).x, splineSupportPoints.get(0).y-splineSupportPoints.get(1).y);
			start.normalize();
			start.scale(r1*8);
			splineSupportPoints.add(0, new Point2D.Double(splineSupportPoints.get(0).x+start.x, splineSupportPoints.get(0).y+start.y));
			
			Vector2d end = new Vector2d(splineSupportPoints.get(splineSupportPoints.size()-1).x-splineSupportPoints.get(splineSupportPoints.size()-2).x, 
					splineSupportPoints.get(splineSupportPoints.size()-1).y-splineSupportPoints.get(splineSupportPoints.size()-2).y);
			end.normalize();
			end.scale(r1*6);
			splineSupportPoints.add(new Point2D.Double(splineSupportPoints.get(splineSupportPoints.size()-1).x+end.x, splineSupportPoints.get(splineSupportPoints.size()-1).y+end.y));
		}
		else{
			Vector2d majordir = new Vector2d(-1, 0);
			majordir.normalize();
			majordir.scale(r1*8);
			splineSupportPoints.add(0, new Point2D.Double(splineSupportPoints.get(0).x+majordir.x, splineSupportPoints.get(0).y+majordir.y));
			majordir.scale(-1);
			splineSupportPoints.add(new Point2D.Double(splineSupportPoints.get(splineSupportPoints.size()-1).x+majordir.x, splineSupportPoints.get(splineSupportPoints.size()-1).y+majordir.y));
		

		}
		
		
		
		
		//Interpolate spline
		double[] supX = new double[splineSupportPoints.size()];
		double[] supY = new double[splineSupportPoints.size()];
		for(int i = 0; i < splineSupportPoints.size(); i++){
			supX[i] = splineSupportPoints.get(i).x;
			supY[i] = splineSupportPoints.get(i).y;
		}
		
		SplineInterpolator sIinter = new SplineInterpolator();
		spline = sIinter.interpolate(supX, supY);
		successfull = true;
		return spline;
	}
public PolynomialSplineFunction calculateSpline ( ) { successfull = false ; ArrayList < Point2D . Double > points = new ArrayList < Point2D . Double > ( ) ; for ( int i = 0 ; i < t . size ( ) ; i ++ ) { Point2D . Double p = new Point2D . Double ( ) ; p . setLocation ( t . get ( i ) . x , t . get ( i ) . y ) ; points . add ( p ) ; } Array2DRowRealMatrix gyr = RadiusGyrationTensor2D . getRadiusOfGyrationTensor ( t ) ; EigenDecomposition eigdec = new EigenDecomposition ( gyr ) ; double inRad = - 1 * Math . atan2 ( eigdec . getEigenvector ( 0 ) . getEntry ( 1 ) , eigdec . getEigenvector ( 0 ) . getEntry ( 0 ) ) ; boolean doTransform = ( Math . abs ( Math . abs ( inRad ) - Math . PI ) > 0.001 ) ; if ( doTransform ) { angleRotated = inRad ; for ( int i = 0 ; i < t . size ( ) ; i ++ ) { double x = t . get ( i ) . x ; double y = t . get ( i ) . y ; double newX = x * Math . cos ( inRad ) - y * Math . sin ( inRad ) ; double newY = x * Math . sin ( inRad ) + y * Math . cos ( inRad ) ; rotatedTrajectory . add ( newX , newY , 0 ) ; points . get ( i ) . setLocation ( newX , newY ) ; } } else { angleRotated = 0 ; rotatedTrajectory = t ; } List < List < Point2D . Double > > pointsInSegments = null ; boolean allSegmentsContainingAtLeastTwoPoints = true ; int indexSmallestX = 0 ; double segmentWidth = 0 ; do { double smallestX = Double . MAX_VALUE ; double largestX = Double . MIN_VALUE ; for ( int i = 0 ; i < points . size ( ) ; i ++ ) { if ( points . get ( i ) . x < smallestX ) { smallestX = points . get ( i ) . x ; indexSmallestX = i ; } if ( points . get ( i ) . x > largestX ) { largestX = points . get ( i ) . x ; } } allSegmentsContainingAtLeastTwoPoints = true ; segmentWidth = ( largestX - smallestX ) / nSegments ; pointsInSegments = new ArrayList < List < Point2D . Double > > ( nSegments ) ; for ( int i = 0 ; i < nSegments ; i ++ ) { pointsInSegments . add ( new ArrayList < Point2D . Double > ( ) ) ; } for ( int i = 0 ; i < points . size ( ) ; i ++ ) { int index = ( int ) Math . abs ( ( points . get ( i ) . x / segmentWidth ) ) ; if ( index > ( nSegments - 1 ) ) { index = ( nSegments - 1 ) ; } pointsInSegments . get ( index ) . add ( points . get ( i ) ) ; } for ( int i = 0 ; i < pointsInSegments . size ( ) ; i ++ ) { if ( pointsInSegments . get ( i ) . size ( ) < 2 ) { if ( nSegments > 2 ) { nSegments -- ; i = pointsInSegments . size ( ) ; allSegmentsContainingAtLeastTwoPoints = false ; } } } } while ( allSegmentsContainingAtLeastTwoPoints == false ) ; double sumSDOrthogonal = 0 ; int Nsum = 0 ; CenterOfGravityFeature cogf = new CenterOfGravityFeature ( rotatedTrajectory ) ; Point2D . Double cog = new Point2D . Double ( cogf . evaluate ( ) [ 0 ] , cogf . evaluate ( ) [ 1 ] ) ; Point2D . Double eMajorP1 = cog ; Point2D . Double eMajorP2 = new Point2D . Double ( cog . getX ( ) + 1 , cog . getY ( ) ) ; for ( int i = 0 ; i < nSegments ; i ++ ) { StandardDeviation sd = new StandardDeviation ( ) ; double [ ] distances = new double [ pointsInSegments . get ( i ) . size ( ) ] ; for ( int j = 0 ; j < pointsInSegments . get ( i ) . size ( ) ; j ++ ) { int factor = 1 ; if ( isLeft ( eMajorP1 , eMajorP2 , pointsInSegments . get ( i ) . get ( j ) ) ) { factor = - 1 ; } distances [ j ] = factor * distancePointLine ( eMajorP1 , eMajorP2 , pointsInSegments . get ( i ) . get ( j ) ) ; } if ( distances . length > 0 ) { sd . setData ( distances ) ; sumSDOrthogonal += sd . evaluate ( ) ; Nsum ++ ; } } double s = sumSDOrthogonal / Nsum ; if ( segmentWidth < Math . pow ( 10 , - 15 ) ) { return null ; } if ( s < Math . pow ( 10 , - 15 ) ) { s = segmentWidth / 2 ; } KDTree < Point2D . Double > kdtree = new KDTree < Point2D . Double > ( 2 ) ; for ( int i = 0 ; i < points . size ( ) ; i ++ ) { try { kdtree . insert ( new double [ ] { points . get ( i ) . x , points . get ( i ) . y } , points . get ( i ) ) ; } catch ( KeySizeException e ) { e . printStackTrace ( ) ; } catch ( KeyDuplicateException e ) { } } List < Point2D . Double > near = null ; Point2D . Double first = points . get ( indexSmallestX ) ; double r1 = 3 * s ; try { near = kdtree . nearestEuclidean ( new double [ ] { first . x , first . y } , r1 ) ; } catch ( KeySizeException e ) { e . printStackTrace ( ) ; } double cx = 0 ; double cy = 0 ; for ( int i = 0 ; i < near . size ( ) ; i ++ ) { cx += near . get ( i ) . x ; cy += near . get ( i ) . y ; } cx /= near . size ( ) ; cy /= near . size ( ) ; splineSupportPoints = new ArrayList < Point2D . Double > ( ) ; splineSupportPoints . add ( new Point2D . Double ( cx , cy ) ) ; int nCircleSegments = 100 ; double deltaRad = 2 * Math . PI / nCircleSegments ; boolean stop = false ; int minN = 7 ; double tempr1 = r1 ; double allowedDeltaDirection = 0.5 * Math . PI ; while ( stop == false ) { List < Point2D . Double > nearestr1 = null ; List < Point2D . Double > nearest2xr1 = null ; try { nearestr1 = kdtree . nearestEuclidean ( new double [ ] { splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . x , splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . y } , tempr1 ) ; nearest2xr1 = kdtree . nearestEuclidean ( new double [ ] { splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . x , splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . y } , 2 * tempr1 ) ; } catch ( KeySizeException e ) { e . printStackTrace ( ) ; } nearest2xr1 . removeAll ( nearestr1 ) ; double lThreshRad = 0 ; double hThreshRad = Math . PI / 2 ; double stopThresh = 2 * Math . PI ; if ( splineSupportPoints . size ( ) > 1 ) { double directionInRad = Math . atan2 ( splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . y - splineSupportPoints . get ( splineSupportPoints . size ( ) - 2 ) . y , splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . x - splineSupportPoints . get ( splineSupportPoints . size ( ) - 2 ) . x ) + Math . PI ; lThreshRad = directionInRad - allowedDeltaDirection / 2 - Math . PI / 4 ; if ( lThreshRad < 0 ) { lThreshRad = 2 * Math . PI + lThreshRad ; } if ( lThreshRad > 2 * Math . PI ) { lThreshRad = lThreshRad - 2 * Math . PI ; } hThreshRad = directionInRad + allowedDeltaDirection / 2 + Math . PI / 4 ; if ( hThreshRad < 0 ) { hThreshRad = 2 * Math . PI + hThreshRad ; } if ( hThreshRad > 2 * Math . PI ) { hThreshRad = hThreshRad - 2 * Math . PI ; } stopThresh = directionInRad + allowedDeltaDirection / 2 - Math . PI / 4 ; if ( stopThresh > 2 * Math . PI ) { stopThresh = stopThresh - 2 * Math . PI ; } } double newCx = 0 ; double newCy = 0 ; int newCN = 0 ; int candN = 0 ; double lastDist = 0 ; double newDist = 0 ; do { lastDist = Math . min ( Math . abs ( lThreshRad - stopThresh ) , 2 * Math . PI - Math . abs ( lThreshRad - stopThresh ) ) ; candN = 0 ; double candCx = 0 ; double candCy = 0 ; for ( int i = 0 ; i < nearest2xr1 . size ( ) ; i ++ ) { Point2D . Double centerOfCircle = splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) ; Vector2d relativeToCircle = new Vector2d ( nearest2xr1 . get ( i ) . x - centerOfCircle . x , nearest2xr1 . get ( i ) . y - centerOfCircle . y ) ; relativeToCircle . normalize ( ) ; double angleInRadians = Math . atan2 ( relativeToCircle . y , relativeToCircle . x ) + Math . PI ; if ( lThreshRad < hThreshRad ) { if ( angleInRadians > lThreshRad && angleInRadians < hThreshRad ) { candCx += nearest2xr1 . get ( i ) . x ; candCy += nearest2xr1 . get ( i ) . y ; candN ++ ; } } else { if ( angleInRadians > lThreshRad || angleInRadians < hThreshRad ) { candCx += nearest2xr1 . get ( i ) . x ; candCy += nearest2xr1 . get ( i ) . y ; candN ++ ; } } } if ( candN > 0 && candN > newCN ) { candCx /= candN ; candCy /= candN ; newCx = candCx ; newCy = candCy ; newCN = candN ; } lThreshRad += deltaRad ; hThreshRad += deltaRad ; if ( lThreshRad > 2 * Math . PI ) { lThreshRad = lThreshRad - 2 * Math . PI ; } if ( hThreshRad > 2 * Math . PI ) { hThreshRad = hThreshRad - 2 * Math . PI ; } newDist = Math . min ( Math . abs ( lThreshRad - stopThresh ) , 2 * Math . PI - Math . abs ( lThreshRad - stopThresh ) ) ; } while ( ( newDist - lastDist ) > 0 ) ; if ( splineSupportPoints . size ( ) > 1 ) { double currentDirectionInRad = Math . atan2 ( splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . y - splineSupportPoints . get ( splineSupportPoints . size ( ) - 2 ) . y , splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . x - splineSupportPoints . get ( splineSupportPoints . size ( ) - 2 ) . x ) + Math . PI ; double candDirectionInRad = Math . atan2 ( newCy - splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . y , newCx - splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . x ) + Math . PI ; double dDir = Math . max ( currentDirectionInRad , candDirectionInRad ) - Math . min ( currentDirectionInRad , candDirectionInRad ) ; if ( dDir > 2 * Math . PI ) { dDir = 2 * Math . PI - dDir ; } if ( dDir > allowedDeltaDirection ) { stop = true ; } } boolean enoughPoints = ( newCN < minN ) ; boolean isNormalRadius = Math . abs ( tempr1 - r1 ) < Math . pow ( 10 , - 18 ) ; boolean isExtendedRadius = Math . abs ( tempr1 - 3 * r1 ) < Math . pow ( 10 , - 18 ) ; if ( enoughPoints && isNormalRadius ) { tempr1 = 3 * r1 ; } else if ( enoughPoints && isExtendedRadius ) { stop = true ; } else if ( stop == false ) { splineSupportPoints . add ( new Point2D . Double ( newCx , newCy ) ) ; tempr1 = r1 ; } } Collections . sort ( splineSupportPoints , new Comparator < Point2D . Double > ( ) { public int compare ( Point2D . Double o1 , Point2D . Double o2 ) { if ( o1 . x < o2 . x ) { return - 1 ; } if ( o1 . x > o2 . x ) { return 1 ; } return 0 ; } } ) ; if ( splineSupportPoints . size ( ) > 1 ) { Vector2d start = new Vector2d ( splineSupportPoints . get ( 0 ) . x - splineSupportPoints . get ( 1 ) . x , splineSupportPoints . get ( 0 ) . y - splineSupportPoints . get ( 1 ) . y ) ; start . normalize ( ) ; start . scale ( r1 * 8 ) ; splineSupportPoints . add ( 0 , new Point2D . Double ( splineSupportPoints . get ( 0 ) . x + start . x , splineSupportPoints . get ( 0 ) . y + start . y ) ) ; Vector2d end = new Vector2d ( splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . x - splineSupportPoints . get ( splineSupportPoints . size ( ) - 2 ) . x , splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . y - splineSupportPoints . get ( splineSupportPoints . size ( ) - 2 ) . y ) ; end . normalize ( ) ; end . scale ( r1 * 6 ) ; splineSupportPoints . add ( new Point2D . Double ( splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . x + end . x , splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . y + end . y ) ) ; } else { Vector2d majordir = new Vector2d ( - 1 , 0 ) ; majordir . normalize ( ) ; majordir . scale ( r1 * 8 ) ; splineSupportPoints . add ( 0 , new Point2D . Double ( splineSupportPoints . get ( 0 ) . x + majordir . x , splineSupportPoints . get ( 0 ) . y + majordir . y ) ) ; majordir . scale ( - 1 ) ; splineSupportPoints . add ( new Point2D . Double ( splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . x + majordir . x , splineSupportPoints . get ( splineSupportPoints . size ( ) - 1 ) . y + majordir . y ) ) ; } double [ ] supX = new double [ splineSupportPoints . size ( ) ] ; double [ ] supY = new double [ splineSupportPoints . size ( ) ] ; for ( int i = 0 ; i < splineSupportPoints . size ( ) ; i ++ ) { supX [ i ] = splineSupportPoints . get ( i ) . x ; supY [ i ] = splineSupportPoints . get ( i ) . y ; } SplineInterpolator sIinter = new SplineInterpolator ( ) ; spline = sIinter . interpolate ( supX , supY ) ; successfull = true ; return spline ; }
Calculates a spline to a trajectory. Attention: The spline is fitted through a rotated version of the trajectory.
To fit the spline the trajectory is rotated into its main direction. You can access this rotated trajectory by
{@link #getRotatedTrajectory() getRotatedTrajectory}.
@return The fitted spline