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 GetNewVelocity(CMeshO::CoordType i_v,CMeshO::FacePointer face,CMeshO::FacePointer new_face,CMeshO::CoordType force,CMeshO::CoordType g,float m,float t){
CMeshO::CoordType n_v;
Point3f n= face->N();
float b=n[0]*force[0]+n[1]*force[1]+n[2]*force[2];
Point3f f;
//Compute force component along the face
f[0]=force[0]-b*n[0];
f[1]=force[1]-b*n[1];
f[2]=force[2]-b*n[2];
CMeshO::CoordType a=f/m;
n_v=i_v+a*t;
return getVelocityComponent(n_v.Norm(),new_face,g);
}
/**
@def Simulate the movement of a point, affected by a force "dir" on a face and a gravity "g".
@param CoordType p - coordinates of the point
@param CoordType v - velocity of the particle
@param float m - mass of the particle
@param FaceType face - pointer to the face
@param CoordType dir - direction of the force
@param float l - length of the movement
@param float t - time step
@return new coordinates of the point
*/
CMeshO::CoordType StepForward(CMeshO::CoordType p,CMeshO::CoordType v,float m,CMeshO::FacePointer &face,CMeshO::CoordType force,float l,float t){
Point3f new_pos;
Point3f n= face->N();
float a=n[0]*force[0]+n[1]*force[1]+n[2]*force[2];
Point3f f;
//Compute force component along the face
f[0]=force[0]-a*n[0];
f[1]=force[1]-a*n[1];
f[2]=force[2]-a*n[2];
new_pos=p+v*t*l+(f/m)*pow(t,2)*0.5*l;
return new_pos;
}
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;
}
/**
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;
}
