I am writing a game in Python (with pygame) that requires me to generate random but nice-looking "sea" for each new game. After a long search I settled on an algorithm that involves Bezier curves as defined in padlib.py. I now need to figure out when the curves generated by padlib intersect a line segment.
The brute force method would be to just use the set of approximating line segments produced by padlib to find the answer. However, I suspect that a better answer can be found analytically. I only have a few dozen spline segments - searching them should be faster than thousand of line segments.
A little search took me down this road: Bezier Curve -> Kochanek-Bartels Spline -> Cubic Hermite spline
On the last page, I found this function:
p(t) = h00(t)p0 + h10(t)m0 + h01(t)p1 + h11(t)m1
where p(t) is a actually a point (2-dimensional vector), hij(t) functions are cubic polynomials, p0, p1, m0 and m1 are points I can get from padlib code.
Now, I can see that the solution to my problem is p(t) = u + v * t1, where u and v are the end of my line segment.
However, working out the analytical solution is beyond me. Does anyone here know of an existing solution? Or can help me with solving the equations?
As a rough outline, rotate and translate the system so that the line segment lies on the X axis. Now the y coordinate is a cubic function of the parameter t. Find the 'zeros' (the analytic formulae will be found in good math texts or wikipedia). Now evaluate the x coordinates corresponding to those zero points and test against your line segment.
I've finally got to a working code to illustrate the method suggested by Mark Thornton. Below is the Python code for the intersection routine, together with pygame code to test it visually. The cubic roots solution can be written based on this question.
import pygame
from pygame.locals import *
import sys
import random
from math import sqrt, fabs, pow
from lines import X, Y
import itertools
import pygame
from pygame import draw, Color
import padlib
from roots_detailed import cubicRoots
def add_points(*points):
X = 0
Y = 0
for (x,y) in points:
X += x
Y += y
return (X,Y)
def diff_points(p2, p1):
# p2 - p1
return (X(p2)-X(p1), Y(p2)-Y(p1));
def scale_point(factor, p):
return (factor * X(p), factor*Y(p))
def between(v0, v, v1):
if v0 > v1: v0, v1 = v1, v0
return v >= v0 and v <= v1
# the point is guaranteed to be on the right line
def pointOnLineSegment(l1, l2, point):
return between(X(l1), X(point), X(l2)) and between(Y(l1), Y(point), Y(l2))
def rotate(x, y, R1, R2, R3, R4):
return (x*R1 + y*R2, x*R3 + y * R4);
def findIntersections(p0, p1, m0, m1, l1, l2):
# We're solving the equation of one segment of Kochanek-Bartels
# spline intersecting with a line segment
# The spline is described at http://en.wikipedia.org/wiki/Cubic_Hermite_spline
# The discussion on the adopted solution can be found at https://stackoverflow.com/questions/1813719/intersection-between-bezier-curve-and-a-line-segment
#
# The equation we're solving is
#
# h00(t) p0 + h10(t) m0 + h01(t) p1 + h11(t) m1 = u + v t1
#
# where
#
# h00(t) = 2t^3 - 3t^2 + 1
# h10(t) = t^3 - 2t^2 + t
# h01(t) = -2t^3 + 3t^2
# h11(t) = t^3 - t^2
# u = l1
# v = l2-l1
u = l1
v = diff_points(l2, l1);
# The first thing we do is to move u to the other side:
#
# h00(t) p0 + h10(t) m0 + h01(t) p1 + h11(t) m1 - u = v t1
#
# Then we're looking for matrix R that would turn (v t1) into
# ({|v|, 0} t1). This is rotation of coordinate system matrix,
# described at http://mathworld.wolfram.com/RotationMatrix.html
#
# R(h00(t) p0 + h10(t) m0 + h01(t) p1 + h11(t) m1 - u) = R(v t1) = {|v|, 0}t1
#
# We only care about R[1,0] and R[1,1] because it lets us solve
# the equation for y coordinate where y == 0 (intersecting the
# spline segment with the x axis of rotated coordinate
# system). I'll call R[1,0] = R3 and R[1,1] = R4 .
v_abs = sqrt(v[0] ** 2 + v[1] ** 2)
R1 = X(v) / v_abs
R2 = Y(v) / v_abs
R3 = -Y(v) / v_abs
R4 = X(v) / v_abs
# The letters x and y are denoting x and y components of vectors
# p0, p1, m0, m1, and u.
p0x = p0[0]; p0y = p0[1]
p1x = p1[0]; p1y = p1[1]
m0x = m0[0]; m0y = m0[1]
m1x = m1[0]; m1y = m1[1]
ux = X(u); uy = Y(u)
#
#
# R3(h00(t) p0x + h10(t) m0x + h01(t) p1x + h11(t) m1x - ux) +
# + R4(h00(t) p0y + h10(t) m0y + h01(t) p1y + h11(t) m1y - uy) = 0
#
# Opening all parentheses and simplifying for hxx we get:
#
# h00(t) p0x R3 + h10(t) m0x R3 + h01(t) p1x R3 + h11(t) m1x R3 - ux R3 +
# + h00(t) p0y R4 + h10(t) m0y R4 + h01(t) p1y R4 + h11(t) m1y R4 - uy R4 = 0
#
# h00(t) p0x R3 + h10(t) m0x R3 + h01(t) p1x R3 + h11(t) m1x R3 - ux R3 +
# + h00(t) p0y R4 + h10(t) m0y R4 + h01(t) p1y R4 + h11(t) m1y R4 - uy R4 = 0
#
# (1)
# h00(t) (p0x R3 + p0y R4) + h10(t) (m0x R3 + m0y R4) +
# h01(t) (p1x R3 + p1y R4) + h11(t) (m1x R3 + m1y R4) - (ux R3 + uy R4) = 0
#
# We now introduce new substitution
K00 = p0x * R3 + p0y * R4
K10 = m0x * R3 + m0y * R4
K01 = p1x * R3 + p1y * R4
K11 = m1x * R3 + m1y * R4
U = ux * R3 + uy * R4
# Expressed in those terms, equation (1) above becomes
#
# h00(t) K00 + h10(t) K10 + h01(t) K01 + h11(t) K11 - U = 0
#
# We will now substitute the expressions for hxx(t) functions
#
# (2t^3 - 3t^2 + 1) K00 + (t^3 - 2t^2 + t) K10 + (-2t^3 + 3t^2) K01 + (t^3 - t^2) K11 - U = 0
#
# 2 K00 t^3 - 3 K00 t^2 + K00 +
# + K10 t^3 - 2 K10 t^2 + K10 t -
# - 2 K01 t^3 + 3 K01 t^2 +
# + K11 t^3 - K11 t^2 - U = 0
#
# 2 K00 t^3 - 3 K00 t^2 + 0t + K00
# + K10 t^3 - 2 K10 t^2 + K10 t
# - 2 K01 t^3 + 3 K01 t^2
# + K11 t^3 - K11 t^2 + 0t - U = 0
#
# (2 K00 + K10 - 2K01 + K11) t^3
# +(-3 K00 - 2K10 + 3 K01 - K11) t^2
# + K10 t
# + K00 - U = 0
#
#
# (2 K00 + K10 - 2K01 + K11) t^3 + (-3 K00 - 2K10 + 3 K01 - K11) t^2 + K10 t + K00 - U = 0
#
# All we need now is to solwe a cubic equation
valuesOfT = cubicRoots((2 * K00 + K10 - 2 * K01 + K11),
(-3 * K00 - 2 * K10 + 3 * K01 - K11),
(K10),
K00 - U)
# We can then put the values of it into our original spline segment
# formula to find the potential intersection points. Any point
# that's on original line segment is an intersection
def h00(t): return 2 * t**3 - 3 * t**2 + 1
def h10(t): return t**3 - 2 * t**2 + t
def h01(t): return -2 * t**3 + 3 * t**2
def h11(t): return t**3 - t**2
intersections = []
for t in valuesOfT:
if t < 0 or t > 1.0: continue
# point = h00(t) * p0 + h10(t) * m0 + h01(t) * p1 + h11(t) * m1
point = add_points(
scale_point(h00(t), p0),
scale_point(h10(t), m0),
scale_point(h01(t), p1),
scale_point(h11(t), m1)
)
if pointOnLineSegment(l1, l2, point): intersections.append(point)
return intersections
def findIntersectionsManyCurves(p0_array, p1_array, m0_array, m1_array, u, v):
result = [];
for (p0, p1, m0, m1) in itertools.izip(p0_array, p1_array, m0_array, m1_array):
result.extend(findIntersections(p0, p1, m0, m1, u, v))
return result
def findIntersectionsManyCurvesManyLines(p0, p1, m0, m1, points):
result = [];
for (u,v) in itertools.izip(*[iter(points)]*2):
result.extend(findIntersectionsManyCurves(p0, p1, m0, m1, u, v))
return result
class EventsEmitter(object):
def __init__(self):
self.consumers = []
def emit(self, eventName, *params):
for method in self.consumers:
funcName = method.im_func.func_name if hasattr(method, "im_func") else method.func_name
if funcName == eventName:
method(*params)
def register(self, method):
self.consumers.append(method)
def unregister(self, method):
self.consumers.remove(method)
class BunchOfPointsModel(EventsEmitter):
def __init__(self):
EventsEmitter.__init__(self)
self.pts = []
def points(self):
return self.pts.__iter__()
def pointsSequence(self):
return tuple(self.pts)
def have(self, point):
return point in self.pts
def addPoint(self,p):
self.pts.append(p)
self.emit("pointsChanged", p)
def replacePoint(self, oldP, newP):
idx = self.pts.index(oldP)
self.pts[idx] = newP
self.emit("pointsChanged", newP)
def removePoint(self, p):
self.point.remove(p)
self.emit("pointsChanged", p)
class BunchOfPointsCompositeModel(object):
def __init__(self, m1, m2):
self.m1 = m1
self.m2 = m2
def points(self):
return itertools.chain(self.m1.points(), self.m2.points())
def have(self, point):
return self.m1.have(point) or self.m2.have(point)
def replacePoint(self, oldP, newP):
if self.m1.have(oldP):
self.m1.replacePoint(oldP, newP)
else:
self.m2.replacePoint(oldP, newP)
def removePoint(self, p):
if self.m1.have(p):
self.m1.removePoint(p)
else:
self.m2.removePoint(p)
def register(self, method):
self.m1.register(method)
self.m2.register(method)
def unregister(self, method):
self.m1.unregister(method)
self.m2.unregister(method)
class BunchOfPointsDragController(EventsEmitter):
def __init__(self, model):
EventsEmitter.__init__(self)
self.model = model
self.draggedPoint = None
def mouseMovedTo(self, x,y):
if self.draggedPoint != None:
newPoint = (x,y)
draggedPoint = self.draggedPoint
self.draggedPoint = newPoint
self.model.replacePoint(draggedPoint, newPoint)
def buttonDown(self, x,y):
if self.draggedPoint == None:
closePoint = self.getCloseEnoughPoint(x,y)
if closePoint != None:
self.draggedPoint = closePoint
self.emit("dragPointChanged",closePoint)
def buttonUp(self, x,y):
self.mouseMovedTo(x,y)
self.draggedPoint = None
self.emit("dragPointChanged", None)
def getCloseEnoughPoint(self, x,y):
minSquareDistance = 25
closestPoint = None
for point in self.model.points():
dx = X(point) - x
dy = Y(point) - y
distance = dx*dx + dy*dy
if minSquareDistance > distance:
closestPoint = point
minSquareDistance = distance
return closestPoint
def isDraggedPoint(self, p):
return p is self.draggedPoint
class CurvesLinesViewPointsView(object):
def __init__(self, screen, modelCurves, modelLines, model, controller):
self.screen = screen
self.modelLines = modelLines
self.modelCurves = modelCurves
self.controller = controller
controller.register(self.dragPointChanged)
model.register(self.pointsChanged)
def draw(self):
self.screen.fill(Color("black"))
pygame.draw.lines(self.screen, Color("cyan"), 0, self.modelLines.pointsSequence(), 3)
(p0, p1, m0, m1) = padlib.BezierCurve(screen,modelCurves.pointsSequence(),3,100,Color("magenta"))
self.drawPointSet(self.modelCurves.points(),
lambda(p):self.controller.isDraggedPoint(p),
Color("white"), Color("red"))
self.drawPointSet(self.modelLines.points(),
lambda(p):self.controller.isDraggedPoint(p),
Color("lightgray"), Color("red"))
self.drawSimplePointSet(findIntersectionsManyCurvesManyLines(p0, p1, m0, m1,self.modelLines.points()),
Color("blue"))
def drawSimplePointSet(self, points, normalColor):
self.drawPointSet(points, lambda(p):True, None, normalColor);
def drawPointSet(self, points, specialPoint, normalColor, specialColor):
for p in points:
if specialPoint(p):
draw.circle(self.screen, specialColor, p, 6)
else:
draw.circle(self.screen, normalColor, p, 2)
pygame.display.update()
def dragPointChanged(self, p): self.draw()
def pointsChanged(self, p): self.draw()
class PygameEventsDistributor(EventsEmitter):
def __init__(self):
EventsEmitter.__init__(self)
def processEvent(self, e):
if e.type == MOUSEMOTION:
self.emit("mouseMovedTo", e.pos[0], e.pos[1])
elif e.type == MOUSEBUTTONDOWN:
self.emit("buttonDown", e.pos[0], e.pos[1])
elif e.type == MOUSEBUTTONUP:
self.emit("buttonUp", e.pos[0], e.pos[1])
modelLines = BunchOfPointsModel()
modelCurves = BunchOfPointsModel()
model = BunchOfPointsCompositeModel(modelLines, modelCurves);
controller = BunchOfPointsDragController(model)
distributor = PygameEventsDistributor()
distributor.register(controller.mouseMovedTo)
distributor.register(controller.buttonUp)
distributor.register(controller.buttonDown)
pygame.init()
screen = pygame.display.set_mode((640, 480))
modelCurves.addPoint((29,34))
modelCurves.addPoint((98,56))
modelCurves.addPoint((200, 293))
modelCurves.addPoint((350, 293))
modelLines.addPoint((23,123))
modelLines.addPoint((78,212))
view = CurvesLinesViewPointsView(screen, modelCurves, modelLines, model, controller)
keepGoing = True
try:
while (keepGoing):
for event in pygame.event.get():
if event.type == QUIT:
keepGoing = False
break
distributor.processEvent(event)
pass
finally:
pygame.quit()
Related
import re
import math
from random
import randint
import hashlib
P = (-0.15, 2.645)
Q = (0.7, 2.71)
max_mod = 1.158 * 10 ** 77
def SHA256_INT(text):
return int(f'0x{hashlib.sha256(text.encode("ascii")).hexdigest()}', 0)
def ecc_double_slope(P):
slope = (3 * P[0] ** 2) / (2 * P[1])
return slope
def ecc_add(P, Q, slope):
xr = slope ** 2 - P[0] - Q[0]
yr = slope * (P[0] - xr) - P[1]
return (xr, yr)
def ecc_double(P):
slope = ecc_double_slope(P)
sum0 = ecc_add(P, P, slope)
return sum0
def ecc_double_for(P, limit):
lis = []
for i in range(limit):
b = ecc_double(P)
P = b
lis.append((2 ** i, b))
return lis
def greedy2(number):
x = 0
dub_lis = []
add_lis = []
while 2 ** x < number:
dub_lis.append(2 ** x)
x += 1
x -= 1
n = 2 ** x
while n != number:
y = x
while n + (2 ** y) > number:
y -= 1
n += (2 ** y)
x += 1
add_lis.append(2 ** y)
return len(dub_lis), add_lis
def ecc_main_add(P, Q):
x1 = P[0]
y1 = P[1]
x2 = Q[0]
y2 = Q[1]
slope = (y2 - y1) / (x2 - y1)
xr = slope ** 2 - x1 - x2
yr = slope * (x1 - xr) - y1
return xr, yr
def gen_points(gen, points):
if gen == 1:
return points
elif gen > 1:
if type(points) == int:
point_curve = (points, math.sqrt(points ** 3 + 7))
elif type(points) == tuple:
point_curve = points
point_steps = greedy2(gen)
point_dub = point_steps[0]
point_add = point_steps[1]
dub_list = ecc_double_for(point_curve, point_dub)
for i in point_add:
b = ecc_main_add(dub_list[int(math.log(i, 2))][1], dub_list[-1][1])
return b, gen
elif gen == 0:
return 0
def sign(messsage, private_key, public_key, G):
global max_mod
msg_hash = SHA256_INT(messsage)
randnum = randint(2, max_mod)
r = gen_points(randnum, G)[0]
r_x = r[0]
msg_hash = SHA256_INT(messsage)
s = (r_x * private_key + msg_hash) / randnum
p1 = gen_points(msg_hash/s, G)
p2 = gen_points(r_x/s, public_key)
p3 = ecc_main_add(p1, p2)
return p3
pub, priv = gen_points(9756, P)
print(sign('hello', priv, pub, P))
Hello, I'm trying to implement a way to sign messages using this tutorial:
https://learnmeabitcoin.com/beginners/digital_signatures_signing_verifying
I've tried to follow the tutorial before, but it's either my key generation is wrong, I didn't follow the tutorial correctly, or both.
My current code generates an error, I've already debugged it and it generates the error: TypeError: 'NoneType' object is not subscriptable.
Does anyone know how to solve this?
I would like to construct an ellipse given the major/minor axes (or radii) and two points. I would like the line between the two points to be on the major axis. This just means that I would like the two points to lie on the major axis, and then construct the ellipse around the major axis. I originally constructed the ellipse at the origin and attempted to rotate and translate the ellipse, which didn't work. The unfinished code I have is listed below. How can I go about constructing an ellipse in this manner? Ideally, this would just return a list. Any insights would be greatly appreciated, and if you have any code for this please feel free to share.
import numpy
import matplotlib.pyplot as plt
import random
import math
from math import sin, cos
# Returns theta in [-pi/2, 3pi/2]
def generate_theta(a, b):
u = random.random() / 4.0
theta = numpy.arctan(b/a * numpy.tan(2*numpy.pi*u))
v = random.random()
if v < 0.25:
return theta
elif v < 0.5:
return numpy.pi - theta
elif v < 0.75:
return numpy.pi + theta
else:
return -theta
def radius(a, b, theta):
return a * b / numpy.sqrt((b*numpy.cos(theta))**2 + (a*numpy.sin(theta))**2)
def random_point(a, b, third_point, center=(0, 0)):
angle = None
if a > b:
random_theta = generate_theta(a, b)
max_radius = radius(a, b, random_theta)
random_radius = max_radius * numpy.sqrt(random.random())
f = round(random_radius * numpy.cos(random_theta))
s = round(random_radius * numpy.sin(random_theta))
angle = math.atan2(third_point[1], third_point[0]) - math.atan2(center[1], center[0])
else:
random_theta = generate_theta(b, a)
max_radius = radius(b, a, random_theta)
random_radius = max_radius * numpy.sqrt(random.random())
f = round(random_radius * numpy.cos(random_theta))
s = round(random_radius * numpy.sin(random_theta))
angle = math.atan2(third_point[1], third_point[0]) - math.atan2(center[1], center[0])
lio = rotate(center, (f, s), angle)
lio = (int(lio[0]), int(lio[1]))
return numpy.array([third, ward])
def rotate(origin, point, angle):
#Rotate a point counterclockwise by a given angle around a given origin.
#The angle should be given in radians.
x = origin[0] + cos(angle) * (point[0] - origin[0]) - sin(angle) * (point[1] - origin[1])
y = origin[1] + sin(angle) * (point[0] - origin[0]) + cos(angle) * (point[1] - origin[1])
return (x, y)
#height
a = 95
#length
b = 25
#rand_p = (-random.randint(300, 400), -random.randint(100, 300))
rand_p = (0, 0)
points = numpy.array([random_point(a, b, (100, 100), (-25, 0)) for _ in range(200)])
#rando = rotate((0, 0), right_most_point, angle)
iopoints = []
# t = x[0] - (int(centroid[0]) - 100)
# t2 = x[1] - (int(centroid[1]) - 100)
centroid = numpy.mean(points, axis=0)
print(centroid)
#plt.plot(rando[0], rando[1], 'ro')
plt.plot(rand_p[0], rand_p[1], 'ro')
plt.scatter(points[:,0], points[:,1])
plt.show()
class ELLIPSE:
def __init__(self, a, b, num_points, start, end):
self.a = a
self.b = b
self.num_points = num_points
self.start = start
self.end = end
self.angle_gen = math.atan2(self.end[1]-self.start[1], self.end[0]-self.start[0])
def generate_theta(self, a, b):
u = random.random() / 4.0
theta = np.arctan(self.b/self.a * np.tan(2*np.pi*u))
v = random.random()
if v < 0.25:
return theta
elif v < 0.5:
return np.pi - theta
elif v < 0.75:
return np.pi + theta
else:
return -theta
def radius(self, a, b, theta):
return self.a * self.b / np.sqrt((b*np.cos(theta))**2 + (a*np.sin(theta))**2)
def random_point(self, major_axis, minor_axis, center, qa):
random_theta = self.generate_theta(self.a, self.b)
max_radius = self.radius(self.a, self.b, random_theta)
random_radius = max_radius * np.sqrt(random.random())
f = round(random_radius * np.cos(random_theta))
s = round(random_radius * np.sin(random_theta))
lio = self.rotate((0, 0), (f, s), self.angle_gen)
return (int(lio[0]+center[0]), int(lio[1]+center[1]))
def rotate(self, origin, point, angle):
"""
Rotate a point counterclockwise by a given angle around a given origin.
The angle should be given in radians.
"""
ox, oy = origin
px, py = point
qx = ox + math.cos(angle) * (px - ox) - math.sin(angle) * (py - oy)
qy = oy + math.sin(angle) * (px - ox) + math.cos(angle) * (py - oy)
return qx, qy
def midpoint(self, p1, p2):
return ((p1[0]+p2[0])/2, (p1[1]+p2[1])/2)
def ret_list(self):
points = [self.random_point(self.a, self.b, self.midpoint(self.start, self.end), self.angle_gen) for _ in range(self.num_points)]
return points
I am trying to produce one plot that contains multiple polar curves; however, only one polar curve is generating. This is my first time using polar plots in matplotlib, but I assumed generating multiple curves on the same plot would work the same as generating multiple lines on a regular graph. I have seen examples of code that demonstrate exactly what I need, and even though my code looks the same, only one curve generates. The curves are also not generating one at a time in separate plots. Only one plot generates.
I have made sure that plt.show() is outside of any while or for loops, and I have even tried moving the plotting functions to an entirely new method with no success. I am unsure of what I am doing wrong and would appreciate any help I could get.
Thank you.
CODE: (Plotting function towards the bottom)
import math
import numpy as np
import matplotlib.pyplot as plt
def rFuncA(a, ec, v):
# calculates current orbit radius from a, eccentricity, and angle
return a * (1 - ec ** 2) / (1 + ec * math.cos(v))
def wFunc(a, ec, v, u):
# calculates current orbit radius from a, eccentricity, and angle
block1 = a * (1 - ec ** 2)
block2 = 1 + ec * math.cos(v)
return (math.sqrt(u) / block1 ** (3 / 2)) * block2 ** 2
def VoFunc(ec, v, r, w):
return r * w * (ec * math.sin(v)) / (1 + ec * math.cos(v)), \
r * w
def dXFunc(Fl, Fd, Ft, gamma, alpha, u, r, m, Isp, Vr, Vv, g):
theta = alpha + gamma
dX1 = (Vv ** 2) / r + (Fl * math.cos(gamma) - Fd * math.sin(gamma) + Ft * math.sin(theta)) / m - u / (r ** 2)
dX2 = -(Vr * Vv / r + (Fl * math.sin(gamma) + Fd * math.cos(gamma) - Ft * math.cos(theta)) / m)
dX3 = Vr
dX4 = Vv / r
dX5 = -Ft / (g * Isp)
return dX1, dX2, dX3, dX4, dX5
def aFuncR(r, u, Vr, Vv):
block1 = 2 * u / r
block2 = Vr ** 2 + Vv ** 2
return u / (block1 - block2)
def ecFuncR(r, u, Vr, Vv):
return (r / u) * math.sqrt((Vv ** 2 - u / r) ** 2 + (Vv * Vr) ** 2)
class Properties:
def __init__(self):
# Preallocate
n = 1000000
self.ec = np.zeros([n])
self.a = np.zeros([n])
self.r = np.zeros([n])
self.gamma = np.zeros([n])
self.X = np.zeros([n, 5])
self.Tt = np.zeros([n])
self.v = np.zeros([n])
self.theta = np.zeros([n])
# Constants
self.Ft = 0.01 # Thrust Force, kN
self.Fl = 0 # Lift Force, kN
self.Fd = 0 # Drag Force, kN
self.g = 0.009806 # gravity, km/s^2
self.u = 3.986E5 # gravitational parameter, km^3/s^2
self.alpha = 0 # Angle of Attack (0 for ballistic trajectory), radians
self.Isp = 2000 # Specific impulse, sec
self.dt = 2000 # Iterator step size, sec
self.r_e = 6561 # Mean earth radius, km
self.r_a = 13200 # Apogee radius at final orbit, km
# Progressive variables initial values
self.a[0] = 8530
self.r[0] = rFuncA(self.a[0], self.ec[0], self.v[0]) # Changing radius, km
self.m = np.zeros([n])
self.m[0] = 1000 + 7.743 + 71.25 # Inital Mass
self.w = np.zeros([n])
self.w[0] = wFunc(self.a[0], self.ec[0], self.v[0], self.u) # Angular velocity, rad/s
Vo1, Vo2 = VoFunc(self.ec[0], self.v[0], self.r[0], self.w[0])
self.Vr = np.zeros([n]) # Creates array for radial velocities
self.Vv = np.zeros(([n])) # Creates array for tangential velocities
self.Vr[0] = Vo1 # Initial radial velocity, km/s
self.Vv[0] = Vo2 # Initial tangential velocity, km/s
# Plotting
self.pt = np.zeros([n])
self.pr = np.zeros([n])
self.cr = np.zeros([n])
self.cf = np.zeros([n])
self.i = 0
def runge_kutta(self):
self.i = 0
self.gamma[0] = math.atan(self.Vr[0] / self.Vv[0]) # Initial angle, rad
self.X[0][:] = np.array([self.Vr[0], self.Vv[0], self.r[0], self.v[0], self.m[0]])
while self.a[self.i] * (1 + self.ec[self.i]) < self.r_a:
# self.Tt[i+1] = self.Tt[0] + self.dt
dX1, dX2, dX3, dX4, dX5 = dXFunc(self.Fl, self.Fd, self.Ft, self.gamma[self.i], self.alpha, self.u, self.X[self.i][2],
self.X[self.i][4], self.Isp, self.X[self.i][0], self.X[self.i][1], self.g)
k1 = np.array([dX1, dX2, dX3, dX4, dX5])
X2 = self.X[self.i] + k1 * self.dt / 2
dX1, dX2, dX3, dX4, dX5 = dXFunc(self.Fl, self.Fd, self.Ft, self.gamma[self.i], self.alpha, self.u, X2[2],
X2[4], self.Isp, X2[0], X2[1], self.g)
k2 = np.array([dX1, dX2, dX3, dX4, dX5])
X3 = X2 + k2 * self.dt / 2
dX1, dX2, dX3, dX4, dX5 = dXFunc(self.Fl, self.Fd, self.Ft, self.gamma[self.i], self.alpha, self.u, X3[2],
X3[4], self.Isp, X3[0], X3[1], self.g)
k3 = np.array([dX1, dX2, dX3, dX4, dX5])
X4 = X3 + k3 * self.dt
dX1, dX2, dX3, dX4, dX5 = dXFunc(self.Fl, self.Fd, self.Ft, self.gamma[self.i], self.alpha, self.u, X4[2],
X4[4], self.Isp, X4[0], X4[1], self.g)
k4 = np.array([dX1, dX2, dX3, dX4, dX5])
self.X[self.i+1][:] = self.X[self.i][:] + (1 / 6) * (k1 + k2 + k3 + k4 * self.dt)
self.a[self.i+1] = aFuncR(self.X[self.i+1][2], self.u, self.X[self.i+1][0], self.X[self.i+1][1])
self.ec[self.i+1] = ecFuncR(self.X[self.i+1][2], self.u, self.X[self.i+1][0], self.X[self.i+1][1])
self.i += 1
print(self.X[self.i][:]) # Final Values for Vr, Vv, r, v, and m
def plot(self):
i = self.i - 1
k = 0
# Orbit period
while self.pt[k] < 2 * math.pi:
self.pt[k+1] = self.pt[k] + (2 * math.pi / i)
k += 1
# Orbit location
for j in range(i):
# Transferring Orbit
self.pr[j] = self.a[j] * (1 + self.ec[j] ** 2) / (1 + self.ec[j] * math.cos(self.pt[j]))
# Low Earth Orbit
self.cr[j] = self.a[0]
plt.polar(self.pt[:i], self.pr[:i], 'r')
plt.polar(self.pt[:i], self.cr[:i], 'g',)
plt.show()
def main():
P = Properties()
P.runge_kutta()
P.plot()
if __name__ == '__main__':
main()
Plot:
First polar curve
Second polar curve (only generates when first plot is commented out)
Upon further investigation, I was using an older version of Matplotlib, and updating Matplotlib via reinstallation fixed the issue.
pip uninstall matplotlib
pip install matplotlib
Excuse me for the length of the title please but this is a pretty specific question. I'm currently simulating a launch of a rocket to mars in the 2022 launch window and I noticed that my rocket is a far distance away from Mars, even though it's traveling in the right direction. After simplifying my code to narrow down the problem, I simply plotted the orbits of the Earth and Mars (Using data from NASA's SPICE library) and propagated the position and velocity given to me by the lambert solver I implemented (Universal variables) to plot the final orbit of the rocket.
I'm only letting the Sun's gravity effect the rocket, not the Earth or Mars, to minimize my problem space. Yet even though I've simplified my problem so far, the intersection between Mars' and my rocket's orbits happens well before the time of flight has been simulated all the way, and the minimum distance between the two bodies is more than a million kilometers at all times.
That being said, something must be wrong but I cannot find the problem. I've made sure the lambert solver code I copied is correct by comparing it to Dario Izzo's method and both gave the same results. Furthermore, I've also checked that my orbit propagator works by propagating Mars' and the Earth's orbits and comparing those ellipses to the data from SPICE.
In conclusion, I assume this must be a stupid little mistake I made somewhere, but cannot find because I lack experience in this field. Thank you for any help! :)
This is the JupyterLab notebook I used:
import numpy as np
import matplotlib.pyplot as plt
import json
import math
import spiceypy as spice
# Physics
G = 6.6741e-11
class Entity:
def __init__(self, x, v, m, do_gravity):
self.x = x
self.v = v
self.a = np.array([0,0,0])
self.m = m
self.do_gravity = do_gravity
def apply_force(self, f):
self.a = np.add(self.a, f / self.m);
def step(self, dt):
self.v = np.add(self.v, self.a * dt)
self.x = np.add(self.x, self.v * dt)
self.a = np.array([0,0,0])
class StaticEntity(Entity):
def __init__(self, body, t, do_gravity):
super().__init__(self.get_state(body, t)[:3], self.get_state(body, t)[3:], self.get_mass(body), do_gravity)
self.body = body
self.t = t
def step(self, dt):
self.t += dt
state = self.get_state(self.body, self.t)
self.x = state[:3]
self.v = state[3:]
#classmethod
def get_state(self, body, t):
[state, lt] = spice.spkezr(body, t, "J2000", "NONE", "SSB")
return state * 1000
#classmethod
def get_mass(self, body):
[dim, gm] = spice.bodvrd(body, "GM", 1)
return gm * 1e9 / G
def get_position(self, t):
return self.get_state(self.body, t)[:3]
def get_velocity(self, t):
return self.get_state(self.body, t)[3:]
class Propagator:
def __init__(self, entities):
self.entities = entities
def step(self, dt):
for e1 in self.entities:
for e2 in self.entities:
if (e1 is e2) or (not e1.do_gravity) or isinstance(e2, StaticEntity):
continue
diff = np.subtract(e1.x, e2.x)
fg = G * e1.m * e2.m / np.dot(diff, diff)
force = fg * diff / np.linalg.norm(diff)
e2.apply_force(force)
for entity in self.entities:
entity.step(dt)
# Lambert solver
def C2(psi):
if psi >= 0.0:
sp = math.sqrt(psi)
return (1 - math.cos(sp)) / psi
else:
sp = math.sqrt(-psi)
return (1 - math.cosh(sp)) / psi
def C3(psi):
if psi >= 0.0:
sp = math.sqrt(psi)
return (sp - math.sin(sp)) / (psi * sp)
else:
sp = math.sqrt(-psi)
return (sp - math.sinh(sp)) / (psi * sp)
def lambert_solve(r1, r2, tof, mu, iterations, tolerance):
R1 = np.linalg.norm(r1)
R2 = np.linalg.norm(r2)
cos_a = np.dot(r1, r2) / (R1 * R2)
A = math.sqrt(R1 * R2 * (1.0 + cos_a))
sqrt_mu = math.sqrt(mu)
if A == 0.0:
return None
psi = 0.0
psi_lower = -4.0 * math.pi * math.pi
psi_upper = 4.0 * math.pi * math.pi
c2 = 1.0 / 2.0
c3 = 1.0 / 6.0
for i in range(iterations):
B = R1 + R2 + A * (psi * c3 - 1.0) / math.sqrt(c2)
if A > 0.0 and B < 0.0:
psi_lower += math.pi
B = -B
chi = math.sqrt(B / c2)
chi3 = chi * chi * chi
tof_new = (chi3 * c3 + A * math.sqrt(B)) / sqrt_mu
if math.fabs(tof_new - tof) < tolerance:
f = 1.0 - B / R1
g = A * math.sqrt(B / mu)
g_dot = 1.0 - B / R2
v1 = (r2 - f * r1) / g
v2 = (g_dot * r2 - r1) / g
return (v1, v2)
if tof_new <= tof:
psi_lower = psi
else:
psi_upper = psi
psi = (psi_lower + psi_upper) * 0.5
c2 = C2(psi)
c3 = C3(psi)
return None
# Set up solar system
spice.furnsh('solar_system.tm')
inject_time = spice.str2et('2022 Sep 28 00:00:00')
exit_time = spice.str2et('2023 Jun 1 00:00:00')
sun = StaticEntity("Sun", inject_time, True)
earth = StaticEntity("Earth", inject_time, False)
mars = StaticEntity("Mars Barycenter", inject_time, False)
(v1, v2) = lambert_solve(earth.get_position(inject_time), mars.get_position(exit_time), exit_time - inject_time, G * sun.m, 1000, 1e-4)
rocket = Entity(earth.x, v1, 100000, False)
propagator = Propagator([sun, earth, mars, rocket])
# Generate data
earth_pos = [[], [], []]
mars_pos = [[], [], []]
rocket_pos = [[], [], []]
t = inject_time
dt = 3600 # seconds
while t < exit_time:
propagator.step(dt)
earth_pos[0].append(earth.x[0])
earth_pos[1].append(earth.x[1])
earth_pos[2].append(earth.x[2])
mars_pos[0].append(mars.x[0])
mars_pos[1].append(mars.x[1])
mars_pos[2].append(mars.x[2])
rocket_pos[0].append(rocket.x[0])
rocket_pos[1].append(rocket.x[1])
rocket_pos[2].append(rocket.x[2])
t += dt
# Plot data
plt.figure()
plt.title('Transfer orbit')
plt.xlabel('x-axis')
plt.ylabel('y-axis')
plt.plot(earth_pos[0], earth_pos[1], color='blue')
plt.plot(mars_pos[0], mars_pos[1], color='orange')
plt.plot(rocket_pos[0], rocket_pos[1], color='green')
EDIT:
I recently remodeled my code so that it uses orbit class to represent the entities. This actually gave me acceptable results, even though the code is, in theory, not doing anything differently (as far as I can tell; obviously something must be different)
def norm(a):
return np.dot(a, a)**0.5
def fabs(a):
return -a if a < 0 else a
def newton_raphson(f, f_dot, x0, n):
res = x0
for i in range(n):
res = res - f(res) / f_dot(res)
return res
def get_ephemeris(body, time):
state, _ = sp.spkezr(body, time, "J2000", "NONE", "SSB")
return np.array(state[:3]) * ap.units.km, np.array(state[3:]) * ap.units.km / ap.units.s
def get_mu(body):
_, mu = sp.bodvrd(body, "GM", 1)
return mu * ap.units.km**3 / ap.units.s**2
class orbit:
def __init__(self, position, velocity, mu):
self.position = position
self.velocity = velocity
self.mu = mu
#staticmethod
def from_body(name, center, time):
return static_orbit(name, center, time)
def get_ephemerides(self, t, dt):
time = 0
positions = []
velocities = []
#M = self.M
position = self.position
velocity = self.velocity
delta_t = dt * ap.units.s
t1 = t * ap.units.s
while time < t1:
g = self.mu / np.dot(position, position)
g_vec = g * -position / norm(position)
velocity = np.add(velocity, g_vec * delta_t)
position = np.add(position, velocity * delta_t)
positions.append(position)
velocities.append(velocity)
time = time + delta_t
return positions, velocities
class static_orbit(orbit):
def __init__(self, name, center, time):
p, v = get_ephemeris(name, time)
pc, vc = get_ephemeris(center, time)
super().__init__(p - pc, v - vc, get_mu(center))
self.name = name
self.center = center
self.time = time
def get_ephemerides(self, t, dt):
time = 0
positions = []
velocities = []
while time < t:
p, v = get_ephemeris(self.name, time + self.time)
pc, vc = get_ephemeris(self.center, time + self.time)
positions.append(p - pc)
velocities.append(v - vc)
time += dt
return positions, velocities
sp.furnsh('solar_system.tm')
t1 = sp.str2et('2022 Sep 28 00:00:00')
t2 = sp.str2et('2023 Jun 10 00:00:00')
eo = orbit.from_body("Earth", "Sun", t1)
mo = orbit.from_body("Mars Barycenter", "Sun", t1)
earth_x, earth_v = eo.get_ephemerides(t2 - t1, 3600)
mars_x, mars_v = mo.get_ephemerides(t2 - t1, 3600)
l = lambert(earth_x[0], mars_x[-1], t2 - t1, get_mu("Sun"), 1000, 1e-6)
ro = orbit(earth_x[0], l.v1, get_mu("Sun"))
rocket_x, rocket_v = ro.get_ephemerides(t2 - t1, 60)
earth_x = np.array(earth_x)
mars_x = np.array(mars_x)
rocket_x = np.array(rocket_x)
fig = go.Figure()
fig.add_trace(go.Scatter3d(x=earth_x[:,0], y=earth_x[:,1], z=earth_x[:,2], marker_size=1, marker_color='blue'))
fig.add_trace(go.Scatter3d(x=mars_x[:,0], y=mars_x[:,1], z=mars_x[:,2], marker_size=1, marker_color='orange'))
fig.add_trace(go.Scatter3d(x=rocket_x[:,0], y=rocket_x[:,1], z=rocket_x[:,2], marker_size=1, marker_color='green'))
fig.show()
This method generated following plot:
Also, before this is mentioned again, I have varied my integration step size and lambert solver tolerance to no avail, the result was qualitatively different.
So, I managed to figure out what the problem was after much head-scratching. I was simply not taking into account that the Sun is not located at (0,0,0) in my coordinate system. I thought this was negligible, but that is what made the difference. In the end, I simply passed the difference between the Earth and Mars's and the Sun's position vectors and passed those into the Lambert solver. This finally gave me the desired results.
The reason that the error ended up being so "small" (It didn't seem like an obvious bug at first) was because my coordinates are centered at the solar system barycenter which is a few million kilometers away from the Sun, as one would expect.
Thanks for the comments!
I am studding automatic and have a matlab/SIMULINK model of nonlinear vibration. I am trying to resolve the same problem using python instead.
Here we have a schema. The black frame is a part that I have already. I also have a Model tłumnika MR (eng. Magnetorheological Damper). Just I don't know how to connect it together?
There are welcome any suggestion how to resolve this including modifying model.
Code for part 1 (black frame) [just pseudo code ]
def rms(v): # ROOT MEAN SQRT
return np.sqrt(np.mean(np.square(v)))
def transmissibility(_in, _out):
# Transmissibility (T) = output/input
return rms(_out) / rms(_in)
def q(ss, t, Ampituda, freqs):
y2 = []
for f in freqs:
u = Ampituda * np.sin(2 * np.pi * f * t) # wektor wejscia
t1, y1, x1 = signal.lsim(ss, u, t) # wyliczenie modelu w dziedzinie czasu
y2.append(transmissibility(u, y1))
return y2
vec_c = np.arange(1000, 6000 , 1000)
for c in vec_c:
A = [[0, 1], [-(k/m), -(c/m)]]
B = [[-c/m], [(k/m) - (c/m)**2]]
C = [1, 0]
D = 0
ss = signal.StateSpace(A,B,C,D) # State Space model
y2 = q(ss, t, Ampituda, freqs)
plt.plot(freqs, y2, label=r'$c = {} \frac{}{} $'.format(c, "{Ns}", "{m}"), linewidth=2.0)
plt.legend()
Code for MR (Model tłumnika MR) [copy paste example]
from scipy import signal
import numpy as np
def I(to_intagrate, dt, y0=0):
i = [y0]
for v in to_intagrate:
i.append(i[-1] + v * dt)
del i[0]
return i
def MR(v, i, dt):
""" #v -- prędkość (z' -w') wektor przyśpieszeń
#i -- natężenie prądu w amperach
return Siła generowan przez tłumnik w N
Slajd 18
http://home.agh.edu.pl/~jastrzeb/images/SSD/SSD_2DOF_v1.pdf
"""
b1 = 3415.7
b2 = 93.324
b3 = 74.487
F0 = b1 * (i**2) + b2 * i + b3
b4 = 2534.1
b5 = 19.55
b6 = 643.1
C1 = b4 * (i**2) + b5*i + b6
beta = 50
p1 = 4
p2 = 0.2
x = I(v, dt)
Ft = []
for x1, v1 in zip(x, v):
part1 = F0 * np.tanh( beta * (v1 + (p1 * x1)))
part2 = C1 * (v1 + (p2 * x1))
Ft.append(part1 + part2)
return Ft, x
import matplotlib.pyplot as plt
plt.rcParams['figure.figsize'] = (20, 16)
plt.rcParams['font.size'] = 18
for f in [2, 5]: # wybrane częstotliwości
# f = 5
i = 0.2
# x_sin wektor wartości x dla wymuszenia sinusoidalnego 201 pkt
# na każdy okres sinusa. Rozpoczęcie pkt pracy w -0.2
x_sin = np.linspace(-np.pi/2, (np.pi * f) - np.pi/2, num=201 * f)
u = np.sin(x_sin) * 0.2 # przeskalowanie przyśpieszenia
dt = 1/(f*201)
ft, x = MR(u, i, dt) # sila
plt.plot(u, ft, label='Freq = {}Hz, I={}A'.format(f, i))
plt.legend()
plt.grid()
plt.show()