int rkqc(double *y,double *dydx, double *x, double *h, double den, double *yscal, double *hdid, double *hnext, double TOL){
double safety = 0.9;
double fcor = 0.0666666667;
double errcon = 6.0E-4;
double pgrow = -0.20;
double pshrnk = -0.25;
double xsav;
int i;
double ysav[2], dysav[2], ytemp[2];
double errmax;
double hh;
xsav = *x;
for (i=0;i<2;i++) {
ysav[i] = y[i];
dysav[i] = dydx[i];
}
do {
hh = 0.5**h;
rk4(xsav, ysav, dysav, hh, ytemp, den);
*x = xsav + hh;
derivs(*x,ytemp,dydx,den); //find derivative
rk4(*x, ytemp, dydx, hh, y, den);
*x = xsav + *h;
if (*x == xsav) {
printf("ERROR: Stepsize not significant in RKQC.\n");
return 0;
}
rk4(xsav, ysav, dysav, *h, ytemp, den);
errmax = 0.0;
for (i=0;i<2;i++) {
ytemp[i] = y[i] - ytemp[i];
errmax = max(errmax, sqrt(pow(ytemp[i]/yscal[i],2)));
}
errmax /= TOL;
if (errmax > 1.0) *h = safety**h*(pow(errmax,pshrnk)); //if integration error is too large, decrease h
} while (errmax > 1.0);
*hdid = *h;
if (errmax > errcon) {
*hnext = safety**h*(pow(errmax,pgrow));//increase step size for next step
} else {
*hnext = 4.0**h;//if integration error is very small increase step size significantly for next step
}
for (i=0;i<2;i++) {
y[i] += ytemp[i]*fcor;
}
return 0;
}
void rkqc(double *y,double *dydt,int n,double t,double htry,double eps,
double *yscal,double *hdid,double *hnext)
{
/* Quality-controlled fifth-order Runge-Kutta. */
int i;
double tsav,hh,h,temp,errmax;
double *dysav,*ysav,*ytemp;
double PGROW,PSHRNK,FCOR,SAFETY,ERRCON;
PGROW=-0.20;
PSHRNK=-0.25;
FCOR=1.0/15.0;
SAFETY=0.9;
ERRCON=6.0e-4;
dysav=malloc(n*sizeof(double));
ysav=malloc(n*sizeof(double));
ytemp=malloc(n*sizeof(double));
tsav=t;
for (i=0; i<n; i++)
{
ysav[i]=y[i];
dysav[i]=dydt[i];
}
h=htry;
for (;;)
{
hh=0.5*h;
rk4(ysav,dysav,n,hh,ytemp);
t=tsav+hh;
derivs(ytemp,dydt);
rk4(ytemp,dydt,n,hh,y);
t=tsav+h;
if (t==tsav) printf("Step size too small in RKQC");
rk4(ysav,dysav,n,h,ytemp);
errmax=0.0;
for (i=0; i<n; i++)
{
ytemp[i]=y[i]-ytemp[i];
temp=fabs(ytemp[i]/yscal[i]);
if (errmax<temp) errmax=temp;
}
errmax/=eps;
if (errmax<1.0)
{
*hdid=h;
*hnext=(errmax>ERRCON ? SAFETY*h*exp(PGROW*log(errmax)) : 4.0*h);
break;
}
h=SAFETY*h*exp(PSHRNK*log(errmax));
}
for (i=0; i<n; i++)
y[i]+=ytemp[i]*FCOR;
free(ytemp);
free(dysav);
free(ysav);
}
void rkdumb(float vstart[], int nvar, float x1, float x2, int nstep,
void (*derivs)(float, float [], float []))
{
void rk4(float y[], float dydx[], int n, float x, float h, float yout[],
void (*derivs)(float, float [], float []));
int i,k;
float x,h;
float *v,*vout,*dv;
v=vector(1,nvar);
vout=vector(1,nvar);
dv=vector(1,nvar);
for (i=1;i<=nvar;i++) {
v[i]=vstart[i];
y[i][1]=v[i];
}
xx[1]=x1;
x=x1;
h=(x2-x1)/nstep;
for (k=1;k<=nstep;k++) {
(*derivs)(x,v,dv);
rk4(v,dv,nvar,x,h,vout,derivs);
if ((float)(x+h) == x) nrerror("Step size too small in routine rkdumb");
x += h;
xx[k+1]=x;
for (i=1;i<=nvar;i++) {
v[i]=vout[i];
y[i][k+1]=v[i];
}
}
free_vector(dv,1,nvar);
free_vector(vout,1,nvar);
free_vector(v,1,nvar);
}
int main(void)
{
int i,j;
float h,x=1.0,*y,*dydx,*yout;
y=vector(1,N);
dydx=vector(1,N);
yout=vector(1,N);
y[1]=bessj0(x);
y[2]=bessj1(x);
y[3]=bessj(2,x);
y[4]=bessj(3,x);
derivs(x,y,dydx);
printf("\n%16s %5s %12s %12s %12s\n",
"Bessel function:","j0","j1","j3","j4");
for (i=1;i<=5;i++) {
h=0.2*i;
rk4(y,dydx,N,x,h,yout,derivs);
printf("\nfor a step size of: %6.2f\n",h);
printf("%12s","rk4:");
for (j=1;j<=4;j++) printf(" %12.6f",yout[j]);
printf("\n%12s %12.6f %12.6f %12.6f %12.6f\n","actual:",
bessj0(x+h),bessj1(x+h),bessj(2,x+h),bessj(3,x+h));
}
free_vector(yout,1,N);
free_vector(dydx,1,N);
free_vector(y,1,N);
return 0;
}
void deom::equilibrium (cx_cube& ddos, const double dt, const double err, const int nk) {
int it = 0;
bool converge = false;
cx_cube ddosBackup = zeros<cx_cube>(size(ddos));
FILE *frho = fopen("equilibrium.log","w");
while (it < ntMax && !converge) {
double t = it*dt;
if (it%nk == 0) {
fprintf (frho, "%16.6e", t/deom_fs2unit);
for (int i=0; i<nsys; ++i) {
fprintf(frho, "%16.6e", real(ddos(i,i,0)));
}
fprintf(frho, "\n");
// check
converge = is_converge (nddo,ddos,ddosBackup,err);
ddosBackup.slices(0,nddo-1) = ddos.slices(0,nddo-1);
}
rk4(ddos,t,dt);
it += 1;
}
if (it >= ntMax || !converge) {
printf ("Fail to reach equilibrium at error %16.6e\n", err);
} else {
printf ("Equilibrium reached with %d steps!\n", it);
}
fclose(frho);
}
void compute_temp(double *power, double *temp, int n_units, double time_elapsed)
{
int i;
double *pow, h, n_iter;
pow = vector(NL*n_units+EXTRA);
/* set power numbers for the virtual nodes */
set_internal_power(power, n_units);
/* find (inv_A)*POWER */
matvectmult(pow, inva, power, NL*n_units+EXTRA);
/* step size for 4th-order Runge-Kutta - assume worst case */
h = PRECISION / max_slope;
n_iter = time_elapsed / h;
n_iter = (n_iter > MIN_ITER) ? n_iter : MIN_ITER; /* do at least MIN_ITER iterations */
h = time_elapsed / n_iter;
if (n_iter >= TOO_LONG)
fprintf(stderr, "warning: calling interval too large, performing %.0f iterations - it may take REALLY long\n", n_iter);
/* Obtain temp at time (t+h).
* Instead of getting the temperature at t+h directly, we do it
* in n_iter steps to reduce the error due to rk4
*/
for (i = 0; i < n_iter; i++)
rk4(c, temp, pow, NL*n_units+EXTRA, h, temp);
free_vector(pow);
}
ConsistentVector Simulable::transition(double& time, double dt,
const ConsistentVector& state,
const ConsistentVector& control) const {
checkStateSize(state);
checkControlSize(control);
ConsistentVector result;
// integration dt to be considered based on passed in dt
double integration_dt = dt*integration_frequency_;
int num_steps = static_cast<int>(std::ceil(dt/integration_dt));
if (integration_dt > min_integration_dt_) {
// Since we are above the min integration point, compute how many integration steps we should
// take in order to be at least at the min dt or lower dt
num_steps = static_cast<int>(std::ceil(dt/min_integration_dt_));
integration_dt = dt/static_cast<double>(num_steps);
}
result = state;
for (int i = 0; i < num_steps; ++i) {
result = rk4(time, integration_dt, result, control);
}
return result;
}
// Programa principal
int main(){
int i,j;
FILE *ptr;
double v[2],t,dt,t_pre,t_max;
// Archivo de salida
ptr=fopen("ej3.dat","w");
dt=0.01;
t_max=20;
// Condiciones iniciales fijas y mu=0.05
v[0] = -0.01;
v[1] = 0.05;
aa.mu= 0.05;
t=0.;
while(t<t_max) {
// Integra las ecuaciones utilizando el metodo de Runge Kutta
rk4(ecuaciones,v,2,t,dt);
// Imprime la integracion
fprintf(ptr,"%lg\t%lg\t%lg\n",t,v[0],v[1]);
t+=dt;
}
fprintf(ptr,"\n");
fclose(ptr);
return(0);
}
//Programa principal
int main(){
int i,j;
FILE *ptr;
double v[2],t,dt,t_pre,t_max;
//Archivo de salida
ptr=fopen("ej1.dat","w");
dt=0.01;
t_max=2;
//Condiciones Iniciales
for (i=0;i<=10;i++) {
v[0]=0.5*i;
/*v[1]=0;*/
t=0.;
while(t<t_max){
//Integra las ecuaciones utilizando el metodo de Runge Kutta
rk4(ecuaciones,v,1,t,dt);
//Imprime la integracion
fprintf(ptr,"%lg\t%lg\n",t,v[0]);
t+=dt;
}
fprintf(ptr,"\n");
}
fclose(ptr);
return(0);
}
//Function for making the actual waveform
void wave_shoot_loud(double energy)
{
double k, y[2];
E = energy;
k = sqrt(-5.1363071e13 * E);
y[0] = exp(-k * a);
y[1] = k * y[0];
rk4(2, schroedinger, -a, a, a / 100.0, y);
}
int plant(plant_inputs_t *inputs, plant_outputs_t *outputs)
{
/* extra field needed by numerical recipes routines: */
static float plant_state[PLANT_STATE_SIZE+1] =
{
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0
};
float derivative_plant_state[PLANT_STATE_SIZE+1];
static int iteration = 0;
float time = iteration * TIME_STEP_SECONDS;
/* the plant state includes the four controller outputs,
because these are used in the calculation of derivatives: */
plant_state[19] = inputs->y19;
plant_state[20] = inputs->y20;
plant_state[21] = inputs->y21;
plant_state[22] = inputs->y22;
derivatives(time,
plant_state,
derivative_plant_state);
rk4(plant_state,
derivative_plant_state,
PLANT_STATE_SIZE,
time,
TIME_STEP_SECONDS,
plant_state,
derivatives);
outputs->y1 = plant_state[1];
outputs->y2 = plant_state[2];
outputs->y3 = plant_state[3];
outputs->y4 = plant_state[4];
outputs->y5 = plant_state[5];
outputs->y6 = plant_state[6];
outputs->y7 = plant_state[7];
outputs->y8 = plant_state[8];
outputs->y9 = plant_state[9];
outputs->y10 = plant_state[10];
outputs->y11 = plant_state[11];
printf("%.3f %.3f %.3f %.3f %.3f %.3f\n",
plant_state[6],
plant_state[7],
plant_state[8],
plant_state[9],
plant_state[10],
plant_state[11]);
iteration++;
return OK;
}
int main()
{
const double ti=0.0;
const double te=17.0652165601579625588917206249;
std::vector<double> position;
std::vector<double> velocity;
initial_condition(position, velocity);
//euler(position, velocity, ti, te);
rk4(position, velocity, ti, te);
return 0;
}
/*
initial_position: the initial configuration of the frisbee at the start of its flight
coeffs: the coefficients used in the current calculation
flight_time: the total time of flight
*/
void driver(void*initial_positionav,void*coeffsav,double flight_time,int n_times,double*all_positions){
//The input arrays came in as void* so we need to cast them back
double*initial_position = (double*)initial_positionav;
double*coeffs = (double*)coeffsav;
//Declare some iteration variables
int i,j;
//These are the the number of variables
int n_vars=12;
//Declare an array for the current positions
double current_position[n_vars];
//Calculate the time step
double dt = flight_time/n_times;
printf("\tTimestep = %e\n",dt);
//Declare the current time
double t=0;
all_positions[0]=t;
//Put in the current position as the initial position
for (i = 0; i < n_vars; ++i)
{
current_position[i] = initial_position[i];
all_positions[i+1] = current_position[i];
}
//Loop over all the times and advance the simulation
//Note: we take n_times-1 steps since we have already stored the initial information
for (i = 0; i < n_times-1; ++i)
{
rk4(current_position,dt,t,coeffs);
t+=dt;
//Store the current position and time
all_positions[(i+1)*(n_vars+1)+0]=t;
for (j = 0; j < n_vars; ++j)
{
all_positions[(i+1)*(n_vars+1)+(j+1)] = current_position[j];
}
}
//Now return the all_positions array
return;
}
void DoublePoleBalancing::stateTransition(const Action& action)
{
OPENANN_CHECK_MATRIX_BROKEN(action);
step++;
this->action = action;
double force = action(0, 0);
if(fabs(force) > maxForce)
force = force / fabs(force) * maxForce;
State s = state;
for(int i = 0; i < 2; i++)
{
State der = derivative(s, force);
s = rk4(s, force, der);
}
state = s;
normalizeState();
}
void one_step()
{
printf("provaonestep");
switch(method){
case 1:
euler();
break;
case 2:
rk2();
break;
case 3:
rk4();
break;
}
}
int main(void)
{
double *y, x, y2;
double x0 = 0, x1 = 10, dx = .1;
int i, n = 1 + (x1 - x0)/dx;
y = malloc(sizeof(double) * n);
for (y[0] = 1, i = 1; i < n; i++)
y[i] = rk4(rate, dx, x0 + dx * (i - 1), y[i-1]);
printf("x\ty\trel. err.\n------------\n");
for (i = 0; i < n; i += 10) {
x = x0 + dx * i;
y2 = pow(x * x / 4 + 1, 2);
printf("%g\t%g\t%g\n", x, y[i], y[i]/y2 - 1);
}
return 0;
}
main()
{
int M = 2;
double x[M], t, h, xold;
extern void derivs();
h = 0.005;
x[0] = 1; x[1] = 1; t = 0; // initial conditions
while (t < 2 - h)
{
xold = x[0];
rk4(t, x, derivs, h, M); // Call RK4
t = t + h;
}
printf("x1 = %f\tx2 = %f", x[0], x[1]);
}
//Remember, tout and xout must have steps+1 cells allocated!
void rkdumb(double* xinit, double* tout, double** xout, int n, double t0, double t1, int steps, void (*derivs)(double, double*, double*)) {
int i,k;
double t,h;
double *dv = malloc(n*sizeof(double));
for(i=0;i<n;i++) {
xout[0][i]=xinit[i];
}
tout[0]=t0;
t=t0;
h=(t1-t0)/steps;
for(k=0;k<steps;k++) {
(*derivs)(t,xout[k],dv);
rk4(xout[k],dv,n,t,h,xout[k+1],derivs);
if((double)(t+h) == t) {
fprintf(stderr, "Step size too small!\n");
}
t+=h;
tout[k+1]=t;
}
free(dv);
}
/* compute_temp: solve for temperature from the equation dT + CT = inv_A * Power
* Given the temperature (temp) at time t, the power dissipation per cycle during the
* last interval (time_elapsed), find the new temperature at time t+time_elapsed.
* power and temp should both be alloced using hotspot_vector
*/
void compute_temp_block(block_model_t *model, double *power, double *temp, double time_elapsed)
{
double t, h, new_h;
#if VERBOSE > 1
unsigned int i = 0;
#endif
if (!model->r_ready || !model->c_ready)
fatal("block model not ready\n");
if (temp == model->t_vector)
fatal("output same as scratch pad\n");
/* set power numbers for the virtual nodes */
set_internal_power_block(model, power);
/* use the scratch pad vector to find (inv_A)*POWER */
diagmatvectmult(model->t_vector, model->inva, power, model->n_nodes);
/* Obtain temp at time (t+time_elapsed).
* Instead of getting the temperature at t+time_elapsed directly, we do it
* in multiple steps with the correct step size at each time
* provided by rk4
*/
for (t = 0, new_h = MIN_STEP; t < time_elapsed && new_h >= MIN_STEP*DELTA; t+=h) {
h = new_h;
new_h = rk4(model, temp, model->t_vector, model->n_nodes, &h,
/* the slope function callback is typecast accordingly */
temp, (slope_fn_ptr) slope_fn_block);
new_h = MIN(new_h, time_elapsed-t-h);
#if VERBOSE > 1
i++;
#endif
}
#if VERBOSE > 1
fprintf(stdout, "no. of rk4 calls during compute_temp: %d\n", i+1);
#endif
}
/* numerical recipies */
void rkdump(double vstart[], int nvar, double t1, double t2, int nstep, void (*derivs)(double, double[], double[]))
{
int i,k;
double x,h;
double v[nvar], vout[nvar], dv[nvar];
for (i=0;i<nvar;i++) {
v[i] = vstart[i];
}
x = t1;
h = (t2-t1)/nstep;
for (k=0;k<nstep;k++) {
(*derivs)(x,v,dv);
rk4(v,dv,nvar,x,h,vout,derivs);
if ((double)(x+h) == x) printf("Step size too small in routine rkdump\n");
x += h;
for (i=0;i<nvar;i++) {
v[i] = vout[i];
}
solution[0][k] = v[0];
solution[1][k] = v[1];
}
}
// Programa principal
int main(){
int i,j;
FILE *ptr;
double v[2],t,dt,t_pre,t_max;
// Archivo de salida
ptr=fopen("ej2.dat","w");
dt=0.01;
t_max=2;
// Condiciones iniciales. Se analizaron 3 casos: sub, sobre y amortiguamiento critico variando gamma y omega con valores opuestos en el rango de 1 a 10, con pasos de 5
for (j=1;j<=10;j=j+4) {
v[0] = 0.5*j;
v[1] = 0.5*(10-j);
aa.gamma=(10-j);
aa.omega=j;
t=0.;
while(t<t_max) {
// Integra las ecuaciones utilizando el metodo de Runge Kutta
rk4(ecuaciones,v,2,t,dt);
// Imprime la integracion
fprintf(ptr,"%lg\t%lg\t%lg\n",t,v[0],v[1]);
t+=dt;
}
fprintf(ptr,"\n");
}
fclose(ptr);
return(0);
}
int
main()
{
double step_size = 0.1; /* integration step */
double x = 1.2; /* initial condition */
int sample_n = 12000; /* total no. of samples, excluding the given
initial condition */
int tau = 17; /* delay constant */
int interval = 10; /* output is printed at every 10 time steps */
double x_tau; /* x(t-tau) */
double x_next;
double time = 0;
int i, index = 0;
double *x_history; /* array to store delay information */
int history_length;
history_length = (int)(tau/step_size);
x_history = (double *)calloc(history_length, sizeof(double));
if (history_length != 0) {
x_history = (double *)calloc(history_length, sizeof(double));
for (i = 0; i < history_length; i++)
x_history[i] = 0;
}
for (i = 0; i <= sample_n; i++) {
if (i%interval == 0) printf("%4d %f\n", i/interval, x);
x_tau = tau == 0 ? x:x_history[index];
x_next = rk4(x, time, step_size, x_tau);
if (tau != 0) {
x_history[index] = x_next;
index = (index + 1)%history_length;
}
time += step_size;
x = x_next;
}
return(0);
}
void StreamLineTracer::step(int steps, dvec3 curPos, IntegralLine &line,bool fwd) {
for (int i = 0; i <= steps; i++) {
if (!volumeSampler_->withinBounds(curPos)) {
break;
}
dvec3 v;
switch (integrationScheme_)
{
case IntegralLineProperties::IntegrationScheme::RK4:
v = rk4(curPos, invBasis_ , fwd);
break;
case IntegralLineProperties::IntegrationScheme::Euler:
default:
v = euler(curPos);
break;
}
if (glm::length(v) == 0) {
break;
}
dvec3 worldVelocty = volumeSampler_->sample(curPos).xyz();
if (normalizeSample_) v = glm::normalize(v);
dvec3 velocity = invBasis_ * (v * stepSize_ * (fwd ? 1.0 : -1.0));
line.positions_.push_back(curPos);
line.metaData_["velocity"].push_back(worldVelocty);
for (auto &m : metaSamplers_) {
line.metaData_[m.first].push_back(m.second->sample(curPos));
}
curPos += velocity;
}
}
// ===================================================================
// Function which simulates the engine.
void engine(struct data *pdat){
FILE *fp1, *fp2;
char b, u;
char COMBUSTION, COMPRESSION, EXPANSION;
int cycl, logical;
double xb, dxb;
double angle, sangle;
double Af, Ut, lt, Sl, hc;
double Vf, Vu, dVu, Vb, dVb;
double densi, denstot, densu ,densb;
double me, dme, mu, dmu, mb, dmb, MBtot;
double Temp, Pres, Temp1, Pres1, Temp2, Pres2, Temp3, Pres3;
double Tempu, Presu, Tempb, Presb, \
Tempu1, Presu1, Tempb1, Presb1, Tempu2, Tempb2, Presb2, Presu2;
double dTempu, dPresu, dTempb, dPresb, dTempu1, dPresu1, dTempb1, dPresb1;
double mu_p, mb_p, Vu_p, Vb_p;
cycl=0;
xb=dxb=0.;
sangle=pi/180.;
Af=Ut=lt=Sl=hc=0.;
Vf=Vu=dVu=Vb=dVb=0.;
densi=denstot=densu=densb=0.;
MBtot=me=dme=mu=dmu=mb=dmb=0.;
Temp=Pres=Temp1=Pres1=Temp2=Pres2=Temp3=Pres3=0.;
Tempu=Presu=Tempb=Presb=Tempu1=Presu1=Tempb1=Presb1=Tempu2=Presu2=Tempb2=Presb2=0.;
dTempu=dPresu=dTempb=dPresb=dTempu1=dPresu1=dTempb1=dPresb1=0.;
mu_p = mb_p = Vu_p = Vb_p = 0.0;
if (((fp1=fopen("results1.dat","w")) == NULL)){
printf("Programm couldn’t open the file.\n");
exit(1);
}
if (((fp2=fopen("results2.dat","w")) == NULL)){
printf("Programm couldn’t open the file.\n");
exit(1);
}
{
fprintf(fp1,"Results1.dat:\n");
fprintf(fp2,"Results2.dat:\n");
for(cycl=1;cycl<=(*pdat).ncycl;cycl++){
//
//Beginning of Intake Process
printf("In loop\n");
densi=density((*pdat).atmotemp,(*pdat).atmopres,MBr);
if(cycl == 1){
Pres=(*pdat).atmopres-1.e-5*.5*densi*pow((*pdat).Ui,2.)*(pow((*pdat).Ap,2.)-pow((*pdat).Av,2.))/pow((*pdat).Ap,2.);
Temp=(*pdat).atmotemp;
printf(".");
fprintf(fp1,"%d \t%lf %lf %lf \n",cycl,270.,Pres,Temp);
}else{
Pres=(*pdat).atmopres-1.e-5*.5*densi*pow((*pdat).Ui,2.)*(pow((*pdat).Ap,2.)-pow((*pdat).Av,2.))/pow((*pdat).Ap,2.);
Temp=(1./3.)*((*pdat).Tw + 2*Temp1);
printf(".");
fprintf(fp1,"%d \t%lf %lf %lf \n",cycl,270.,Pres,Temp);
}
Vu=(*pdat).Vc+.25*pi*(*pdat).B*(*pdat).B*(*pdat).L;
mu=density(Temp,Pres,MBr)*Vu;
//
//
//Beginning of compression Process
angle=pi;
sangle=pi/180.;
Temp1=Pres1=Temp2=Pres2=0.;
while(angle<2*pi-(*pdat).sacomb-2.*pi/180.){
logical=1;
while(logical == 1){
Pres1=Pres;Temp1=Temp;
Pres2=Pres;Temp2=Temp;
Vu=volume(angle,pdat);
check(&Vu);
rk4("COMPRESSION","u",cycl,sangle,angle,Pres,Temp,&Pres1,&Temp1,Vu,mu,pdat);
// BYPASS PRESSURE CALCULATION-CALCULATE USING IDEAL GAS LAW
Pres1 = (mu/Vu) * ( Rgas*Temp1/MBr ) *1.e-5;
rk4("COMPRESSION","u",cycl,.5*sangle,angle,Pres,Temp,&Pres2,&Temp2,Vu,mu,pdat);
Pres2 = (mu/Vu) * ( Rgas*Temp2/MBr ) *1.e-5;
Vu=volume(angle+.5*sangle,pdat) ;
check(&Vu);
rk4("COMPRESSION","u",cycl,.5*sangle,angle+.5*sangle,Pres,Temp,&Pres3,&Temp3, \
Vu,mu,pdat);
Pres3 = (mu/Vu) * ( Rgas*Temp3/MBr ) *1.e-5;
if((error(Pres1,Pres3)<=tol)&&(error(Temp1,Temp3)<=tol)){
Pres=Pres1;Temp=Temp1;
printf(".");
fprintf(fp1,"%d %lf %lf %lf %lf \n",cycl,180.*sangle/pi,180.*angle/pi,Pres,Temp);
Temp1=Pres1=Temp2=Pres2=Temp3=Pres3=0.;
angle += sangle;
logical=0;
}else{
sangle=.5*sangle;
if(sangle<1.e-6){
Pres=Pres1;Temp=Temp1;
Temp1=Pres1=Temp2=Pres2=Temp3=Pres3=0.;
angle += sangle;
logical=0;
}
}
}
}
//
//
/*Still in Compression, 1 deg before the start of Combustion
Reduce step to 1/10 of what it is
Save values at time (n-1) in order to use in combustion
*/
sangle=.1*sangle;
while(angle<2.*pi-(*pdat).sacomb){
Pres1=Pres;Temp1=Temp;
Vu=volume(angle,pdat);
check(&Vu);
rk4("COMPRESSION","u",cycl,sangle,angle,Pres,Temp,&Pres1,&Temp1,Vu,mu,pdat);
//BYPASS PRESSURE CALCULATION-CALCULATE USING IDEAL GAS LAW
Pres1 = (mu/Vu) * ( Rgas*Temp1/MBr ) *1.e-5;
Temp2=Pres2=0.;
Pres2=Pres;Temp2=Temp;
Pres=Pres1;Temp=Temp1;
Pres1=Pres2;Temp1=Temp2;
printf(".");
fprintf(fp1,"%d %lf %lf %lf %lf \n",cycl,180.*sangle/pi,180.*angle/pi,Pres,Temp);
mu_p = mu;
Vu_p = Vu;
angle += sangle;
}
//Beginning of Combustion Process
Vu = volume(angle,pdat);
densu=mu/Vu;
check(&densu);
Presu=Pres;Presu1=Pres1;
Tempu=Temp;Tempu1=Temp1;
while(angle<2.*pi+(*pdat).facomb){
Wiebe(angle,2.*pi-(*pdat).sacomb,2.*pi+(*pdat).facomb,&xb,&dxb);
//CAREFUL!! INCONSISTENCY BETWEEN VOLUMES
//SWITCH SUDDENLY BETWEEN Vc+sthing small TO FRACTION OF Vc
Vb=xb*volume(angle,pdat);
check(&Vb);
dVb=dxb*volume(angle,pdat)+xb*(*pdat).Vc+.25*pi*(*pdat).B*(*pdat).B*.5*(*pdat).L*(sin(angle)+(*pdat).R*sin(angle)*cos(angle));
check(&dVb);
Vu=(1-xb)*volume(angle,pdat);
check(&Vb);
dVu=-dVb;
check(&dVu);
if((mb<=1.e-9)&&(Vb<=1.e-10)){ densb=1.e-10;}
else{ if(mb<=1.e-9){
densb=1.e-10;}
else{
densb=mb/Vb;
check(&densb);}
}
Vf=Vb+(me-mb)/densu;
if (Vf<=0.0){Vf = 1.e-10;}
check(&Vf);
hc=Vf/(pi*pow((*pdat).B,2.)/4.);
check(&hc);
Af=.25*pi*pow(.5*hc,2.);
check(&Af);
Ut=.08*(*pdat).Ui*sqrt(densu/densi);
check(&Ut);
lt=.8*(*pdat).Liv*pow(densi/densu,3./4.);
check(<);
Sl=(*pdat).B*(*pdat).omega/( (*pdat).facomb+(*pdat).sacomb );
check(&Sl);
dme=densu*Af*(Ut+Sl)*(1./(*pdat).omega);
check(&dme);
me += sangle*dme;
check(&me);
dmb=(densu*Af*Sl+Sl*(me-mb)/lt)*(1./(*pdat).omega); check(&dmb);
mb += sangle*dmb;
check(&mb);
dmu=-dmb;
check(&dmu);
mu += sangle*dmu;
check(&mu);
dTempb=funcf1("COMBUSTION","b",cycl,angle,\
Tempb,Tempb1,Tempu,Presb,Presb1, \
Vu,Vb,Vu_p,Vb_p,dVu,dVb,mu,mb,mu_p,mb_p,dmu,dmb,pdat);
Tempb2 += sangle*dTempb;
dPresb=funcf2("COMBUSTION","b",cycl,angle,\
Tempb,Tempb1,Tempu,Presb,Presb1, \
Vu,Vb,Vu_p,Vb_p,dVu,dVb,mu,mb,mu_p,mb_p,dmu,dmb,pdat);
Presb2 += sangle*dPresb;
//Bypass Pressure calculation-Use ideal gas law
if (mb<=1.e-5) {Presb2 = 1.e-10;}
//Bypass Pressure calculation-Use ideal gas law
if (mb<=1.e-5) {Presb2 = 1.e-10;}
else
{Presb2 = (mb/Vb) * (Rgas/MBp) * Tempb2 *1.e-5;}
dTempu=funcf1("COMBUSTION","u",cycl,angle, \
Tempu,Tempu1,0.,Presu,Presu1, \
Vu,Vb,Vu_p,Vb_p,dVu,dVb,mu,mb,mu_p,mb_p,dmu,dmb,pdat);
Tempu2 += sangle*dTempu;
dPresu=funcf2("COMBUSTION","u",cycl,angle, \
Tempu,Tempu1,0.,Presu,Presu1, \
Vu,Vb,Vu_p,Vb_p,dVu,dVb,mu,mb,mu_p,mb_p,dmu,dmb,pdat);
Presu2 += sangle*dPresu;
//Bypass Pressure calculation-Use ideal gas law
if (mu<=1.e-5) {Presu2 = 1.e-10;}
else
Presu2 = (mu/Vu) * (Rgas/MBr) * Tempu2 *1.e-5;
Pres=Presu2+Presb2;
MBtot=xb*MBp+(1.-xb)*MBr;
denstot=xb*densb+(1.-xb)*densu;
Temp=(Pres*MBtot)/(denstot*Rgas);
printf(".");
fprintf(fp1,"%d %lf %lf %lf %lf \t %lf %lf %lf %lf \n" \
,cycl,180.*sangle/pi,180.*angle/pi,Pres,Temp,Presu2,Tempu2,Presb2,Tempb2);
fprintf(fp2,"%d %lf %lf %lf %lf \n" \
,cycl,180.*sangle/pi,180.*angle/pi,xb,1.-xb);
Presu1=Presu;Presb1=Presb;
Tempu1=Tempu;Presb1=Tempb;
Presu=Presu2;Presb=Presb2;
Tempu=Tempu2;Presb=Tempb2;
if((mu<=1.e-10)&&(Vu<=1.e-10)){ densu=1.e-10;}
//Added conditions
else{ if(mu<=1.e-10){
densu=1.e-10;}
else{
densu=mu/Vu;
check(&densu);}
}
//Allocate mass-volume previous values
mu_p = mu;
Vu_p = Vu;
mb_p = mb;
Vb_p = Vb;
angle += sangle;
}
// Beginning of Expansion Process
sangle=pi/180.;
Temp1=Pres1=Temp2=Pres2=0.;
while(angle<=3*pi){
logical=1;
while(logical == 1){
Pres1=Pres;Temp1=Temp;
Pres2=Pres;Temp2=Temp;
rk4("EXPANSION","b",cycl,sangle,angle \
,Pres,Temp,&Pres1,&Temp1,Vb,mb,pdat);
rk4("EXPANSION","b",cycl,.5*sangle,angle \
,Pres,Temp,&Pres2,&Temp2,Vb,mb,pdat);
Vb=volume(angle+.5*sangle,pdat);
check(&Vu);
rk4("EXPANSION","b",cycl,.5*sangle,angle+.5*sangle \
,Pres2,Temp2,&Pres3,&Temp3,Vb,mb,pdat);
if((error(Pres1,Pres3)<=tol)&&(error(Temp1,Temp3)<=tol)){
Pres=Pres1;Temp=Temp1;
printf(".");
fprintf(fp1,"%d %lf %lf %lf \n",cycl,180.*angle/pi,Pres,Temp);
Pres1=Temp1=Pres2=Temp2=Pres3=Temp3=0.;
angle += sangle;
logical=0;
}else{
sangle=.5*sangle;
}
}
}
// Beginning of Exhaust Process
Pres1=1.15*(*pdat).atmopres;
Temp1=Temp / pow((Pres/Pres1),(1./3.));
Pres=Pres1;Temp=Temp1;
printf(".");
fprintf(fp1,"%d \t%lf %lf %lf",cycl,180.*angle/pi,Pres,Temp);
fprintf(fp1,"\n");
fprintf(fp2,"\n");
}
}
fclose(fp1);
fclose(fp2);
}
int main(void){
initGLFW();
initOpenGL();
mcs = createFloppyThing();
cam = new Camera(window, mcs);
matrices = new MatrixHandler(cam);
// Create and compile the shader
programID = LoadShaders( "../../data/shaders/simple.vert", "../../data/shaders/simple.frag" );
textureID = loadBMP_custom("../../data/textures/empty.bmp");
faces_textureID = loadBMP_custom("../../data/textures/empty1.bmp");
waffle_textureID = loadBMP_custom("../../data/textures/waffle.bmp");
cloth1_textureID = loadBMP_custom("../../data/textures/cloth1.bmp");
cloth2_textureID = loadBMP_custom("../../data/textures/cloth2.bmp");
cloth3_textureID = loadBMP_custom("../../data/textures/cloth3.bmp");
cloth4_textureID = loadBMP_custom("../../data/textures/cloth4.bmp");
cloth5_textureID = loadBMP_custom("../../data/textures/cloth5.bmp");
slime_textureID = loadBMP_custom("../../data/textures/slime3.bmp");
floor2_textureID = loadBMP_custom("../../data/textures/floor2.bmp");
floor3_textureID = loadBMP_custom("../../data/textures/floor3.bmp");
die_textureID = loadBMP_custom("../../data/textures/die.bmp");
induction_textureID = loadBMP_custom("../../data/textures/induction.bmp");
tiling_textureID = loadBMP_custom("../../data/textures/tiling.bmp");
wall1_textureID = loadBMP_custom("../../data/textures/wall1.bmp");
wall2_textureID = loadBMP_custom("../../data/textures/wall2.bmp");
wall3_textureID = loadBMP_custom("../../data/textures/wall3.bmp");
// INIT SIMULATION
int simulations_per_frame = 12;
float dt = 1.0f/(60.0f*simulations_per_frame);
std::vector<float> w;
w.push_back(1.0f);
w.push_back(3.0f);
w.push_back(3.0f);
w.push_back(1.0f);
RungeKutta rk4(w);
EulerExplicit ee;
NumericalMethod * nm = ⅇ //Make use of polymorphism
float current_time = glfwGetTime();;
int FPS = 0;
//OpenGL_Drawer //od;
////od.add(mcs);
ground_material = new Material;
object_material = new Material;
object_material->wetness = 0.1f;
drawable_mcs = new OpenGL_drawable(mcs, *object_material, programID, cloth1_textureID);
for (int i=0; i<mcs->collisionPlanes.size(); ++i) {
drawable_planes.push_back(new OpenGL_drawable(&mcs->collisionPlanes[i], *ground_material, programID, floor3_textureID));
}
int frame = 0;
std::cout << "�" << std::endl;
while (!glfwWindowShouldClose(window)){
// Moving one mass
//double x_mouse, y_mouse;
//glfwGetCursorPos(window, &x_mouse, &y_mouse);
//glm::vec2 pos2d = glm::vec2(float(x_mouse-0.5*width)*2*scale/height, -float(y_mouse-0.5*height)*2*scale/height);
//mcs->setAvgPosition(glm::vec3(pos2d[0],pos2d[1],-50));
//mcs->vertices.positions[0] = glm::vec3(pos2d[0],pos2d[1],-50);
//mcs->particles.velocities[0] = glm::vec3(0);
if(!pause){
for (int i = 0; i < simulations_per_frame; ++i){
nm->update(*mcs,dt);
}
}
else{
for (int i = 0; i < simulations_per_frame; ++i){
if(forward) nm->update(*mcs,dt/10.0f);
if(backward) nm->update(*mcs,-dt/10.0f);
}
}
mcs->updateVertexPositions();
mcs->updateNormals();
// DRAW
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glfwGetFramebufferSize(window, &width, &height);
//od.ratio = width / (float) height;
//if(!od.draw()) break;
drawable_mcs->updateRuntimeBuffers(mcs, matrices);
/*
for (int i=0; i<mcs->collisionPlanes.size(); ++i) {
drawable_planes[i].draw();
}
*/
//drawable_plane->draw();
for (int i=0; i<mcs->collisionPlanes.size(); ++i) {
drawable_planes[i]->draw();
}
drawable_mcs->draw();
//Swap draw buffers
glfwSwapBuffers(window);
glfwPollEvents();
// Print FPS
++FPS;
++frame;
if((glfwGetTime() - current_time) > 1){
std::string title = "Elastic materials, ";
std::ostringstream ss;
ss << FPS;
std::string s(ss.str());
title.append(s);
title.append(" FPS");
glfwSetWindowTitle (window, title.c_str());
FPS = 0;
current_time = glfwGetTime();
}
}
delete mcs;
delete cam;
delete drawable_mcs;
for (int i = 0; i<drawable_planes.size(); ++i) {
delete drawable_planes[i];
}
cleanUpGLFW();
}
void run()
{
int j,k;
double Rblob,temp,lambdaM;
fprintf(comp_log,"Time N Split Merge MPlev lambdaM\n");
while (SimTime < EndTime)
{
tot_cputime_ref = clock();
numk2 = 0;
rk4(TimeStep);
SimTime += TimeStep;
for (j=0; j<N; ++j)
if (blobguts[j].a2 < 0.0)
{
fprintf(diag_log,
"Time: %12.4e. WARNING: Negative a2 in element %d\n",
SimTime,j);
for (k=0; k<N; ++k)
fprintf(diag_log,
"str=%10.3e x=%10.3e y=%10.3e s2=%10.3e a2=%10.3e\n",
mblob[k].blob0.strength,
mblob[k].blob0.x,mblob[k].blob0.y,
blobguts[k].s2,blobguts[k].a2);
exit(0);
}
/* Constrain the bdy conditions */
/*
if ( (BoundaryStep > 0.0) &&
((SimTime+0.499*TimeStep) >= BoundaryStep*BoundaryFrame) )
{
++BoundaryFrame;
BoundaryConstrain();
}
*/
if ( (InterpStep > 0.0) &&
((SimTime+0.499*TimeStep) >= InterpStep*Interps) )
{
RHE_interp2(InterpVar,InterpPopulationControl);
++Interps;
}
if ((SimTime+0.499*TimeStep) >= FrameStep*Frame)
{
if (write_vel) write_vels(Frame);
#ifdef MULTIPROC
/* Have only the 'root' node do the writes */
MPI_Comm_rank(MPI_COMM_WORLD, &rank);
if (rank == 0)
{
if (write_vtx) write_vorts(Frame);
if (write_partitions) write_partition(Frame);
}
#else
/* single processor */
if (write_vel) write_vels(Frame);
if (write_partitions) write_partition(Frame);
#endif
++Frame;
Rblob = 0.0;
lambdaM = 0.0;
for (j=0; j<N; ++j)
{
temp = 2.0*
(tmpparms[j].du11*(tmpparms[j].cos2-tmpparms[j].sin2)+
(tmpparms[j].du12+tmpparms[j].du21)*tmpparms[j].sincos);
if (lambdaM < temp)
lambdaM = temp;
temp = 2.0*
(tmpparms[j].du11*(tmpparms[j].cos2-tmpparms[j].sin2)-
(tmpparms[j].du12+tmpparms[j].du21)*tmpparms[j].sincos);
if (lambdaM < temp)
lambdaM = temp;
}
fprintf(comp_log,"%8.2e %-5d %-5d %-5d %-5d %8.2e\n",
SimTime,N,totsplit+nsplit,totmerge+nmerge,
mplevels,lambdaM);
fflush(comp_log);
fflush(diag_log);
fflush(mpi_log);
}
} /* end while loop */
fprintf(comp_log,"Simulation complete.\n");
fprintf(comp_log,"Total splitting events: %d\n",totsplit);
fprintf(comp_log,"Total merging events: %d\n",totmerge);
fclose(cpu_log);
}
double loop_simulation(Loop_inputs *loop_inputs)
{
// -- Variables decalration -- //
int i,k;
int nvar, nstep;
int simu_go;
double x, h;
double x1, x2;
double *ystart;
double *v,*vout,*dv;
LocalDataStruct *lds;
MBSdataStruct *MBSdata;
#ifdef WRITE_FILES
Write_files *write_files;
#endif
#if defined(JNI) & defined (REAL_TIME)
JNI_struct* jni_struct;
#endif
#ifdef REAL_TIME
int cur_t_usec;
int init_t_sec, init_t_usec;
// simulation time
double tsim;
// real-time (in us) vs simulation (in s) variables
int speed_last_t_usec, speed_new_t_usec;
double speed_last_tsim, speed_new_tsim;
// Real-time constraints
Real_time_constraint **constraints;
// Real-time constraints main structure
Simu_real_time *real_time;
#endif
#if defined(SDL) & defined(REAL_TIME)
double y_vec[NB_CURVES_MAX];
Screen_sdl *screen_sdl;
#endif
// -- Variables initialization -- //
nvar = loop_inputs->nvar;
nstep = loop_inputs->nstep;
x1 = loop_inputs->x1;
x2 = loop_inputs->x2;
ystart = loop_inputs->ystart;
lds = loop_inputs->lds;
MBSdata = loop_inputs->MBSdata;
v = dvector(1,nvar);
vout = dvector(1,nvar);
dv = dvector(1,nvar);
// Load starting values
for (i=1; i <= nvar; i++)
{
v[i] = ystart[i];
}
x = x1;
h = (x2 - x1) / nstep; // [s]
#ifdef WRITE_FILES
write_files = loop_inputs->write_files;
#endif
#if defined(JNI) & defined (REAL_TIME)
jni_struct = loop_inputs->jni_struct;
#endif
#ifdef REAL_TIME
// simulation time
tsim = TSIM_INIT;
init_t_sec = loop_inputs->init_t_sec;
init_t_usec = loop_inputs->init_t_usec;
// current eral-time [us]
cur_t_usec = t_usec(init_t_sec, init_t_usec);
// real-time variables
speed_last_t_usec = cur_t_usec;
speed_new_t_usec = cur_t_usec;
speed_last_tsim = TSIM_INIT;
speed_new_tsim = TSIM_INIT;
// structures
real_time = loop_inputs->real_time;
constraints = real_time->constraints;
#endif
#if defined(SDL) & defined(REAL_TIME)
// strcuture
screen_sdl = loop_inputs->screen_sdl;
// first plot
plot_screen_sdl(screen_sdl, real_time, tsim, 2);
#endif
// flag : 0 if the simulation must be stopped (1 otherwise)
simu_go = 1;
// Simulation over nstep steps: loop
for (k = 1; (k <= nstep) && simu_go; k++)
{
// user own functions (controller_files and simulation_files)
user_compute_output(MBSdata,lds);
// .anim
#ifdef WRITE_FILES
update_write_files(write_files, MBSdata);
#endif
// SDL window
#if defined(SDL) & defined(REAL_TIME)
// assign values for the plot
get_screen_sdl_functions(y_vec, MBSdata);
// update simulation vectors for the plot
update_full_vectors(screen_sdl, tsim, y_vec);
#endif
#ifdef REAL_TIME
// check actions related to the Real-time constraints
for (i=0; i<NB_REAL_TIME_CONSTRAINTS; i++)
{
if (tsim >= constraints[i]->next_tsim)
{
// plot screen sdl
#if defined(SDL) & defined (REAL_TIME)
if (!i)
{
update_plot_vectors(screen_sdl, real_time, tsim, y_vec);
plot_screen_sdl(screen_sdl, real_time, tsim, 0);
}
#endif
// JNI visualization
#if defined(JNI) & defined (REAL_TIME)
if (i == 1)
{
update_jni(jni_struct, MBSdata, real_time);
}
#endif
// new real-time constraints
update_real_time_constraint(constraints[i], real_time->simu_speed_flag);
}
}
// handle events (coming from the keyboard...)
#if defined (SDL) & defined(JNI) & defined (REAL_TIME)
events_simu(screen_sdl, real_time, MBSdata, &simu_go, &speed_last_t_usec, init_t_sec, init_t_usec, tsim, jni_struct);
#elif defined (SDL) & defined (REAL_TIME)
events_simu(screen_sdl, real_time, MBSdata, &simu_go, &speed_last_t_usec, init_t_sec, init_t_usec, tsim);
#endif
// gate locked: waiting time
if (tsim >= real_time->next_tsim_gate)
{
// current time
cur_t_usec = t_usec(init_t_sec, init_t_usec);
// loop in order to wait to respect the real-time constraints
while (real_time->next_t_usec_gate > cur_t_usec)
{
// current time
cur_t_usec = t_usec(init_t_sec, init_t_usec);
// handle events (coming from the keyboard...)
#if defined (SDL) & defined(JNI) & defined (REAL_TIME)
events_simu(screen_sdl, real_time, MBSdata, &simu_go, &speed_last_t_usec, init_t_sec, init_t_usec, tsim, jni_struct);
#elif defined (SDL) & defined (REAL_TIME)
events_simu(screen_sdl, real_time, MBSdata, &simu_go, &speed_last_t_usec, init_t_sec, init_t_usec, tsim);
#endif
}
// in case there is no additional constraint
if (real_time->no_additional_constraint)
{
update_real_time_constraint(real_time->constraints[0], real_time->simu_speed_flag);
}
// update real-time strcuture and related variables
update_simu_real_time(real_time);
}
// computes the real time speed factor of the simulation (every REAL_TIME_SPEED_PERIOD_USEC s)
if (cur_t_usec - speed_last_t_usec > REAL_TIME_SPEED_PERIOD_USEC)
{
speed_new_t_usec = cur_t_usec;
speed_new_tsim = tsim;
real_time->real_simu_speed_factor = (speed_new_tsim - speed_last_tsim) / (1.0e-6 * (speed_new_t_usec - speed_last_t_usec));
// change the plot
#if defined(SDL) & defined (REAL_TIME)
screen_sdl->bottom_flag = 1;
#endif
speed_last_t_usec = speed_new_t_usec;
speed_last_tsim = speed_new_tsim;
}
#endif
/*
* Simbody external forces
*/
#ifdef SIMBODY
// kinematics from Robotran
update_simbody_kinematics(MBSdata->user_IO->simbodyBodies, MBSdata);
// Simbody functions
loop_Simbody(p_simbodyVariables, MBSdata->user_IO->simbodyBodies);
#endif
/*
* Main routine of the integrator.
*
* Starting from initial values ystart[1..nvar] known at x1 use fourth-order Runge-Kutta
* to advance nstep equal increments h to x2. The user-supplied routine derivs(x,v,dvdx)
* evaluates derivatives. Results are stored in the global variables y[1..nvar][1..nstep+1]
* and xx[1..nstep+1].
*/
(*derivs)(x,v,dv,lds,MBSdata);
rk4(v,dv,nvar,x,h,vout,derivs,lds,MBSdata);
if ((double)(x+h) == x)
{
nrerror("Step size too small in routine odeint");
}
x += h;
for (i=1;i<=nvar;i++)
{
v[i] = vout[i];
}
/*
* Real-time update
*/
#ifdef REAL_TIME
// update simulation time
tsim = MBSdata->tsim;
// update real time
cur_t_usec = t_usec(init_t_sec, init_t_usec);
#endif
// stop the simulation if 'stop_out == 1'
#ifdef FLAG_STOP
if (MBSdata->user_IO->stop_out)
{
simu_go = 0;
}
#endif
}
// -- Free memory -- //
free_dvector(dv,1,nvar);
free_dvector(vout,1,nvar);
free_dvector(v,1,nvar);
return MBSdata->user_IO->fitness_opti;
}
void Physics::step( real_t dt )
{
std::vector< SphereBody* >::iterator it;
std::vector< SphereBody* >::iterator it2;
std::vector< PlaneBody* >::iterator pt;
std::vector< TriangleBody* >::iterator tt;
for (it = spheres.begin(); it != spheres.end(); ++it)
{
SphereBody *sb = *it;
for (tt = triangles.begin(); tt != triangles.end(); ++tt)
{
if (collides(*sb, **tt, collision_damping))
{
Vector3 n =
normalize(
cross(
(*tt)->vertices[0] - (*tt)->vertices[1],
(*tt)->vertices[1] - (*tt)->vertices[2]
)
);
Vector3 u = sb->velocity - 2 * dot(sb->velocity, n) * n;
sb->velocity = u - (collision_damping * u);
if (squared_length(sb->velocity) <= 1.0f)
sb->velocity = Vector3::Zero();
}
}
for (it2 = spheres.begin(); it2 != spheres.end(); ++it2)
{
if (collides(**it, **it2, collision_damping) && *it != *it2)
{
SphereBody *sb2 = *it2;
Vector3 v1 = sb->velocity - sb2->velocity;
Vector3 d = ((sb2->position - sb->position)
/ distance(sb->position, sb2->position));
Vector3 v22 = 2 * d
* (sb->mass / (sb->mass + sb2->mass))
* (dot(v1, d));
Vector3 u2 = sb2->velocity + v22;
Vector3 u1 =
((sb->mass * sb->velocity)
+ (sb2->mass * sb2->velocity)
- (sb2->mass * u2))
/ sb->mass;
sb->velocity = u1 - collision_damping * u1;
sb2->velocity = u2 - collision_damping * u2;
if (squared_length(sb->velocity) <= 1.0f)
sb->velocity = Vector3::Zero();
if (squared_length(sb2->velocity) <= 1.0f)
sb2->velocity = Vector3::Zero();
}
}
for (pt = planes.begin(); pt != planes.end(); ++pt)
{
if (collides(*sb, **pt, collision_damping))
{
Vector3 u = sb->velocity - 2 * dot(sb->velocity, (*pt)->normal) * (*pt)->normal;
sb->velocity = u - collision_damping * u;
if (squared_length(sb->velocity) <= 1.0f)
sb->velocity = Vector3::Zero();
}
}
sb->force = Vector3::Zero();
for (SpringList::iterator si = springs.begin(); si != springs.end(); ++si) {
(*si)->step(dt);
}
sb->apply_force(gravity, Vector3::Zero());
// si pas de force et velocite, ne pas faire les tests
State initMove = {sb->position, sb->velocity, sb->force / sb->mass};
State s = rk4(dt, initMove);
sb->position += s.x;
sb->sphere->position = sb->position;
sb->velocity += s.v;
real_t i = (2.0f / 5.0f) * sb->mass * (sb->radius * sb->radius);
Vector3 angular_accel = sb->torque / i;
State initRot = {Vector3::Zero(), sb->angular_velocity, angular_accel};
s = rk4(dt, initRot);
if (s.x != Vector3::Zero())
{
Quaternion q(s.x, length(s.x));
Quaternion qq = q * sb->orientation;
sb->orientation = qq;
sb->sphere->orientation = sb->orientation;
}
}
}
void Ukf(VEC *omega, VEC *mag_vec, VEC *mag_vec_I, VEC *sun_vec, VEC *sun_vec_I, VEC *Torq_ext, double t, double h, int eclipse, VEC *state, VEC *st_error, VEC *residual, int *P_flag, double sim_time)
{
static VEC *omega_prev = VNULL, *mag_vec_prev = VNULL, *sun_vec_prev = VNULL, *q_s_c = VNULL, *x_prev = VNULL, *Torq_prev, *x_m_o;
static MAT *Q = {MNULL}, *R = {MNULL}, *Pprev = {MNULL};
static double alpha, kappa, lambda, sqrt_lambda, w_m_0, w_c_0, w_i, beta;
static int n_states, n_sig_pts, n_err_states, iter_num, initialize=0;
VEC *x = VNULL, *x_priori = VNULL, *x_err_priori = VNULL, *single_sig_pt = VNULL, *v_temp = VNULL, *q_err_quat = VNULL,
*err_vec = VNULL, *v_temp2 = VNULL, *x_ang_vel = VNULL, *meas = VNULL, *meas_priori = VNULL,
*v_temp3 = VNULL, *x_posteriori_err = VNULL, *x_b_m = VNULL, *x_b_g = VNULL;
MAT *sqrt_P = {MNULL}, *P = {MNULL}, *P_priori = {MNULL}, *sig_pt = {MNULL}, *sig_vec_mat = {MNULL},
*err_sig_pt_mat = {MNULL}, *result = {MNULL}, *result_larger = {MNULL}, *result1 = {MNULL}, *Meas_err_mat = {MNULL},
*P_zz = {MNULL}, *iP_vv = {MNULL}, *P_xz = {MNULL}, *K = {MNULL}, *result2 = {MNULL}, *result3 = {MNULL}, *C = {MNULL};
int update_mag_vec, update_sun_vec, update_omega, i, j;
double d_res;
if (inertia == MNULL)
{
inertia = m_get(3,3);
m_ident(inertia);
inertia->me[0][0] = 0.007;
inertia->me[1][1] = 0.014;
inertia->me[2][2] = 0.015;
}
if (initialize == 0){
iter_num = 1;
n_states = (7+6);
n_err_states = (6+6);
n_sig_pts = 2*n_err_states+1;
alpha = sqrt(3);
kappa = 3 - n_states;
lambda = alpha*alpha * (n_err_states+kappa) - n_err_states;
beta = -(1-(alpha*alpha));
w_m_0 = (lambda)/(n_err_states + lambda);
w_c_0 = (lambda/(n_err_states + lambda)) + (1 - (alpha*alpha) + beta);
w_i = 0.5/(n_err_states +lambda);
initialize = 1;
sqrt_lambda = (lambda+n_err_states);
if(q_s_c == VNULL)
{
q_s_c = v_get(4);
q_s_c->ve[0] = -0.020656;
q_s_c->ve[1] = 0.71468;
q_s_c->ve[2] = -0.007319;
q_s_c->ve[3] = 0.6991;
}
if(Torq_prev == VNULL)
{
Torq_prev = v_get(3);
v_zero(Torq_prev);
}
quat_normalize(q_s_c);
}
result = m_get(9,9);
m_zero(result);
result1 = m_get(n_err_states, n_err_states);
m_zero(result1);
if(x_m_o == VNULL)
{
x_m_o = v_get(n_states);
v_zero(x_m_o);
}
x = v_get(n_states);
v_zero(x);
x_err_priori = v_get(n_err_states);
v_zero(x_err_priori);
x_ang_vel = v_get(3);
v_zero(x_ang_vel);
sig_pt = m_get(n_states, n_err_states);
m_zero(sig_pt);
if (C == MNULL)
{
C = m_get(9, 12);
m_zero(C);
}
if (P_priori == MNULL)
{
P_priori = m_get(n_err_states, n_err_states);
m_zero(P_priori);
}
if (Q == MNULL)
{
Q = m_get(n_err_states, n_err_states);
m_ident(Q);
//
Q->me[0][0] = 0.0001;
Q->me[1][1] = 0.0001;
Q->me[2][2] = 0.0001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.000001;
Q->me[7][7] = 0.000001;
Q->me[8][8] = 0.000001;
Q->me[9][9] = 0.000001;
Q->me[10][10] = 0.000001;
Q->me[11][11] = 0.000001;
}
if( Pprev == MNULL)
{
Pprev = m_get(n_err_states, n_err_states);
m_ident(Pprev);
Pprev->me[0][0] = 1e-3;
Pprev->me[1][1] = 1e-3;
Pprev->me[2][2] = 1e-3;
Pprev->me[3][3] = 1e-3;
Pprev->me[4][4] = 1e-3;
Pprev->me[5][5] = 1e-3;
Pprev->me[6][6] = 1e-4;
Pprev->me[7][7] = 1e-4;
Pprev->me[8][8] = 1e-4;
Pprev->me[9][9] = 1e-3;
Pprev->me[10][10] = 1e-3;
Pprev->me[11][11] = 1e-3;
}
if (R == MNULL)
{
R = m_get(9,9);
m_ident(R);
R->me[0][0] = 0.034;
R->me[1][1] = 0.034;
R->me[2][2] = 0.034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.000012;
R->me[7][7] = 0.000012;
R->me[8][8] = 0.000012;
}
if(eclipse==0)
{
R->me[0][0] = 0.00034;
R->me[1][1] = 0.00034;
R->me[2][2] = 0.00034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;//0.000012;//0.0175;//1e-3;
Q->me[4][4] = 0.0001;//0.0175;//1e-3;
Q->me[5][5] = 0.0001;//0.0175;//1e-3;
Q->me[6][6] = 0.0000000001;//1e-6;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
else
{
R->me[0][0] = 0.34;
R->me[1][1] = 0.34;
R->me[2][2] = 0.34;
R->me[3][3] = 0.0027;
R->me[4][4] = 0.0027;
R->me[5][5] = 0.0027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.0000000001;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
if(omega_prev == VNULL)
{
omega_prev = v_get(3);
v_zero(omega_prev);
}
if(mag_vec_prev == VNULL)
{
mag_vec_prev = v_get(3);
v_zero(mag_vec_prev);
}
if(sun_vec_prev == VNULL)
{
sun_vec_prev = v_get(3);
v_zero(sun_vec_prev);
}
if (err_sig_pt_mat == MNULL)
{
err_sig_pt_mat = m_get(n_err_states, n_sig_pts);
m_zero(err_sig_pt_mat);
}
if(q_err_quat == VNULL)
{
q_err_quat = v_get(4);
// q_err_quat = v_resize(q_err_quat,4);
v_zero(q_err_quat);
}
if(err_vec == VNULL)
{
err_vec = v_get(3);
v_zero(err_vec);
}
v_temp = v_get(9);
v_resize(v_temp,3);
if(x_prev == VNULL)
{
x_prev = v_get(n_states);
v_zero(x_prev);
x_prev->ve[3] = 1;
quat_mul(x_prev,q_s_c,x_prev);
x_prev->ve[4] = 0.0;
x_prev->ve[5] = 0.0;
x_prev->ve[6] = 0.0;
x_prev->ve[7] = 0.0;
x_prev->ve[8] = 0.0;
x_prev->ve[9] = 0.0;
x_prev->ve[10] = 0.0;
x_prev->ve[11] = 0.0;
x_prev->ve[12] = 0.0;
}
sqrt_P = m_get(n_err_states, n_err_states);
m_zero(sqrt_P);
//result = m_resize(result, n_err_states, n_err_states);
result_larger = m_get(n_err_states, n_err_states);
int n, m;
for(n = 0; n < result->n; n++)
{
for(m = 0; m < result->m; m++)
{
result_larger->me[m][n] = result->me[m][n];
}
}
//v_resize(v_temp, n_err_states);
V_FREE(v_temp);
v_temp = v_get(n_err_states);
symmeig(Pprev, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_copy(Pprev, result1);
sm_mlt(sqrt_lambda, result1, result_larger);
catchall(CHfactor(result_larger), printerr(sim_time));
for(i=0; i<n_err_states; i++){
for(j=i+1; j<n_err_states; j++){
result_larger->me[i][j] = 0;
}
}
expandstate(result_larger, x_prev, sig_pt);
sig_vec_mat = m_get(n_states, n_sig_pts);
m_zero(sig_vec_mat);
for(j = 0; j<(n_err_states+1); j++)
{
for(i = 0; i<n_states; i++)
{
if(j==0)
{
sig_vec_mat->me[i][j] = x_prev->ve[i];
}
else if(j>0)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-1];
}
}
}
sm_mlt(-1,result_larger,result_larger);
expandstate(result_larger, x_prev, sig_pt);
for(j = (n_err_states+1); j<n_sig_pts; j++)
{
for(i = 0; i<n_states; i++)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-(n_err_states+1)];
}
}
single_sig_pt = v_get(n_states);
quat_rot_vec(q_s_c, Torq_ext);
for(j=0; j<(n_sig_pts); j++)
{
//v_temp = v_resize(v_temp,n_states);
V_FREE(v_temp);
v_temp = v_get(n_states);
get_col(sig_vec_mat, j, single_sig_pt);
v_copy(single_sig_pt, v_temp);
rk4(t, v_temp, h, Torq_prev);
set_col(sig_vec_mat, j, v_temp);
}
v_copy(Torq_ext, Torq_prev);
x_priori = v_get(n_states);
v_zero(x_priori);
v_resize(v_temp,n_states);
v_zero(v_temp);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp);
if(j == 0)
{
v_mltadd(x_priori, v_temp, w_m_0, x_priori);
}
else
{
v_mltadd(x_priori, v_temp, w_i, x_priori);
}
}
v_copy(x_priori, v_temp);
v_resize(v_temp,4);
quat_normalize(v_temp);//zaroori hai ye
for(i=0; i<4; i++)
{
x_priori->ve[i] = v_temp->ve[i];
}
v_resize(v_temp, n_states);
v_copy(x_priori, v_temp);
v_resize(v_temp, 4);
quat_inv(v_temp, v_temp);
for(i=0; i<3; i++)
{
x_ang_vel->ve[i] = x_priori->ve[i+4];
}
x_b_m = v_get(3);
v_zero(x_b_m);
x_b_g = v_get(3);
v_zero(x_b_g);
/////////////////////////check it!!!!!!!! checked... doesnt change much the estimate
for(i=0; i<3; i++)
{
x_b_m->ve[i] = x_priori->ve[i+7];
x_b_g->ve[i] = x_priori->ve[i+10];
}
v_temp2 = v_get(n_states);
v_zero(v_temp2);
for(j=0; j<n_sig_pts; j++)
{
v_resize(v_temp2, n_states);
get_col( sig_vec_mat, j, v_temp2);
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+4];
}
v_resize(v_temp2, 4);
quat_mul(v_temp2, v_temp, q_err_quat);
v_resize(q_err_quat, n_err_states);
v_sub(err_vec, x_ang_vel, err_vec);
for(i=3; i<6; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-3];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+7];
}
v_sub(err_vec, x_b_m, err_vec);
for(i=6; i<9; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-6];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+10];
}
v_sub(err_vec, x_b_g, err_vec);
for(i=9; i<12; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-9];
}
set_col(err_sig_pt_mat, j, q_err_quat);
if(j==0){
v_mltadd(x_err_priori, q_err_quat, w_m_0, x_err_priori);
}
else{
v_mltadd(x_err_priori, q_err_quat, w_i, x_err_priori);
}
}
v_resize(v_temp,n_err_states);
for (j=0;j<13;j++)
{
get_col(err_sig_pt_mat, j, v_temp);
v_sub(v_temp, x_err_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
}
else{
sm_mlt(w_i, result_larger, result_larger);
}
m_add(P_priori, result_larger, P_priori);
}
symmeig(P_priori, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_priori, Q, P_priori);
v_resize(v_temp,3);
meas = v_get(9);
if (!(is_vec_equal(sun_vec, sun_vec_prev)) /*&& (eclipse==0)*/ ){
update_sun_vec =1;
v_copy(sun_vec, sun_vec_prev);
v_copy(sun_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i = 0; i<3;i++){
meas->ve[i] = v_temp->ve[i];
}
}
else{
update_sun_vec =0;
for(i = 0; i<3;i++){
meas->ve[i] = 0;
}
}
if (!(is_vec_equal(mag_vec, mag_vec_prev)) ){
update_mag_vec =1;
v_copy(mag_vec, mag_vec_prev);
v_copy(mag_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i=3; i<6; i++){
meas->ve[i] = v_temp->ve[i-3];
}
}
else{
update_mag_vec =0;
for(i=3; i<6; i++){
meas->ve[i] = 0;//mag_vec_prev->ve[i-3];
}
}
if (!(is_vec_equal(omega, omega_prev) ) ){
update_omega =1;
v_copy(omega, omega_prev);
v_copy(omega, v_temp);
quat_rot_vec(q_s_c, v_temp);
for(i=6; i<9; i++){
meas->ve[i] = v_temp->ve[i-6];
}
}
else{
update_omega =0;
for(i=6; i<9; i++){
meas->ve[i] = 0;
}
}
v_resize(v_temp, 9);
v_resize(v_temp2, n_states);
v_temp3 = v_get(3);
Meas_err_mat = m_get(9, n_sig_pts);
m_zero(Meas_err_mat);
meas_priori = v_get(9);
v_zero(meas_priori);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp2);
if(update_omega){
for(i=6;i<9;i++){
v_temp->ve[i] = v_temp2->ve[i-2] + x_b_g->ve[i-6];
}
}
else{
for(i=6;i<9;i++){
v_temp->ve[i] = 0;
}
}
v_resize(v_temp2, 4);
if(update_sun_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = sun_vec_I->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp->ve[i] = v_temp3->ve[i];
}
}
else{
for(i=0;i<3;i++){
v_temp->ve[i] = 0;
}
}
if(update_mag_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = mag_vec_I->ve[i];
}
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp3->ve[i] = v_temp3->ve[i] + x_b_m->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=3;i<6;i++){
v_temp->ve[i] = v_temp3->ve[i-3];
}
}
else{
for(i=3;i<6;i++){
v_temp->ve[i] = 0;
}
}
set_col(Meas_err_mat, j, v_temp);
if(j==0){
v_mltadd(meas_priori, v_temp, w_m_0, meas_priori);
}
else{
v_mltadd(meas_priori, v_temp, w_i, meas_priori);
}
}
v_resize(v_temp, 9);
m_resize(result_larger, 9, 9);
m_zero(result_larger);
P_zz = m_get(9, 9);
m_zero(P_zz);
iP_vv = m_get(9, 9);
m_zero(iP_vv);
P_xz = m_get(n_err_states, 9);
m_zero(P_xz);
v_resize(v_temp2, n_err_states);
result1 = m_resize(result1,n_err_states,9);
for (j=0; j<n_sig_pts; j++)
{
get_col( Meas_err_mat, j, v_temp);
get_col( err_sig_pt_mat, j, v_temp2);
v_sub(v_temp, meas_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
get_dyad(v_temp2, v_temp, result1);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
sm_mlt(w_c_0, result1, result1);
}
else{
sm_mlt(w_i, result_larger, result_larger);
sm_mlt(w_i, result1, result1);
}
m_add(P_zz, result_larger, P_zz);
m_add(P_xz, result1, P_xz);
}
symmeig(P_zz, result_larger, v_temp);
i = 0;
for (j=0; j<9; j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_zz, R, P_zz);
m_inverse(P_zz, iP_vv);
K = m_get(n_err_states, 9);
m_zero(K);
m_mlt(P_xz, iP_vv, K);
if(x_posteriori_err == VNULL)
{
x_posteriori_err = v_get(n_err_states);
v_zero(x_posteriori_err);
}
v_resize(v_temp,9);
v_sub(meas, meas_priori, v_temp);
v_copy(v_temp, residual);
mv_mlt(K, v_temp, x_posteriori_err);
v_resize(v_temp2,3);
for(i=0;i<3;i++){
v_temp2->ve[i] = x_posteriori_err->ve[i];
}
for(i=4; i<n_states; i++){
x_prev->ve[i] = (x_posteriori_err->ve[i-1] + x_priori->ve[i]);
}
d_res = v_norm2(v_temp2);
v_resize(v_temp2,4);
if(d_res<=1 /*&& d_res!=0*/){
v_temp2->ve[0] = v_temp2->ve[0];
v_temp2->ve[1] = v_temp2->ve[1];
v_temp2->ve[2] = v_temp2->ve[2];
v_temp2->ve[3] = sqrt(1-d_res);
}
else//baad main daala hai
{
v_temp2->ve[0] = (v_temp2->ve[0])/(sqrt(1+d_res));
v_temp2->ve[1] = (v_temp2->ve[1])/(sqrt(1+d_res));
v_temp2->ve[2] = (v_temp2->ve[2])/(sqrt(1+d_res));
v_temp2->ve[3] = 1/sqrt(1 + d_res);
}
v_resize(x_posteriori_err, n_states);
for(i=(n_states-1); i>3; i--){
x_posteriori_err->ve[i] = x_posteriori_err->ve[i-1];
}
for(i=0; i<4; i++){
x_posteriori_err->ve[i] = v_temp2->ve[i];
}
quat_mul(v_temp2, x_priori, v_temp2);
for(i=0;i<4;i++){
x_prev->ve[i] = v_temp2->ve[i];
}
m_resize(result_larger, n_err_states, 9);
m_mlt(K, P_zz, result_larger);
result2 = m_get(9, n_err_states);
m_transp(K,result2);
m_resize(result1, n_err_states, n_err_states);
m_mlt(result_larger, result2, result1);
v_resize(v_temp, n_err_states);
m_sub(P_priori, result1, Pprev);
symmeig(Pprev, result1 , v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
v_copy(x_prev, v_temp);
v_resize(v_temp,4);
v_copy(x_prev, v_temp2);
v_resize(v_temp2,4);
v_copy(x_prev, x_m_o);
//v_resize(x_m_o, 4);
v_resize(v_temp,3);
quat_inv(q_s_c, v_temp2);
v_copy( x_prev, state);
quat_mul(state, v_temp2, state);
for(i=0; i<3; i++){
v_temp->ve[i] = state->ve[i+4];
}
quat_rot_vec(v_temp2, v_temp);
for(i=0; i<3; i++){
state->ve[i+4] = v_temp->ve[i];
}
v_copy( x_posteriori_err, st_error);
iter_num++;
V_FREE(x);
V_FREE(x_priori);
V_FREE(x_err_priori);
V_FREE(single_sig_pt);
V_FREE(v_temp);
V_FREE(q_err_quat);
V_FREE(err_vec);
V_FREE(v_temp2);
V_FREE(x_ang_vel);
V_FREE(meas);
V_FREE(meas_priori);
V_FREE(v_temp3);
V_FREE(x_posteriori_err);
V_FREE(x_b_m);
V_FREE(x_b_g);
M_FREE(sqrt_P);
M_FREE(P);
M_FREE(P_priori);
M_FREE(sig_pt);
M_FREE(sig_vec_mat);
M_FREE(err_sig_pt_mat);
M_FREE(result);
M_FREE(result_larger);
M_FREE(result1);
M_FREE(Meas_err_mat);
M_FREE(P_zz);
M_FREE(iP_vv);
M_FREE(P_xz);
M_FREE(K);
M_FREE(result2);
M_FREE(result3);
}
void Object::move() {
// std::cout << "Moving" << std::endl;
// Falling
vel[1] = rk4(*this, vel[1], st->getTime(), st->getStep());
}
