bool IMEXIntegrator::integrate(Clock& c) {
PROFILER_START("Total");
const real ConvergenceThreshold = 0.0125 * r.numCPs();
const real ConvergenceTolerance = 1.25 * r.numCPs();
bool evalSuccess = false;
bool newtonConverge = false;
int newtonIterations = 0;
while (!evalSuccess) {
VecXe Fx = VecXe::Zero(r.numDOF());
VecXe FxEx = VecXe::Zero(r.numDOF());
VecXe dqdot = VecXe::Zero(r.numDOF());
VecXe sol = VecXe::Zero(r.numDOF());
Eigen::SparseMatrix<real> GradFx(r.numDOF(), r.numDOF());
std::vector<Triplet> triplets;
// TODO: Figure out a thread-safe way to fill this
// TODO: Query energies to figure out a good estimate
std::size_t numTriplets = 9*9*r.numIntCPs();
// Calculate Fx contribution from Explicit energies
// (these do not change between Newton iterations)
for (RodEnergy* e : energies) {
if (e->evalType() == Explicit) {
evalSuccess = e->eval(&FxEx);
CHECK_NAN_VEC(FxEx);
if (!evalSuccess) break;
}
}
if (!evalSuccess) continue;
// Perform Newton iteration to solve IMEX equations
while (!newtonConverge) {
triplets.clear();
triplets.reserve(numTriplets);
Fx = FxEx;
// Find offset for implicit evaluation
VecXe offset = c.timestep() * (r.cur().vel + dqdot);
// Add up energies
for (RodEnergy* e : energies) {
if (e->evalType() == Implicit) {
evalSuccess = e->eval(&Fx, &triplets, &offset);
if (!evalSuccess) break;
}
}
if (!evalSuccess) break;
Fx *= -c.timestep();
Fx += r.getMass().sparse * dqdot;
CHECK_NAN_VEC(Fx);
// Test for convergence
real residual = Fx.norm();
if (residual < ConvergenceThreshold) {
newtonConverge = true;
break;
} else if (newtonIterations > 4) {
std::cout << "resid: " << residual << "\n";
if (residual < ConvergenceTolerance) {
newtonConverge = true;
}
break;
}
// Error too high; perform one Newton iteration to update dqdot
newtonIterations++;
// Create sparse Jacobian of Fx
GradFx.setFromTriplets(triplets.begin(), triplets.end()); // sums up duplicates automagically
GradFx *= -c.timestep() * c.timestep();
GradFx += r.getMass().sparse;
CHECK_NAN_VEC(GradFx.toDense());
PROFILER_START("CG Solver");
Eigen::ConjugateGradient<Eigen::SparseMatrix<real>, Eigen::Upper,
Eigen::IncompleteLUT<real>> cg;
cg.compute(GradFx);
assert(cg.preconditioner().info() == Eigen::ComputationInfo::Success);
VecXe guess = sol;
sol = cg.solveWithGuess(-Fx, guess); // H(x_n) (x_n+1 - x_n) = -F(x_n)
if (cg.info() == Eigen::NoConvergence) {
if (c.canDecreaseTimestep()) {
PROFILER_STOP("CG Solver");
c.suggestTimestep(c.timestep()/2.0);
std::cout << "No convergence in CG solver. New timestep: " << c.timestep() << "\n";
evalSuccess = false;
break;
}
std::cerr << "No convergence!! Newton iterate: " << newtonIterations << "\n";
std::cerr << "Fx has NaN: " << Fx.hasNaN() << "\n";
std::cerr << "GradFx has NaN: " << GradFx.toDense().hasNaN() << "\n";
std::cerr << "Fx max coeff: " << Fx.maxCoeff() << "\n";
std::cerr << "GradFx max coeff: " << GradFx.toDense().maxCoeff() << "\n";
assert(false);
}
dqdot += sol;
PROFILER_STOP("CG Solver");
}
// Update rod positions
if (newtonConverge) {
#ifdef NEWMARK_BETA
// Newmark-Beta update
const real gamma = 0.5;
const real beta = 0.25;
r.next().dVel = dqdot;
r.next().vel = r.cur().vel + (1.0-gamma) * r.cur().dVel + gamma * dqdot;
r.next().pos = r.cur().pos + c.timestep() * (r.cur().vel +
((1.0-2.0*beta) / 2.0) * r.cur().dVel +
beta * dqdot);
#else // ifdef NEWMARK_BETA
// Update changes to position and velocity
r.next().vel = r.cur().vel + dqdot;
r.next().pos = r.cur().pos + c.timestep() * r.next().vel;
#endif // ifdef NEWMARK_BETA
}
}
PROFILER_STOP("Total");
PROFILER_PRINT_ELAPSED();
PROFILER_RESET_ALL();
return newtonConverge;
}
bool idCameraDef::getCameraInfo(long time, idVec3 &origin, idVec3 &direction, float *fv)
{
char buff[1024];
if((time - startTime) / 1000 > totalTime)
{
return false;
}
for(int i = 0; i < events.Num(); i++)
{
if(time >= startTime + events[i]->getTime() && !events[i]->getTriggered())
{
events[i]->setTriggered(true);
if(events[i]->getType() == idCameraEvent::EVENT_TARGET)
{
setActiveTargetByName(events[i]->getParam());
getActiveTarget()->start(startTime + events[i]->getTime());
//Com_Printf("Triggered event switch to target: %s\n",events[i]->getParam());
}
else if(events[i]->getType() == idCameraEvent::EVENT_TRIGGER)
{
// empty!
}
else if(events[i]->getType() == idCameraEvent::EVENT_FOV)
{
memset(buff, 0, sizeof(buff));
strcpy(buff, events[i]->getParam());
const char *param1 = strtok(buff, " \t,\0");
const char *param2 = strtok(NULL, " \t,\0");
float len = (param2) ? atof(param2) : 0;
float newfov = (param1) ? atof(param1) : 90;
fov.reset(fov.getFOV(time), newfov, time, len);
//*fv = fov = atof(events[i]->getParam());
}
else if(events[i]->getType() == idCameraEvent::EVENT_FADEIN)
{
float time = atof(events[i]->getParam());
Cbuf_AddText(va("fade 0 0 0 0 %f", time));
Cbuf_Execute();
}
else if(events[i]->getType() == idCameraEvent::EVENT_FADEOUT)
{
float time = atof(events[i]->getParam());
Cbuf_AddText(va("fade 0 0 0 255 %f", time));
Cbuf_Execute();
}
else if(events[i]->getType() == idCameraEvent::EVENT_CAMERA)
{
memset(buff, 0, sizeof(buff));
strcpy(buff, events[i]->getParam());
const char *param1 = strtok(buff, " \t,\0");
const char *param2 = strtok(NULL, " \t,\0");
if(param2)
{
loadCamera(atoi(param1), va("cameras/%s.camera", param2));
startCamera(time);
}
else
{
loadCamera(0, va("cameras/%s.camera", events[i]->getParam()));
startCamera(time);
}
return true;
}
else if(events[i]->getType() == idCameraEvent::EVENT_STOP)
{
return false;
}
}
}
origin = *cameraPosition->getPosition(time);
CHECK_NAN_VEC(origin);
*fv = fov.getFOV(time);
idVec3 temp = origin;
int numTargets = targetPositions.Num();
if(numTargets == 0)
{
// empty!
}
else
{
temp = *getActiveTarget()->getPosition(time);
}
temp -= origin;
temp.Normalize();
direction = temp;
return true;
}
