from math import *
from numpy import *
from numpy.linalg import *
from pylab import *

R = 20.0 # Radius of center of gravity
w = 5.5  # Weight at center of gravity
k = 1.0  # Spring constant
D = 16.0 # Distance at which spring is connected
x = 1.0  # Initial spring displacement

N = 200  # Number of incremental steps in curve simulation

# Vary any of the parameters by placing here
for w in [4,5,6,7]:

	Q = array([0.0, 0.0])
	D0 = norm(Q - array([D,0]))
	x0 = x
	
	cam_pts = []
	cir_pts = []

	for i in range(N):
		ang0 = i * pi/2/N
		ang1 = (i+1) * pi/2/N
		P0 = D * array([cos(ang0), sin(ang0)])
		P1 = D * array([cos(ang1), sin(ang1)])
	
		# Vector magic... http://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Vector_formulation
		r = dot(P1-Q,P0-Q) / norm(P0-Q)**2
		h = norm(r*(P0-Q) - (P1-Q))
		l = r*norm(P0-Q)

		# Potential energy transferred from gravity into spring
		K = sqrt( 2 * w*R/k * (cos(ang0)-cos(ang1)) + (norm(P0 - Q) + (x0-D0) )**2 ) - (x0-D0)

		if not l < K < l+h: break
		#assert l<K<l+h

		# Move point the distance required to extend the spring by the proper amount 
		d = (l + K + h*h/(l-K))/2
		Q = Q + d*(P0-Q)/norm(P0-Q)
		x0 += d

		#print '(%f,%f),'%(Q[0],Q[1]),
		cam_pts.append(Q)
		cir_pts.append(P1)

		if not i % (N/20):
			plot(*zip(Q,P1),color=(0,0,0,.1))
	
	plot(*zip(*cam_pts))
	plot(*zip(*cir_pts))

print
print 'Gravitational energy:', w * R
print '       Spring energy:', .5 * k * (x0 + norm(P1-Q) - D0)**2

axes().set_aspect('equal','datalim')
grid(1)
show()