import math
import matplotlib.pyplot
import matplotlib.pyplot as plt
import matplotlib as mpl
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.collections import PolyCollection
from matplotlib.colors import colorConverter
import numpy as np
import matplotlib.pyplot as plt
from numpy import *
from pylab import *
import pylab
class vec(object):
def __init__(self, x, y, z):
self.vec=(x, y, z)
def unit(self, vec):
dist=math.sqrt(vec[0]**2+vec[1]**2+vec[2]**2)
u=(vec[0]/dist,vec[1]/dist,vec[2]/dist)
return u
def add(self, vector1, vector2):
sum=(vector1[0]+vector2[0], vector1[1]+vector2[1], vector1[2]+vector2[2])
return sum
def addfour(self, vec1, vec2, vec3, vec4):
comebine1=vector.add(vec1,vec2)
comebine2=vector.add(vec3,vec4)
outcome=vector.add(comebine1, comebine2)
return outcome
def smultiply(self,scalar, vec):
product=(scalar*vec[0],scalar*vec[1],scalar*vec[2])
return product
def innerproduct(self, vec1, vec2):
result=(vec1[0]*vec2[0]+vec1[1]*vec2[1]+vec1[2]*vec2[2])
return result
def crossproduct(self, vec1, vec2):
res=(vec1[1]*vec2[2]-vec1[2]*vec2[1], vec1[2]*vec2[0]-vec1[0]*vec2[2], vec1[0]*vec2[1]-vec1[1]*vec2[0])
return res
def function(self, vec1, vec2):
coef1=c1*inner(vec2, vec2)
term1=self.smultiply(coef1, vec2)
term2=self.smultiply(c2,self.crossproduct(vec2,vec1))
term3=self.smultiply(c3,(0,0,1))
term4=self.smultiply(c4*vec1[2],vec1)
sumoffour=self.addfour(term1,term2,term3,term4)
out=self.smultiply(h, sumoffour)
return out
def rungekutta1(self,vec1,vec):
k11=self.smultiply(h,vec)
k11_half=self.smultiply(.5, k11)
y12=self.add(vec, k11_half)
k12=self.smultiply(h,y12)
k12_half=self.smultiply(.5, k12)
y13=self.add(vec, k12_half)
k13=self.smultiply(h,y13)
y14=self.add(vec, k13)
k14=self.smultiply(h,y14)
k12_double=self.smultiply(2.0, k12)
k13_double=self.smultiply(2.0, k13)
k1s=self.addfour(k11, k12_double, k13_double, k14)
n_next=self.add(vec1, self.smultiply(1.0/6, k1s))
return n_next
def rungekutta2(self, vec1, vec2):
k21=self.function(vec1, vec2)
n22=self.add(vec1,self.smultiply(.5*h,(1,1,1)))
d_n22=self.add(vec2,self.smultiply(.5,k21))
k22=self.function(n22,d_n22)
n23=self.add(vec1,self.smultiply(.5*h,(1,1,1)))
d_n23=self.add(vec2,self.smultiply(.5,k22))
k23=self.function(n23,d_n23)
n24=self.add(vec1,self.smultiply(h,(1,1,1)))
d_n24=self.add(vec2,k23)
k24=self.function(n24,d_n24)
k22_double=self.smultiply(2.0,k22)
k23_double=self.smultiply(2.0,k23)
k2s=self.addfour(k21, k22_double, k23_double, k24)
d_n_next=self.add(vec2, self.smultiply(1.0/6, k2s))
return d_n_next
def plotlines(x ,y ,z):
line = ax.plot([x,0],[y,0],[z,0],color='#0000A0',marker='.')
#ax.lines.pop(0)
ax.set_xlabel('X-axis')
ax.set_ylabel('Y-axis')
ax.set_zlabel('Z-axis')
#del ax.lines[0]
matplotlib.pyplot.show()
def plotdots(x,y,z):
l = ax.scatter(x, y, z, c='#387C44',marker='.')
ax.set_xlabel('X-axis')
ax.set_ylabel('Y-axis')
ax.set_zlabel('Z-axis')
matplotlib.pyplot.show()
def plotvel(dx, dy, dz):
print dx,dy,dz
ax.scatter(dx, dy, dz, c='m')
ax.set_xlabel('X-axis')
ax.set_ylabel('Y-axis')
ax.set_zlabel('Z-axis')
matplotlib.pyplot.show()
def xyproj(xs,ys):
print xs,ys
#plot(xs,ys,'o')
plotpos(xs ,ys ,0)
show()
def drawstaticplot(m,n, d_n):
for i in range(0,m):
n=vector.rungekutta1(n, d_n)
d_n=vector.rungekutta2(n, d_n)
x1 = n[0]
y1 = n[1]
z1 = n[2]
#print x1,y1,z1
xarray.append(x1)
yarray.append(y1)
zarray.append(z1)
for j in range(0,m-20):
if j%20 == 0:
ax.plot([xarray[j],xarray[j+20]],[yarray[j],yarray[j+20]],[zarray[j],zarray[j+20]],color='#817339',marker='.')
matplotlib.pyplot.show()
def connectpoints(x1,y1,z1,x2,y2,z2):
ax.plot([x2,x2],[y2,y2],[z2,z2],color='#817339',marker='.')
matplotlib.pyplot.show()
def realtimeplotline(m,n,d_n):
x1=n[0]
y1=n[1]
z1=n[2]
x2=0
y2=0
z2=0
for i in range(0,m):
n=vector.rungekutta1(n, d_n)
d_n=vector.rungekutta2(n, d_n)
x2= n[0]
y2= n[1]
z2= n[2]
if i%20 == 0:
connectpoints(x2,y2,z2,x1,y1,z1)
x1=x2
y1=y2
z1=z2
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
theta = np.linspace(-4 * np.pi, 4 * np.pi, 100)
c1=-1.0
c2=-1.0
c3=1.0
c4=-1.0
h=.004
vector=vec(1,2,3)
allone=vec(1.0,1.0,1.0)
n=(1.0,1.0,1.0)
d_n=(1.0,-1.0,0.0)
ion()
x = n[0]
y = n[1]
z = n[2]
n=(1.0,1.0,1.0)
d_n=(1.0,-1.0,0.0)
m=10000
xarray=[]
yarray=[]
zarray=[]
#drawstaticplot(m,n, d_n)
realtimeplotline(m,n, d_n)
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