float GetVelocity(CMeshO::CoordType o_p,CMeshO::CoordType n_p,CMeshO::FacePointer f,CMeshO::CoordType g,float m,float v){
Point3f n=f->N();
float b=n[0]*g[0]+n[1]*g[1]+n[2]*g[2];
float distance=Distance(o_p,n_p);
Point3f force;
force[0]=g[0]-b*n[0];
force[1]=g[1]-b*n[1];
force[2]=g[2]-b*n[2];
if(force.Norm()==0) return 0;
float acceleration=(force/m).Norm();
float n_v=math::Sqrt(pow(v,2)+(2*acceleration*distance));
return n_v;
}
CMeshO::CoordType getVelocityComponent(float v,CMeshO::FacePointer f,CMeshO::CoordType g){
CMeshO::CoordType cV;
Point3f n= f->N();
float a=n[0]*g[0]+n[1]*g[1]+n[2]*g[2];
Point3f d;
d[0]=g[0]-a*n[0];
d[1]=g[1]-a*n[1];
d[2]=g[2]-a*n[2];
cV=d/d.Norm();
cV.Normalize();
cV[0]=v*d[0];
cV[1]=v*d[1];
cV[2]=v*d[2];
return cV;
}
/**
@def This function move a particle over the mesh
*/
void MoveParticle(Particle<CMeshO> &info,CMeshO::VertexPointer p,float l,int t,Point3f dir,Point3f g,float a){
if(CheckFallPosition(info.face,g,a)){
p->SetS();
return;
}
float time=t;
if(dir.Norm()==0) dir=getRandomDirection();
Point3f new_pos;
Point3f current_pos;
Point3f int_pos;
CMeshO::FacePointer current_face=info.face;
CMeshO::FacePointer new_face;
new_face=current_face;
current_pos=p->P();
new_pos=StepForward(current_pos,info.v,info.mass,current_face,g+dir,l,time);
while(!IsOnFace(new_pos,current_face)){
int edge=ComputeIntersection(current_pos,new_pos,current_face,new_face,int_pos);
if(edge!=-1){
Point3f n = new_face->N();
if(CheckFallPosition(new_face,g,a)) p->SetS();
float elapsed_time=GetElapsedTime(current_pos,int_pos,new_pos,time);
info.v=GetNewVelocity(info.v,current_face,new_face,g+dir,g,info.mass,elapsed_time);
time=time-elapsed_time;
current_pos=int_pos;
current_face->Q()+=elapsed_time*5;
current_face=new_face;
new_pos=int_pos;
if(time>0){
if(p->IsS()) break;
new_pos=StepForward(current_pos,info.v,info.mass,current_face,g+dir,l,time);
}
current_face->C()=Color4b::Green;//Just Debug!!!!
}else{
//We are on a border
new_pos=int_pos;
current_face=new_face;
p->SetS();
break;
}
}
p->P()=new_pos;
info.face=current_face;
}
/**
Verify if a point on that face fall out because of the inclination
@param FacePointer f - Pointer to the face
@param Point3f g - Direction of the gravity
@param float a - Adhesion Factor
return true if a particle of that face fall out
*/
bool CheckFallPosition(CMeshO::FacePointer f,Point3f g,float a){
Point3f n=f->N();
if(a>1) return false;
if(acos(n.dot(g)/(n.Norm()*g.Norm()))<((PI/2)*(1-a))) return true;
return false;
}
