Commit 6a420ce1 authored by Arash HABIBI's avatar Arash HABIBI
Browse files

unidir

parent ebf53f8a
......@@ -50,6 +50,7 @@ public:
std::vector<Dart> getMemoCross(const VEC3& pos, const VEC3& dest, Dart& d1,Dart& d2);
VEC3 getDilatedPosition(unsigned int ind); //vertex position with velocity dilatation
VEC3 getPosition(unsigned int ind); // vertex position
void initForces();
void update();
PFP::REAL computeMVC(PFP::VEC3 p, Dart vertex);
......
......@@ -599,6 +599,117 @@ void MovingObstacle::updateObstacleNeighbors() // obstacles voisins , distance p
}
}
//-------------------------------------------------------------
int matrixInverse(VEC3 v1, VEC3 v2, VEC3 v3, VEC3 *inv1, VEC3 *inv2, VEC3 *inv3)
{
int res = 0;
float determinant = v1[0]*v2[1]*v3[2] + v2[0]*v3[1]*v1[2] + v3[0]*v1[1]*v2[2] - v3[0]*v2[1]*v1[2] - v2[0]*v1[1]*v3[2] - v1[0]*v3[1]*v2[2];
if(determinant==0.0)
res=1;
else
{
float un_sur = 1.0 / determinant;
*inv1 = VEC3( v2[1]*v3[2]-v3[1]*v2[2], v2[0]*v3[2]-v3[0]*v2[2], v2[0]*v3[1]-v3[0]*v2[1] );
*inv2 = VEC3( v1[1]*v3[2]-v3[1]*v1[2], v1[0]*v3[2]-v3[0]*v1[2], v1[0]*v3[1]-v3[0]*v1[1] );
*inv3 = VEC3( v1[1]*v2[2]-v2[1]*v1[2], v1[0]*v2[2]-v2[0]*v1[2], v1[0]*v2[1]-v2[0]*v1[1] );
res=0;
*inv1 = *inv1 * un_sur;
*inv2 = *inv2 * un_sur;
*inv3 = *inv3 * un_sur;
}
return res;
}
//-------------------------------------------------------------
float distToLine(VEC3 M, VEC3 P, VEC3 u)
{
u.normalize();
VEC3 MP = P - M;
return(MP*(u^(MP^u)));
}
//-------------------------------------------------------------
// Première stratégie : appliquer à p1 et à p2 une force
// colinéaire à f, mais d'intensité différente. Dans ce cas,
// il est nécessaire de calculer la somme des moments et de
// l'annuler. *f1 = f*d2/(d1+d2) et *f2 = f*d1/(d1+d2).
void getResponse1(VEC3 f, VEC3 p, VEC3 p1, VEC3 p2, VEC3 *f1, VEC3 *f2)
{
float d1 = distToLine(p1,p,f);
float d2 = distToLine(p2,p,f);
*f1 = f*(-d2/(d1+d2));
*f2 = f*(-d1/(d1+d2));
}
//-------------------------------------------------------------
// Deuxième stratégie, les forces appliquées à p1 et p2
// doivent etre colinaires respectivement à pp1 et pp2.
// Donc la somme des moments est automatiquement nulle.
// Pour calculer les intensites, il faut décomposer f
// dans la base (pp1,pp2), ce qui donnera l'intensite de
// chaque force. Problème : on peut générer des forces
// d'étirement importantes.
void getResponse2(VEC3 f, VEC3 p, VEC3 p1, VEC3 p2, VEC3 *f1, VEC3 *f2)
{
float d1 = (p1-p).norm();
float d2 = (p2-p).norm();
float d12 = (p2-p1).norm();
VEC3 ortho = (p1-p) ^ (p2-p);
if(ortho.norm()==0.0)
{
if(d12==0) // comprend le cas où d1==0 et d2==0
{
*f2 = (-0.5)*f;
*f1 = (-0.5)*f;
}
else if(d1==0)
{
*f2 = VEC3(0,0,0);
*f1 = -f;
// meilleure idee : projeter f sur p1p2 -> f2 et f1 <- f-f2
}
else if(d2==0)
{
*f1 = VEC3(0,0,0);
*f2 = -f;
// meilleure idee : projeter f sur p1p2 -> f1 et f2 <- f-f1
}
else // trois points distincts, mais alignés
{
// A suivre
}
}
// int res = matrixInverse(p1-p,p2-p,ortho, VEC3 *inv1, VEC3 *inv2, VEC3 *inv3);
// inv1, inv2 et inv3 sont les composantes de la matrice inverse. L'image de
// p1-p et p2-p par cette matrice donne la valeur des forces f1 et f2.
}
//-------------------------------------------------------------
void MovingObstacle::initForces()
{
if(!rigid_)
for (unsigned int i = 1; i < nbVertices; ++i)
{
//initialisation of forces
forces[dd] = VEC3(0.0);
map.next(dd);
}
}
//-------------------------------------------------------------
void MovingObstacle::update()
{
assert(sim_->envMap_.map.getCurrentLevel() == sim_->envMap_.map.getMaxLevel()) ;
......@@ -643,7 +754,7 @@ void MovingObstacle::update()
for (unsigned int i = 1; i < nbVertices; ++i)
{
//initialisation of forces with viscosity
forces[dd] = -0.2f*velocity[dd];
forces[dd] += -0.2f*velocity[dd];
// forces[dd] = VEC3(0.0);
map.next(dd);
}
......@@ -747,6 +858,19 @@ void MovingObstacle::update()
Obstacle * obst = it->second;
VEC3 p1=obst->p1 ;
VEC3 p2=obst->p2 ;
MovingObstacle *mo = obst->mo;
if(mo!=NULL)
{
// int N = mo->nbVertices;
// int ind1 = obst->index;
// int ind2 = (obst->index+1)%N;
// fprintf(stderr,"%d %d [%d]\n",ind1,ind2,N);
// cout << "-------------------" << mo->forces[ind1] << endl;
}
else
fprintf(stderr,"NULL\n");
double longueur2 = (p1-p2).norm2();
double rest_sum_of_dists = 2 * sqrt(obst_radius_infl*obst_radius_infl + longueur2/4);
......@@ -765,10 +889,23 @@ void MovingObstacle::update()
normal.normalize();
// force_value *= 10;
forces[dd] += force_value * normal;
// This force is not symmetrically applied
// We assume that a homologous vertex in the other obstacle
// is also intersecting the current obstacle
VEC3 force_vector = force_value * normal;
forces[dd] += force_vector;
// obst->index ??? indice local de l'obstacle dans le mo
// obst->mo : le moving_obstacle
// obst->mo->nbVertices
// obst->mo->forces[obst->index]
// obst->mo->forces[obst->index+1%obst->mo->nbVertices]
VEC3 force_vector1, force_vector2;
getResponse1(force_vector,p,p1,p2,&force_vector1,&force_vector2);
int indice1 = obst->index;
int indice2 = (obst->index+1)%(obst->mo->nbVertices);
// obst->mo->forces[indice1] += force_vector1;
// obst->mo->forces[indice2] += force_vector2;
VEC3 p11 = obst->mo->position[indice1];
VEC3 p22 = obst->mo->position[indice2];
// cerr << "p1=" << p1 << " p11=" << p11 << " p2=" << p2 << " p22=" << p22 << endl;
}
}
}
......
......@@ -129,10 +129,13 @@ void Simulator::doStep()
#ifndef SPATIAL_HASHING
envMap_.map.setCurrentLevel(envMap_.map.getMaxLevel()) ;
#endif
movingObstacles_[i]->update() ;
movingObstacles_[i]->initForces();
}
// commente par Arash
movingObstacles_[i]->updateMesh() ;
for (unsigned int i = 0 ; i < movingObstacles_.size() ; ++i)
{
movingObstacles_[i]->update() ;
movingObstacles_[i]->updateMesh() ;
}
endTime=clock() ;
time_obstacle+= endTime-begTime;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment