void DragDampingForce::addHessVToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, MatrixXs& hessE )
{
assert( x.size() == v.size() );
assert( x.size() == m.size() );
assert( x.size() == hessE.rows() );
assert( x.size() == hessE.cols() );
assert( x.size()%2 == 0 );
// Compute the force Jacboian here!
hessE.diagonal().array() += m_b;
}
void VortexForce::addHessXToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, MatrixXs& hessE )
{
assert( x.size() == v.size() );
assert( x.size() == m.size() );
assert( x.size() == hessE.rows() );
assert( x.size() == hessE.cols() );
assert( x.size()%2 == 0 );
// scalar m1 = m(2*m_particles.second);
// scalar m2 = m(2*m_particles.first);
//
// Vector2s nhat = x.segment<2>(2*m_particles.second)-x.segment<2>(2*m_particles.first);
// scalar r = nhat.norm();
// assert( r != 0.0 );
// nhat /= r;
//
// Matrix2s entry = Matrix2s::Identity()-3.0*nhat*nhat.transpose();
// entry *= m_G*m1*m2/r*r*r;
//
// hessE.block<2,2>(2*m_particles.first,2*m_particles.first) += entry;
// hessE.block<2,2>(2*m_particles.second,2*m_particles.second) += entry;
// hessE.block<2,2>(2*m_particles.first,2*m_particles.second) -= entry;
// hessE.block<2,2>(2*m_particles.second,2*m_particles.first) -= entry;
std::cerr << outputmod::startred << "ERROR IN VORTEXFORCE: " << outputmod::endred << "No addHessXToTotal defined for VortexForce." << std::endl;
exit(1);
}
void DragDampingForce::addHessXToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, MatrixXs& hessE )
{
assert( x.size() == v.size() );
assert( x.size() == m.size() );
assert( x.size() == hessE.rows() );
assert( x.size() == hessE.cols() );
assert( x.size()%3 == 0 );
// Nothing to do.
}
void TwoDScene::accumulateddUdxdv( MatrixXs& A, const VectorXs& dx, const VectorXs& dv )
{
assert( A.rows() == m_x.size() );
assert( A.cols() == m_x.size() );
assert( dx.size() == dv.size() );
assert( dx.size() == 0 || dx.size() == A.rows() );
if( dx.size() == 0 ) for( std::vector<Force*>::size_type i = 0; i < m_forces.size(); ++i ) m_forces[i]->addHessVToTotal( m_x, m_v, m_m, A );
else for( std::vector<Force*>::size_type i = 0; i < m_forces.size(); ++i ) m_forces[i]->addHessVToTotal( m_x+dx, m_v+dv, m_m, A );
}
void VortexForce::addHessVToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, MatrixXs& hessE )
{
assert( x.size() == v.size() );
assert( x.size() == m.size() );
assert( x.size() == hessE.rows() );
assert( x.size() == hessE.cols() );
assert( x.size()%2 == 0 );
std::cerr << outputmod::startred << "ERROR IN VORTEXFORCE: " << outputmod::endred << "No addHessXToTotal defined for VortexForce." << std::endl;
exit(1);
}
void DragDampingForce::addHessVToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, MatrixXs& hessE )
{
assert( x.size() == v.size() );
assert( x.size() == m.size() );
assert( x.size() == hessE.rows() );
assert( x.size() == hessE.cols() );
assert( x.size()%3 == 0 );
Matrix3s J = -m_b * Matrix3s::Identity();
for (int i=0; i < x.size(); i += 3) {
for (int j=0; j < x.size(); j += 3) {
hessE.block<3, 3>(i, j) += J;
}
}
}
bool LinearizedImplicitEuler::stepScene( TwoDScene& scene, scalar dt )
{
VectorXs& x = scene.getX();
VectorXs& v = scene.getV();
const VectorXs& m = scene.getM();
assert(x.size() == v.size());
assert(x.size() == m.size());
// Implement implicit euler here!
VectorXs F(x.size());
F.setZero();
scene.accumulateGradU(F, dt*v, VectorXs(x.size()).setZero());
// Force is negative the energy gradient
F *= -1.0;
MatrixXs M(x.size(), x.size());
M.setZero();
for (int i=0;i<x.size();i+=2) {
M(i, i) = m[i];
M(i+1, i+1) = m[i+1];
}
MatrixXs MatQ(x.size(), x.size()); // dF/dq
MatQ.setZero();
scene.accumulateddUdxdx(MatQ, dt*v, VectorXs(x.size()).setZero());
MatrixXs MatV(x.size(), x.size()); // dF/dv
MatV.setZero();
scene.accumulateddUdxdv(MatV, dt*v, VectorXs(x.size()).setZero());
MatrixXs A = M-(dt*dt*MatQ+dt*MatV);
// Zero the force for fixed DoFs
for( int i = 0; i < scene.getNumParticles(); ++i ) if( scene.isFixed(i) ) F.segment<2>(2*i).setZero();
for( int i = 0; i < scene.getNumParticles(); ++i ) if( scene.isFixed(i) ) {
A.row(2*i).setZero();
A.row(2*i+1).setZero();
A.col(2*i).setZero();
A.col(2*i+1).setZero();
A(2*i, 2*i) = 1;
A(2*i+1, 2*i+1) = 1;
}
VectorXs dv = A.fullPivLu().solve(dt*F);
v = v+dv;
x = x+v*dt;
return true;
}
