struct __verlet_node__{
    double __x, __y, __z, __xp, __yp, __zp, __mass, __damp, __grav;
    __verlet_node__()
    {
        __x = 0; __y = 0; __z = 0; __xp = __x; __yp = __y; __zp = __z;
        __mass = 1; __damp = 1; __grav = .1;
    }
    __verlet_node__( double x, double y, double z, double mass = 1, double damp = 1 )
    {
        __x = x; __y = y; __z = z; __xp = __x; __yp = __y; __zp = __z;
        __mass = mass; __damp = damp; __grav = .1;
    }
    __verlet_node__( double mass, double damp = 1 )
    {
        __x = 0; __y = 0; __z = 0; __xp = __x; __yp = __y; __zp = __z;
        __mass = mass; __damp = damp; __grav = .1;
    }
    void move()
    {
        double xp2 = __x, yp2 = __y, zp2 = __z;
        __x += ( __x - __xp ) * __damp * (1 - 1 / (__mass+50));
        __y += ( __y - __yp ) * __damp * (1 - 1 / (__mass+50));
        __z += ( __z - __zp ) * __damp * (1 - 1 / (__mass+50)) + __grav;
        __xp = xp2; __yp = yp2; __zp = zp2;
    }
    void moveTo( double x, double y, double z )
    {
        __x = x;
        __y = y;
        __z = z;
    }
    void moveRel( double x, double y, double z )
    {
        __x += x;
        __y += y;
        __z += z;
    }
    void moveDir( double latDir, double vertDir, double distance )
    {
        __x += cos( latDir ) * cos( vertDir );
        __y += sin( latDir ) * cos( vertDir );
        __z += sin( vertDir );
    }
};
struct __verlet_constraint__{
    struct __verlet_node__ *n1;
    struct __verlet_node__ *n2;
    double __len;
    int __ret_time;
    __verlet_constraint__( struct __verlet_node__ *_n1, struct __verlet_node__ *_n2 )
    {
        n1 = _n1;
        n2 = _n2;
        __ret_time = 1;
        double dx = n2->__x - n1->__x;
        double dy = n2->__y - n1->__y;
        double dz = n2->__z - n1->__z;
        __len = sqrt( dx*dx+dy*dy+dz*dz );
    }
    __verlet_constraint__( struct __verlet_node__ *_n1, struct __verlet_node__ *_n2, double len )
    {
        n1 = _n1;
        n2 = _n2;
        __ret_time = 1;
        __len = len;
    }

    void satisfy()
    {
        double dx = n2->__x - n1->__x;
        double dy = n2->__y - n1->__y;
        double dz = n2->__z - n1->__z;
        double h = sqrt( dx*dx+dy*dy+dz*dz);
        double scale = (__len - h) / h;
        double massRatio1 = n2->__mass / (n1->__mass+n2->__mass);
        double massRatio2 = n1->__mass / (n1->__mass+n2->__mass);

        n1->moveRel( -( dx * scale ) / __ret_time * massRatio1, -( dy * scale ) / __ret_time * massRatio1, -( dz * scale ) / __ret_time * massRatio1 );
        n2->moveRel(  ( dx * scale ) / __ret_time * massRatio2,  ( dy * scale ) / __ret_time * massRatio2,  ( dz * scale ) / __ret_time * massRatio2 );
    }
};


typedef struct __verlet_constraint__ CONSTRAINT;
typedef struct __verlet_node__ NODE;


std::vector<NODE*> Nodes;
std::vector<CONSTRAINT*> Cons;

NODE* new_node( double x, double y, double z, double mass = 1, double damp = 1 )
{
    NODE* a = new NODE( x, y, z, mass, damp );
    Nodes.push_back( a );
    return a;
}
CONSTRAINT* constrain( NODE* n1, NODE* n2 )
{
    CONSTRAINT* a = new CONSTRAINT( n1, n2 );
    Cons.push_back( a );
    return a;
}
CONSTRAINT* constrain( NODE* n1, NODE* n2, double len )
{
    CONSTRAINT* a = new CONSTRAINT( n1, n2, len );
    Cons.push_back( a );
    return a;
}