#Charles Peter Nystrom "Smokey"

from visual import *

#robot1


#screen.settings

scene.center=(0,5,0)
scene.autoscale=1


ceiling=box(pos=vector(0,10,0),height=.1,width=4,length=10,color=color.green)
floor=box(pos=vector(0,0,0),height=.1,width=4,length=10,color=color.green)

#konstants
g=9.8
t=0
dt=0.01


#parts list robot1
parts=[]
nparts=14

struts=[]
nstruts=19

stone=[]
nstone=0

#robot1.foot.1
parts.append(cone(pos=vector(-2,0,0),axis=(0,.5,0),radius=.5,color=color.blue))
parts[0].m=2.
parts[0].v=vector(0,0,0)
parts[0].jet=vector(0,0,0)


#robot1.foot.2
parts.append(cone(pos=vector(2,0,0),axis=(0,.5,0),radius=.5,color=color.blue))
parts[1].m=2.
parts[1].v=vector(0,0,0)
parts[1].jet=vector(0,0,0)


#robot1.torso
parts.append(sphere(pos=(0,3,0),color=color.red,radius=1.6))
parts[2].m=20.
parts[2].v=vector(.1,0,0)
parts[2].jet=vector(0,8,0)


#robot1.head
parts.append(sphere(pos=(0,5,0),color=color.orange,radius=.5))
parts[3].m=1.
parts[3].v=vector(0,10,0)
parts[3].jet=vector(0,10,0)


#robot1.foot.3
parts.append(cone(pos=vector(0,0,-2),axis=(0,.5,0),radius=.5,color=color.blue))
parts[4].m=2.
parts[4].v=vector(0,0,0)
parts[4].jet=vector(10,0,0)


#robot1.foot.4
parts.append(cone(pos=vector(0,0,2),axis=(0,.5,0),radius=.5,color=color.blue))
parts[5].m=2.
parts[5].v=vector(0,0,0)
parts[5].jet=vector(-10,0,0)


#robot1.ball.1
parts.append(sphere(pos=(3,3,0),color=color.orange,radius=.25))
parts[6].m=1
parts[6].v=vector(-2,0,0)
parts[6].jet=vector(2,1,0)


#robot1.ball.2
parts.append(sphere(pos=(-3,3,0),color=color.orange,radius=.15))
parts[7].m=2
parts[7].v=vector(2,3,0)
parts[7].jet=vector(-2,0,4)


#robot1.ball.3
parts.append(sphere(pos=(0,3,2),color=color.orange,radius=.32))
parts[8].m=1.5
parts[8].v=vector(-1,0,1)
parts[8].jet=vector(1,0,1)


#robot1.ball.4
parts.append(sphere(pos=(0,1,0),color=color.orange,radius=.4))
parts[9].m=1.8
parts[9].v=vector(-4,0,3)
parts[9].jet=vector(4,0,3)


#robot1.ball.5
parts.append(sphere(pos=(2,0,-1),color=color.orange,radius=.12))
parts[10].m=2.4
parts[10].v=vector(-5,2,1)
parts[10].jet=vector(0,1,3)


#robot1.ball.6
parts.append(sphere(pos=(-1,3,4),color=color.orange,radius=.23))
parts[11].m=2.1
parts[11].v=vector(-2,2,3)
parts[11].jet=vector(4,-1,0)


#robot1.ball.7
parts.append(sphere(pos=(0,7,-.1),color=color.orange,radius=.1))
parts[12].m=3.8
parts[12].v=vector(-1,0,3)
parts[12].jet=vector(1,1,-2)


#robot1.ball.8
parts.append(sphere(pos=(.3,0,-7),color=color.orange,radius=.3))
parts[13].m=.7
parts[13].v=vector(0,-5,1)
parts[13].jet=vector(1,-2,4)
#===================================================================================

#===================================================================================



#robot1. struts
struts.append(helix(radius=.1))
struts[0].side1=0
struts[0].side2=1
struts[0].k=60


struts.append(helix(radius=.1))
struts[1].side1=1
struts[1].side2=2
struts[1].k=3000


struts.append(helix(radius=.1))
struts[2].side1=0
struts[2].side2=2
struts[2].k=3000


struts.append(helix(radius=.1))
struts[3].side1=3
struts[3].side2=2
struts[3].k=3000


struts.append(helix(radius=.1))
struts[4].side1=4
struts[4].side2=2
struts[4].k=3000


struts.append(helix(radius=.1))
struts[5].side1=1
struts[5].side2=4
struts[5].k=60


struts.append(helix(radius=.1))
struts[6].side1=5
struts[6].side2=2
struts[6].k=3000


struts.append(helix(radius=.1))
struts[7].side1=0
struts[7].side2=5
struts[7].k=60


struts.append(helix(radius=.1))
struts[8].side1=1
struts[8].side2=5
struts[8].k=60


struts.append(helix(radius=.1))
struts[9].side1=0
struts[9].side2=4
struts[9].k=60


struts.append(helix(radius=.1))
struts[10].side1=4
struts[10].side2=5
struts[10].k=60


struts.append(helix(radius=.1))
struts[11].side1=6
struts[11].side2=2
struts[11].k=60


struts.append(helix(radius=.1))
struts[12].side1=7
struts[12].side2=2
struts[12].k=60


struts.append(helix(radius=.1))
struts[13].side1=8
struts[13].side2=2
struts[13].k=60


struts.append(helix(radius=.1))
struts[14].side1=9
struts[14].side2=2
struts[14].k=60


struts.append(helix(radius=.1))
struts[15].side1=10
struts[15].side2=2
struts[15].k=60


struts.append(helix(radius=.1))
struts[16].side1=11
struts[16].side2=2
struts[16].k=60


struts.append(helix(radius=.1))
struts[17].side1=12
struts[17].side2=2
struts[17].k=60


struts.append(helix(radius=.1))
struts[18].side1=13
struts[18].side2=2
struts[18].k=60


#
#====================================================================================
#
#
#====================================================================================



#struts.init
for j in range (nstruts):
	struts[j].pos=parts[struts[j].side1].pos
	struts[j].axis=parts[struts[j].side2].pos-parts[struts[j].side1].pos
	struts[j].x0=mag(struts[j].axis)



while(t < 20):
	rate(100)
	#parts[i].F
	for i in range(nparts):
		parts[i].F=parts[i].m*vector(0,-g,0)+parts[i].jet



	for j in range(nstruts):
		#springs.F
		dF=struts[j].k*struts[j].axis*(mag(struts[j].axis)-struts[j].x0)/\
			mag(struts[j].axis)
		parts[struts[j].side1].F=parts[struts[j].side1].F+dF
		parts[struts[j].side2].F=parts[struts[j].side2].F-dF



	#update.parts[i].pos
	for i in range(nparts):
		parts[i].v=parts[i].v+parts[i].F/parts[i].m*dt
		#floor.bound
		if ((parts[i].y < 0) and (parts[i].v.y < 0)):
			parts[i].v.y=-parts[i].v.y
			#"friction"
			parts[i].v=parts[i].v*0.9
		parts[i].pos=parts[i].pos+parts[i].v*dt
		


	#update.struts
	for j in range (nstruts):
		struts[j].pos=parts[struts[j].side1].pos
		struts[j].axis=parts[struts[j].side2].pos-parts[struts[j].side1].pos

	
	t=t+dt