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