void
NewtonSolution::init(const Manager &manager)
{
_y = manager.getActiveY();
_z = manager.getCurrentZ();
warmStart(manager);
if (manager.inPhase2()) {
_ss = manager.getPointGen()._ss;
_zs = manager.getPointGen()._zs;
/* double f2 = manager.getSmoothObj();
_zs = f2;//std::max(f2*(1 + 1e-10), 1.0);
_ss = 100;//_zs - f2;
_s0 = 100; */
} else {
_ss = manager.getPointGen()._ss;
_zs = manager.getPointGen()._zs;
}
}
void PhysicsWorld::solve()
{
if (m_narrowphase->contacts().empty())
{
return;
}
/**
* Warm starting
*/
for (auto & contact : m_narrowphase->contacts())
{
warmStart(*contact);
}
/**
* Iterative solving
*/
// std::cout << "Solve velocities:" << std::endl;
for (auto i = 0; i < m_numVelocityIterations; i++)
{
for (auto & contact : m_narrowphase->contacts())
{
solveContactVelocities(*contact);
}
}
std::cout << "Solve positions:" << std::endl;
for (auto i = 0; i < m_numPositionIterations; i++)
{
for (auto & contact : m_narrowphase->contacts())
{
solvePositions(*contact);
}
}
}
