Commits

Vertexwahn committed 1caa519

Inproved pyhton clothoid.

Comments (0)

Files changed (2)

EmbeddedPython/clothoid.py

+from math import factorial, pow, pi, acos, sqrt
+from euclid import Vector2, Vector3, Matrix4
+
 ##################################
 # User Properties
-clockwise = true
-startCurvature = 434
-endCurvature = 0
+clockwise = False
+length		= 77.500241
+radiusStart	= 500
+radiusEnd	= 367
+start = Vector2(861.369007, 537.722476)
+end = Vector2(930.775243, 503.483954)
 ##################################
 
-from math import factorial, pow
-#from eucli
+startCurvature = 1/radiusStart
+endCurvature = 1/radiusEnd
+
+def getClothoidConstant(): 
+	clothidConstant = 0;
+
+	if startCurvature < endCurvature: # R1>R2
+		clothidConstant = sqrt(length/(endCurvature-startCurvature))
+	elif startCurvature > endCurvature: # R1<R2
+		clothidConstant = sqrt(length_/(startCurvature-endCurvature))
+	else:
+		raise(-13.0)
+
+	return clothidConstant
 
 def isEven(number):
     if number%2==0:
 	for i in range(0, iterations):
 		sign = isEven(i)
 		
-		L_exponent = 3+i*4
-		A_exponent = 2+i*4
-		factor = factorial(2*i+1) * pow(2.0, i*2) * 2  * (A_exponent + 1);
+		L_exponent = 3.0+i*4.0
+		A_exponent = 2.0+i*4.0
+		factor = factorial(2*i+1) * pow(2.0, i*2.0) * 2.0  * (A_exponent + 1.0);
 		
 		y += sign * pow(L, L_exponent) / (factor * pow(A, A_exponent));
 	return y
+
+def minimalComponent(v):
+	if v.x < v.y:
+		return v.x
+	else:
+		return v.y
 	
-def calculateGlobalRotation(A):
+def calculateAngleBetweenVectorsHalfCircle(v1, v2):
+	if v1 == Vector2(0, 0) or v2 == Vector2(0, 0):
+		return 0
+
+	a = v1.magnitude_squared()
+	b = v2.magnitude_squared()
+	
+	if a * b == 0.0:
+		return 0.0
+		
+	if a > 0 and b > 0:
+		v3_1 = Vector3(v1.x, v1.y, 0)
+		v3_2 = Vector3(v2.x, v2.y, 0)
+		sign = minimalComponent(v3_1.cross(v3_2))
+
+		if sign < 0:
+			return -acos(v1.dot(v2) / sqrt(a * b))
+		else:
+			return acos(v1.dot(v2) / sqrt(a * b))
+		
+	return 0
+
+def calculateAngleBetweenVectors(v1,v2):
+	angle = calculateAngleBetweenVectorsHalfCircle(v1, v2)
+	
+	if angle < 0.0:
+		angle += 2.0 * pi
+	
+	return angle;
+	
+def calculateGlobalRotation():
+	A = getClothoidConstant()
+	
 	signY = 1
-	if (clockwise==True): # && startCurvature < endCurvature) || (clockwise==False && endCurvature < startCurvature):
+	if (clockwise==True and startCurvature < endCurvature) or (clockwise==False and endCurvature < startCurvature):
 		signY = -1
 	else:
 		signY = 1
 	localEndX = computeX(greaterLength, A)-computeX(smallerLength,A)
 	localEndY = signY * (computeY(greaterLength, A)-computeY(smallerLength,A))
 	
-	return 16
+	# transfer coordinates in global system
+	calculatedEnd = Vector2(localEndX,localEndY) + start;
+	
+	# calculate angle between expected StartEnd and calculated StartEnd vector
+	globalRotationAngle = pi * 2.0 - calculateAngleBetweenVectors(end-start, calculatedEnd-start);
+
+	# rotate by 180° if endCurvature<startCurvature
+	if endCurvature<startCurvature:
+		globalRotationAngle -= pi	
+		
+	return globalRotationAngle
+	
+def getLength():
+	return length
+
+def lerp(_a, _b, _t) :
+  return _a+(_b-_a)*_t
+	
+def getPosition(lerpParameter):
+	A = getClothoidConstant()
+	
+	signY = 1
+	
+	# flip clothoid vertical if clockwise or endCurvature<startCurvature
+	if (clockwise and startCurvature < endCurvature) or (not clockwise and endCurvature < startCurvature):
+		signY = -1
+	else:
+		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
+	localX = computeX(currentLength, A)-computeX(smallerLength,A);
+	localY = signY * (computeY(currentLength, A)-computeY(smallerLength,A));
+
+	# calculate the global rotation angle and create rotation matrix
+	rotation = Matrix4()
+	rotation = rotation.rotatez(calculateGlobalRotation())
+
+	# rotate point with global rotation angle
+	tP_3 = rotation.transform(Vector3(localX,localY,0))
+	tP = Vector2(tP_3.x, tP_3.y)
+	
+	# output global coordinates on clothoid intersection point
+	if startCurvature <endCurvature:
+		return tP + start
+	elif endCurvature<startCurvature: 
+		return start - tP
+	else:
+		raise(-66)
 	
 	
 print(factorial(5))
-print(isEven(5))
+print(isEven(5))
+calculateGlobalRotation()
+print(getPosition(0.1))

EmbeddedPython/euclid.py

             return tuple([(self.x, self.y)['xy'.index(c)] \
                           for c in name])
         except ValueError:
-            raise AttributeError, name
+            raise( AttributeError, name)
 
     if _enable_swizzle_set:
         # This has detrimental performance on ordinary setattr as well
                         l['xy'.index(c)] = v
                     self.x, self.y = l
                 except ValueError:
-                    raise AttributeError, name
+                    raise (AttributeError, name)
 
     def __add__(self, other):
         if isinstance(other, Vector2):
             return tuple([(self.x, self.y, self.z)['xyz'.index(c)] \
                           for c in name])
         except ValueError:
-            raise AttributeError, name
+            raise (AttributeError, name)
 
     if _enable_swizzle_set:
         # This has detrimental performance on ordinary setattr as well
                         l['xyz'.index(c)] = v
                     self.x, self.y, self.z = l
                 except ValueError:
-                    raise AttributeError, name
+                    raise (AttributeError, name)
 
 
     def __add__(self, other):
 
 class Geometry:
     def _connect_unimplemented(self, other):
-        raise AttributeError, 'Cannot connect %s to %s' % \
-            (self.__class__, other.__class__)
+        raise (AttributeError, 'Cannot connect %s to %s' % \
+            (self.__class__, other.__class__))
 
     def _intersect_unimplemented(self, other):
-        raise AttributeError, 'Cannot intersect %s and %s' % \
-            (self.__class__, other.__class__)
+        raise (AttributeError, 'Cannot intersect %s and %s' % \
+            (self.__class__, other.__class__))
 
     _intersect_point2 = _intersect_unimplemented
     _intersect_line2 = _intersect_unimplemented
                 self.p = args[0].copy()
                 self.v = args[1].copy()
             else:
-                raise AttributeError, '%r' % (args,)
+                raise (AttributeError, '%r' % (args,))
         elif len(args) == 1:
             if isinstance(args[0], Line2):
                 self.p = args[0].p.copy()
                 self.v = args[0].v.copy()
             else:
-                raise AttributeError, '%r' % (args,)
+                raise (AttributeError, '%r' % (args,))
         else:
-            raise AttributeError, '%r' % (args,)
+            raise (AttributeError, '%r' % (args,))
         
         if not self.v:
-            raise AttributeError, 'Line has zero-length vector'
+            raise (AttributeError, 'Line has zero-length vector')
 
     def __copy__(self):
         return self.__class__(self.p, self.v)
                 self.p = args[0].copy()
                 self.v = args[1].copy()
             else:
-                raise AttributeError, '%r' % (args,)
+                raise (AttributeError, '%r' % (args,))
         elif len(args) == 1:
             if isinstance(args[0], Line3):
                 self.p = args[0].p.copy()
                 self.v = args[0].v.copy()
             else:
-                raise AttributeError, '%r' % (args,)
+                raise (AttributeError, '%r' % (args,))
         else:
-            raise AttributeError, '%r' % (args,)
+            raise (AttributeError, '%r' % (args,))
         
         # XXX This is annoying.
         #if not self.v:
                 self.n = args[0].normalized()
                 self.k = args[1]
             else:
-                raise AttributeError, '%r' % (args,)
+                raise (AttributeError, '%r' % (args,))
 
         else:
-            raise AttributeError, '%r' % (args,)
+            raise (AttributeError, '%r' % (args,))
         
         if not self.n:
-            raise AttributeError, 'Points on plane are colinear'
+            raise (AttributeError, 'Points on plane are colinear')
 
     def __copy__(self):
         return self.__class__(self.n, self.k)