// Detects whether a particle and a half-plane are overlapping (including the
// radius of the particle) and are approaching.
// If the two objects overlap and are approaching, returns true and sets the
// vector n to be the shortest vector between the particle and the half-plane.
// Inputs:
// scene: The scene data structure.
// vidx: The index of the particle.
// pidx: The index of the halfplane. The vectors (px, py) and (nx, ny) can
// be retrieved by calling scene.getHalfplane(pidx).
// Outputs:
// n: The shortest vector between the particle and the half-plane.
// Returns true if the two objects overlap and are approaching.
bool SimpleCollisionHandler::detectParticleHalfplane(TwoDScene &scene, int vidx, int pidx, VectorXs &n)
{
VectorXs x1 = scene.getX().segment<2>(2*vidx);
VectorXs px = scene.getHalfplane(pidx).first;
VectorXs pn = scene.getHalfplane(pidx).second;
pn.normalize();
n = (px-x1).dot(pn)*pn;
if(n.norm() < scene.getRadius(vidx))
{
double relvel = scene.getV().segment<2>(2*vidx).dot(n);
if(relvel > 0)
return true;
}
return false;
}
// Adds the gradient of the penalty potential (-1 * force) for a particle-
// half-plane pair to the total.
// Read the positions of the particle from the input variable x.
// Inputs:
// x: The positions of the particles in the scene.
// vidx: The index of the particle.
// pidx: The index of the half-plane, i.e. the position and normal vectors
// for the half-plane can be retrieved by calling
// m_scene.getHalfplane(pidx).
// Outputs:
// gradE: The total gradient of the penalty force. *ADD* the particle-
// half-plane gradient to this total gradient.
void PenaltyForce::addParticleHalfplaneGradEToTotal(const VectorXs &x, int vidx, int pidx, VectorXs &gradE)
{
// VectorXs x1 = x.segment<2>(2*vidx);
// VectorXs nh = m_scene.getHalfplane(pidx).second;
//
// // your implementation here
//
VectorXs x1 = x.segment<2>(2*vidx);
VectorXs nh = m_scene.getHalfplane(pidx).second;
VectorXs n = (m_scene.getHalfplane(pidx).first - x1).dot(nh)/(nh.dot(nh))*nh;
VectorXs nhat = n;
nhat.normalize();
double r = m_scene.getRadius(vidx);
if(n.norm() < r + m_thickness)
{
gradE.segment<2>(2*vidx) -= m_k*(n.norm()-r - m_thickness)*nhat.dot(nh)/(nh.dot(nh))*nh;
}
}
// Responds to a collision detected between two particles by applying an impulse
// to the velocities of each one.
// You can get the COR of the simulation by calling getCOR().
// Inputs:
// scene: The scene data structure.
// idx1: The index of the first particle.
// idx2: The index of the second particle.
// n: The vector between the first and second particle.
// Outputs:
// None.
void SimpleCollisionHandler::respondParticleParticle(TwoDScene &scene, int idx1, int idx2, const VectorXs &n)
{
const VectorXs &M = scene.getM();
VectorXs &v = scene.getV();
VectorXs nhat = n;
nhat.normalize();
double cfactor = (1.0 + getCOR())/2.0;
double m1 = scene.isFixed(idx1) ? std::numeric_limits<double>::infinity() : M[2*idx1];
double m2 = scene.isFixed(idx2) ? std::numeric_limits<double>::infinity() : M[2*idx2];
double numerator = 2*cfactor * (v.segment<2>(2*idx2) - v.segment<2>(2*idx1) ).dot(nhat);
double denom1 = 1+m1/m2;
double denom2 = m2/m1 + 1;
if(!scene.isFixed(idx1))
v.segment<2>(2*idx1) += numerator/denom1 * nhat;
if(!scene.isFixed(idx2))
v.segment<2>(2*idx2) -= numerator/denom2 * nhat;
}
// Responds to a collision detected between a particle and an edge by applying
// an impulse to the velocities of each one.
// Inputs:
// scene: The scene data structure.
// vidx: The index of the particle.
// eidx: The index of the edge.
// n: The shortest vector between the particle and the edge.
// Outputs:
// None.
void SimpleCollisionHandler::respondParticleEdge(TwoDScene &scene, int vidx, int eidx, const VectorXs &n)
{
const VectorXs &M = scene.getM();
int eidx1 = scene.getEdges()[eidx].first;
int eidx2 = scene.getEdges()[eidx].second;
VectorXs x1 = scene.getX().segment<2>(2*vidx);
VectorXs x2 = scene.getX().segment<2>(2*eidx1);
VectorXs x3 = scene.getX().segment<2>(2*eidx2);
VectorXs v1 = scene.getV().segment<2>(2*vidx);
VectorXs v2 = scene.getV().segment<2>(2*eidx1);
VectorXs v3 = scene.getV().segment<2>(2*eidx2);
VectorXs nhat = n;
nhat.normalize();
double alpha = (x1-x2).dot(x3-x2)/(x3-x2).dot(x3-x2);
alpha = std::min(1.0, std::max(0.0, alpha) );
VectorXs vedge = v2 + alpha*(v3-v2);
double cfactor = (1.0 + getCOR())/2.0;
double m1 = scene.isFixed(vidx) ? std::numeric_limits<double>::infinity() : M[2*vidx];
double m2 = scene.isFixed(eidx1) ? std::numeric_limits<double>::infinity() : M[2*eidx1];
double m3 = scene.isFixed(eidx2) ? std::numeric_limits<double>::infinity() : M[2*eidx2];
double numerator = 2*cfactor*(vedge-v1).dot(nhat);
double denom1 = 1.0 + (1-alpha)*(1-alpha)*m1/m2 + alpha*alpha*m1/m3;
double denom2 = m2/m1 + (1-alpha)*(1-alpha) + alpha*alpha*m2/m3;
double denom3 = m3/m1 + (1-alpha)*(1-alpha)*m3/m2 + alpha*alpha;
if(!scene.isFixed(vidx))
scene.getV().segment<2>(2*vidx) += numerator/denom1 * nhat;
if(!scene.isFixed(eidx1))
scene.getV().segment<2>(2*eidx1) -= (1.0-alpha)*numerator/denom2 * nhat;
if(!scene.isFixed(eidx2))
scene.getV().segment<2>(2*eidx2) -= alpha * numerator/denom3 * nhat;
}
