void Workspace::LobattoTwo(double **&vcm,double **&omega,double **&torque, double **&fcm){
int numsys = currentIndex;
int numbodies;
double time = 0.0;
int * mappings;
double SysKE =0.0;
for (int i = 0; i <= numsys; i++){
mappings = system[i].system->GetMappings();
numbodies = system[i].system->GetNumBodies() - 1;
Matrix FF(6,numbodies);
for (int j=1; j<=numbodies; j++){
FF(1,j) = torque[mappings[j - 1]-1][0]*ConFac;
FF(2,j) = torque[mappings[j - 1]-1][1]*ConFac;
FF(3,j) = torque[mappings[j - 1]-1][2]*ConFac;
FF(4,j) = fcm[mappings[j - 1]-1][0]*ConFac;
FF(5,j) = fcm[mappings[j - 1]-1][1]*ConFac;
FF(6,j) = fcm[mappings[j - 1]-1][2]*ConFac;
}
//------------------------------------//
// Get a solver and solve that system.
Solver * theSolver = Solver::GetSolver((SolverType)system[i].solver);
theSolver->SetSystem(system[i].system);
theSolver->Solve(time, FF);
ColMatrix tempv = *((*theSolver).GetStateDerivative());
ColMatrix tempa = *((*theSolver).GetStateDerivativeDerivative());
*((*theSolver).GetStateDerivative()) = tempv + Thalf*tempa;
int numjoints = system[i].system->joints.GetNumElements();
for(int k = 0; k < numjoints; k++){
system[i].system->joints(k)->ForwardKinematics();
}
for(int k = 0; k < numbodies; k++){
Vect3 temp1 =system[i].system->bodies(k+1)->r;
Vect3 temp2 =system[i].system->bodies(k+1)->v;
Vect3 temp3 =system[i].system->bodies(k+1)->omega;
SysKE = SysKE + system[i].system->bodies(k+1)->KE;
for(int m = 0; m < 3; m++){
vcm[mappings[k]-1][m] = temp2(m+1);
omega[mappings[k]-1][m] = temp3(m+1);
}
}
delete theSolver;
}
}
void Workspace::RKStep(double **&xcm, double **&vcm,double **&omega,double **&torque, double **&fcm, double **&ex_space, double **&ey_space, double **&ez_space){
double a[6];
double b[6][6];
double c[6];
//double e[6];
a[1] = 0.2;
a[2] = 0.3;
a[3] = 0.6;
a[4] = 1.0;
a[5] = 0.875;
b[1][0] = 0.2;
b[2][0] = 0.075; b[2][1] = 0.225;
b[3][0] = 0.3; b[3][1] = -0.9; b[3][2] = 1.2;
b[4][0] = -11.0/54.0; b[4][1] = 2.5; b[4][2] = -70.0/27.0; b[4][3] = 35.0/27.0;
b[5][0] = 1631.0/55296.0; b[5][1] = 175.0/512.0; b[5][2] = 575.0/13824.0; b[5][3] = 44275.0/110592.0; b[5][4] = 253.0/4096.0;
c[0] = 37.0/378.0;
c[1] = 0.0;
c[2] = 250.0/621.0;
c[3] = 125.0/594.0;
c[4] = 0.0;
c[5] = 512.0/1771.0;
int numsys = currentIndex;
int numbodies;
double time = 0.0;
int * mappings;
double SysKE =0.0;
for (int i = 0; i <= numsys; i++){
mappings = system[i].system->GetMappings();
numbodies = system[i].system->GetNumBodies() - 1;
Matrix FF(6,numbodies);
for (int j=1; j<=numbodies; j++){
FF(1,j) = ConFac*torque[mappings[j - 1]-1][0];
FF(2,j) = ConFac*torque[mappings[j - 1]-1][1];
FF(3,j) = ConFac*torque[mappings[j - 1]-1][2];
FF(4,j) = ConFac*fcm[mappings[j - 1]-1][0];
FF(5,j) = ConFac*fcm[mappings[j - 1]-1][1];
FF(6,j) = ConFac*fcm[mappings[j - 1]-1][2];
}
//------------------------------------//
// Get a solver and solve that system.
Solver * theSolver = Solver::GetSolver((SolverType)system[i].solver);
theSolver->SetSystem(system[i].system);
theSolver->Solve(time, FF);
ColMatrix initial_x;
ColMatrix initial_xdot;
ColMatMap* x;
ColMatMap* xdot;
ColMatMap* xdotdot;
x = theSolver->GetState();
xdot = theSolver->GetStateDerivative();
xdotdot=theSolver->GetStateDerivativeDerivative();
initial_x = *x;
initial_xdot = *xdot;
ColMatrix f[6];
ColMatrix ff[6];
ff[0] = initial_xdot;
f[0] = *xdotdot;
for(int ii=1;ii<6;ii++){
time = a[ii] * Tfull;
(*x) = initial_x;
(*xdot) = initial_xdot;
for(int j=0;j<ii;j++){
(*x) = (*x) + (b[ii][j]*Tfull)*ff[j];
(*xdot) = (*xdot) + (b[ii][j]*Tfull)*f[j];
}
theSolver->Solve(time,FF);
f[ii] = (*xdotdot);
ff[ii] = (*xdot);
}
(*x) = initial_x + (c[0]*Tfull)*ff[0] + (c[2]*Tfull)*ff[2] + (c[3]*Tfull)*ff[3] + (c[5]*Tfull)*ff[5];
(*xdot) = initial_xdot + (c[0]*Tfull)*f[0] + (c[2]*Tfull)*f[2] + (c[3]*Tfull)*f[3] + (c[5]*Tfull)*f[5];
int numjoints = system[i].system->joints.GetNumElements();
for(int k = 0; k < numjoints; k++){
system[i].system->joints(k)->ForwardKinematics();
}
for(int k = 0; k < numbodies; k++){
Vect3 temp1 =system[i].system->bodies(k+1)->r;
Vect3 temp2 =system[i].system->bodies(k+1)->v;
Vect3 temp3 =system[i].system->bodies(k+1)->omega;
Mat3x3 temp4 =system[i].system->bodies(k+1)->n_C_k;
SysKE = SysKE + system[i].system->bodies(k+1)->KE;
for(int m = 0; m < 3; m++){
xcm[mappings[k]-1][m] = temp1(m+1);
vcm[mappings[k]-1][m] = temp2(m+1);
omega[mappings[k]-1][m] = temp3(m+1);
ex_space[mappings[k]-1][m] = temp4(m+1,1);
ey_space[mappings[k]-1][m] = temp4(m+1,2);
ez_space[mappings[k]-1][m] = temp4(m+1,3);
}
}
delete theSolver;
}
}
