# coin.mbd

#-----------------------------------------------------------------------------
# [Data Block]

begin: data;
   problem: initial value;
end: data;

#-----------------------------------------------------------------------------
# [<Problem> Block]

begin: initial value;
   initial time:   0.;
   final time:     1.;
   time step:      1.e-4;
   max iterations: 500;
   tolerance:      1.e-7;
end: initial value;

#-----------------------------------------------------------------------------
# [Control Data Block]

begin: control data;
   output frequency: 10;
   structural nodes: 4;
   abstract nodes:   12;
   rigid bodies:     1;
   joints:           1;
   forces:           16;
   genels:           12;
   gravity;
end: control data;

#-----------------------------------------------------------------------------
# Design Variables
set: real m = 5.0e-3; #[kg] Mass of the coin
set: real d = 1.5e-3; #[m]  Thickness of the coin
set: real R = 6.8e-3; #[m]  Radius of the coin

set: real X0 = 0.;    #[m] Initial X position
set: real Y0 = 0.;    #[m] Initial Y position
set: real Z0 = 5.e-2; #[m] Initial Z position

set: real E1 = 45.*deg2rad; #[rad] Initial Euler angle 1
set: real E2 = 0.;          #[rad] Initial Euler angle 2
set: real E3 = 0.;          #[rad] Initial Euler angle 3

set: real VX0 = 0.; #[m/s] Initial X velocity
set: real VY0 = 0.; #[m/s] Initial Y velocity
set: real VZ0 = 0.; #[m/s] Initial Z velocity

set: real wx0 = 0.; #[rad/s] Initial angular velocity about x axis of the coin
set: real wy0 = 5.; #[rad/s] Initial angular velocity about y axis of the coin
set: real wz0 = 0.; #[rad/s] Initial angular velocity about z axis of the coin

set: real Kc = 100000.; #[N/m]  Contact stiffness
set: real Ec = 1.2;     #[-]    Contact stiffness exponent
set: real Cc = 10.;     #[Ns/m] Contact damping
set: real Dc = 0.0005;  #[m]    Contact damping activation depth

set: real mu = 0.1;  #[-]   Friction coefficient
set: real Vt = 0.05; #[m/s] Friction threshold velocity

set: real eps = 1e-6;

#-----------------------------------------------------------------------------
# Intermediate Variables
set: real Ixx = 1./12.*m*(3*R^2+d^2);
set: real Iyy = 1./12.*m*(3*R^2+d^2);
set: real Izz = m*R^2/2.;

#-----------------------------------------------------------------------------
# Reference Labels
set: integer Ref_Coin   = 1;
set: integer Ref_Top    = 2;
set: integer Ref_Bottom = 3;

# Structural Node Labels
set: integer NoSta_Ground = 1;
set: integer NoDyn_Coin   = 2;
set: integer NoDum_Top    = 3;
set: integer NoDum_Bottom = 4;

# Abstract Node Labels
set: integer NoAbs_X_Normal  = 1;
set: integer NoAbs_Y_Normal  = 2;
set: integer NoAbs_Z_Normal  = 3;
set: integer NoAbs_X_Contact = 4;
set: integer NoAbs_Y_Contact = 5;
set: integer NoAbs_Z_Contact = 6;
set: integer NoAbs_FN_Top    = 7;
set: integer NoAbs_FX_Top    = 8;
set: integer NoAbs_FY_Top    = 9;
set: integer NoAbs_FN_Bottom = 10;
set: integer NoAbs_FX_Bottom = 11;
set: integer NoAbs_FY_Bottom = 12;

# Body Labels
set: integer Body_Coin = 1;

# Joint Labels
set: integer JoClamp_Ground = 1;

# Force Labels
set: integer FoStr_NormalFN_Top = 1;
set: integer CoStr_NormalTX_Top = 2;
set: integer CoStr_NormalTY_Top = 3;

set: integer FoStr_FrictionFX_Top = 4;
set: integer FoStr_FrictionFY_Top = 5;

set: integer CoStr_FrictionTX_Top = 6;
set: integer CoStr_FrictionTY_Top = 7;
set: integer CoStr_FrictionTZ_Top = 8;

set: integer FoStr_NormalFN_Bottom = 9;
set: integer CoStr_NormalTX_Bottom = 10;
set: integer CoStr_NormalTY_Bottom = 11;

set: integer FoStr_FrictionFX_Bottom = 12;
set: integer FoStr_FrictionFY_Bottom = 13;

set: integer CoStr_FrictionTX_Bottom = 14;
set: integer CoStr_FrictionTY_Bottom = 15;
set: integer CoStr_FrictionTZ_Bottom = 16;

# Genel Labels
set: integer GeClamp_X_Normal  = 1;
set: integer GeClamp_Y_Normal  = 2;
set: integer GeClamp_Z_Normal  = 3;
set: integer GeClamp_X_Contact = 4;
set: integer GeClamp_Y_Contact = 5;
set: integer GeClamp_Z_Contact = 6;
set: integer GeClamp_FN_Top    = 7;
set: integer GeClamp_FX_Top    = 8;
set: integer GeClamp_FY_Top    = 9;
set: integer GeClamp_FN_Bottom = 10;
set: integer GeClamp_FX_Bottom = 11;
set: integer GeClamp_FY_Bottom = 12;

#-----------------------------------------------------------------------------
# Scalar Functions
scalar function: "cubstep",
   cubicspline, do not extrapolate,
      -0.03, 0.00,
      -0.02, 0.00,
      -0.01, 0.00,
       0.00, 0.00,
       1.00, 1.00,
       1.01, 1.00,
       1.02, 1.00,
       1.03, 1.00;

scalar function: "cubsign",
   cubicspline, do not extrapolate,
      -1.03, -1.00,
      -1.02, -1.00,
      -1.01, -1.00,
      -1.00, -1.00,
       1.00,  1.00,
       1.01,  1.00,
       1.02,  1.00,
       1.03,  1.00;
   
#-----------------------------------------------------------------------------
# References
reference: Ref_Coin,
   X0, Y0, Z0,        # absolute position
   euler, E1, E2, E3, # absolute orientation
   VX0, VY0, VZ0,     # absolute velocity
   null;              # absolute angular velocity
   
reference: Ref_Top,
   reference, Ref_Coin, 0., 0., d/2., # absolute position
   reference, Ref_Coin, eye,          # absolute orientation
   reference, Ref_Coin, null,         # absolute velocity
   reference, Ref_Coin, null;         # absolute angular velocity
   
reference: Ref_Bottom,
   reference, Ref_Coin, 0., 0., -d/2., # absolute position
   reference, Ref_Coin, eye,           # absolute orientation
   reference, Ref_Coin, null,          # absolute velocity
   reference, Ref_Coin, null;          # absolute angular velocity
      
#-----------------------------------------------------------------------------
# [Nodes Block]

begin: nodes;

   #-----------------------------------------------------------------------------
   # Structural Nodes
   structural: NoSta_Ground, static,
      null, # absolute position
      eye,  # absolute orientation
      null, # absolute velocity
      null; # absolute angular velocity
      
   structural: NoDyn_Coin, dynamic,
      reference, Ref_Coin, null,          # absolute position
      reference, Ref_Coin, eye,           # absolute orientation
      reference, Ref_Coin, null,          # absolute velocity
      reference, Ref_Coin, wx0, wy0, wz0, # absolute angular velocity
      accelerations, yes;
      
   structural: NoDum_Top, dummy, NoDyn_Coin,
      offset, 
         reference, Ref_Top, null, # relative offset
         reference, Ref_Top, eye;  # relative orientation
         
   structural: NoDum_Bottom, dummy, NoDyn_Coin,
      offset,
         reference, Ref_Bottom, null, # relative offset
         reference, Ref_Bottom, eye;  # relative orientation
   
   #-----------------------------------------------------------------------------
   # Intermediate Variables     
   set: real DX0_Top = model::xposition(NoDum_Top);
   set: real DY0_Top = model::yposition(NoDum_Top);
   set: real DZ0_Top = model::zposition(NoDum_Top);
   
   set: real DX0_Bottom = model::xposition(NoDum_Bottom);
   set: real DY0_Bottom = model::yposition(NoDum_Bottom);
   set: real DZ0_Bottom = model::zposition(NoDum_Bottom);
   
   set: real X0_Normal = (DX0_Top-DX0_Bottom)/d;
   set: real Y0_Normal = (DY0_Top-DY0_Bottom)/d;
   set: real Z0_Normal = (DZ0_Top-DZ0_Bottom)/d;

   set: real X0_Contact = R*X0_Normal*Z0_Normal
                             /sqrt(max(eps,X0_Normal^2+Y0_Normal^2));
   set: real Y0_Contact = R*Y0_Normal*Z0_Normal
                             /sqrt(max(eps,X0_Normal^2+Y0_Normal^2));
   set: real Z0_Contact = -R*sqrt(X0_Normal^2+Y0_Normal^2);
   
   #-----------------------------------------------------------------------------
   # Abstract Nodes   
   abstract: NoAbs_X_Normal, algebraic,
      value, X0_Normal;
      
   abstract: NoAbs_Y_Normal, algebraic,
      value, Y0_Normal;
   
   abstract: NoAbs_Z_Normal, algebraic,
      value, Z0_Normal;
        
   abstract: NoAbs_X_Contact, algebraic,
      value, X0_Contact;
   
   abstract: NoAbs_Y_Contact, algebraic,
      value, Y0_Contact;
      
   abstract: NoAbs_Z_Contact, algebraic,
      value, Z0_Contact;      

   abstract: NoAbs_FN_Top, algebraic;
   abstract: NoAbs_FX_Top, algebraic;
   abstract: NoAbs_FY_Top, algebraic;
   
   abstract: NoAbs_FN_Bottom, algebraic;
   abstract: NoAbs_FX_Bottom, algebraic;
   abstract: NoAbs_FY_Bottom, algebraic;
      
end: nodes;

#-----------------------------------------------------------------------------
# Plugin Variables
set: [node, DX_Top, NoDum_Top, structural, string="X[1]"];
set: [node, DY_Top, NoDum_Top, structural, string="X[2]"];
set: [node, DZ_Top, NoDum_Top, structural, string="X[3]"];

set: [node, VX_Top, NoDum_Top, structural, string="XP[1]"];
set: [node, VY_Top, NoDum_Top, structural, string="XP[2]"];
set: [node, VZ_Top, NoDum_Top, structural, string="XP[3]"];

set: [node, DX_Bottom, NoDum_Bottom, structural, string="X[1]"];
set: [node, DY_Bottom, NoDum_Bottom, structural, string="X[2]"];
set: [node, DZ_Bottom, NoDum_Bottom, structural, string="X[3]"];

set: [node, VX_Bottom, NoDum_Bottom, structural, string="XP[1]"];
set: [node, VY_Bottom, NoDum_Bottom, structural, string="XP[2]"];
set: [node, VZ_Bottom, NoDum_Bottom, structural, string="XP[3]"];

set: [node, WX, NoDyn_Coin, structural, string="Omega[1]"];
set: [node, WY, NoDyn_Coin, structural, string="Omega[2]"];
set: [node, WZ, NoDyn_Coin, structural, string="Omega[3]"];

set: [dof, X_Normal, NoAbs_X_Normal, abstract, algebraic];
set: [dof, Y_Normal, NoAbs_Y_Normal, abstract, algebraic];
set: [dof, Z_Normal, NoAbs_Z_Normal, abstract, algebraic];

set: [dof, X_Contact, NoAbs_X_Contact, abstract, algebraic];
set: [dof, Y_Contact, NoAbs_Y_Contact, abstract, algebraic];
set: [dof, Z_Contact, NoAbs_Z_Contact, abstract, algebraic];

set: [dof, FN_Top, NoAbs_FN_Top, abstract, algebraic];
set: [dof, FX_Top, NoAbs_FX_Top, abstract, algebraic];
set: [dof, FY_Top, NoAbs_FY_Top, abstract, algebraic];

set: [dof, FN_Bottom, NoAbs_FN_Bottom, abstract, algebraic];
set: [dof, FX_Bottom, NoAbs_FX_Bottom, abstract, algebraic];
set: [dof, FY_Bottom, NoAbs_FY_Bottom, abstract, algebraic];

#-----------------------------------------------------------------------------
# [Elements Block]

begin: elements;

   #-----------------------------------------------------------------------------
   # Bodies
   body: Body_Coin, NoDyn_Coin,
      m,                   # mass
      null,                # relative center of mass
      diag, Ixx, Iyy, Izz; # inertia matrix
     
   #-----------------------------------------------------------------------------
   # Genels
   genel: GeClamp_X_Normal,
      clamp,
         NoAbs_X_Normal, abstract,
         string, "(DX_Top-DX_Bottom)/d";
         
   genel: GeClamp_Y_Normal,
      clamp,
         NoAbs_Y_Normal, abstract,
         string, "(DY_Top-DY_Bottom)/d";
         
   genel: GeClamp_Z_Normal,
      clamp,
         NoAbs_Z_Normal, abstract,
         string, "(DZ_Top-DZ_Bottom)/d";
         
   genel: GeClamp_X_Contact,
      clamp,
         NoAbs_X_Contact, abstract,
         string, "R*X_Normal*Z_Normal\
                     /sqrt(max(eps,X_Normal^2+Y_Normal^2))";
         
   genel: GeClamp_Y_Contact,
      clamp,
         NoAbs_Y_Contact, abstract,
         string, "R*Y_Normal*Z_Normal\
                     /sqrt(max(eps,X_Normal^2+Y_Normal^2))";
         
   genel: GeClamp_Z_Contact,
      clamp,
         NoAbs_Z_Contact, abstract,
         string, "-R*sqrt(X_Normal^2+Y_Normal^2)";
 
   genel: GeClamp_FN_Top,
      clamp,
         NoAbs_FN_Top, abstract,
         string, "max(0.,Kc*sign(-Z_Contact-DZ_Top)*abs(-Z_Contact-DZ_Top)^Ec\
                       +Cc*(0-(VZ_Top+WX*Y_Contact-WY*X_Contact))\
                          *model::sf::cubstep((-Z_Contact-DZ_Top)/Dc))";
                       
   genel: GeClamp_FX_Top,
      clamp,
         NoAbs_FX_Top, abstract,
         string, "-model::sf::cubsign((VX_Top+WY*Z_Contact-WZ*Y_Contact)/Vt)*mu*FN_Top";
         
   genel: GeClamp_FY_Top,
      clamp,
         NoAbs_FY_Top, abstract,
         string, "-model::sf::cubsign((VY_Top+WZ*X_Contact-WX*Z_Contact)/Vt)*mu*FN_Top";
               
   genel: GeClamp_FN_Bottom,
      clamp,
         NoAbs_FN_Bottom, abstract,
         string, "max(0.,Kc*sign(-Z_Contact-DZ_Bottom)*abs(-Z_Contact-DZ_Bottom)^Ec\
                       +Cc*(0-(VZ_Bottom+WX*Y_Contact-WY*X_Contact))\
                          *model::sf::cubstep((-Z_Contact-DZ_Bottom)/Dc))";
                          
   genel: GeClamp_FX_Bottom,
      clamp,
         NoAbs_FX_Bottom, abstract,
         string, "-model::sf::cubsign((VX_Bottom+WY*Z_Contact-WZ*Y_Contact)/Vt)*mu*FN_Bottom";
         
   genel: GeClamp_FY_Bottom,
      clamp,
         NoAbs_FY_Bottom, abstract,
         string, "-model::sf::cubsign((VY_Bottom+WZ*X_Contact-WX*Z_Contact)/Vt)*mu*FN_Bottom";
   
   #-----------------------------------------------------------------------------
   # Joints
   joint: JoClamp_Ground,
      clamp,
         NoSta_Ground,
            position, node,
            orientation, node;
   
   #-----------------------------------------------------------------------------
   # Forces
   force: FoStr_NormalFN_Top,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Top, null, # relative arm
         single, 0., 0., 1., string, "FN_Top";  # force value
         
   couple: CoStr_NormalTX_Top,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Top, null,          # relative arm
         single, 1., 0., 0., string, "Y_Contact*FN_Top"; # couple value
         
   couple: CoStr_NormalTY_Top,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Top, null,           # relative arm
         single, 0., 1., 0., string, "-X_Contact*FN_Top"; # couple value
         
   force: FoStr_FrictionFX_Top,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Top, null, # relative arm
         single, 1., 0., 0., string, "FX_Top";  # force value
                                                       
   force: FoStr_FrictionFY_Top,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Top, null, # relative arm
         single, 0., 1., 0., string, "FY_Top";  # force value
                                                       
   couple: CoStr_FrictionTX_Top,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Top, null,           # relative arm
         single, 1., 0., 0., string, "-Z_Contact*FY_Top"; # couple value
            
   couple: CoStr_FrictionTY_Top,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Top, null,          # relative arm
         single, 0., 1., 0., string, "Z_Contact*FX_Top"; # couple value
            
   couple: CoStr_FrictionTZ_Top,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Top, null,                              # relative arm
         single, 0., 0., 1., string, "-Y_Contact*FX_Top + X_Contact*FY_Top"; # couple value
         
   force: FoStr_NormalFN_Bottom,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Bottom, null, # relative arm
         single, 0., 0., 1., string, "FN_Bottom";  # force value
         
   couple: CoStr_NormalTX_Bottom,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Bottom, null,          # relative arm
         single, 1., 0., 0., string, "Y_Contact*FN_Bottom"; # couple value
         
   couple: CoStr_NormalTY_Bottom,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Bottom, null,           # relative arm
         single, 0., 1., 0., string, "-X_Contact*FN_Bottom"; # couple value
         
   force: FoStr_FrictionFX_Bottom,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Bottom, null, # relative arm
         single, 1., 0., 0., string, "FX_Bottom";  # force value
                                                       
   force: FoStr_FrictionFY_Bottom,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Bottom, null, # relative arm
         single, 0., 1., 0., string, "FY_Bottom";  # force value
                                                       
   couple: CoStr_FrictionTX_Bottom,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Bottom, null,           # relative arm
         single, 1., 0., 0., string, "-Z_Contact*FY_Bottom"; # couple value
            
   couple: CoStr_FrictionTY_Bottom,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Bottom, null,          # relative arm
         single, 0., 1., 0., string, "Z_Contact*FX_Bottom"; # couple value
            
   couple: CoStr_FrictionTZ_Bottom,
      absolute,
         NoDyn_Coin,
            position, reference, Ref_Bottom, null,                                 # relative arm
         single, 0., 0., 1., string, "-Y_Contact*FX_Bottom + X_Contact*FY_Bottom"; # couple value   
 
   #-----------------------------------------------------------------------------
   # Gravity 
   gravity: 0., 0., -1., const, 9.81;
   
end: elements;