unidir

include/moving_obstacle.h

@@ -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);

src/moving_obstacle.cpp

@@ -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;
 						}
 					}
 				}

src/simulator.cpp

@@ -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;