Esempio n. 1
0
int luaV_tostring (lua_State *L, StkId obj) {
  if (!ttisnumber(obj))
    return 0;
  else {
    char s[LUAI_MAXNUMBER2STR];
    lua_Number n = nvalue(obj);
    // SPRING -- synced safety change
    //        -- need a custom number formatter?
    if (isfinite(n)) {
      lua_number2str(s, n);
    }
    else {
      if (isnan(n)) {
        strcpy(s, "nan");
      }
      else {
        const int inf_type = isinf(n);
        if (inf_type == 1) {
          strcpy(s, "+inf");
        } else if (inf_type == -1) {
          strcpy(s, "-inf");
        } else {
          strcpy(s, "weird_number");
        }
      }
    } 
    setsvalue2s(L, obj, luaS_new(L, s));
    return 1;
  }
}
/* solve a multi contact problem by using a global successive overrelaxation method
 * with a local nonlinear solver
 */
prox_result reference_sequential_g_sor_prox::solve_multi_contact_problem_sor() {
  bool converged          = false;
  bool diverged           = false;
  unsigned int iteration  = 0;
  while(!converged && !diverged && iteration < m_max_global_iterations) {
    converged = true;
    
    for(index_t i = 0; i < m_contacts.size(); ++i) {
      contact & ci = m_contacts[i];
      vec3 rhs = ci.c;
      index_t cbegin = m_gij_rows[i];
      index_t cend   = m_gij_rows[i + 1];
      
      //step 0. get contributions from all the other contacts (gij off-diagonal terms)
      for(index_t j = cbegin; j < cend; ++j) {
        index_t cj = m_gij_columns[j];
        rhs = rhs + m_gij_blocks[j] * m_percussions[cj];
      }
      
      vec3 pold = m_percussions[i];
      //step 1. solve a single contact under the assumption, that all others are known
      vec3 pnew = solve_one_contact_problem_alart_curnier(ci, pold, rhs, m_tol_rel, m_tol_abs);
      
      using std::abs;
      //step 2. check for global convergence
      converged &= 
      abs(pnew[0] - pold[0]) <= m_tol_rel * abs(pnew[0]) + m_tol_abs
      &&  abs(pnew[1] - pold[1]) <= m_tol_rel * abs(pnew[1]) + m_tol_abs
      &&  abs(pnew[2] - pold[2]) <= m_tol_rel * abs(pnew[2]) + m_tol_abs;
      //and check whether a force became infinite or NaN
      using std::isinf; using std::isnan;
      diverged 
      |= isinf(pnew[0]) || isnan(pnew[0])
      || isinf(pnew[1]) || isnan(pnew[1])
      || isinf(pnew[2]) || isnan(pnew[2]);
      m_percussions[i] = pnew;
    }
    ++iteration;
  }
  std::cout << "done\n    # iterations = " << iteration << std::endl;
  return
  converged ? CONVERGED
  : (diverged ? DIVERGED
     : (!(iteration < m_max_global_iterations) ? ITERATION_LIMIT_REACHED
        /*: (time_limit_reached ? TIME_LIMIT_REACHED */: OOPS/*)*/));
}
Esempio n. 3
0
double Stat::requestRate(const uint64_t elapsed_t) const {
  double rate = (getCount() / (double)elapsed_t);
  if (isinf(rate)) {
    return 1.0;
  } else {
    return rate;
  }
}
 std::pair<bool, bool> multicolor_parallel_g_sor_prox::work_function(
                                            sub_problem const & sub,
                                            index_t & l_i,
                                            index_t & g_i,
                                            index_t l_end,
                                            std::vector<vec3> & percussions,
                                            real tol_rel, real tol_abs,
                                            index_t max_local_iterations
                                            ) {
   bool diverged   = false;
   bool converged  = true;
   
   for(; l_i < l_end; ++l_i, ++g_i) {
     contact const & ci = sub.contacts[l_i];
     vec3 rhs = ci.c;
     index_t cbegin = sub.gij_rows[l_i];
     index_t cend   = sub.gij_rows[l_i + 1];
     
     //step 0. get contributions from all the other contacts (gij off-diagonal terms)
     for(index_t j = cbegin; j < cend; ++j) {
       index_t g_j = sub.gij_columns[j];
       rhs = rhs + sub.gij_blocks[j] * percussions[g_j];
     }
     
     vec3 pold = percussions[g_i];
     //step 1. solve a single contact under the assumption, that all others are known
     vec3 pnew = solve_one_contact_problem_alart_curnier(ci, pold, rhs, tol_rel, tol_abs, max_local_iterations);
     
     using std::abs;
     //step 2. check for global convergence
     converged &= 
     abs(pnew[0] - pold[0]) <= tol_rel * abs(pnew[0]) + tol_abs
     && abs(pnew[1] - pold[1]) <= tol_rel * abs(pnew[1]) + tol_abs
     && abs(pnew[2] - pold[2]) <= tol_rel * abs(pnew[2]) + tol_abs;
     //and check whether a force became infinite or NaN
     using std::isinf; using std::isnan;
     diverged 
     |= isinf(pnew[0]) || isnan(pnew[0])
     || isinf(pnew[1]) || isnan(pnew[1])
     || isinf(pnew[2]) || isnan(pnew[2]);
     percussions[g_i] = pnew;
     
   }
   return std::make_pair(converged, diverged);
 }
Esempio n. 5
0
// function that standarize printing NaN and Inf values on
// Windows (where they are in 1.#INF, 1.#NAN format) and all
// others platform
inline void
safe_double_print (double val)
{
    if (isnan (val))
        std::cout << "nan";
    else if (isinf (val))
        std::cout << "inf";
    else
        std::cout << val;
    std::cout << '\n';
}
Esempio n. 6
0
void R2d2lri::setBoundary(const Boundary& boundary)
{
	if(boundary.isValid())
	{
		const double a = boundary.l(0);
		const double b = boundary.u(0);
		this->a = isinf(a) ? copysign(INFINITE, a) : a;
		this->b = isinf(b) ? copysign(INFINITE, b) : b;

		adapterSetFunctor(g, Constant(boundary.l(1)));
		adapterSetFunctor(h, Constant(boundary.u(1)));

		if(this->hasIntegrand())
		{
			this->setIntegrand(this->getIntegrand());
		}
	}
	else
	{
		this->a = NaN;
		this->b = NaN;
	}
}
Esempio n. 7
0
void EntityFollowSystem::update() {
    for (Entity& e : this->getEntities()) {
        // For each entity we have...
        EntityFollowComponent& ef = e.getComponent<EntityFollowComponent>();
        if (isinf(ef.maxSpeed) && ef.target.isValid() && ef.target.hasComponent<PositionComponent>()) {
            // If the entity we're trying to face exists and has a position...

            PositionComponent& pos = e.getComponent<PositionComponent>();
            PositionComponent& follow_pos = ef.target.getComponent<PositionComponent>();

            pos.position = follow_pos.position;
        }
    }
}
Esempio n. 8
0
String::String(const double d) {
    if (isinf(d)) {
        if (copysign(1.0, d) == -1.0) {
            fromAscii("-Infinity");
        } else {
            fromAscii("Infinity");
        }
    } else if (isnan(d)) {
        fromAscii("NaN");
    } else {
        char * buf = static_cast<char*>(GC_MALLOC_ATOMIC(20));
        sprintf(buf,"%.16g",d);
        fromAscii(buf);
    }
}
Esempio n. 9
0
String::String(const float f) {
    if (isinf(f)) {
        if (copysign(1.0, f) == -1.0) {
            fromAscii("-Infinity");
        } else {
            fromAscii("Infinity");
        }
    } else if (isnan(f)) {
        fromAscii("NaN");
    } else {
        char * buf = static_cast<char*>(GC_MALLOC_ATOMIC(20));
        sprintf(buf,"%.7g",f);
        fromAscii(buf);
    }
}
Esempio n. 10
0
static int math_modf (lua_State *L) {
  // FIXME -- streflop does not have modf()
  // double fp = math::modf(luaL_checknumber(L, 1), &ip);
  const float in = (float)luaL_checknumber(L, 1);
  if (isnan(in)) {
    lua_pushnumber(L, in);
    lua_pushnumber(L, in);
  }
  else if (isinf(in)) {
    lua_pushnumber(L, in);
    lua_pushnumber(L, 0.0f);
  }
  else {
    const float fp = math::fmod(in, 1.0f);
    const float ip = (in - fp);
    lua_pushnumber(L, ip);
    lua_pushnumber(L, fp);
  }
  return 2;
}
Esempio n. 11
0
    typename boost::math::tools::promote_args<T_y,T_dof,T_loc,T_scale>::type
    multi_student_t_log(const Eigen::Matrix<T_y,Eigen::Dynamic,1>& y,
                        const T_dof& nu,
                        const Eigen::Matrix<T_loc,Eigen::Dynamic,1>& mu,
                        const 
                        Eigen::Matrix<T_scale,
                                      Eigen::Dynamic,Eigen::Dynamic>& Sigma) {
      static const char* function = "stan::prob::multi_student_t(%1%)";

      using stan::math::check_size_match;
      using stan::math::check_finite;
      using stan::math::check_not_nan;
      using stan::math::check_symmetric;
      using stan::math::check_positive;      
      using boost::math::tools::promote_args;
      using boost::math::lgamma;
      using stan::math::log_determinant_ldlt;
      using stan::math::mdivide_left_ldlt;
      using stan::math::LDLT_factor;
      
      typename promote_args<T_y,T_dof,T_loc,T_scale>::type lp(0.0);
      if (!check_size_match(function, 
          y.size(), "Size of random variable",
          mu.size(), "size of location parameter",
          &lp))
        return lp;
      if (!check_size_match(function, 
          y.size(), "Size of random variable",
          Sigma.rows(), "rows of scale parameter",
          &lp))
        return lp;
      if (!check_size_match(function, 
          y.size(), "Size of random variable",
          Sigma.cols(), "columns of scale parameter",
          &lp))
        return lp;
      if (!check_finite(function, mu, "Location parameter", &lp))
        return lp;
      if (!check_not_nan(function, y, "Random variable", &lp)) 
        return lp;
      if (!check_symmetric(function, Sigma, "Scale parameter", &lp))
        return lp;

      LDLT_factor<T_scale,Eigen::Dynamic,Eigen::Dynamic> ldlt_Sigma(Sigma);
      if (!ldlt_Sigma.success()) {
        std::ostringstream message;
        message << "Scale matrix is not positive definite. " 
        << "Sigma(0,0) is %1%.";
        std::string str(message.str());
        stan::math::dom_err(function,Sigma(0,0),"Scale matrix",str.c_str(),"",&lp);
        return lp;
      }

      // allows infinities
      if (!check_not_nan(function, nu, 
                         "Degrees of freedom parameter", &lp))
        return lp;
      if (!check_positive(function, nu, 
                          "Degrees of freedom parameter", &lp))
        return lp;
      
      using std::isinf;

      if (isinf(nu)) // already checked nu > 0
        return multi_normal_log(y,mu,Sigma);

      double d = y.size();

      if (include_summand<propto,T_dof>::value) {
        lp += lgamma(0.5 * (nu + d));
        lp -= lgamma(0.5 * nu);
        lp -= (0.5 * d) * log(nu);
      }

      if (include_summand<propto>::value) 
        lp -= (0.5 * d) * LOG_PI;

      using stan::math::multiply;
      using stan::math::dot_product;
      using stan::math::subtract;
      using Eigen::Array;


      if (include_summand<propto,T_scale>::value) {
        lp -= 0.5*log_determinant_ldlt(ldlt_Sigma);
      }

      if (include_summand<propto,T_y,T_dof,T_loc,T_scale>::value) {
        
        Eigen::Matrix<typename promote_args<T_y,T_loc>::type,
                      Eigen::Dynamic,
                      1> y_minus_mu = subtract(y,mu);
        Eigen::Matrix<typename promote_args<T_scale,T_y,T_loc>::type,
                      Eigen::Dynamic,
                      1> invSigma_dy = mdivide_left_ldlt(ldlt_Sigma, y_minus_mu);
        lp -= 0.5 
          * (nu + d)
          * log(1.0 + dot_product(y_minus_mu,invSigma_dy) / nu);
      }
      return lp;
    }
Esempio n. 12
0
void launcher::Launcher::loadSceneFromFile(string fileName, string initialStateGroup)
{
    Engine& engine = Engine::getInstance();

    // FIXME should we validate task file against xml schema?
    auto& ffls = FileFolderLookupService::getInstance();
    string fname = ffls.lookupFile(fileName);
    LOG_DEBUG("Loading scene from file " << fname);
// parse file
    Doc doc = Doc::fromFile(fname);
    xml::Node rootNode = doc.getRootElement();
    // read task parameters
    NodeList taskNodes = rootNode.xpath("/task");
    if( taskNodes.size() != 1 )
        THROW_INVALID_INPUT("Config file should contain one <task/> element");
    for(auto& taskNode: taskNodes)
    {
        int numberOfSnaps = lexical_cast<int>(taskNode["numberOfSnaps"]);
        int stepsPerSnap = lexical_cast<int>(taskNode["stepsPerSnap"]);
        engine.setNumberOfSnaps(numberOfSnaps);
        engine.setStepsPerSnap(stepsPerSnap);
    }

    NodeList loadPluginsList = rootNode.xpath("/task/system/loadPlugin");
    for (auto& plugin: loadPluginsList){
        engine.loadPlugin(plugin["name"]);
    }

    // reading system properties
    NodeList defaultContactCalculatorList = rootNode.xpath("/task/system/defaultContactCalculator");
    if( defaultContactCalculatorList.size() > 1 )
        THROW_INVALID_INPUT("Config file can contain only one <defaultContactCalculator/> element");
    if( defaultContactCalculatorList.size() == 1 )
    {
        xml::Node defaultContactCalculator = defaultContactCalculatorList.front();
        string type = defaultContactCalculator["type"];
        if( engine.getContactCalculator(type) == NULL )
        {
            THROW_INVALID_INPUT("Unknown contact calculator requested: " + type);
        }
        engine.replaceDefaultContactCondition( 
                new ContactCondition(NULL, new StepPulseForm(-1, -1), engine.getContactCalculator(type) ) 
        );
        LOG_INFO("Default contact calculator set to: " + type);
        if (type == "AdhesionContactDestroyCalculator")
        {
            real adhesionThreshold = lexical_cast<real>(defaultContactCalculator["adhesionThreshold"]);
            engine.getContactCondition(0)->setConditionParam(adhesionThreshold);
        }
		if (type == "ClosedFractureContactCalculator")
        {
			NodeList areaNodes = defaultContactCalculator.getChildrenByName("area");
			if (areaNodes.size() != 1)
				THROW_INVALID_INPUT("Exactly one area element can be provided for ClosedFractureCalculator");
			Area* area = readArea(areaNodes[0]);
			(static_cast<gcm::ClosedFractureContactCalculator*>
				(engine.getContactCalculator(type)))->setFracArea(area);
        }
		if (type == "OpenFractureContactCalculator")
        {
			NodeList areaNodes = defaultContactCalculator.getChildrenByName("area");
			if (areaNodes.size() != 1)
				THROW_INVALID_INPUT("Exactly one area element can be provided for ClosedFractureCalculator");
			Area* area = readArea(areaNodes[0]);
			(static_cast<gcm::OpenFractureContactCalculator*>
				(engine.getContactCalculator(type)))->setFracArea(area);
        }
    }
    
    NodeList defaultBorderCalculatorList = rootNode.xpath("/task/system/defaultBorderCalculator");
    if( defaultBorderCalculatorList.size() > 1 )
        THROW_INVALID_INPUT("Config file can contain only one <defaultBorderCalculator/> element");
    if( defaultBorderCalculatorList.size() == 1 )
    {
        xml::Node defaultBorderCalculator = defaultBorderCalculatorList.front();
        string type = defaultBorderCalculator["type"];
        if( engine.getBorderCalculator(type) == NULL )
        {
            THROW_INVALID_INPUT("Unknown border calculator requested: " + type);
        }
        engine.replaceDefaultBorderCondition( 
                new BorderCondition(NULL, new StepPulseForm(-1, -1), engine.getBorderCalculator(type) ) 
        );
        LOG_INFO("Default border calculator set to: " + type);
    }

    NodeList defaultRheoCalculatorList = rootNode.xpath("/task/system/defaultRheologyCalculator");
    if( defaultRheoCalculatorList.size() > 1 )
        THROW_INVALID_INPUT("Config file can contain only one <defaultRheologyCalculator/> element");
    if( defaultRheoCalculatorList.size() == 1 )
    {
        xml::Node defaultRheoCalculator = defaultRheoCalculatorList.front();
        string type = defaultRheoCalculator["type"];
        engine.setDefaultRheologyCalculatorType(type);
        LOG_INFO("Default rheology calculator set to: " + type);
    }

    NodeList defaultFailureModelList = rootNode.xpath("/task/system/defaultFailureModel");
    if( defaultFailureModelList.size() > 1 )
        THROW_INVALID_INPUT("Config file can contain only one <defaultFailureModelList/> element");
    if( defaultFailureModelList.size() == 1 )
    {
        xml::Node defaultFailureModel = defaultFailureModelList.front();
        string type = defaultFailureModel["type"];
        engine.setDefaultFailureModelType(type);
        LOG_INFO("Default failure model set to: " + type);
    }
    
    NodeList contactThresholdList = rootNode.xpath("/task/system/contactThreshold");
    if( contactThresholdList.size() > 1 )
        THROW_INVALID_INPUT("Config file can contain only one <contactThreshold/> element");
    if( contactThresholdList.size() == 1 )
    {
        xml::Node contactThreshold = contactThresholdList.front();
        string measure = contactThreshold["measure"];
        real value = lexical_cast<real>(contactThreshold["value"]);
        if( measure == "avgH" )
        {
            engine.setContactThresholdType(CONTACT_THRESHOLD_BY_AVG_H);
            engine.setContactThresholdFactor(value);
        }
        else if( measure == "lambdaTau" )
        {
            engine.setContactThresholdType(CONTACT_THRESHOLD_BY_MAX_LT);
            engine.setContactThresholdFactor(value);
        }
        else if( measure == "abs" )
        {
            engine.setContactThresholdType(CONTACT_THRESHOLD_FIXED);
            engine.setContactThresholdFactor(value);
        }
        else
        {
            THROW_INVALID_INPUT("Unknown units of measure for <contactThreshold/>");
        }
    }

    NodeList collisionDetectorList = rootNode.xpath("/task/system/collisionDetector");
    if( collisionDetectorList.size() > 1 )
        THROW_INVALID_INPUT("Config file can contain only one <collisionDetector/> element");
    if( collisionDetectorList.size() == 1 )
    {
        xml::Node collisionDetector = collisionDetectorList.front();
        string isStatic = collisionDetector["static"];
        if( isStatic == "true" )
        {
            engine.setCollisionDetectorStatic(true);
        }
        else if( isStatic == "false" )
        {
            engine.setCollisionDetectorStatic(false);
        }
    }
    
    NodeList meshMovementList = rootNode.xpath("/task/system/meshMovement");
    if( meshMovementList.size() > 1 )
        THROW_INVALID_INPUT("Config file can contain only one <meshMovement/> element");
    if( meshMovementList.size() == 1 )
    {
        xml::Node meshMovement = meshMovementList.front();
        string meshMovementType = meshMovement["type"];
        if( meshMovementType == "none" )
        {
            engine.setMeshesMovable(false);
        }
    }
    
    NodeList timeStepList = rootNode.xpath("/task/system/timeStep");
    if( timeStepList.size() > 1 )
        THROW_INVALID_INPUT("Config file can contain only one <timeStepList/> element");
    if( timeStepList.size() == 1 )
    {
        xml::Node timeStep = timeStepList.front();
        real value = lexical_cast<real>(timeStep["multiplier"]);
        engine.setTimeStepMultiplier(value);
        LOG_INFO("Using time step multiplier: " << value);
    }
    
    NodeList plasticityTypeList = rootNode.xpath("/task/system/plasticity");
    if( plasticityTypeList.size() > 1 )
        THROW_INVALID_INPUT("Config file can contain only one <plasticity/> element");
    string plasticityType = PLASTICITY_TYPE_NONE;
    if( plasticityTypeList.size() == 1 )
    {
        plasticityType = plasticityTypeList.front()["type"];
    }
    
    NodeList failureModeList = rootNode.xpath("/task/system/failure");
    if( failureModeList.size() > 1 )
        THROW_INVALID_INPUT("Config file can contain only one <failure/> element");
    string failureMode = FAILURE_MODE_DISCRETE;
    if( failureModeList.size() == 1 )
    {
        failureMode = failureModeList.front()["mode"];
    }

    string matrixDecompositionImplementation = "numerical";
    NodeList matrixDecompositionList = rootNode.xpath("/task/system/matrixDecomposition");
    if( matrixDecompositionList.size() > 1 )
        THROW_INVALID_INPUT("Config file can contain only one <matrixDecomposition/> element");
    if( matrixDecompositionList.size() == 1 )
    {
        xml::Node matrixDecomposition = matrixDecompositionList.front();
        matrixDecompositionImplementation = matrixDecomposition["implementation"];
    }

    LOG_INFO("Using matrix decomposition: " << matrixDecompositionImplementation);

    loadMaterialLibrary("materials");
    
    // reading materials
    loadMaterialsFromXml(rootNode.xpath("/task/materials/material"));


    AABB globalScene;

    // search for bodies
    NodeList bodyNodes = rootNode.xpath("/task/bodies/body");

    // prepare basic bodies parameters
    for(auto& bodyNode: bodyNodes)
    {
        string id = bodyNode.getAttributes()["id"];
        LOG_DEBUG("Loading body '" << id << "'");
        // create body instance
        Body* body = new Body(id);
        body->setRheologyCalculatorType(engine.getDefaultRheologyCalculatorType());
        // set rheology
        NodeList rheologyNodes = bodyNode.getChildrenByName("rheology");
        if (rheologyNodes.size() > 1)
            THROW_INVALID_INPUT("Only one rheology element allowed for body declaration");
        if (rheologyNodes.size()) {
            // We can do smth here when we have more than one rheology calculators
        }

        // preload meshes for dispatcher
        NodeList meshNodes = bodyNode.getChildrenByName("mesh");
        for(auto& meshNode: meshNodes)
        {         
            string type = meshNode["type"];

            LOG_INFO("Preparing mesh for body '" << id << "'");

            AABB localScene;
            int slicingDirection;
            int numberOfNodes;

            if (type == Geo2MeshLoader::MESH_TYPE)
                Geo2MeshLoader::getInstance().preLoadMesh(meshNode, localScene, slicingDirection, numberOfNodes);
            else if (type == Msh2MeshLoader::MESH_TYPE)
                Msh2MeshLoader::getInstance().preLoadMesh(meshNode, localScene, slicingDirection, numberOfNodes);
            else if (type == Ani3D2MeshLoader::MESH_TYPE)
                Ani3D2MeshLoader::getInstance().preLoadMesh(meshNode, localScene, slicingDirection, numberOfNodes);
            else if (type == Vtu2MeshLoader::MESH_TYPE)
                Vtu2MeshLoader::getInstance().preLoadMesh(meshNode, localScene, slicingDirection, numberOfNodes);
            else if (type == Vtu2MeshZoneLoader::MESH_TYPE)
                Vtu2MeshZoneLoader::getInstance().preLoadMesh(meshNode, localScene, slicingDirection, numberOfNodes);
            else if (type == BasicCubicMeshLoader::MESH_TYPE)
                BasicCubicMeshLoader::getInstance().preLoadMesh(meshNode, localScene, slicingDirection, numberOfNodes);
            else if (type == RectangularCutCubicMeshLoader::MESH_TYPE)
                RectangularCutCubicMeshLoader::getInstance().preLoadMesh(meshNode, localScene, slicingDirection, numberOfNodes);
            else if (type == MarkeredMeshGeoLoader::MESH_TYPE)
                MarkeredMeshGeoLoader::getInstance().preLoadMesh(meshNode, localScene, slicingDirection, numberOfNodes);
            else
                THROW_UNSUPPORTED("Specified mesh loader is not supported");

            // transform meshes
            NodeList transformNodes = bodyNode.getChildrenByName("transform");
            for(auto& transformNode: transformNodes)
            {
                string transformType = transformNode["type"];
                if ( transformType == "translate" )
                {
                    real x = lexical_cast<real>(transformNode["moveX"]);
                    real y = lexical_cast<real>(transformNode["moveY"]);
                    real z = lexical_cast<real>(transformNode["moveZ"]);
                    LOG_DEBUG("Moving body: [" << x << "; " << y << "; " << z << "]");
                    localScene.transfer(x, y, z);
                } 
                if ( transformType == "scale" )
                {
                    real x0 = lexical_cast<real>(transformNode["x0"]);
                    real y0 = lexical_cast<real>(transformNode["y0"]);
                    real z0 = lexical_cast<real>(transformNode["z0"]);
                    real scaleX = lexical_cast<real>(transformNode["scaleX"]);
                    real scaleY = lexical_cast<real>(transformNode["scaleY"]);
                    real scaleZ = lexical_cast<real>(transformNode["scaleZ"]);
                    LOG_DEBUG("Scaling body: [" << x0 << "; " << scaleX << "; " 
                                        << y0 << "; " << scaleY << "; " << z0 << "; " << scaleZ << "]");
                    localScene.scale(x0, y0, z0, scaleX, scaleY, scaleZ);
                }
            }
            LOG_DEBUG("Mesh preloaded. Mesh size: " << localScene << " Number of nodes: " << numberOfNodes);

            engine.getDispatcher()->addBodyOutline(id, localScene);
            engine.getDispatcher()->addBodySlicingDirection(id, slicingDirection);
            engine.getDispatcher()->addBodyNodesNumber(id, numberOfNodes);

            if( isinf(globalScene.maxX) )
            {
                globalScene = localScene;
            }
            else
            {
                for( int k = 0; k < 3; k++ )
                {
                    if( globalScene.min_coords[k] > localScene.min_coords[k] )
                        globalScene.min_coords[k] = localScene.min_coords[k];
                    if( globalScene.max_coords[k] < localScene.max_coords[k] )
                        globalScene.max_coords[k] = localScene.max_coords[k];
                }
            }
        }

        // add body to scene
        engine.addBody(body);
    }

    engine.setScene(globalScene);
    LOG_DEBUG("Total scene: " << engine.getScene());

    // run dispatcher
    engine.getDispatcher()->prepare(engine.getNumberOfWorkers(), &globalScene);
    engine.getDataBus()->syncOutlines();
    for( int i = 0; i < engine.getNumberOfWorkers(); i++)
    {
        LOG_DEBUG("Area scheduled for worker " << i << ": " << *(engine.getDispatcher()->getOutline(i)));
    }

    // read meshes for all bodies
    for(auto& bodyNode: bodyNodes)
    {
        string id = bodyNode.getAttributes()["id"];
        LOG_DEBUG("Loading meshes for body '" << id << "'");
        // get body instance
        Body* body = engine.getBodyById(id);

        // FIXME - WA - we need this to determine isMine() correctly for moved points
        real dX = 0;
        real dY = 0;
        real dZ = 0;
        NodeList tmpTransformNodes = bodyNode.getChildrenByName("transform");
        for(auto& transformNode: tmpTransformNodes)
        {
            string transformType = transformNode["type"];
            if ( transformType == "translate" )
            {
                dX += lexical_cast<real>(transformNode["moveX"]);
                dY += lexical_cast<real>(transformNode["moveY"]);
                dZ += lexical_cast<real>(transformNode["moveZ"]);
            }
            if ( transformType == "scale" )
            {
                //real x0 = lexical_cast<real>(transformNode["x0"]);
                //real y0 = lexical_cast<real>(transformNode["y0"]);
                //real z0 = lexical_cast<real>(transformNode["z0"]);
                //real scaleX = lexical_cast<real>(transformNode["scaleX"]);
                //real scaleY = lexical_cast<real>(transformNode["scaleY"]);
                //real scaleZ = lexical_cast<real>(transformNode["scaleZ"]);
            }
        }
        engine.getDispatcher()->setTransferVector(dX, dY, dZ, id);

        // load meshes
        NodeList meshNodes = bodyNode.getChildrenByName("mesh");
        for(auto& meshNode: meshNodes)
        {
            LOG_INFO("Loading mesh for body '" << id << "'");

            string type = meshNode["type"];

            Mesh* mesh = nullptr;

            if (type == Geo2MeshLoader::MESH_TYPE)
                mesh = Geo2MeshLoader::getInstance().load(meshNode, body);
            else if (type == Msh2MeshLoader::MESH_TYPE)
                mesh = Msh2MeshLoader::getInstance().load(meshNode, body);
            else if (type == Ani3D2MeshLoader::MESH_TYPE)
                mesh = Ani3D2MeshLoader::getInstance().load(meshNode, body);
            else if (type == Vtu2MeshLoader::MESH_TYPE)
                mesh = Vtu2MeshLoader::getInstance().load(meshNode, body);
            else if (type == Vtu2MeshZoneLoader::MESH_TYPE)
                mesh = Vtu2MeshZoneLoader::getInstance().load(meshNode, body);
            else if (type == BasicCubicMeshLoader::MESH_TYPE)
                mesh = BasicCubicMeshLoader::getInstance().load(meshNode, body);
            else if (type == RectangularCutCubicMeshLoader::MESH_TYPE)
                mesh = RectangularCutCubicMeshLoader::getInstance().load(meshNode, body);
            else if (type == MarkeredMeshGeoLoader::MESH_TYPE)
                mesh = MarkeredMeshGeoLoader::getInstance().load(meshNode, body);            
	    LOG_INFO("Loaded mesh for body '" << id << "', started attaching to body");
            // attach mesh to body
            body->attachMesh(mesh);
            mesh->setBodyNum( engine.getBodyNum(id) );
            LOG_INFO("Mesh '" << mesh->getId() << "' of type '" <<  type << "' created. "
                        << "Number of nodes: " << mesh->getNodesNumber() << ".");
        }

        // transform meshes
        NodeList transformNodes = bodyNode.getChildrenByName("transform");
        for(auto& transformNode: transformNodes)
        {
            string transformType = transformNode["type"];
            if( transformType == "translate" )
            {
                real x = lexical_cast<real>(transformNode["moveX"]);
                real y = lexical_cast<real>(transformNode["moveY"]);
                real z = lexical_cast<real>(transformNode["moveZ"]);
                LOG_DEBUG("Moving body: [" << x << "; " << y << "; " << z << "]");
                body->getMeshes()->transfer(x, y, z);
            }
            if ( transformType == "scale" )
            {
                real x0 = lexical_cast<real>(transformNode["x0"]);
                real y0 = lexical_cast<real>(transformNode["y0"]);
                real z0 = lexical_cast<real>(transformNode["z0"]);
                real scaleX = lexical_cast<real>(transformNode["scaleX"]);
                real scaleY = lexical_cast<real>(transformNode["scaleY"]);
                real scaleZ = lexical_cast<real>(transformNode["scaleZ"]);
                LOG_DEBUG("Scaling body: [" << x0 << "; " << scaleX << "; " 
                                << y0 << "; " << scaleY << "; " << z0 << "; " << scaleZ << "]");
                body->getMeshes()->scale(x0, y0, z0, scaleX, scaleY, scaleZ);
            }
        }

        // FIXME - Part of the WA above
        if( engine.getNumberOfWorkers() != 1 )
            engine.getDispatcher()->setTransferVector(/*-dX, -dY, -dZ,*/0, 0, 0, id);

        // set material properties
        NodeList matNodes = bodyNode.getChildrenByName("material");
        if (matNodes.size() < 1)
            THROW_INVALID_INPUT("Material not set");
        for(auto& matNode: matNodes)
        {
            string id = matNode["id"];
            // FIXME this code seems to be dead
            //Material* mat = engine.getMaterial(id);
            Mesh* mesh = body->getMeshes();

            NodeList areaNodes = matNode.getChildrenByName("area");
            int matId = engine.getMaterialIndex(id);
            usedMaterialsIds.push_back(matId);
            
            if (areaNodes.size() == 0)
            {
                mesh->setRheology( matId );
            }
            else if (areaNodes.size() == 1)
            {
                Area* matArea = readArea(areaNodes.front());
                if(matArea == NULL)
                    THROW_INVALID_INPUT("Can not read area");
                mesh->setRheology( matId, matArea );
            }
            else
            {
                THROW_INVALID_INPUT("Only one or zero area elements are allowed for material");
            }
        }
        LOG_DEBUG("Body '" << id << "' loaded");
    }

    NodeList initialStateNodes = rootNode.xpath("/task/initialState" + (initialStateGroup == "" ? "" : "[@group=\"" + initialStateGroup + "\"]"));
    if (initialStateGroup != "" && initialStateNodes.size() == 0)
        THROW_INVALID_ARG("Initial state group not found");
    for(auto& initialStateNode: initialStateNodes)
    {
        NodeList areaNodes = initialStateNode.getChildrenByName("area");
        NodeList valuesNodes = initialStateNode.getChildrenByName("values");
        NodeList pWaveNodes = initialStateNode.getChildrenByName("pWave");
        if (areaNodes.size() == 0)
            THROW_INVALID_INPUT("Area element should be provided for initial state");
        if (valuesNodes.size() > 1)
            THROW_INVALID_INPUT("Only one values element allowed for initial state");
        if (pWaveNodes.size() > 1)
            THROW_INVALID_INPUT("Only one pWave element allowed for initial state");
        if ((valuesNodes.size() == 1 && pWaveNodes.size() == 1) || (valuesNodes.size() == 0 && pWaveNodes.size() == 0))
            THROW_INVALID_INPUT("You have to provide initial state by using exactly one tag of allowed ones: values, pWave");;

        auto useValues = valuesNodes.size() == 1;
        real values[9];

        std::function<void(CalcNode&)> setter;

        if (useValues)
        {
            xml::Node valuesNode = valuesNodes.front();

            vector<string> names = {"vx", "vy", "vz", "sxx", "sxy", "sxz", "syy", "syz", "szz"};

            int i = 0;
            for (auto value_name: names)
            {
                string v = valuesNode.getAttributes()[value_name];
                values[i++] = v.empty() ? 0.0 : lexical_cast<real>(v);

            }
            
            LOG_DEBUG("Initial state values: "
                            << values[0] << " " << values[1] << " " << values[2] << " "
                            << values[3] << " " << values[4] << " " << values[5] << " "
                            << values[6] << " " << values[7] << " " << values[8] );
            
        }
        else
        {
            xml::Node pWaveNode = pWaveNodes.front();

            auto attrs = pWaveNode.getAttributes();

            auto direction = attrs["direction"];
            if (direction.empty())
                THROW_INVALID_INPUT("P-wave direction is not specified");

            vector<string> _direction;
            split(_direction, direction, is_any_of(";"));

            if (_direction.size() != 3)
                THROW_INVALID_INPUT("Invalid P-wave direction specified");

            auto dx = lexical_cast<real>(_direction[0]);
            auto dy = lexical_cast<real>(_direction[1]);
            auto dz = lexical_cast<real>(_direction[2]);

            Vector3 dir({dx, dy, dz});

            if (dx == 0.0 && dy == 0.0 && dz == 0.0)
                THROW_INVALID_INPUT("Invalid P-wave direction specified");

            auto scale = attrs["amplitudeScale"];
            if (scale.empty())
                THROW_INVALID_INPUT("P-wave amplitude scale is not specified");

            auto amplitudeScale = lexical_cast<real>(scale);
            if (amplitudeScale <= 0.0)
                THROW_INVALID_INPUT("P-wave amplitude must be positive");

            auto type = attrs["type"];
            if (type.empty())
                THROW_INVALID_INPUT("P-wave type is not specified");
            if (type != "compression" && type != "rarefaction")
                THROW_INVALID_INPUT("Invalid P-wave type specified");
            auto compression = type == "compression";

            setter = [=](CalcNode& node)
            {
                setIsotropicElasticPWave(node, dir, amplitudeScale, compression);
            };

        }
        for(auto& areaNode: areaNodes)
        {
            Area* stateArea = readArea(areaNode);
            if(stateArea == NULL)
                THROW_INVALID_INPUT("Can not read area");

            for( int i = 0; i < engine.getNumberOfBodies(); i++ )
            {
                if (useValues)
                   engine.getBody(i)->setInitialState(stateArea, values);
                else
                   engine.getBody(i)->setInitialState(stateArea, setter);
                engine.getBody(i)->getMeshes()->processStressState();
            }
        }
    }
    
    NodeList borderConditionNodes = rootNode.xpath("/task/borderCondition");
    for(auto& borderConditionNode: borderConditionNodes)
    {
        string calculator = borderConditionNode["calculator"];
        if( engine.getBorderCalculator(calculator) == NULL )
        {
            THROW_INVALID_INPUT("Unknown border calculator requested: " + calculator);
        }
        
        // FIXME_ASAP: calculators became statefull
        engine.getBorderCalculator(calculator)->setParameters( borderConditionNode );
        
        float startTime = lexical_cast<real>(borderConditionNode.getAttributeByName("startTime", "-1"));
        float duration = lexical_cast<real>(borderConditionNode.getAttributeByName("duration", "-1"));
        
        unsigned int conditionId = engine.addBorderCondition(
                new BorderCondition(NULL, new StepPulseForm(startTime, duration), engine.getBorderCalculator(calculator) ) 
        );
        LOG_INFO("Border condition created with calculator: " + calculator);
        
        NodeList areaNodes = borderConditionNode.getChildrenByName("area");
        if (areaNodes.size() == 0)
            THROW_INVALID_INPUT("Area should be specified for border condition");
        
        for(auto& areaNode: areaNodes)
        {
            Area* conditionArea = readArea(areaNode);
            if(conditionArea == NULL)
                THROW_INVALID_INPUT("Can not read area");

            for( int i = 0; i < engine.getNumberOfBodies(); i++ )
            {
                engine.getBody(i)->setBorderCondition(conditionArea, conditionId);
            }
        }
    }
    
    NodeList contactConditionNodes = rootNode.xpath("/task/contactCondition");
    for(auto& contactConditionNode: contactConditionNodes)
    {
        string calculator = contactConditionNode["calculator"];
        if( engine.getContactCalculator(calculator) == NULL )
        {
            THROW_INVALID_INPUT("Unknown border calculator requested: " + calculator);
        }
        
        float startTime = lexical_cast<real>(contactConditionNode.getAttributeByName("startTime", "-1"));
        float duration = lexical_cast<real>(contactConditionNode.getAttributeByName("duration", "-1"));
        
        unsigned int conditionId = engine.addContactCondition(
                new ContactCondition(NULL, new StepPulseForm(startTime, duration), engine.getContactCalculator(calculator) ) 
        );
        if (calculator == "AdhesionContactDestroyCalculator")
        {
            real adhesionThreshold = lexical_cast<real>(contactConditionNode["adhesionThreshold"]);
            engine.getContactCondition(conditionId)->setConditionParam(adhesionThreshold);
        }
        LOG_INFO("Contact condition created with calculator: " + calculator);
        
        NodeList areaNodes = contactConditionNode.getChildrenByName("area");
        if (areaNodes.size() == 0)
            THROW_INVALID_INPUT("Area should be specified for contact condition");
        
        for(auto& areaNode: areaNodes)
        {
            Area* conditionArea = readArea(areaNode);
            if(conditionArea == NULL)
                THROW_INVALID_INPUT("Can not read area");

            for( int i = 0; i < engine.getNumberOfBodies(); i++ )
            {
                engine.getBody(i)->setContactCondition(conditionArea, conditionId);
            }
        }
    }

    // create rheology matrixes
    vector<RheologyMatrixPtr> matrices;
    for (int i = 0; i < engine.getNumberOfMaterials(); i++)
    {
        MaterialPtr material = engine.getMaterial(i);
        
        bool materialUsedInTask = (std::find(usedMaterialsIds.begin(), usedMaterialsIds.end(), i) != usedMaterialsIds.end());
        
        auto props = material->getPlasticityProperties();
        bool plasticityPropsPresent = ( (props[plasticityType].size() != 0) 
                                        || (plasticityType == PLASTICITY_TYPE_NONE) );
        
        SetterPtr setter;
        DecomposerPtr decomposer;
        CorrectorPtr corrector;
        RheologyMatrixPtr matrix;

        if (material->isIsotropic())
        {
            if(materialUsedInTask)
            {
                LOG_INFO("Using \"" << plasticityType << "\" plasticity model " 
                        << "and \""  + failureMode + "\" failure mode "
                        << "for isotropic material \"" << material->getName() << "\".");
                if( !plasticityPropsPresent )
                    THROW_UNSUPPORTED("Required plasticity properties were not found.");
            }
            
            if (plasticityType == PLASTICITY_TYPE_NONE)
            {
                corrector = nullptr;
                setter = makeSetterPtr<IsotropicRheologyMatrixSetter>();
                decomposer = makeDecomposerPtr<IsotropicRheologyMatrixDecomposer>();
            }
            else if (plasticityType == PLASTICITY_TYPE_PRANDTL_RAUSS)
            {
                corrector = nullptr;
                setter = makeSetterPtr<PrandtlRaussPlasticityRheologyMatrixSetter>();
                if (matrixDecompositionImplementation == "numerical")
                    decomposer = makeDecomposerPtr<NumericalRheologyMatrixDecomposer>();
                else
                    decomposer = makeDecomposerPtr<AnalyticalRheologyMatrixDecomposer>();
            }
            else if (plasticityType == PLASTICITY_TYPE_PRANDTL_RAUSS_CORRECTOR)
            {
                corrector = makeCorrectorPtr<IdealPlasticFlowCorrector>();
                setter = makeSetterPtr<IsotropicRheologyMatrixSetter>();
                decomposer = makeDecomposerPtr<IsotropicRheologyMatrixDecomposer>();
            }
            else
            {
                THROW_UNSUPPORTED("Plasticity type\"" + plasticityType + "\" is not supported.");
            }
            
            if (failureMode == FAILURE_MODE_DISCRETE)
            {
                corrector = nullptr;
                setter = makeSetterPtr<IsotropicRheologyMatrixSetter>();
                decomposer = makeDecomposerPtr<IsotropicRheologyMatrixDecomposer>();
            }
            else if (failureMode == FAILURE_MODE_CONTINUAL)
            {
                corrector = nullptr;
                setter = makeSetterPtr<IsotropicDamagedRheologyMatrixSetter>();
                decomposer = makeDecomposerPtr<IsotropicRheologyMatrixDecomposer>();
            }
            else
            {
                THROW_UNSUPPORTED("Failure mode \"" + failureMode + "\" is not supported.");
            }
        } else 
        {
            if(materialUsedInTask)
            {
                LOG_INFO("Using \"" << plasticityType << "\" plasticity model for anisotropic material \"" << material->getName() << "\".");
                if (plasticityType != PLASTICITY_TYPE_NONE)
                    LOG_WARN("Plasticity is not supported for anisotropic materials, using elastic instead.");
            }
            
            if (failureMode == FAILURE_MODE_DISCRETE)
            {
                corrector = nullptr;
                setter = makeSetterPtr<AnisotropicRheologyMatrixSetter>();
            }
            else if (failureMode == FAILURE_MODE_CONTINUAL)
            {
                corrector = nullptr;
                setter = makeSetterPtr<AnisotropicDamagedRheologyMatrixSetter>();
            }
            else
            {
                THROW_UNSUPPORTED("Failure mode \"" + failureMode + "\" is not supported.");
            }          
            
            if( matrixDecompositionImplementation == "numerical" )
                decomposer = makeDecomposerPtr<NumericalRheologyMatrixDecomposer>();
            else
                decomposer = makeDecomposerPtr<AnalyticalRheologyMatrixDecomposer>();
        }

        matrices.push_back(makeRheologyMatrixPtr(material, setter, decomposer, corrector));
    }

    engine.setRheologyMatrices([&matrices](const CalcNode& node) -> RheologyMatrixPtr
        {
            return matrices[node.getMaterialId()];
        }
    );

    LOG_DEBUG("Running plugin-specific initializations");

    for (auto plugin: engine.getPlugins())
        plugin ->parseTask(doc);

    LOG_DEBUG("Scene loaded");
}
Esempio n. 13
0
    typename boost::math::tools::promote_args<T_y,T_dof,T_loc,T_scale>::type
    multi_student_t_log(const Eigen::Matrix<T_y,Eigen::Dynamic,1>& y,
                        const T_dof& nu,
                        const Eigen::Matrix<T_loc,Eigen::Dynamic,1>& mu,
                        const 
                        Eigen::Matrix<T_scale,
                                      Eigen::Dynamic,Eigen::Dynamic>& Sigma,
                        const Policy&) {
      static const char* function = "stan::prob::multi_student_t(%1%)";

      using stan::math::check_size_match;
      using stan::math::check_finite;
      using stan::math::check_not_nan;
      using stan::math::check_symmetric;
      using stan::math::check_positive;      
      using stan::math::check_pos_definite;
      using boost::math::tools::promote_args;

      typename promote_args<T_y,T_dof,T_loc,T_scale>::type lp(0.0);
      if (!check_size_match(function, 
          y.size(), "Size of random variable",
          mu.size(), "size of location parameter",
          &lp, Policy()))
        return lp;
      if (!check_size_match(function, 
          y.size(), "Size of random variable",
          Sigma.rows(), "rows of scale parameter",
          &lp, Policy()))
        return lp;
      if (!check_size_match(function, 
          y.size(), "Size of random variable",
          Sigma.cols(), "columns of scale parameter",
          &lp, Policy()))
        return lp;
      if (!check_finite(function, mu, "Location parameter", &lp, Policy()))
        return lp;
      if (!check_not_nan(function, y, "Random variable", &lp, Policy())) 
        return lp;
      if (!check_symmetric(function, Sigma, "Scale parameter", &lp, Policy()))
        return lp;
      if (!check_pos_definite(function, Sigma, "Scale parameter", &lp, Policy()))
        return lp;

      // allows infinities
      if (!check_not_nan(function, nu, 
                         "Degrees of freedom parameter", &lp,
                         Policy()))
        return lp;
      if (!check_positive(function, nu, 
                          "Degrees of freedom parameter", &lp,
                          Policy()))
        return lp;
      
      using std::isinf;

      if (isinf(nu)) // already checked nu > 0
        return multi_normal_log(y,mu,Sigma,Policy());

/*
      Eigen::LLT< Eigen::Matrix<T_scale,Eigen::Dynamic,Eigen::Dynamic> > LLT = Sigma.llt();
      if (LLT.info() != Eigen::Success) {
        lp = stan::math::policies::raise_domain_error<T_scale>(function,
                                              "Sigma is not positive definite (%1%)",
                                              0,Policy());
        return lp;
      }
      Eigen::Matrix<T_scale,Eigen::Dynamic,Eigen::Dynamic> L = LLT.matrixL();
*/
      double d = y.size();

      if (include_summand<propto,T_dof>::value) {
        lp += lgamma(0.5 * (nu + d));
        lp -= lgamma(0.5 * nu);
        lp -= (0.5 * d) * log(nu);
      }

      if (include_summand<propto>::value) 
        lp -= (0.5 * d) * LOG_PI;

      using stan::math::multiply;
//      using stan::math::dot_self;
      using stan::math::dot_product;
      using stan::math::subtract;
      using Eigen::Array;
//      using stan::math::mdivide_left_tri;
      using stan::math::mdivide_left;
      using stan::math::log_determinant;


      if (include_summand<propto,T_scale>::value) {
//        lp -= L.diagonal().array().log().sum();
        lp -= 0.5*log_determinant(Sigma);
      }

      if (include_summand<propto,T_y,T_dof,T_loc,T_scale>::value) {
//      Eigen::Matrix<T_scale,Eigen::Dynamic,Eigen::Dynamic> I(d,d);
//      I.setIdentity();
        
        Eigen::Matrix<typename promote_args<T_y,T_loc>::type,
                      Eigen::Dynamic,
                      1> y_minus_mu = subtract(y,mu);
//        Eigen::Matrix<typename promote_args<T_scale,T_y,T_loc>::type,
//                      Eigen::Dynamic,
//                      1> half = L = mdivide_left_tri<Eigen::Lower>(L, y_minus_mu);
        Eigen::Matrix<typename promote_args<T_scale,T_y,T_loc>::type,
                      Eigen::Dynamic,
                      1> invSigma_dy = mdivide_left(Sigma, y_minus_mu);
        lp -= 0.5 
          * (nu + d)
          * log(1.0 + dot_product(y_minus_mu,invSigma_dy) / nu);
//          * log(1.0 + dot_self(half) / nu);
      }
      return lp;
    }
Esempio n. 14
0
int WsolveWKT(double *W, double *gamma, double *T,
              double qdotn, double qdotb, double Q2, double B2, double D)
{
  Iterations = 0;

  int fail   = 0;
  int count  = 0;
  int Nmax   = 30;
  int Nmax2  = 30;
  int Nmaxsafe = 100;
  int Nextra = 5;

  double x = *W;
  double y = pow(*gamma,2.0)-1.0;
  double z = *T;
  double Told = z;
  double err1,err2,err3;

  double error = 1.0;
  double deltax,deltay,deltaz;

  while (error>NR_TOL && fail==0) {

    fghWKT(x,y,z,&deltax,&deltay,&deltaz,qdotn,qdotb,Q2,B2,D,&err1,&err2,&err3);
    count++;
    error = NR_ERROR;

    if (verbose) {
      printf("W=%e K=%e T=%e dx=%e dy=%e dz=%e error=%e\n",
             x,y,z,deltax,deltay,deltaz,error);
    }

    if (isnan(deltax) || isinf(deltax)) { deltax=0.0; error=1.0; }
    if (isnan(deltay) || isinf(deltay)) { deltay=0.0; error=1.0; }
    if (isnan(deltaz) || isinf(deltaz)) { deltaz=0.0; error=1.0; }

    x += deltax;
    y += deltay;
    z += deltaz;

    if (y < 0.0) y = 0.0;

    if (y>YMAX || count>=Nmax) {
      fail = 1;
    }
    ++Iterations;
  }

  if (fail==1) {
    if (verbose) {
      printf("Resetting...\n");
    }

    count = 0;
    fail  = 0;
    error = 1.0;
    y = (qdotb*qdotb + 2.0*Q2*D)/(B2*D*D + 2.0*D*D*D);
    z = Told;
    double a =  qdotn + .5*B2*(y+2.0)/(y+1.0);
    double b = -qdotb;
    x = - (b + a*a*a)/(pow(b*b,1./3.)+a*a);

    while (error>NR_TOL && fail==0) {

      fghWKT(x,y,z,&deltax,&deltay,&deltaz,qdotn,qdotb,Q2,B2,D,&err1,&err2,&err3);
      count++;
      error = NR_ERROR;

      if (verbose) {
        printf("W=%e K=%e T=%e dx=%e dy=%e dz = %e error = %e\n",
               x,y,z,deltax,deltay,deltaz,error);
      }

      if (isnan(deltax) || isinf(deltax)) { deltax=0.0; error=1.0; }
      if (isnan(deltay) || isinf(deltay)) { deltay=0.0; error=1.0; }
      if (isnan(deltaz) || isinf(deltaz)) { deltaz=0.0; error=1.0; }

      x += deltax;
      y += deltay;
      z += deltaz;

      if (y < 0.0) y = 0.0;

      if (y>YMAX || count>=Nmax2) {
        fail = 1;
      }
      ++Iterations;
    }
  }

  if (fail==1) {

    if (verbose) {
      printf("Resetting...\n");
    }

    count = 0;
    fail  = 0;
    z = eos->Temperature_u(D / *gamma, -qdotn-D-.5*B2);
    double P = eos->Pressure(D, z);

    x = -qdotn + P - .5*B2;
    y = 1.0e8;

    while (error>NR_TOL && fail==0) {

      fghWKT(x,y,z,&deltax,&deltay,&deltaz,qdotn,qdotb,Q2,B2,D,&err1,&err2,&err3);
      count++;
      error = NR_ERROR;

      if (verbose) {
        printf("W=%e K=%e T=%e dx=%e dy=%e dz = %e error = %e\n",
               x,y,z,deltax,deltay,deltaz,error);
      }

      if (isnan(deltax) || isinf(deltax)) { deltax=0.0; error=1.0; }
      if (isnan(deltay) || isinf(deltay)) { deltay=0.0; error=1.0; }
      if (isnan(deltaz) || isinf(deltaz)) { deltaz=0.0; error=1.0; }

      x += deltax;
      y += deltay;
      z += deltaz;

      if (y < 0.0) y = 0.0;

      if (y>YMAX || count>=Nmaxsafe) {
        fail = 1;
      }
    }
    ++Iterations;
  }

  for (count=0; count<Nextra; count++) {

    fghWKT(x,y,z,&deltax,&deltay,&deltaz,qdotn,qdotb,Q2,B2,D,&err1,&err2,&err3);
    double test = deltax + deltay + deltaz;

    if (isnan(test) || isinf(test)) {
      deltax = 0.0; deltay = 0.0; deltaz = 0.0;
    }

    x += deltax;
    y += deltay;
    z += deltaz;

    x = fabs(x);
    y = fabs(y);
    z = fabs(z);
  }

  if ((x < 0.0) || (y < 0.0)) { // || (z < 0.0)) {
    fail = 1;
  }
  if (fail==1) {
    if (verbose) {
      printf("c2p failed: W=%e K=%e T=%e qdotn=%e qdotb=%e Q2=%e B2=%e, D=%e, count=%d\n",
             x,y,z,qdotn,qdotb,Q2,B2,D,count);
    }
    return RMHD_C2P_MAXITER;
  }

  *W = x;
  *T = z;
  *gamma = sqrt(y+1.0);

  if (verbose) {
    printf("success! Final K=%e\n", y);
  }

  return 0;
}
Esempio n. 15
0
 bool hasInf() const {
     using std::isinf;
     return isinf(this->x) || isinf(this->y) || isinf(this->z);
 }