#--- bruno.chareyre@hmg.inpg.fr ---
#!/usr/bin/python
# -*- coding: utf-8 -*-

# Experiment beam-like behaviour with chained cylinders + CohFrict connexions

from woo import utils
young=1.0e6
poisson=4
density=2.60e3 
frictionAngle=radians(30)
O.materials.append(CohFrictMat(young=young,poisson=poisson,density=density,frictionAngle=frictionAngle,normalCohesion=1e13,shearCohesion=1e13,momentRotationLaw=True,label='mat'))
O.dt=1e-4

O.engines=[
    ForceResetter(),
    InsertionSortCollider([
        Bo1_ChainedCylinder_Aabb(),
        Bo1_Sphere_Aabb()
    ]),
    InteractionLoop(
        [Ig2_ChainedCylinder_ChainedCylinder_ScGeom6D(),Ig2_Sphere_ChainedCylinder_CylScGeom()],
        [Ip2_2xCohFrictMat_CohFrictPhys(setCohesionNow=True,setCohesionOnNewContacts=True)],
        [Law2_ScGeom6D_CohFrictPhys_CohesionMoment(label='law')]
    ),
    ## Apply gravity
    GravityEngine(gravity=[0,-9.81,0]),
    ## Motion equation
    NewtonIntegrator(damping=0.15),
    PyRunner(iterPeriod=500,command='history()'),
    #PyRunner(iterPeriod=5000,command='if O.iter<21000 : woo.qt.center()')
]

#Generate a spiral
Ne=200
for i in range(0, Ne):
    omega=60.0/float(Ne); hy=0.10; hz=0.15;
    px=float(i)*(omega/60.0); py=sin(float(i)*omega)*hy; pz=cos(float(i)*omega)*hz;
    px2=float(i+1.)*(omega/60.0); py2=sin(float(i+1.)*omega)*hy; pz2=cos(float(i+1.)*omega)*hz;
    utils.chainedCylinder(begin=Vector3(pz,py,px), radius=0.005,end=Vector3(pz2,py2,px2),color=Vector3(0.6,0.5,0.5))

def outp(id=1):
    for i in O.interactions:
        if i.id1 == 1:
            print(i.phys.shearForce)
            print(i.phys.normalForce)
            return  i

O.bodies[Ne-1].state.blockedDOFs='xyzXYZ'
woo.qt.View();


 #plot some results
from math import *
from woo import plot
plot.plots={'t':('pos1',None,'vel1')}
def history():
      plot.addData(pos1=O.bodies[0].state.pos[1], # potential elastic energy
             vel1=O.bodies[0].state.vel[1],
             t=O.time)

#woo.qt.Renderer().bound=True
plot.plot()
O.saveTmp()
#O.bodies[0].state.angVel=Vector3(0.05,0,0)

