C++ FunctionSet类代码示例

/// Add the functions called by a function to the function set
/// \param func
///   The function for which all called functions will be added
/// \param funcSet
///   The set of currently called functions
/// \param funcList
///   The list of all functions
/// \return
///   True if all functions are found in the funcList, false otherwise.
bool HlslLinker::addCalledFunctions( GlslFunction *func, FunctionSet& funcSet, std::vector<GlslFunction*> &funcList )
{
	const std::set<std::string> &cf = func->getCalledFunctions();

	for (std::set<std::string>::const_iterator cit=cf.begin(); cit != cf.end(); cit++)
	{
		std::vector<GlslFunction*>::iterator it = funcList.begin();

		//This might be better as a more efficient search
		while (it != funcList.end())
		{
			if ( *cit == (*it)->getMangledName())
				break;
			it++;
		}

		//check to see if it really exists
		if ( it == funcList.end())
		{
			infoSink.info << "Failed to find function '" << *cit <<"'\n";
			return false;
		}

		//add the function (if it's not there already) and recurse
		if (std::find (funcSet.begin(), funcSet.end(), *it) == funcSet.end())
			funcSet.push_back (*it);
		addCalledFunctions( *it, funcSet, funcList); 
	}

	return true;
}

/** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau. 
    Now the state is not given, but is constructed from known coordinates, q as functions of time.
	Coordinate functions must be twice differentiable. 
	NOTE: state dependent forces and other applied loads are NOT computed since these may depend on
	state variables (like muscle fiber lengths) that are not known */
Vector InverseDynamicsSolver::solve(State &s, const FunctionSet &Qs, double time)
{
	int nq = getModel().getNumCoordinates();

	if(Qs.getSize() != nq){
		throw Exception("InverseDynamicsSolver::solve invalid number of q functions.");
	}

	if( nq != getModel().getNumSpeeds()){
		throw Exception("InverseDynamicsSolver::solve using FunctionSet, nq != nu not supported.");
	}

	// update the State so we get the correct gravity and coriolis effects
	// direct references into the state so no allocation required
	s.updTime() = time;
	Vector &q = s.updQ();
	Vector &u = s.updU();
	Vector &udot = s.updUDot();

	for(int i=0; i<nq; i++){
		q[i] = Qs.evaluate(i, 0, time);
		u[i] = Qs.evaluate(i, 1, time);
		udot[i] = Qs.evaluate(i, 2, time);
	}

	// Perform general inverse dynamics
	return solve(s,	udot);
}

bool
query_function_entry(void *addr)
{
    FunctionSet::iterator it = function_entries.find(addr);

    if (it == function_entries.end()) return false;
    else return true;
}

void FunctionManager::destroy_functions_in_set(FunctionSet &function_set)
{
    // We iterate over the set like this because destroy_function() will erase
    // the function from function_set, thereby invalidating any iterator we are
    // holding on to.
    while (!function_set.empty())
    {
        destroy_function(*function_set.begin());
    }
}

/**
 * Set the velocity functions for the tasks.  Functions are set based on the
 * correspondence of the function and the task.  For example,
 * a task with the name "x" will search for a function or functions
 * with the name "x".  For tasks that require 3 functions, such
 * as CMC_Point tasks, the assumption is that there will be three
 * consecutive functions named "x" in the function set.  If the correct
 * number of functions is not found, the task is disabled.
 *
 * @param aFuncSet Function set.
 * @return Pointer to the previous function set.
 */
void CMC_TaskSet::
setFunctionsForVelocity(FunctionSet &aFuncSet)
{
    // LOOP THROUGH TRACK OBJECTS
    int i,j,iFunc=0;
    int nTrk;
    string name;
    Function *f[3];

    const CoordinateSet& coords = getModel()->getCoordinateSet(); 

    for(i=0;i<getSize();i++) {

        // OBJECT
        TrackingTask& ttask = get(i);

        // If CMC_Task process same way as pre 2.0.2
        if (dynamic_cast<CMC_Task*>(&ttask)==NULL) 
            continue;

        CMC_Task& task = dynamic_cast<CMC_Task&>(ttask);

        // NAME
        name = task.getName();
        if(name.empty()) continue;

        const Coordinate& coord = coords.get(name);

        // FIND FUNCTION(S)
        f[0] = f[1] = f[2] = NULL;
        nTrk = task.getNumTaskFunctions();
        iFunc = aFuncSet.getIndex(coord.getSpeedName(),iFunc);

        if (iFunc < 0){
            name = coord.getJoint().getName() + "/" + coord.getSpeedName();
            iFunc = aFuncSet.getIndex(name, iFunc);
            if (iFunc < 0){
                string msg = "CMC_TaskSet::setFunctionsForVelocity: function for task '";
                msg += name + " not found.";
                throw Exception(msg);
            }
        }

        for(j=0;j<nTrk;j++) {
            try {
                f[j] = &aFuncSet.get(iFunc);
            } catch(const Exception& x) {
                x.print(cout);
            }
            if(f[j]==NULL) break;
        }
        task.setTaskFunctionsForVelocity(f[0],f[1],f[2]);
    }
}

// Free the function_entries map, the Function objects in the map, the
// excluded_function_entries set and cbranges intervals.
//
void
function_entries_reinit(void)
{
    FunctionSet::iterator it;

    for (it = function_entries.begin(); it != function_entries.end(); it++) {
        Function *f = it->second;
        delete f->comment;
        delete f;
    }
    function_entries.clear();
    excluded_function_entries.clear();
    cbranges.clear();
    num_entries_total = 0;
}

static void EmitCalledFunctions (std::stringstream& shader, const FunctionSet& functions)
{
	if (functions.empty())
		return;

	// Functions must be emitted in reverse order as they are sorted topologically in top to bottom.
	for (FunctionSet::const_reverse_iterator fit = functions.rbegin(); fit != functions.rend(); fit++)
	{
		shader << "\n";
		OutputLineDirective(shader, (*fit)->getLine());
		shader << (*fit)->getPrototype() << " {\n";
		shader << (*fit)->getCode() << "\n"; //has embedded }
		shader << "\n";
	}
}

void
entries_in_range(void *start, void *end, vector<void *> &result)
{
#ifdef DEBUG_ENTRIES_IN_RANGE
    printf("function entries for range [%p, %p]\n", start, end);
#endif
    FunctionSet::iterator it = function_entries.find(start);
    for (; it != function_entries.end(); it++) {
        void *addr = (*it).first;
        if (addr > end) return;
#ifdef DEBUG_ENTRIES_IN_RANGE
        printf("  %p\n", addr);
#endif
        result.push_back(addr);
    }
}

static void EmitCalledFunctions (std::stringstream& shader, const FunctionSet& functions)
{
	if (functions.empty())
		return;

	for (FunctionSet::const_reverse_iterator fit = functions.rbegin(); fit != functions.rend(); fit++) // emit backwards, will put least used ones in front
	{
		shader << (*fit)->getPrototype() << ";\n";
	}

	for (FunctionSet::const_reverse_iterator fit = functions.rbegin(); fit != functions.rend(); fit++) // emit backwards, will put least used ones in front
	{
		shader << (*fit)->getPrototype() << " {\n";
		shader << (*fit)->getLocalDecls(1) << "\n";
		shader << (*fit)->getCode() << "\n"; //has embedded }
		shader << "\n";
	}
}

void
add_function_entry(void *addr, const string *comment, bool isvisible,
                   int call_count)
{
    FunctionSet::iterator it = function_entries.find(addr);

    if (it == function_entries.end()) {
        new_function_entry(addr, comment ? new string(*comment) : NULL,
                           isvisible, call_count);
    } else {
        Function *f = (*it).second;
        if (comment) {
            f->AppendComment(comment);
        } else if (f->comment) {
            f->isvisible = true;
        }
        f->call_count += call_count;
    }
}

void
dump_reachable_functions()
{
    char buffer[1024];
    FunctionSet::iterator i = function_entries.begin();

    for (; i != function_entries.end();) {
        Function *f = (*i).second;
        ++i;

        const char *name;
        if (!f->isvisible && !(f->call_count > 1) && !is_possible_fn(f->address)) continue;
        if (f->comment) {
            name = f->comment->c_str();
        }
        else {
            // inferred functions must be at least 16 bytes long
            if (i != function_entries.end()) {
                Function *nextf = (*i).second;
                if (f->call_count == 0 &&
                        (((unsigned long) nextf->address) -
                         ((unsigned long) f->address)) < 16)  {
                    long offset = offset_for_fn(f->address);
                    if (offset == 0) {
                        // if f->address lies within a valid code range, its
                        // offset will be non-zero. if offset is zero, the
                        // address cannot be a valid function start.
                        continue;
                    }
                    if (!range_contains_control_flow((char *) f->address + offset,
                                                     ((char *) nextf->address +
                                                      offset)))
                        continue;
                }
            }
            sprintf(buffer,"stripped_%p", f->address);
            name = buffer;
        }
        dump_function_entry(f->address, name);
    }
}

void HlslLinker::buildUniformsAndLibFunctions(const FunctionSet& calledFunctions, std::vector<GlslSymbol*>& constants, std::set<TOperator>& libFunctions)
{
	for (FunctionSet::const_iterator it = calledFunctions.begin(); it != calledFunctions.end(); ++it) {
		const std::vector<GlslSymbol*> &symbols = (*it)->getSymbols();
		
		unsigned n_symbols = symbols.size();
		for (unsigned i = 0; i != n_symbols; ++i) {
			GlslSymbol* s = symbols[i];
			if (s->getQualifier() == EqtUniform || s->getQualifier() == EqtMutableUniform)
				constants.push_back(s);
		}
		
		//take each referenced library function, and add it to the set
		const std::set<TOperator> &referencedFunctions = (*it)->getLibFunctions();
		libFunctions.insert( referencedFunctions.begin(), referencedFunctions.end());
	}
	
    // std::unique only removes contiguous duplicates, so vector must be sorted to remove them all
    std::sort(constants.begin(), constants.end(), GlslSymbolSorter());

	// Remove duplicates
	constants.resize(std::unique(constants.begin(), constants.end()) - constants.begin());
}

/**
 * This method creates a SimmMotionTrial instance with the markerFile and
 * timeRange parameters. It also creates a Storage instance with the
 * coordinateFile parameter. Then it updates the coordinates and markers in
 * the model, if specified. Then it does IK to fit the model to the static
 * pose. Then it uses the current model pose to relocate all non-fixed markers
 * according to their locations in the SimmMotionTrial. Then it writes the
 * output files selected by the user.
 *
 * @param aModel the model to use for the marker placing process.
 * @return Whether the marker placing process was successful or not.
 */
bool MarkerPlacer::processModel(Model* aModel,
        const string& aPathToSubject) const {

    if(!getApply()) return false;

    cout << endl << "Step 3: Placing markers on model" << endl;

    if (_timeRange.getSize()<2) 
        throw Exception("MarkerPlacer::processModel, time_range is unspecified.");

    /* Load the static pose marker file, and average all the
    * frames in the user-specified time range.
    */
    TimeSeriesTableVec3 staticPoseTable{aPathToSubject + _markerFileName};
    const auto& timeCol = staticPoseTable.getIndependentColumn();

    // Users often set a time range that purposely exceeds the range of
    // their data with the mindset that all their data will be used.
    // To allow for that, we have to narrow the provided range to data
    // range, since the TimeSeriesTable will correctly throw that the 
    // desired time exceeds the data range.
    if (_timeRange[0] < timeCol.front())
        _timeRange[0] = timeCol.front();
    if (_timeRange[1] > timeCol.back())
        _timeRange[1] = timeCol.back();

    const auto avgRow = staticPoseTable.averageRow(_timeRange[0],
                                                   _timeRange[1]);
    for(size_t r = staticPoseTable.getNumRows(); r-- > 0; )
        staticPoseTable.removeRowAtIndex(r);
    staticPoseTable.appendRow(_timeRange[0], avgRow);
    
    OPENSIM_THROW_IF(!staticPoseTable.hasTableMetaDataKey("Units"),
                     Exception,
                     "MarkerPlacer::processModel -- Marker file does not have "
                     "'Units'.");
    Units
    staticPoseUnits{staticPoseTable.getTableMetaData<std::string>("Units")};
    double scaleFactor = staticPoseUnits.convertTo(aModel->getLengthUnits());
    OPENSIM_THROW_IF(SimTK::isNaN(scaleFactor),
                     Exception,
                     "Model has unspecified units.");
    if(std::fabs(scaleFactor - 1) >= SimTK::Eps) {
        for(unsigned r = 0; r < staticPoseTable.getNumRows(); ++r)
            staticPoseTable.updRowAtIndex(r) *= scaleFactor;

        staticPoseUnits = aModel->getLengthUnits();
        staticPoseTable.removeTableMetaDataKey("Units");
        staticPoseTable.addTableMetaData("Units",
                                         staticPoseUnits.getAbbreviation());
    }
    
    MarkerData* staticPose = new MarkerData(aPathToSubject + _markerFileName);
    staticPose->averageFrames(_maxMarkerMovement, _timeRange[0], _timeRange[1]);
    staticPose->convertToUnits(aModel->getLengthUnits());

    /* Delete any markers from the model that are not in the static
     * pose marker file.
     */
    aModel->deleteUnusedMarkers(staticPose->getMarkerNames());

    // Construct the system and get the working state when done changing the model
    SimTK::State& s = aModel->initSystem();
    s.updTime() = _timeRange[0];
    
    // Create references and WeightSets needed to initialize InverseKinemaicsSolver
    Set<MarkerWeight> markerWeightSet;
    _ikTaskSet.createMarkerWeightSet(markerWeightSet); // order in tasks file
    // MarkersReference takes ownership of marker data (staticPose)
    MarkersReference markersReference(staticPoseTable, &markerWeightSet);
    SimTK::Array_<CoordinateReference> coordinateReferences;

    // Load the coordinate data
    // create CoordinateReferences for Coordinate Tasks
    FunctionSet *coordFunctions = NULL;
    // bool haveCoordinateFile = false;
    if(_coordinateFileName != "" && _coordinateFileName != "Unassigned"){
        Storage coordinateValues(aPathToSubject + _coordinateFileName);
        aModel->getSimbodyEngine().convertDegreesToRadians(coordinateValues);
        // haveCoordinateFile = true;
        coordFunctions = new GCVSplineSet(5,&coordinateValues);
    }
    
    int index = 0;
    for(int i=0; i< _ikTaskSet.getSize(); i++){
        IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i]);
        if (coordTask && coordTask->getApply()){
            std::unique_ptr<CoordinateReference> coordRef{};
//.........这里部分代码省略.........

/**
 * This method creates a SimmMotionTrial instance with the markerFile and
 * timeRange parameters. It also creates a Storage instance with the
 * coordinateFile parameter. Then it updates the coordinates and markers in
 * the model, if specified. Then it does IK to fit the model to the static
 * pose. Then it uses the current model pose to relocate all non-fixed markers
 * according to their locations in the SimmMotionTrial. Then it writes the
 * output files selected by the user.
 *
 * @param aModel the model to use for the marker placing process.
 * @return Whether the marker placing process was successful or not.
 */
bool MarkerPlacer::processModel(Model* aModel, const string& aPathToSubject)
{
    if(!getApply()) return false;

    cout << endl << "Step 3: Placing markers on model" << endl;

    /* Load the static pose marker file, and average all the
     * frames in the user-specified time range.
     */
    MarkerData staticPose(aPathToSubject + _markerFileName);
    if (_timeRange.getSize()<2) 
        throw Exception("MarkerPlacer::processModel, time_range is unspecified.");

    staticPose.averageFrames(_maxMarkerMovement, _timeRange[0], _timeRange[1]);
    staticPose.convertToUnits(aModel->getLengthUnits());

    /* Delete any markers from the model that are not in the static
     * pose marker file.
     */
    aModel->deleteUnusedMarkers(staticPose.getMarkerNames());

    // Construct the system and get the working state when done changing the model
    SimTK::State& s = aModel->initSystem();
    
    // Create references and WeightSets needed to initialize InverseKinemaicsSolver
    Set<MarkerWeight> markerWeightSet;
    _ikTaskSet.createMarkerWeightSet(markerWeightSet); // order in tasks file
    MarkersReference markersReference(staticPose, &markerWeightSet);
    SimTK::Array_<CoordinateReference> coordinateReferences;

    // Load the coordinate data
    // create CoordinateReferences for Coordinate Tasks
    FunctionSet *coordFunctions = NULL;
    bool haveCoordinateFile = false;
    if(_coordinateFileName != "" && _coordinateFileName != "Unassigned"){
        Storage coordinateValues(aPathToSubject + _coordinateFileName);
        aModel->getSimbodyEngine().convertDegreesToRadians(coordinateValues);
        haveCoordinateFile = true;
        coordFunctions = new GCVSplineSet(5,&coordinateValues);
    }
    
    int index = 0;
    for(int i=0; i< _ikTaskSet.getSize(); i++){
        IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i]);
        if (coordTask && coordTask->getApply()){
            CoordinateReference *coordRef = NULL;
            if(coordTask->getValueType() == IKCoordinateTask::FromFile){
                index = coordFunctions->getIndex(coordTask->getName(), index);
                if(index >= 0){
                    coordRef = new CoordinateReference(coordTask->getName(),coordFunctions->get(index));
                }
            }
            else if((coordTask->getValueType() == IKCoordinateTask::ManualValue)){
                Constant reference(Constant(coordTask->getValue()));
                coordRef = new CoordinateReference(coordTask->getName(), reference);
            }
            else{ // assume it should be held at its current/default value
                double value = aModel->getCoordinateSet().get(coordTask->getName()).getValue(s);
                Constant reference = Constant(value);
                coordRef = new CoordinateReference(coordTask->getName(), reference);
            }

            if(coordRef == NULL)
                throw Exception("MarkerPlacer: value for coordinate "+coordTask->getName()+" not found.");

            // We have a valid coordinate reference so now set its weight according to the task
            coordRef->setWeight(coordTask->getWeight());
            coordinateReferences.push_back(*coordRef);      
        }           
    }
    double constraintWeight = std::numeric_limits<SimTK::Real>::infinity();

    InverseKinematicsSolver ikSol(*aModel, markersReference,
                                  coordinateReferences, constraintWeight);
    ikSol.assemble(s);

    // Call realize Position so that the transforms are updated and  markers can be moved correctly
    aModel->getMultibodySystem().realize(s, SimTK::Stage::Position);
    // Report marker errors to assess the quality 
    int nm = markerWeightSet.getSize();
    SimTK::Array_<double> squaredMarkerErrors(nm, 0.0);
    SimTK::Array_<Vec3> markerLocations(nm, Vec3(0));
    double totalSquaredMarkerError = 0.0;
    double maxSquaredMarkerError = 0.0;
    int worst = -1;
    // Report in the same order as the marker tasks/weights
    ikSol.computeCurrentSquaredMarkerErrors(squaredMarkerErrors);
    for(int j=0; j<nm; ++j){
//.........这里部分代码省略.........

bool HlslLinker::link(HlslCrossCompiler* compiler, const char* entryFunc, bool usePrecision)
{
	std::vector<GlslFunction*> globalList;
	std::vector<GlslFunction*> functionList;
	std::string entryPoint;
	GlslFunction* funcMain = NULL;
	FunctionSet calledFunctions;
	std::set<TOperator> libFunctions;
	std::map<std::string,GlslSymbol*> globalSymMap;
	std::map<std::string,GlslStruct*> structMap;

	if (!compiler)
	{
		infoSink.info << "No shader compiler provided\n";
		return false;
	}

	EShLanguage lang = compiler->getLanguage();

	if (!entryFunc)
	{
		infoSink.info << "No shader entry function provided\n";
		return false;
	}

	entryPoint = GetEntryName (entryFunc);

	//build the list of functions
	HlslCrossCompiler *comp = static_cast<HlslCrossCompiler*>(compiler);

	std::vector<GlslFunction*> &fl = comp->functionList;

	for ( std::vector<GlslFunction*>::iterator fit = fl.begin(); fit < fl.end(); fit++)
	{
		if ( (*fit)->getName() == "__global__")
			globalList.push_back( *fit);
		else
			functionList.push_back( *fit);

		if ((*fit)->getName() == entryPoint)
		{
			if (funcMain)
			{
				infoSink.info << kShaderTypeNames[lang] << " entry function cannot be overloaded\n";
				return false;
			}
			funcMain = *fit;
		}
	}

	// check to ensure that we found the entry function
	if (!funcMain)
	{
		infoSink.info << "Failed to find entry function: '" << entryPoint <<"'\n";
		return false;
	}

	//add all the called functions to the list
	calledFunctions.push_back (funcMain);
	if (!addCalledFunctions (funcMain, calledFunctions, functionList))
	{
		infoSink.info << "Failed to resolve all called functions in the " << kShaderTypeNames[lang] << " shader\n";
	}

	//iterate over the functions, building a global list of structure declaractions and symbols
	// assume a single compilation unit for expediency (eliminates name clashes, as type checking
	// withing a single compilation unit has been performed)
	for (FunctionSet::iterator it=calledFunctions.begin(); it != calledFunctions.end(); it++)
	{
		//get each symbol and each structure, and add them to the map
		// checking that any previous entries are equivalent
		const std::vector<GlslSymbol*> &symList = (*it)->getSymbols();

		for (std::vector<GlslSymbol*>::const_iterator cit = symList.begin(); cit < symList.end(); cit++)
		{
			if ( (*cit)->getIsGlobal())
			{
				//should check for already added ones here
				globalSymMap[(*cit)->getName()] = *cit;
			}
		}

		//take each referenced library function, and add it to the set
		const std::set<TOperator> &libSet = (*it)->getLibFunctions();

		libFunctions.insert( libSet.begin(), libSet.end());
	}


	// The following code is what is used to generate the actual shader and "main"
	// function. The process is to take all the components collected above, and
	// write them to the appropriate code stream. Finally, a main function is
	// generated that calls the specified entrypoint. That main function uses
	// semantics on the arguments and return values to connect items appropriately.

	//
	// Write Library Functions & required extensions
	std::string shaderExtensions, shaderLibFunctions;
	if (!libFunctions.empty())
	{
//.........这里部分代码省略.........