void chain( model &M, int N)
{
cout << "Creating a chain model" << endl;
// make a chain model
double m = 1.0; // mass of links
double L = 1.0; // length of each chain link
// define N, parent, pitch, Xtree, I
M.setN(N);
// deifne size
if (N == 1)
{
M.setParent( zeros<irowvec>(1) );
}
else
{
M.setParent( linspace<irowvec>( 0, N-1, N) );
}
// define vector (3D pos) argument for xtree
vec r = zeros<vec>(3);
M.setXtree( 0, xlt(r) );
// cdefine center of mass 3D pos vector
vec com = zeros<vec>(3);
com(0) = L/2.0;
// define inertia
vec inertia = m*pow(L,2)/12.0 * ones<vec>(3);
inertia(0) = 0.01; // no thickness in this direction
M.setI(0, mcI( m, com, diagmat(inertia)) );
// RIGID BODY
RBbox* box1 = new RBbox( m, L, L/5, L/5);
/* char* torus = const_cast<char*>("torus.obj");
RBobj* box1 = new RBobj( torus );
box1->normalize();
*/
M.setRB( 0, box1);
r(0) = L; // for the rest of the links
// fill in the fields
for (int i = 1; i < N; i++)
{
M.setXtree( i, xlt(r) );
M.setI( i, mcI( m, com, diagmat(inertia)) );
M.setRB( i, box1);
}
// define pitch
M.setPitch( 2*ones<rowvec>(N) );
}
void chain3DOF2( model &M, int nlinks)
{
cout << "Creating a chain3DOF2 model" << endl;
// make a chain model
double m = 0.0; // mass of links
double L = 1.0; // length of each chain link
int N = 3*nlinks;
// define N, parent, pitch, Xtree, I
M.setN(N);
// deifne size
M.setParent( linspace<irowvec>( 0, N-1, N) );
// define vector (3D pos) argument for xtree
vec com;
vec inertia;
RBbox* box1 = new RBbox( m, L, L/5, L/5);
RBnull* null1 = new RBnull();
M.setXtree(0, xlt( zeros<vec>(3) ) );
vec r(3);
r(0) = L;
for (int i = 0; i < N-2; i+=3)
{
if ( i != 0 )
{
M.setXtree(0+i, xlt( r ) );
}
M.setXtree(1+i, xlt( zeros<vec>(3) ) );
M.setXtree(2+i, xlt( zeros<vec>(3) ) );
//
m = 0.0;
com = zeros<vec>(3);
inertia = zeros<vec>(3);
M.setI(0+i, mcI( m, com, diagmat(inertia)) );
M.setI(1+i, mcI( m, com, diagmat(inertia)) );
com(0) = L/2.0; m = 1.0;
inertia = m*pow(L,2)/12.0 * ones<vec>(3); inertia(0) = 0.01;
M.setI(2+i, mcI( m, com, diagmat(inertia)) );
//
M.setRB( 0+i, null1);
M.setRB( 1+i, null1);
M.setRB( 2+i, box1);
M.setPitch(i+0, 0);
M.setPitch(i+1, 1);
M.setPitch(i+2, 2);
}
// define pitch
}
void chain3DOF( model &M)
{
cout << "Creating a chain3DOF model" << endl;
// make a chain model
double m = 0.0; // mass of links
double L = 1.0; // length of each chain link
int N = 3;
// define N, parent, pitch, Xtree, I
M.setN(N);
// deifne size
M.setParent( linspace<irowvec>( 0, N-1, N) );
// define vector (3D pos) argument for xtree
M.setXtree(0, xlt( zeros<vec>(3) ) );
M.setXtree(1, rotx( -math::pi()/2 ) );
M.setXtree(2, roty( math::pi()/2 ) );
// cdefine center of mass 3D pos vector
vec com = zeros<vec>(3);
vec inertia = zeros<vec>(3);
M.setI(0, mcI( m, com, diagmat(inertia)) );
M.setI(1, mcI( m, com, diagmat(inertia)) );
com(0) = L/2.0;
m = 1.0;
// define inertia
inertia = m*pow(L,2)/12.0 * ones<vec>(3);
inertia(0) = 0.01; // no thickness in this direction
M.setI(2, mcI( m, com, diagmat(inertia)) );
// RIGID BODY
RBbox* box1 = new RBbox( m, L, L/5, L/5);
RBnull* null1 = new RBnull();
M.setRB( 0, null1);
M.setRB( 1, null1);
M.setRB( 2, box1);
// define pitch
M.setPitch( 2*ones<rowvec>(N) );
/*
M.setXtree(0, xlt(zeros<vec>(3)));
M.setXtree(1, rotx( math::pi()/2) );
M.setXtree(2, roty( math::pi()/2) );
*/
}
void chainrot( model &M, int nlinks)
{
cout << "Creating a chainrot model" << endl;
// make a chain model
int N = nlinks + 1;
double m = 1.0; // mass of links
double L = 1.0; // length of each chain link
// define N, parent, pitch, Xtree, I
M.setN(N);
// deifne size
M.setParent( linspace<irowvec>( 0, N-1, N) );
// define vector (3D pos) argument for xtree
vec r = zeros<vec>(3);
// RIGID BODY
RBnull* null1 = new RBnull();
RBbox* box1 = new RBbox( m, L, L/6, L/6);
// cdefine center of mass 3D pos vector
vec com = zeros<vec>(3);
com(0) = L/2.0;
// define inertia
vec inertia = m*pow(L,2)/12.0 * ones<vec>(3);
inertia(0) = 0.01; // no thickness in this direction
M.setXtree( 0, xlt(r) );
M.setI(0, mcI( 0, zeros<vec>(3), diagmat(zeros<vec>(3) ) ) );
M.setRB( 0, null1);
M.setXtree( 1, xlt(r) );
M.setI(1, mcI( m, com, diagmat(inertia)) );
M.setRB( 1, box1);
r(0) = L; // for the rest of the links
// fill in the fields
for (int i = 2; i < N; i++)
{
M.setXtree( i, xlt(r) );
M.setI( i, mcI( m, com, diagmat(inertia)) );
M.setRB( i, box1);
}
// define pitch
rowvec pitch = 2*ones<rowvec>(N);
pitch(0) = 1;
M.setPitch( pitch );
}
