// 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
int eidx1 = m_scene.getEdge(eidx).first;
int eidx2 = m_scene.getEdge(eidx).second;
scalar alpha = (x1-x2).dot(x3-x2)/(x3-x2).squaredNorm();
if(alpha<0) alpha = 0;
if(alpha>1) alpha = 1;
Vector2s n = alpha*x3+(1-alpha)*x2-x1;
if(n.norm()>r1+r2+m_thickness) return;
Vector2s nhat = n/n.norm();
MatrixXs gradn(2, 6);
Matrix2s I = I.Identity();
gradn<<-I, (1-alpha)*I, alpha*I;
VectorXs gradV = m_k*(n.norm()-r1-r2-m_thickness)*gradn.transpose()*nhat;
gradE(2*vidx) += gradV(0);
gradE(2*vidx+1) += gradV(1);
gradE(2*eidx1) += gradV(2);
gradE(2*eidx1+1) += gradV(3);
gradE(2*eidx2) += gradV(4);
gradE(2*eidx2+1) += gradV(5);
}
bool MathUtilities::isRightHandedOrthoNormal( const Vector2s& a, const Vector2s& b, const scalar& tol )
{
// All basis vectors should be unit
if( fabs( a.norm() - 1.0 ) > tol ) { return false; }
if( fabs( b.norm() - 1.0 ) > tol ) { return false; }
// All basis vectors should be mutually orthogonal
if( fabs( a.dot( b ) ) > tol ) { return false; }
// Coordinate system should be right handed
assert( fabs( cross( a, b ) - 1.0 ) <= 1.0e-6 || fabs( cross( a, b ) + 1.0 ) <= 1.0e-6 );
if( cross( a, b ) <= 0.0 ) { return false; }
return true;
}
void VortexForce::addGradEToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, VectorXs& gradE )
{
assert( x.size() == v.size() );
assert( x.size() == m.size() );
assert( x.size() == gradE.size() );
assert( x.size()%2 == 0 );
assert( m_particles.first >= 0 ); assert( m_particles.first < x.size()/2 );
assert( m_particles.second >= 0 ); assert( m_particles.second < x.size()/2 );
Vector2s rhat = x.segment<2>(2*m_particles.second)-x.segment<2>(2*m_particles.first);
scalar r = rhat.norm();
assert( r != 0.0 );
rhat /= r;
rhat *= m_kbs;
// Rotate rhat 90 degrees clockwise
scalar temp = rhat.x();
rhat.x() = -rhat.y();
rhat.y() = temp;
rhat -= v.segment<2>(2*m_particles.second)-v.segment<2>(2*m_particles.first);
rhat /= r*r;
rhat *= m_kvc;
gradE.segment<2>(2*m_particles.first) -= -rhat;
gradE.segment<2>(2*m_particles.second) += -rhat;
}
scalar HertzianPenaltyForce::computePotential( const VectorXs& q, const SparseMatrixsc& M, const VectorXs& r ) const
{
assert( q.size() % 2 == 0 ); assert( q.size() == M.rows() ); assert( q.size() == M.cols() ); assert( r.size() == q.size() / 2 );
scalar U{ 0.0 };
// For each ball
for( unsigned ball0 = 0; ball0 < r.size(); ++ball0 )
{
// For each subsequent ball
for( unsigned ball1 = ball0 + 1; ball1 < r.size(); ++ball1 )
{
// Compute the total radius
const scalar total_radius{ r(ball0) + r(ball1) };
// Compute a vector pointing from ball0 to ball1
const Vector2s n{ q.segment<2>( 2 * ball1 ) - q.segment<2>( 2 * ball0 ) };
// If the squared distance is greater or equal to the sum of the radii squared, no force
if( n.squaredNorm() > total_radius * total_radius )
{
continue;
}
// Compute the penetration depth
const scalar delta{ n.norm() - total_radius };
assert( delta < 0.0 );
// U = 0.5 * k * pen_depth^(5/2)
U += 0.5 * m_k * std::pow( -delta, scalar( 2.5 ) );
}
}
return U;
}
// 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;
VectorXs px = m_scene.getHalfplane(pidx).first;
// your implementation here
Vector2s n = (px-x1).dot(nh)/nh.squaredNorm()*nh;
Vector2s nhat = n/n.norm();
scalar r1 = m_scene.getRadius(vidx);
if(n.norm()>r1+m_thickness) return;
Matrix2s gradn = -nh*nh.transpose()/nh.squaredNorm();
Vector2s gradV = m_k*(n.norm()-r1-m_thickness)*gradn.transpose()*nhat;
gradE(2*vidx) += gradV(0);
gradE(2*vidx+1) += gradV(1);
}
void TeleportedCircleCircleConstraint::computeContactBasis( const VectorXs& q, const VectorXs& v, MatrixXXsc& basis ) const
{
assert( fabs( m_n.norm() - 1.0 ) <= 1.0e-6 );
const Vector2s t{ -m_n.y(), m_n.x() };
assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 ); assert( fabs( m_n.dot( t ) ) <= 1.0e-6 );
basis.resize( 2, 2 );
basis.col( 0 ) = m_n;
basis.col( 1 ) = t;
}
// 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
Vector2s n = x2-x1;
if(n.norm()>r1+r2+m_thickness) return;
Vector2s nhat = n/n.norm();
MatrixXs gradn(2, 4);
Matrix2s I = I.Identity();
gradn<<-I, I;
VectorXs gradV = m_k*(n.norm()-r1-r2-m_thickness)*gradn.transpose()*nhat;
gradE(2*idx1) += gradV(0);
gradE(2*idx1+1) += gradV(1);
gradE(2*idx2) += gradV(2);
gradE(2*idx2+1) += gradV(3);
}
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;
}
// TODO: Don't do the if here, have children do what they need to do
scalar Constraint::computeLambda( const VectorXs& q, const VectorXs& v ) const
{
MatrixXXsc basis;
computeBasis( q, v, basis );
assert( basis.rows() == basis.cols() );
assert( basis.cols() == 2 || basis.cols() == 3 );
const VectorXs rel_vel = computeRelativeVelocity( q, v );
assert( rel_vel.size() == basis.rows() );
if( basis.cols() == 3 )
{
const Vector2s tangent_vel( rel_vel.dot( basis.col( 1 ) ), rel_vel.dot( basis.col( 2 ) ) );
return tangent_vel.norm();
}
else if( basis.cols() == 2 )
{
return fabs( rel_vel.dot( basis.col( 1 ) ) );
}
std::cerr << "Unhandled case in Constraint::computeLambda" << std::endl;
std::exit( EXIT_FAILURE );
}
