from visual import *
import math
#
# this program makes an object and rotates it
# this version is like rotator1, but draws acceleration and
# velocity vectors
#    pmb, 10/24/02
#
# make axes
xaxis = cylinder ( pos=(0,0,0), axis=(5,0,0), radius=0.05, color=color.red )
yaxis = cylinder ( pos=(0,0,0), axis=(0,5,0), radius=0.05, color=color.green )
zaxis = cylinder ( pos=(0,0,0), axis=(0,0,5), radius=0.05, color=color.blue )
# label axes
xlabel = label ( pos= (5,0,0),text='X',height=10, border=5 )
ylabel = label ( pos= (0,5,0),text='Y',height=10, border=5 )
zlabel = label ( pos= (0,0,5),text='Z',height=10, border=5 )
#
# make a box
whiteBox = box(pos=(0,0,0), length= 5.0, height=1.0, width=0.1, color=color.white)
#
# setup for rotation
t = 0.0
dt = 0.05
angPos = 0.0
angVel = 0.0
angAxis = ( 0,1,0 )
#
# draw acc and vel?
drawAcc = 1
rPoint = 2.0
if drawAcc:
    acc = arrow ( pos = ( rPoint,0,0 ), axis = ( 0,0,1 ), shaftwidth= 0.05, color= color.red ) 
    vel = arrow ( pos = ( rPoint,0,0 ), axis = ( 0,0,1 ), shaftwidth= 0.05, color= color.blue ) 
    posNow = vector ( rPoint,0,0 )
    posPrev1 = vector ( rPoint,0,0 )
    posPrev2 = vector ( rPoint,0,0 )
#
# main loop
while 1:
    rate(20)
    t = t+ dt

    # find angular velocity
    angAcc = 2 * 3.14159/100.0
    angVel = angVel + angAcc * dt
    angPos = angPos + angVel * dt
    
    # the vpython "rotate" takes CHANGES to omega as argument, not omega
    whiteBox.rotate( angle= angVel * dt, axis = angAxis )

    # draw acc, vel if wanted
    if ( drawAcc ):
        # find position now
        posNow.x = rPoint * cos ( angPos )
        posNow.y = 0.0
        posNow.z = -rPoint * sin ( angPos )
        print "poses ", posNow, posPrev1, posPrev2
        
        # draw velocity
        vel.pos = posNow
        vel.axis = (posNow - posPrev1)/ dt 
        print " vel= ", vel.pos, vel.axis
        
        # draw acc
        acc.pos = posNow
        acc.axis = (( posNow - posPrev1 )/ dt - ( posPrev1 - posPrev2 )/ dt ) /dt
        acc.axis = acc.axis 
        print " acc= ", acc.pos, acc.axis
        
        # save new pos
        posPrev2.x  = posPrev1.x
        posPrev2.y  = posPrev1.y
        posPrev2.z  = posPrev1.z
        posPrev1.x  = posNow.x
        posPrev1.y  = posNow.y
        posPrev1.z  = posNow.z