'''
Problem can be adimensionalized like this
m d^2x/dt^2 = m g - k (|x| - lo) x/|x| - d dx/dt
d^2x/dt^2 = g - k/m (|x| - lo) x/|x| - d/m dx/dt
x' = x/lo
d^2x'/dt^2 = g - k/m (|x'| - 1) x'/|x'| - dlo/m dx'/dt
t' = t*sqrt(k/m)
k/m d^2x'/dt'^2 = g - k/m (|x'| - 1) x'/|x'| - d lo sqrt(k/m)/m dx'/dt
d^2x'/dt'^2 = mg/k - (|x'| - 1) x'/|x'| - d lo sqrt(k m) dx'/dt
so we just get two constants (plus the initial conditions):
G = m*g/k y D = d lo sqrt(k m)
d^2x'/dt'^2 = G - (|x'| - 1) x'/|x'| - D dx'/dt
'''
from __future__ import division
from visual import vector, dot
from math import *
L0 = 1
k = 25
g = vector(0,-9.8,0)
mass = 0.2
vec_scale = 0.1 # How long is the vector for a 1N force
gamma = 100
tmax = 2000
dt = 0.0001
def get_components(vec, axis):
axis_90 = (-axis[1], axis[0], 0)
return dot(vec, axis), dot(vec, axis_90)
# Create objects
class Point_Mass(object):
def __init__(self, pos, velocity):
self.pos = vector(pos)
self.velocity = vector(velocity)
def simulate(G, D, initial_length, theta0, end):
pivot = Point_Mass(vector(0,L0,0), (0,0,0))
bob = Point_Mass(pos=(initial_length * sin(theta0),
L0 - initial_length * cos(theta0),
0),
velocity=(0,0,0))
# First iteration step
# Get forces
fgrav = G
axis = bob.pos - pivot.pos
axis_n = axis.norm()
deltal = axis.mag - 1
if deltal > 0:
fstring = -deltal*axis_n
else:
fstring = vector(0,0,0)
v_rad, v_trans = get_components(bob.velocity, axis_n)
fdamp = -D * v_rad * axis_n
olddeltal = deltal
t = 0
done = False
final_pos = final_vel = None
while t < tmax and not done:
# Update objects
# m*v = m*v0 + F_net * dt
oldvelocity = bob.velocity
bob.velocity = bob.velocity + (fgrav+fstring+fdamp)*dt
# x = x0 + (m*v - m*v0)*dt/m/2
bob.pos = bob.pos + 0.5*(bob.velocity + oldvelocity)*dt
# Update counters and indicators
axis = bob.pos - pivot.pos
axis_n = axis.norm()
deltal = axis.mag - 1
old_vtrans = v_trans
v_rad, v_trans = get_components(bob.velocity, axis_n)
if v_trans < 0 and old_vtrans > 0:
print deltal
if abs(deltal-olddeltal) < end:
done = True
final_pos = bob.pos
final_vel = bob.velocity
olddeltal = deltal
# Update force(s)
if deltal > 0:
fstring = -deltal*axis_n
else:
fstring = vector(0,0,0)
fdamp = -D * v_rad * axis_n
t += dt
return deltal, final_pos, final_vel
G = mass*g/k
D = gamma*L0*sqrt(k*mass)
initial_length = (1+0.08476)*L0
angle = atan(0.60461/(L0-0.099364))
deltal, final_pos, final_vel = simulate(G, D, initial_length, angle, 1e-5)
print (deltal, final_pos, final_vel)
print ("initial length: ", L0+deltal, "initial angle: ", angle)