//----------------------------------------------------------------------------
void robot_t::calc_frottements()
{
vector_t O,F;
F = N*(-coeff_frott_n*(speed|N));
add_force(O,F);
F = T*(-coeff_frott_t*(speed|T)*masse/simul_info->dt);
add_force(O,F);
}
/**
adds force in positive or negative direction depending on flip parameter
**/
void Movement::add_flip_force(SDL_RendererFlip newflip, float forcemultiplierx, float forcemultipliery) {
if (newflip == SDL_RendererFlip::SDL_FLIP_NONE) {
add_force(forcemultiplierx, forcemultipliery);
flip = SDL_RendererFlip::SDL_FLIP_HORIZONTAL;
}else {
add_force(-forcemultiplierx, forcemultipliery);
flip = SDL_RendererFlip::SDL_FLIP_NONE;
}
}
void MACStableFluids::step(Scalar dt)
{
float t=0;
int count=0;
while(t<dt)
{
float substep=cfl();
if(substep<0.000001)
{
std::cerr << "SUBSTEP GOING TOO LOW QUITTING\n";
return;
}
if(t+substep > dt)
substep=dt-t;
add_force(dt);
advect(dt);
diffuse(dt);
project(dt);
advect_particles(dt);
++count;
t+=substep;
}
time += dt;
}
//----------------------------------------------------------------------------
void EltOrange::calc_frottements()
{
// frottements
vector_t C1,C2,F;
double norme;
norme=-1.*simul_info->coeff_frott[mBois][mBois]*masse;
F = speed*norme;
add_force(G_rot,F);
C1=N*rayon;
C2=-C1;
norme=(get_speed(C1)^N)/rayon*0.0008;
F=T*norme;
add_force(C1,F);
F=-F;
add_force(C2,F);
}
//The main fluid simulation step
void FluidSim::advance(float dt) {
float t = 0;
while(t < dt) {
float substep = cfl();
if(t + substep > dt)
substep = dt - t;
//Passively advect particles
advect_particles(substep);
//Estimate the liquid signed distance
compute_phi();
//Advance the velocity
advect(substep);
add_force(substep);
apply_viscosity(substep);
apply_projection(substep);
//Pressure projection only produces valid velocities in faces with non-zero associated face area.
//Because the advection step may interpolate from these invalid faces,
//we must extrapolate velocities from the fluid domain into these zero-area faces.
extrapolate(u, u_valid);
extrapolate(v, v_valid);
//For extrapolated velocities, replace the normal component with
//that of the object.
constrain_velocity();
t+=substep;
}
}
//----------------------------------------------------------------------------
void robot_t::move_palets()
{
vector_t C;
vector_t F;
double d;
double v=(33./6.)*((double)tension_courroie)*(tension/12)*M_PI*0.022;
double modif;
object_t *clamp = ((union_obj_t*)objects[3])->objects[1];
palet_t *palet;
vector< palet_t* > palets_in;
palets_in.clear();
for(unsigned int i=0;i<simul_info->palets.size();i++)
{
palet=((palet_t*)simul_info->palets[i]);
if(palet->z-palet->hauteur/2. != 0.) continue;
C=(palet->position-position).rotate_spec(cosinus,-sinus)+G_init;
if(C.y>-0.04 && C.y<0.04 && C.x<dimX/2. && C.x>(dimX/2.-longueur_recept))
{
palets_in.resize(palets_in.size()+1);
palets_in[palets_in.size()-1] = palet;
}
}
modif=(v<0?0.05 + 0.07*(palets_in.size()-1):0.);
if(clamp->type==OBJ_PINCE_FERMEE && clamp->z-clamp->hauteur/2.-clamp_minZ<0.03)
modif += 0.07;
for(unsigned int i=0;i<palets_in.size();i++)
{
C=(palets_in[i]->position-position).rotate_spec(cosinus,-sinus)+G_init;
if(C.y>-0.04 && C.y<0.04 && C.x<dimX/2. && C.x>(dimX/2.-longueur_recept+modif))
{
/* s=((2.*(speed|N)+v)/2.-(palets_in[i]->speed|N))*palets_in[i]->masse/simul_info->dt;
d=(v/(2.*palets_in[i]->rayon)+palets_in[i]->omega)*palets_in[i]->J/(palets_in[i]->rayon*simul_info->dt);
C=T*palets_in[i]->rayon;
F=N*(s+d)/2.;
palets_in[i]->add_force(C,F);
C=C+palets_in[i]->position-position;
F=-F;
add_force(C,F);
C=-T*palets_in[i]->rayon;
F=N*(s-d)/2.;
palets_in[i]->add_force(C,F);
C=C+palets_in[i]->position-position;
F=-F;
add_force(C,F); */
d = (speed|N) + v - (palets_in[i]->speed|N);
C = vector_t(0., 0.);
F = N * (d * palets_in[i]->masse/simul_info->dt);
palets_in[i]->add_force(C,F);
F = -F;
add_force(C,F);
}
}
}
void cell_cell_transfer(GhostCommunication *gc, int data_parts)
{
int pl, p, offset;
Particle *part1, *part2, *pt1, *pt2;
GHOST_TRACE(fprintf(stderr, "%d: local_transfer: type %d data_parts %d\n", this_node,gc->type,data_parts));
/* transfer data */
offset = gc->n_part_lists/2;
for (pl = 0; pl < offset; pl++) {
Cell *src_list = gc->part_lists[pl];
Cell *dst_list = gc->part_lists[pl + offset];
if (data_parts == GHOSTTRANS_PARTNUM) {
prepare_ghost_cell(dst_list, src_list->n);
}
else {
int np = src_list->n;
part1 = src_list->part;
part2 = dst_list->part;
for (p = 0; p < np; p++) {
pt1 = &part1[p];
pt2 = &part2[p];
if (data_parts & GHOSTTRANS_PROPRTS) {
memcpy(&pt2->p, &pt1->p, sizeof(ParticleProperties));
#ifdef GHOSTS_HAVE_BONDS
realloc_intlist(&(pt2->bl), pt2->bl.n = pt1->bl.n);
memcpy(&pt2->bl.e, &pt1->bl.e, pt1->bl.n*sizeof(int));
#ifdef EXCLUSIONS
realloc_intlist(&(pt2->el), pt2->el.n = pt1->el.n);
memcpy(&pt2->el.e, &pt1->el.e, pt1->el.n*sizeof(int));
#endif
#endif
}
if (data_parts & GHOSTTRANS_POSSHFTD) {
/* ok, this is not nice, but perhaps fast */
int i;
memcpy(&pt2->r, &pt1->r, sizeof(ParticlePosition));
for (i = 0; i < 3; i++)
pt2->r.p[i] += gc->shift[i];
}
else if (data_parts & GHOSTTRANS_POSITION)
memcpy(&pt2->r, &pt1->r, sizeof(ParticlePosition));
if (data_parts & GHOSTTRANS_MOMENTUM)
memcpy(&pt2->m, &pt1->m, sizeof(ParticleMomentum));
if (data_parts & GHOSTTRANS_FORCE)
add_force(&pt2->f, &pt1->f);
#ifdef LB
if (data_parts & GHOSTTRANS_COUPLING)
memcpy(&pt2->lc, &pt1->lc, sizeof(ParticleLatticeCoupling));
#endif
}
}
}
}
void cell_cell_transfer(GhostCommunication *gc, int data_parts)
{
int pl, p, np, offset;
Particle *part1, *part2, *pt1, *pt2;
GHOST_TRACE(fprintf(stderr, "%d: local_transfer: type %d data_parts %d\n", this_node,gc->type,data_parts));
/* transfer data */
offset = gc->n_part_lists/2;
for (pl = 0; pl < offset; pl++) {
np = gc->part_lists[pl]->n;
if (data_parts == GHOSTTRANS_PARTNUM) {
realloc_particlelist(gc->part_lists[pl + offset],
gc->part_lists[pl + offset]->n = gc->part_lists[pl]->n);
#ifdef GHOST_FLAG
{
//init ghost variable
int i;
for (i=0; i<gc->part_lists[pl + offset]->n; i++) {
gc->part_lists[pl + offset]->part[i].l.ghost=1;
}
}
#endif
}
else {
part1 = gc->part_lists[pl]->part;
part2 = gc->part_lists[pl + offset]->part;
for (p = 0; p < np; p++) {
pt1 = &part1[p];
pt2 = &part2[p];
if (data_parts & GHOSTTRANS_PROPRTS)
memcpy(&pt2->p, &pt1->p, sizeof(ParticleProperties));
if (data_parts & GHOSTTRANS_POSSHFTD) {
/* ok, this is not nice, but perhaps fast */
int i;
memcpy(&pt2->r, &pt1->r, sizeof(ParticlePosition));
for (i = 0; i < 3; i++)
pt2->r.p[i] += gc->shift[i];
}
else if (data_parts & GHOSTTRANS_POSITION)
memcpy(&pt2->r, &pt1->r, sizeof(ParticlePosition));
if (data_parts & GHOSTTRANS_MOMENTUM)
memcpy(&pt2->m, &pt1->m, sizeof(ParticleMomentum));
if (data_parts & GHOSTTRANS_FORCE)
add_force(&pt2->f, &pt1->f);
#ifdef LB
if (data_parts & GHOSTTRANS_COUPLING)
memcpy(&pt2->lc, &pt1->lc, sizeof(ParticleLatticeCoupling));
#endif
}
}
}
}
// ********************************************************** Engine::update
void update( const TNumber& delta_time )
{
for (auto it = _bodies.begin(); it != _bodies.end(); ++it) {
auto bd = *it;
if( _use_gravity ) {
// compute gravity force
auto weight = _gravity;
_gravity *= bd->_mass,
bd->add_force( weight );
}
bd->update( delta_time );
}
}
void reduce_forces_sum(void *add, void *to, int *len, MPI_Datatype *type)
{
ParticleForce
*cadd = (ParticleForce*)add,
*cto = (ParticleForce*)to;
int i, clen = *len/sizeof(ParticleForce);
if (*type != MPI_BYTE || (*len % sizeof(ParticleForce)) != 0) {
fprintf(stderr, "%d: transfer data type wrong\n", this_node);
errexit();
}
for (i = 0; i < clen; i++)
add_force(&cto[i], &cadd[i]);
}
//----------------------------------------------------------------------------
void robot_t::calc_forces()
{
vector_t R[2],F;
double force_value;
R[0]=roue[0].rotate_spec(cosinus,sinus);
R[1]=roue[1].rotate_spec(cosinus,sinus);
move_clamp();
move_palets();
calc_frottements();
/* Antidérapage de le pic */
/*
double U[2];
double Umax[2];
double Umin[2];
for(int i=0;i<2;i++)
{
U[i] = ((double)tension_moteur[i])*tension/((double)RANGE_MOTOR);
Umax[i] = moteur[i].w/moteur[i].Kv+(moteur[i].m*moteur[i].g*moteur[i].e*rayon_roue)/(1.5*moteur[i].Kc*moteur[i].k*moteur[i].n*0.98);
Umin[i] = moteur[i].w/moteur[i].Kv-(moteur[i].m*moteur[i].g*moteur[i].e*rayon_roue)/(1.5*moteur[i].Kc*moteur[i].k*moteur[i].n*0.98);
}
for(int i=0;i<2;i++)
{
if(U[i]>Umax[i])
U[i] = Umax[i];
if(U[i]<Umin[i])
U[i] = Umin[i];
}
*/
for(int i=0;i<2;i++)
{
force_value=moteur[i].calc_force(((double)tension_moteur[i])*tension/((double)RANGE_MOTOR),get_speed(R[i])|N);
F = N*force_value;
add_force(R[i],F);
capacite-=fabs(moteur[i].I)*simul_info->dt;
}
}
//The main fluid simulation step
void FluidSim::advance(float dt) {
//Passively advect particles
advect_particles(dt);
//Advance the velocity
advect(dt);
add_force(dt);
project(dt);
//Pressure projection only produces valid velocities in faces with non-zero associated face area.
//Because the advection step may interpolate from these invalid faces,
//we must extrapolate velocities from the fluid domain into these zero-area faces.
extrapolate(u, u_weights, valid, old_valid);
extrapolate(v, v_weights, valid, old_valid);
//For extrapolated velocities, replace the normal component with
//that of the object.
constrain_velocity();
}
//The main fluid simulation step
void FluidSim::advance(float dt) {
float t = 0;
while(t < dt) {
float substep = cfl();
if(t + substep > dt)
substep = dt - t;
printf("Taking substep of size %f (to %0.3f%% of the frame)\n", substep, 100 * (t+substep)/dt);
printf(" Surface (particle) advection\n");
advect_particles(substep);
printf(" Velocity advection\n");
//Advance the velocity
advect(substep);
add_force(substep);
printf(" Solve viscosity");
apply_viscosity(substep);
printf(" Pressure projection\n");
project(substep);
//Pressure projection only produces valid velocities in faces with non-zero associated face area.
//Because the advection step may interpolate from these invalid faces,
//we must extrapolate velocities from the fluid domain into these invalid faces.
printf(" Extrapolation\n");
extrapolate(u, u_valid);
extrapolate(v, v_valid);
extrapolate(w, w_valid);
//For extrapolated velocities, replace the normal component with
//that of the object.
printf(" Constrain boundary velocities\n");
constrain_velocity();
t+=substep;
}
}
void add_forces_from_recv_buffer(GhostCommunication *gc)
{
int pl, p, np;
Particle *part, *pt;
char *retrieve;
/* put back data */
retrieve = r_buffer;
for (pl = 0; pl < gc->n_part_lists; pl++) {
np = gc->part_lists[pl]->n;
part = gc->part_lists[pl]->part;
for (p = 0; p < np; p++) {
pt = &part[p];
add_force(&pt->f, (ParticleForce *)retrieve);
retrieve += sizeof(ParticleForce);
}
}
if (retrieve - r_buffer != n_r_buffer) {
fprintf(stderr, "%d: recv buffer size %d differs "
"from what I put in %ld\n",
this_node, n_r_buffer, retrieve - r_buffer);
errexit();
}
}
void cell_cell_transfer(GhostCommunication *gc, int data_parts)
{
int pl, p, offset;
Particle *part1, *part2, *pt1, *pt2;
GHOST_TRACE(fprintf(stderr, "%d: local_transfer: type %d data_parts %d\n", this_node,gc->type,data_parts));
/* transfer data */
offset = gc->n_part_lists/2;
for (pl = 0; pl < offset; pl++) {
Cell *src_list = gc->part_lists[pl];
Cell *dst_list = gc->part_lists[pl + offset];
if (data_parts & GHOSTTRANS_PARTNUM) {
prepare_ghost_cell(dst_list, src_list->n);
}
else {
int np = src_list->n;
part1 = src_list->part;
part2 = dst_list->part;
for (p = 0; p < np; p++) {
pt1 = &part1[p];
pt2 = &part2[p];
if (data_parts & GHOSTTRANS_PROPRTS) {
memcpy(&pt2->p, &pt1->p, sizeof(ParticleProperties));
#ifdef GHOSTS_HAVE_BONDS
realloc_intlist(&(pt2->bl), pt2->bl.n = pt1->bl.n);
memcpy(&pt2->bl.e, &pt1->bl.e, pt1->bl.n*sizeof(int));
#ifdef EXCLUSIONS
realloc_intlist(&(pt2->el), pt2->el.n = pt1->el.n);
memcpy(&pt2->el.e, &pt1->el.e, pt1->el.n*sizeof(int));
#endif
#endif
}
if (data_parts & GHOSTTRANS_POSSHFTD) {
/* ok, this is not nice, but perhaps fast */
int i;
memcpy(&pt2->r, &pt1->r, sizeof(ParticlePosition));
for (i = 0; i < 3; i++)
pt2->r.p[i] += gc->shift[i];
#ifdef LEES_EDWARDS
/* special wrapping conditions for x component of y LE shift */
if( gc->shift[1] != 0.0 ){
/* LE transforms are wrapped */
if( pt2->r.p[0]
- (my_left[0] + dst_list->myIndex[0]*dd.cell_size[0]) > 2*dd.cell_size[0] )
pt2->r.p[0]-=box_l[0];
if( pt2->r.p[0]
- (my_left[0] + dst_list->myIndex[0]*dd.cell_size[0]) < -2*dd.cell_size[0] )
pt2->r.p[0]+=box_l[0];
}
#endif
}
else if (data_parts & GHOSTTRANS_POSITION)
memcpy(&pt2->r, &pt1->r, sizeof(ParticlePosition));
if (data_parts & GHOSTTRANS_MOMENTUM) {
memcpy(&pt2->m, &pt1->m, sizeof(ParticleMomentum));
#ifdef LEES_EDWARDS
/* special wrapping conditions for x component of y LE shift */
if( gc->shift[1] > 0.0 )
pt2->m.v[0] += lees_edwards_rate;
else if( gc->shift[1] < 0.0 )
pt2->m.v[0] -= lees_edwards_rate;
#endif
}
if (data_parts & GHOSTTRANS_FORCE)
add_force(&pt2->f, &pt1->f);
#ifdef LB
if (data_parts & GHOSTTRANS_COUPLING)
memcpy(&pt2->lc, &pt1->lc, sizeof(ParticleLatticeCoupling));
#endif
#ifdef ENGINE
if (data_parts & GHOSTTRANS_SWIMMING)
memcpy(&pt2->swim, &pt1->swim, sizeof(ParticleParametersSwimming));
#endif
}
}
}
}
