S[workpt][0] -= (kappa/gamma)*M*dt*(S[workpt][0]-physarray[workpt][1].phi(PHI_DIHEDRAL))
S[workpt][1] -= (kappa/gamma)*M*dt*(S[workpt][1]-physarray[workpt][1].phi(PSI_DIHEDRAL))
# UPDATE CARTESIAN
# Dr. Izaguirre: I have checked and this constraint
# is correct. The energy is harmonic, but the force (the gradient)
# is not harmonic. In fact it is exactly what is in the paper.
proparray[workpt][0].propagate(scheme="LangevinImpulse", steps=1, dt=dt, forcefield=ff)
proparray[workpt][1].propagate(scheme="LangevinImpulse", steps=1, dt=dt, forcefield=ff)
# My own function which sets phi and psi for individual force objects
# Saves performance since I only change 'angle', I don't want to define
# all new force objects by changing params.
if (workpt == numpoints-1):
FTSM.setPhiPsi(ff, S[0][0], S[0][1])
else:
FTSM.setPhiPsi(ff, S[workpt+1][0], S[workpt+1][1])
#print "\nI"+str(iter+1)+" BEFORE SMOOTH: ",S
# REPARAMETERIZE S
#S.reverse()
#FTSM.switchPhiPsi(S)
S = FTSM.arclenChris(FTSM.extractPhi(S), FTSM.extractPsi(S))
#FTSM.switchPhiPsi(S)
#S.reverse()
print "\nI"+str(iter+1)+": ",S
io.plotVector(proparray[0][0],stringgraph,S, rangex=[-numpy.pi, 0], rangey=[-100*numpy.pi/180, numpy.pi])
