Commits

committed 1caa519

Inproved pyhton clothoid.

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)`