// Adds the gradient of the penalty potential (-1 * force) for a pair of
// particles to the total.
// Read the positions of the particles from the input variable x. Radii can
// be obtained from the member variable m_scene, the penalty force stiffness
// from member variable m_k, and penalty force thickness from member variable
// m_thickness.
// Inputs:
// x: The positions of the particles in the scene.
// idx1: The index of the first particle, i.e. the position of this particle
// is ( x[2*idx1], x[2*idx1+1] ).
// idx2: The index of the second particle.
// Outputs:
// gradE: The total gradient of penalty force. *ADD* the particle-particle
// gradient to this total gradient.
void PenaltyForce::addParticleParticleGradEToTotal(const VectorXs &x, int idx1, int idx2, VectorXs &gradE)
{
// VectorXs x1 = x.segment<2>(2*idx1);
// VectorXs x2 = x.segment<2>(2*idx2);
//
// double r1 = m_scene.getRadius(idx1);
// double r2 = m_scene.getRadius(idx2);
//
// // your implementation here
//
VectorXs x1 = x.segment<2>(2*idx1);
VectorXs x2 = x.segment<2>(2*idx2);
double r1 = m_scene.getRadius(idx1);
double r2 = m_scene.getRadius(idx2);
VectorXs n = x2-x1;
VectorXs nhat = n;
nhat.normalize();
if(n.norm() < r1 + r2 + m_thickness)
{
gradE.segment<2>(2*idx1) -= m_k * (n.norm() - r1 - r2 - m_thickness) * nhat;
gradE.segment<2>(2*idx2) += m_k * (n.norm() - r1 - r2 - m_thickness) * nhat;
}
}
void SpringForce::addGradEToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, VectorXs& gradE )
{
assert( x.size() == v.size() );
assert( x.size() == gradE.size() );
assert( x.size()%2 == 0 );
assert( m_endpoints.first >= 0 ); assert( m_endpoints.first < x.size()/2 );
assert( m_endpoints.second >= 0 ); assert( m_endpoints.second < x.size()/2 );
//GET PARTICLES
int size = 2;
int xistart = m_endpoints.first*2;
int xjstart = m_endpoints.second*2;
VectorXs xi = x.segment(xistart, size);
VectorXs xj = x.segment(xjstart, size);
//SPRING FORCE
VectorXs nhat = xi-xj;
double lxixj = nhat.norm();
nhat /= lxixj;
VectorXs dxiU = m_k*(lxixj-m_l0)*nhat;
VectorXs dxjU = -dxiU;
//SPRING DAMPING FORCE
VectorXs vi = v.segment(xistart, size);
VectorXs vj = v.segment(xjstart, size);
double lvivj = (vi-vj).dot(nhat);
dxiU += m_b * nhat * lvivj; //F-
dxjU += -m_b * nhat * lvivj; //F+
//CHANGE SYSTEM
gradE[xistart+0] += dxiU[0];
gradE[xistart+1] += dxiU[1];
gradE[xjstart+0] += dxjU[0];
gradE[xjstart+1] += dxjU[1];
}
void StaticPlaneSphereConstraint::computeGeneralizedFrictionGivenTangentSample( const VectorXs& q, const VectorXs& t, const unsigned column, SparseMatrixsc& D ) const
{
assert( t.size() == 3 );
assert( column < unsigned( D.cols() ) );
assert( q.size() % 12 == 0 );
assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 );
assert( fabs( m_plane.n().dot( t ) ) <= 1.0e-6 );
// Effect on center of mass
D.insert( 3 * m_sphere_idx + 0, column ) = t.x();
D.insert( 3 * m_sphere_idx + 1, column ) = t.y();
D.insert( 3 * m_sphere_idx + 2, column ) = t.z();
// Effect on orientation
{
const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
// Compute the displacement from the center of mass to the point of contact
assert( fabs( m_plane.n().norm() - 1.0 ) <= 1.0e-10 );
assert( m_r >= 0.0 );
const Vector3s r_world{ - m_r * m_plane.n() };
const Vector3s ntilde{ r_world.cross( Eigen::Map<const Vector3s>( t.data() ) ) };
D.insert( 3 * ( nbodies + m_sphere_idx ) + 0, column ) = ntilde.x();
D.insert( 3 * ( nbodies + m_sphere_idx ) + 1, column ) = ntilde.y();
D.insert( 3 * ( nbodies + m_sphere_idx ) + 2, column ) = ntilde.z();
}
}
void BodyBodyConstraint::computeGeneralizedFrictionGivenTangentSample( const VectorXs& q, const VectorXs& t, const unsigned column, SparseMatrixsc& D ) const
{
assert( column < unsigned( D.cols() ) );
assert( q.size() % 12 == 0 );
assert( t.size() == 3 );
assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 );
assert( fabs( m_n.dot( t ) ) <= 1.0e-6 );
assert( m_idx0 < m_idx1 );
const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
// Effect on center of mass of body i
D.insert( 3 * m_idx0 + 0, column ) = t.x();
D.insert( 3 * m_idx0 + 1, column ) = t.y();
D.insert( 3 * m_idx0 + 2, column ) = t.z();
// Effect on orientation of body i
{
const Vector3s ntilde0{ m_r0.cross( Eigen::Map<const Vector3s>{ t.data() } ) };
D.insert( 3 * ( m_idx0 + nbodies ) + 0, column ) = ntilde0.x();
D.insert( 3 * ( m_idx0 + nbodies ) + 1, column ) = ntilde0.y();
D.insert( 3 * ( m_idx0 + nbodies ) + 2, column ) = ntilde0.z();
}
// Effect on center of mass of body j
D.insert( 3 * m_idx1 + 0, column ) = - t.x();
D.insert( 3 * m_idx1 + 1, column ) = - t.y();
D.insert( 3 * m_idx1 + 2, column ) = - t.z();
// Effect on orientation of body j
{
const Vector3s ntilde1{ m_r1.cross( Eigen::Map<const Vector3s>{ t.data() } ) };
D.insert( 3 * ( m_idx1 + nbodies ) + 0, column ) = - ntilde1.x();
D.insert( 3 * ( m_idx1 + nbodies ) + 1, column ) = - ntilde1.y();
D.insert( 3 * ( m_idx1 + nbodies ) + 2, column ) = - ntilde1.z();
}
}
VectorXs Constraint::computeWorldSpaceContactNormal( const VectorXs& q ) const
{
VectorXs n;
getWorldSpaceContactNormal( q, n );
assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );
return n;
}
static VectorXs parallelTransport2D( const VectorXs& n0, const VectorXs& n1, const VectorXs& t0 )
{
assert( fabs( n0.norm() - 1.0 ) <= 1.0e-6 ); assert( fabs( n1.norm() - 1.0 ) <= 1.0e-6 );
// x is cos of angle, y is sin of angle
const Vector2s r{ n0.dot( n1 ), MathUtilities::cross( n0, n1 ) };
assert( fabs( r.norm() - 1.0 ) <= 1.0e-6 );
// Rotate t0
VectorXs t1{ 2 };
t1( 0 ) = r.x() * t0.x() - r.y() * t0.y();
t1( 1 ) = r.y() * t0.x() + r.x() * t0.y();
assert( fabs( t0.norm() - t1.norm() ) <= 1.0e-6 );
// TODO: Assert n0,t0 angle is same as n1,t1 angle
return t1;
}
// 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;
}
}
// Detects whether two particles are overlapping (including the radii of each)
// and approaching.
// If the two particles overlap and are approaching, returns true and sets
// the vector n to be the vector between the first and second particle.
// Inputs:
// scene: The scene data structure. The positions and radii of the particles
// can be obtained from here.
// idx1: The index of the first particle. (Ie, the degrees of freedom
// corresponding to this particle are entries 2*idx1 and 2*idx1+1 in
// scene.getX().
// idx2: The index of the second particle.
// Outputs:
// n: The vector between the two particles.
// Returns true if the two particles overlap and are approaching.
bool SimpleCollisionHandler::detectParticleParticle(TwoDScene &scene, int idx1, int idx2, VectorXs &n)
{
VectorXs x1 = scene.getX().segment<2>(2*idx1);
VectorXs x2 = scene.getX().segment<2>(2*idx2);
n = x2-x1;
if(n.norm() < scene.getRadius(idx1) + scene.getRadius(idx2))
{
double relvel = (scene.getV().segment<2>(2*idx1)-scene.getV().segment<2>(2*idx2)).dot(n);
if(relvel > 0)
return true;
}
return false;
}
// 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-edge
// pair to the total.
// Read the positions of the particle and edge endpoints from the input
// variable x.
// Inputs:
// x: The positions of the particles in the scene.
// vidx: The index of the particle.
// eidx: The index of the edge, i.e. the indices of the particle making up the
// endpoints of the edge are given by m_scene.getEdge(eidx).first and
// m_scene.getEdges(eidx).second.
// Outputs:
// gradE: The total gradient of penalty force. *ADD* the particle-edge
// gradient to this total gradient.
void PenaltyForce::addParticleEdgeGradEToTotal(const VectorXs &x, int vidx, int eidx, VectorXs &gradE)
{
// VectorXs x1 = x.segment<2>(2*vidx);
// VectorXs x2 = x.segment<2>(2*m_scene.getEdge(eidx).first);
// VectorXs x3 = x.segment<2>(2*m_scene.getEdge(eidx).second);
//
// double r1 = m_scene.getRadius(vidx);
// double r2 = m_scene.getEdgeRadii()[eidx];
//
// // your implementation here
//
VectorXs x1 = x.segment<2>(2*vidx);
VectorXs x2 = x.segment<2>(2*m_scene.getEdge(eidx).first);
VectorXs x3 = x.segment<2>(2*m_scene.getEdge(eidx).second);
double r1 = m_scene.getRadius(vidx);
double r2 = m_scene.getEdgeRadii()[eidx];
double alpha = (x1-x2).dot(x3-x2)/(x3-x2).dot(x3-x2);
if(alpha < 0)
alpha = 0;
if(alpha > 1)
alpha = 1;
VectorXs n = x2 + alpha*(x3-x2) - x1;
VectorXs nhat=n;
nhat.normalize();
if(n.norm() < r1+r2 + m_thickness)
{
gradE.segment<2>(2*vidx) -= m_k * (n.norm() - r1 - r2 - m_thickness) * nhat;
gradE.segment<2>(2*m_scene.getEdge(eidx).first) += m_k*(1-alpha)*(n.norm()-r1-r2 - m_thickness)*nhat;
gradE.segment<2>(2*m_scene.getEdge(eidx).second) += m_k*alpha*(n.norm()-r1-r2 - m_thickness)*nhat;
}
}
// TODO: Unspecialize from 3D
void Constraint::computeNormalAndRelVelAlignedTangent( const VectorXs& q, const VectorXs& v, VectorXs& n, VectorXs& t, VectorXs& tangent_rel_vel ) const
{
MatrixXXsc basis;
computeBasis( q, v, basis );
assert( basis.rows() == basis.cols() ); assert( basis.rows() == 3 );
n = basis.col( 0 );
t = basis.col( 1 );
// Compute the relative velocity
tangent_rel_vel = computeRelativeVelocity( q, v );
// Project out normal component of relative velocity
tangent_rel_vel = tangent_rel_vel - tangent_rel_vel.dot( n ) * n;
#ifndef NDEBUG
// Relative velocity and tangent should be parallel
assert( Eigen::Map<Vector3s>( tangent_rel_vel.data() ).cross( Eigen::Map<Vector3s>( t.data() ) ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
// If tangent relative velocity is non-negligble, tangent should oppose
if( tangent_rel_vel.norm() > 1.0e-9 )
{
assert( fabs( tangent_rel_vel.normalized().dot( t ) + 1.0 ) <= 1.0e-6 );
}
#endif
}
// Detects whether a particle and an edge are overlapping (including the radii
// of both) 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 edge.
// Inputs:
// scene: The scene data structure.
// vidx: The index of the particle.
// eidx: The index of the edge. (Ie, the indices of particle with index e are
// scene.getEdges()[e].first and scene.getEdges()[e].second.)
// Outputs:
// n: The shortest vector between the particle and the edge.
// Returns true if the two objects overlap and are approaching.
bool SimpleCollisionHandler::detectParticleEdge(TwoDScene &scene, int vidx, int eidx, VectorXs &n)
{
VectorXs x1 = scene.getX().segment<2>(2*vidx);
VectorXs x2 = scene.getX().segment<2>(2*scene.getEdges()[eidx].first);
VectorXs x3 = scene.getX().segment<2>(2*scene.getEdges()[eidx].second);
double alpha = (x1-x2).dot(x3-x2)/(x3-x2).dot(x3-x2);
alpha = std::min(1.0, std::max(0.0, alpha));
VectorXs closest = x2 + alpha*(x3-x2);
n = closest-x1;
if(n.norm() < scene.getRadius(vidx)+scene.getEdgeRadii()[eidx])
{
VectorXs v1 = scene.getV().segment<2>(2*vidx);
VectorXs v2 = scene.getV().segment<2>(2*scene.getEdges()[eidx].first);
VectorXs v3 = scene.getV().segment<2>(2*scene.getEdges()[eidx].second);
double relvel = (v1 - v2 - alpha*(v3-v2)).dot(n);
if(relvel > 0)
{
return true;
}
}
return false;
}