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;
}
示例#2
0
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;
}