ClothoidIFCPL.txt

{

func getClothoidConstant() 
{
	clothoidConstant = 0;
	# R1>R2
	if (startCurvature < endCurvature)  do 
	{
		clothoidConstant = sqrt(length / (endCurvature - startCurvature));
		#print (clothoidConstant);
	}
	# R1<R2
	else if (startCurvature > endCurvature) do 
	{
		clothoidConstant = sqrt(length/ (startCurvature - endCurvature));
		
	}
	else do
	{
		#raise(-13.0);
		return -13.0;
	};

	return clothoidConstant;
};

		
func sgnEven(double number)
{
    if ((number%2)==0) do
	{
        # Even Number
		return 1.0; 
	}
    else do
	{
        return -1.0	;
	};
};
	
func computeX(double L,double A)
{
	x = L;
	iterations = 5;
	i = 1;
	while(i < 6) do
	{
		sign = sgnEven(i);
		
		L_exponent = 5 + (i - 1) * 4.0;

		A_exponent = i*4.0;
		Atemp = factorial(2.0*i);
		B = pow(2.0, 2.0*i);
		C = 5.0 + (i - 1) * 4;
		factor = Atemp * B * C;
		

	    D = pow(L, L_exponent);
		E = factor * pow(A, A_exponent);
		x = x + (sign * (D / E));
		i = i + 1;
	};

	return x;
	#print (x);
};
	

func computeY(double L, double A) 
{
	y = 0;
	iterations = 5;
	i = 0;
	while(i < 5) do
	{
		sign = sgnEven(i);
		
		L_exponent = 3.0+(i*4.0);
		A_exponent = 2.0+(i*4.0);
		Atemp = factorial((2*i)+1);
		B = pow(2.0, i * 2.0);
		C = A_exponent + 1.0;
		factor = Atemp * B * 2.0  * C;

		D = pow(L, L_exponent); 
		E = factor * pow(A, A_exponent);
		y = y + (sign * (D / E));

		i = i + 1;
	};
	return y;
	#print(y);
};
	
func minimalComponent(double v)
{
	return min(v0, v1, v2);
};
	
func calculateAngleBetweenVectorsHalfCircle(double vOne, double vTwo)
{
    vector VectorTwo = (0, 0);
	#print (vOne);
	#print (vTwo);
	if ( (vOne0 == VectorTwo0) && (vOne1 == VectorTwo1) ) do
	{
		returnVal = 0.0;
    };
	
	if (  (vTwo0 == VectorTwo0) && (vTwo1 == VectorTwo1) ) do
	{
		returnVal = 0.0;
    };

	a = magnitude_squared(vOne);
	b = magnitude_squared(vTwo);
	
	
	if ((a * b) == 0.0) do
	{
		returnVal = 0.0;
	};
	

	if ((a > 0) && (b >0)) do  
	{
		vector vThreeOne = (vOne0, vOne1, 0);
		vector vThreeTwo = (vTwo0, vTwo1, 0);

		A_cross = cross(vThreeOne,vThreeTwo);
		sign = minimalComponent(A_cross);
		
		
	};
	
		if (sign < 0) do
		{
		    #print(0);
		    A = dot(vOne,vTwo);
			B = sqrt(a*b);
			C = A / B;
			returnVal = acos(C);
			return -returnVal;
		}
		else do
		{
			A = dot(vOne,vTwo);
			
			B = sqrt(a*b);
			#print(B);
			C = A / B;
			#print(C);
			returnVal = acos(C);
			#print(returnVal);
			return returnVal;
		};
		
	return returnVal;
	#print (returnVal);
};

func calculateAngleBetweenVectors(double vOne,double vTwo)
{
   
	angle = calculateAngleBetweenVectorsHalfCircle(vOne, vTwo);
	
	if (angle < 0.0) do
	{
	    
		angle = angle + ( 2.0 * PI);
	};
	
	return angle;
	#print (angle);
};

	
func calculateGlobalRotation()
{
	A = getClothoidConstant();
		
	signY = 1;
	
	if ( (clockwise == 1) && (startCurvature < endCurvature) ) do
	{
		signY = -1;
	}
	else if ( (clockwise == 0) && (endCurvature < startCurvature) ) do
	{
		signY = -1;
	}
	else do
	{
		signY = 1;
	};
	
	# calculate length on clothoid at start- and endpoint
	smallerLength = A*A*startCurvature;
	greaterLength = A*A*endCurvature;
	
	# calculate clothoid end coordinates in local system
	M = computeX(greaterLength, A);
	N = computeX(smallerLength,A);
	localEndX = M - N;
	
	M = computeY(greaterLength, A);
	N = computeY(smallerLength,A);
	localEndY = signY * (M - N);
	#print(localEndX);
	#print(localEndY);
	
	
	# transfer coordinates in global system
	vector Vector2 = (localEndX,localEndY);
	calculatedEnd = Vector2 + start;
	
	# calculate angle between expected StartEnd and calculated StartEnd vector
	A = calculateAngleBetweenVectors(end-start, calculatedEnd-start);
	#print (A);
	globalRotationAngle = (PI * 2.0) - A;

	
	# rotate by 180° if endCurvature<startCurvature
	if (endCurvature < startCurvature) do
	{
		globalRotationAngle = globalRotationAngle - PI;
	};
		
		
	return globalRotationAngle;
	#print (globalRotationAngle);
  };



func getLength()
{
	return length;
};

func lerp(double a, double b, double t) 
{
	return a + ( (b - a) * t);
};
	
func getPosition(double lerpParameter) 
{
    
	A = getClothoidConstant();
	
	signY = 1;
	
	# flip clothoid vertical if clockwise or endCurvature<startCurvature
	#and startCurvature < endCurvature) or (not clockwise and endCurvature < startCurvature
	if ( (clockwise == 1) && (startCurvature < endCurvature) ) do
	{
		signY = -1;
	}
	else if ( (clockwise == 0) && (endCurvature < startCurvature) ) do
	{
		signY = -1;
	}
	else do
	{
		signY = 1;
	};
		
	# calculate length on clothoid at start- and endpoint
	smallerLength = A * A * startCurvature;
	greaterLength = A * A * endCurvature;
	
	
	# calculate length on clothoid at interpolation point
	currentLength = lerp(smallerLength,greaterLength,lerpParameter);
	

	# calculate clothoid coordinates in local system
	M = computeX(currentLength, A);
	N = computeX(smallerLength,A);
	localX = M - N;

	M = computeY(currentLength, A);
	N = computeY(smallerLength,A);
	localY = signY * (M - N);
	vector vPlc = (localX,localY,0);
	
	

	#rotatez accepts angle of rotation and rotates a
	# given matrix(x,y,z) on z-axis. Returns the teransformed x,y coords
	rotationAngle = calculateGlobalRotation();
	#print(0);
	#print(vPlc);
	tP = rotatez(rotationAngle,vPlc);
	#print (tP);
	
	# output global coordinates on clothoid intersection point
	if (startCurvature < endCurvature) do
	{
	    vector returnVec = tP + start;
		return returnVec;
	}
	else if (endCurvature < startCurvature) do
	{ 
		vector returnVec = start - tP;
		return returnVec;
	}
	else do
	{
		#raise(-66);
		return start;
	};

}; #function ends

  
  x = getPosition(0.84);
  print (x);
};