#!/usr/bin/env python
# encoding: utf-8

################################################################################
# CONSTITUTIVE LAW TESTING: Law2_ScGeom_ViscElPhys_Basic()
#
# Two spheres with velocities (v,0,0) and (-v,0,0) collide.
# This script checks if:
#  - the numerical displacement equals the analytical displacement at a certain
#    time before the spheres separate, for instance in t = 0.05 s
#    This also implies the consistency of the results in terms of velocity
#    (and acceleration).
#  - the normal stiffness and normal damping coefficients have been calculated
#    correctly in function of the normal coefficient (en) of restitution and the
#    impact time (tc); this is a consequence of the previous condition.
#
# Notice that:
#  - this script only checks the displacement before the separation because the
#    analytical solution (given below) supposes that the damping term is still
#    present, when the spheres separate.  This is however not true in the
#    numerical model (see source code, prevent appearing of attraction forces
#    due to a viscous component).
#  - this script does not check the tangential (or shear) behaviour of
#    the constitutive law.
#

from yade import plot, ymport
import math

################################################################################
# MATERIAL

rho = 2000  # [kg/m^3] density
mu = 0.75  # [-]      friction coefficient
tc = 0.2  # [s]      contact time
en = 0.3  # [-]      normal restitution coefficient
et = 0.2  # [-]      tangential restitution coefficient

frictionAngle = math.atan(mu)
mat = O.materials.append(ViscElMat(tc=tc, en=en, et=et, frictionAngle=frictionAngle, density=rho))

################################################################################
# GEOMETRY

r = 0.1  # [m] sphere radius

b1 = O.bodies.append(sphere(center=(2 * r, 0, 0), radius=r, material=mat))
b2 = O.bodies.append(sphere(center=(0, 0, 0), radius=r, material=mat))

################################################################################
# SIMULATION

O.dt = 1e-5  # [s]   fixed time step
v = 2  # [m/s] velocity along x-axis

O.bodies[b1].state.vel[0] = -v
O.bodies[b2].state.vel[0] = v

O.engines = [
        ForceResetter(),
        InsertionSortCollider([Bo1_Sphere_Aabb(), Bo1_Facet_Aabb(), Bo1_Wall_Aabb()]),
        InteractionLoop(
                [Ig2_Sphere_Sphere_ScGeom(), Ig2_Facet_Sphere_ScGeom(), Ig2_Wall_Sphere_ScGeom()], [Ip2_ViscElMat_ViscElMat_ViscElPhys()],
                [Law2_ScGeom_ViscElPhys_Basic()]
        ),
        NewtonIntegrator(damping=0)
]

################################################################################
# RUN

O.run(5001, True)

################################################################################
# COMPARE ANALYTICAL AND NUMERICAL SOLUTIONS

m = 4. / 3. * math.pi * r**3 * rho  # [kg] mass of the sphere

# Normal stiffness and damping coefficients according to [Pournin2001]
meff = m / 2
kn = 2.0 * meff / tc**2 * (math.pi**2 + math.log(en)**2)
cn = -4.0 * meff / tc * math.log(en)

# Analytical solution of a linear spring damper system
omega0 = math.sqrt(kn / m)
zeta = cn / (2 * math.sqrt(kn * m))
omegad = omega0 * math.sqrt(1 - zeta**2)
xAnalytical = v / omegad * math.exp(-zeta * omega0 * O.time) * math.sin(omegad * O.time)

# Comparison (if ok, YadeCheckError Exception is not thrown)
tolerance = 0.0001

xNumerical = O.bodies[b2].state.pos[0]

if ((abs(xNumerical - xAnalytical) / xAnalytical) > tolerance):
	raise YadeCheckError("((abs(xNumerical-xAnalytical)/xAnalytical)>tolerance)")
