C++ Array_::size方法代码示例

void testVisModel(string fileName)
{

    Model* model = new Model(fileName, true);
    SimTK::State& si = model->initSystem();
    ModelDisplayHints mdh; 
    SimTK::Array_<SimTK::DecorativeGeometry> geometryToDisplay;
    model->generateDecorations(true, mdh, si, geometryToDisplay);
    cout << geometryToDisplay.size() << endl;
    model->generateDecorations(false, mdh, si, geometryToDisplay);
    cout << geometryToDisplay.size() << endl;
    DecorativeGeometryImplementationText dgiText;
    for (unsigned i = 0; i < geometryToDisplay.size(); i++)
        geometryToDisplay[i].implementGeometry(dgiText);

    std::string baseName = fileName.substr(0, fileName.find_last_of('.'));
    std::ifstream t("vis_" + baseName + ".txt");
    if (!t.good()) throw OpenSim::Exception("Could not open file.");
    std::stringstream buffer;
    buffer << t.rdbuf();
    std::string fromFile = buffer.str();
    std::string fromModel = dgiText.getAsString();
    cout << "From Model " << endl << "=====" << endl;
    cout << fromModel << endl;
    cout << "From File " << endl << "=====" << endl;
    cout << fromFile << endl;
    int same = fromFile.compare(fromModel);
    delete model;
    ASSERT(same == 0, __FILE__, __LINE__, "Files do not match.");
}

// Handle conversion from older format
void VisibleObject::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)
{ 
    SimTK::Array_<SimTK::String> oldGeometryFiles;
	if ( versionNumber < XMLDocument::getLatestVersion()){
		if (versionNumber<20101){
			SimTK::Xml::element_iterator visPropIter = aNode.element_begin("VisibleProperties");
			// Get geometry files and Preferences if any and set them into 
			if (visPropIter!=aNode.element_end()){
				// Move display_prference, and show_axes nodes up to VisibleObject
				SimTK::Xml::element_iterator  prefIter = visPropIter->element_begin("display_preference");
				if (prefIter!= visPropIter->element_end()){
					SimTK::Xml::Node moveNode = visPropIter->removeNode(prefIter);
					aNode.insertNodeAfter(aNode.element_end(), moveNode);
				}
				SimTK::Xml::element_iterator  showAxesIter = visPropIter->element_begin("show_axes");
				if (showAxesIter!=aNode.element_end()){
					SimTK::Xml::Node moveNode = visPropIter->removeNode(showAxesIter);
					aNode.insertNodeAfter(aNode.element_end(), moveNode);
				}
			}
			SimTK::Xml::element_iterator geometryIter = aNode.element_begin("geometry_files");
			string propValue="";
			bool hasPieces=false;
			if (geometryIter!= aNode.element_end()){
				geometryIter->getValueAs(oldGeometryFiles);
			}
        }
    }
	Object::updateFromXMLNode(aNode, versionNumber);
    if (oldGeometryFiles.size()>0){
        for(unsigned i=0; i< oldGeometryFiles.size(); i++) 
            setGeometryFileName(i, oldGeometryFiles[i]);
    }
}

/** Update all markers weights by order in the markersReference passed in to
    construct the solver. */
void InverseKinematicsSolver::updateMarkerWeights(const SimTK::Array_<double> &weights)
{
    if(_markersReference.updMarkerWeightSet().getSize() == weights.size()){
        for(unsigned int i=0; i<weights.size(); i++){
            _markersReference.updMarkerWeightSet()[i].setWeight(weights[i]);
            _markerAssemblyCondition->changeMarkerWeight(SimTK::Markers::MarkerIx(i), weights[i]);
        }
    }
    else
        throw Exception("InverseKinematicsSolver::updateMarkerWeights: invalid size of weights.");
}

void Geometry::generateDecorations(bool fixed, const ModelDisplayHints& hints, const SimTK::State& state,
    SimTK::Array_<SimTK::DecorativeGeometry>& appendToThis) const
{
    if (!fixed) return; // serialized Geometry is assumed fixed
    SimTK::Array_<SimTK::DecorativeGeometry> decos;
    implementCreateDecorativeGeometry(decos);
    if (decos.size() == 0) return;
    setDecorativeGeometryTransform(decos, state);
    for (unsigned i = 0; i < decos.size(); i++){
        setDecorativeGeometryAppearance(decos[i]);
        appendToThis.push_back(decos[i]);
    }
}

int SegmentedQuinticBezierToolkit::calcIndex(double x, 
                                             const SimTK::Array_<SimTK::Vector>& bezierPtsX)
{
    int idx = 0;
    bool flag_found = false;

    int n = bezierPtsX.size(); 
    for(int i=0; i<n; i++){
        if( x >= bezierPtsX[i](0) && x < bezierPtsX[i](5) ){
            idx = i;
            flag_found = true;
            break; 
        }
    }

    //Check if the value x is identically the last point
    if(!flag_found && x == bezierPtsX[n-1](5)){
        idx = n-1;
        flag_found = true;
    }

    SimTK_ERRCHK_ALWAYS( (flag_found == true), 
        "SegmentedQuinticBezierToolkit::calcIndex", 
        "Error: A value of x was used that is not within the Bezier curve set.");

    return idx;
}

void checkMarkersReferenceConsistencyFromTool(InverseKinematicsTool& ik)
{
    MarkersReference markersReference;
    SimTK::Array_<CoordinateReference> coordinateReferences;

    ik.populateReferences(markersReference, coordinateReferences);
    const IKTaskSet& tasks = ik.getIKTaskSet();

    // Need a model to get a state, doesn't matter which model.
    Model model;
    SimTK::State& state = model.initSystem();

    SimTK::Array_<string> names = markersReference.getNames();
    SimTK::Array_<double> weights;
    markersReference.getWeights(state, weights);

    for (unsigned int i=0; i < names.size(); ++i) {
        std::cout << names[i] << ": " << weights[i];
        int ix = tasks.getIndex(names[i]);
        if (ix > -1) {
            cout << " in TaskSet: " << tasks[ix].getWeight() << endl;
            SimTK_ASSERT_ALWAYS(weights[i] == tasks[ix].getWeight(),
                "Mismatched weight to marker task");
        }
        else {
            cout << " default: " << markersReference.get_default_weight() << endl;
            SimTK_ASSERT_ALWAYS(
                weights[i] == markersReference.get_default_weight(),
                "Mismatched weight to default weight");
        }
    }
}

	double calcDerivative(const SimTK::Array_<int>& derivComponents, const SimTK::Vector& x) const {
		if (derivComponents.size() == 1){
			if (derivComponents[0]==0){
				SimTK::Vector x1(1);
				x1[0] = x[0];
				return scale*f1->calcDerivative(derivComponents, x1);
			}
			else if (derivComponents[0]==1)
				return -1;
		}
		else if(derivComponents.size() == 2){
			if (derivComponents[0]==0 && derivComponents[1] == 0){
				SimTK::Vector x1(1);
				x1[0] = x[0];
				return scale*f1->calcDerivative(derivComponents, x1);
			}
		}
        return 0;
    }

int main()
{
    SimTK::Array_<std::string> failures;

    // get all registered Components
    SimTK::Array_<Component*> availableComponents;

    // starting with type Frame
    ArrayPtrs<Frame> availableFrames;
    Object::getRegisteredObjectsOfGivenType(availableFrames);
    for (int i = 0; i < availableFrames.size(); ++i) {
        availableComponents.push_back(availableFrames[i]);
    }

    // then type Joint
    ArrayPtrs<Joint> availableJoints;
    Object::getRegisteredObjectsOfGivenType(availableJoints);
    for (int i = 0; i < availableJoints.size(); ++i) {
        availableComponents.push_back(availableJoints[i]);
    }

    // continue with Constraint, Actuator, Frame, ...
    //Example of an updated force that passes
    ArrayPtrs<PointToPointSpring> availablePointToPointSpring;
    Object::getRegisteredObjectsOfGivenType(availablePointToPointSpring);
    availableComponents.push_back(availablePointToPointSpring[0]);

    for (unsigned int i = 0; i < availableComponents.size(); i++) {
        try {
            testComponent(*availableComponents[i]);
        }
        catch (const std::exception& e) {
            cout << "*******************************************************\n";
            cout<< "FAILURE: " << availableComponents[i]->getConcreteClassName() << endl;
            cout<< e.what() << endl;
            failures.push_back(availableComponents[i]->getConcreteClassName());
        }
    }
    
    if (!failures.empty()) {
        cout << "*******************************************************\n";
        cout << "Done, with failure(s): " << failures << endl;
        cout << failures.size() << "/" << availableComponents.size() 
            << " components failed test." << endl;
        cout << 100 * (availableComponents.size() - failures.size()) / availableComponents.size()
            << "% components passed." << endl;
        cout << "*******************************************************\n" << endl;
        return 1;
    }
    cout << "\ntestComponents PASSED. " << availableComponents.size() 
        << " components were tested." << endl;
}

int main()
{
    try {
        Storage result("Arm26_optimized_states.sto"),
                standard("std_Arm26_optimized_states.sto");
        CHECK_STORAGE_AGAINST_STANDARD(result, standard, Array<double>(0.01, 16),
            __FILE__, __LINE__, "Arm26 states failed comparison test");
        cout << "Arm26 states comparison test passed\n";

        // Ensure the optimization result acheived a velocity of at least
        // REF_MAX_VEL, and that the control values are within either 20% of the
        // reference values or 0.05 in absolute value.
        ifstream resFile;
        resFile.open("Arm26_optimization_result");
        ASSERT(resFile.is_open(), __FILE__, __LINE__,
               "Can't open optimization result file" );

        SimTK::Array_<double> resVec;
        for ( ; ; ) {
            double tmp;
            resFile >> tmp;
            if (!resFile.good())
                break;
            resVec.push_back(tmp);
        }

        ASSERT(resVec.size() == ARM26_DESIGN_SPACE_DIM+1, __FILE__, __LINE__,
               "Optimization result size mismatch" );
        
        // Ensure the optimizer found a local minimum we expect.
        for (int i = 0; i < ARM26_DESIGN_SPACE_DIM-1; ++i) {
            ASSERT(fabs(resVec[i] - refControls[i])/refControls[i] < 0.2 ||
                   fabs(resVec[i] - refControls[i]) < 0.05, __FILE__, __LINE__,
                   "Control value does not match reference" );
        }
        ASSERT(resVec[ARM26_DESIGN_SPACE_DIM] > REF_MAX_VEL, __FILE__, __LINE__,
               "Optimized velocity smaller than reference" );

        cout << "Arm26 optimization results passed\n";
    }
    catch (const Exception& e) {
        e.print(cerr);
        return 1;
    }
    cout << "Done" << endl;
    return 0;
}

/**
 * Compute Transform of a Geometry w.r.t. passed in Frame
 * Both Frame(s) could be Bodies, state is assumed to be realized to position
*/
void Geometry::setDecorativeGeometryTransform(SimTK::Array_<SimTK::DecorativeGeometry>& decorations, const SimTK::State& state) const
{
    const Frame& myFrame = getFrame();
    const Frame& bFrame = myFrame.findBaseFrame();
    const PhysicalFrame* bPhysicalFrame = dynamic_cast<const PhysicalFrame*>(&bFrame);
    if (bPhysicalFrame == nullptr){
        // throw exception something is wrong
        throw (Exception("Frame for Geometry " + getName() + " is not attached to a PhysicalFrame."));
    }
    const SimTK::MobilizedBodyIndex& idx = bPhysicalFrame->getMobilizedBodyIndex();
    SimTK::Transform transformInBaseFrame = myFrame.findTransformInBaseFrame();
    for (unsigned i = 0; i < decorations.size(); i++){
        decorations[i].setBodyId(idx);
        decorations[i].setTransform(transformInBaseFrame);
        decorations[i].setIndexOnBody(i);
    }
}

int main()
{
	try {	
		Storage result("Arm26_noActivation_states.sto"), standard("std_Arm26_noActivation_states.sto");
		CHECK_STORAGE_AGAINST_STANDARD(result, standard, Array<double>(0.01, 16), __FILE__, __LINE__, "Arm26 no activation states failed");
		cout << "Arm26 no activation states passed\n";

		// Check the optimization result acheived at least a velocity of 5.43 m/s, and that the control values are within either 20% 
		// of the reference values or 0.05 in absolute value.
		ifstream resFile; 
		resFile.open("Arm26_optimization_result"); 
		ASSERT(resFile.is_open(), __FILE__, __LINE__, "Can't open optimization result file" );

		SimTK::Array_<double> resVec;
		for ( ; ; ) {
			double tmp;
			resFile >> tmp;   
			if (!resFile.good()) 
				break; 
			resVec.push_back(tmp); 
		}
	
		ASSERT(resVec.size() == ARM26_DESIGN_SPACE_DIM+1, __FILE__, __LINE__, "Optimization result size mismatch" );
		
		// We don't enforce the optimizer to find precisedly the same local minimum for now.   	
		/*for (int i = 0; i < ARM26_DESIGN_SPACE_DIM-1; i++) {
			ASSERT(fabs(resVec[i] - refControls[i])/refControls[i] < 0.2 || fabs(resVec[i] - refControls[i]) < 0.05, __FILE__, __LINE__, "Control value does not match reference" );
		}*/
		ASSERT(resVec[ARM26_DESIGN_SPACE_DIM] > REF_MAX_VEL, __FILE__, __LINE__, "Optimized velocity smaller than reference" );
		
		cout << "Arm26 optimization results passed\n";
	}
	catch (const Exception& e) {
        e.print(cerr);
        return 1;
    }
    cout << "Done" << endl;
    return 0;
}

/** Override of the default implementation to account for versioning. */
void CustomJoint::
updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)
{
	int documentVersion = versionNumber;
	if ( documentVersion < XMLDocument::getLatestVersion()){
		if (Object::getDebugLevel()>=1)
			cout << "Updating CustomJoint to latest format..." << endl;
		// Version before refactoring spatialTransform
		if (documentVersion<10901){
			// replace TransformAxisSet with SpatialTransform
			/////renameChildNode("TransformAxisSet", "SpatialTransform");
			// Check how many TransformAxes are defined
			SimTK::Xml::element_iterator spatialTransformNode = 
                aNode.element_begin("TransformAxisSet");
			if (spatialTransformNode == aNode.element_end()) return;

			SimTK::Xml::element_iterator axesSetNode = 
                spatialTransformNode->element_begin("objects");
			/////if (axesSetNode != NULL)
			/////	spatialTransformNode->removeChild(axesSetNode);
			/////DOMElement*grpNode = XMLNode::GetFirstChildElementByTagName(spatialTransformNode,"groups");
			/////if (grpNode != NULL)
			/////	spatialTransformNode->removeChild(grpNode);
			// (0, 1, 2 rotations & 3, 4, 5 translations then remove the is_rotation node)
			SimTK::Array_<SimTK::Xml::Element> list = 
                axesSetNode->getAllElements();
			unsigned int listLength = list.size();
			int objectsFound = 0;
			Array<int> translationIndices(-1, 0);
			Array<int>  rotationIndices(-1, 0);
			int nextAxis = 0;
			std::vector<TransformAxis *> axes;
			// Add children for all six axes here
			//////_node->removeChild(SpatialTransformNode);
			
			// Create a blank Spatial Transform and use it to populate the 
            // XML structure
			for(int i=0; i<6; i++)
				updSpatialTransform()[i].setFunction(new OpenSim::Constant(0));
			
			Array<OpenSim::TransformAxis*> oldAxes;
			for(unsigned int j=0;j<listLength;j++) {
				// getChildNodes() returns all types of DOMNodes including 
                // comments, text, etc., but we only want
				// to process element nodes
				SimTK::Xml::Element objElmt = list[j];
				string objectType = objElmt.getElementTag();
                // (sherm) this is cleaning up old TransformAxis here but
                // that should really be done in TransformAxis instead.
				if (objectType == "TransformAxis"){
					OpenSim::TransformAxis* readAxis = 
                        new OpenSim::TransformAxis(objElmt);
					assert(nextAxis <=5);
					bool isRotation = false;
					SimTK::Xml::element_iterator rotationNode = 
                        objElmt.element_begin("is_rotation");
					if (rotationNode != objElmt.element_end()){
						SimTK::String sValue = 
                            rotationNode->getValueAs<SimTK::String>();
						bool value = (sValue.toLower() == "true");
								isRotation = value;
						objElmt.eraseNode(rotationNode);
							}
					SimTK::Xml::element_iterator coordinateNode = 
                        objElmt.element_begin("coordinate");
					SimTK::String coordinateName = 
                        coordinateNode->getValueAs<SimTK::String>();
					Array<std::string> names("");
					names.append(coordinateName);
					readAxis->setCoordinateNames(names);
					SimTK::Xml::element_iterator axisNode = 
                        objElmt.element_begin("axis");
					
					SimTK::Vec3 axisVec= axisNode->getValueAs<SimTK::Vec3>();
					readAxis->setAxis(axisVec);
					if (isRotation){
						rotationIndices.append(nextAxis);
					}
					else {
						translationIndices.append(nextAxis);
					}
					axes.push_back(readAxis);
					nextAxis++;
				}
			}
			assert(rotationIndices.getSize() <=3);
			assert(translationIndices.getSize() <=3);
			//XMLNode::RemoveChildren(SpatialTransformAxesNode);
			int nRotations = rotationIndices.getSize();
			int nTranslations = translationIndices.getSize();
			// Now copy coordinateName, Axis, Function into proper slot
			for (int i=0; i<nRotations; i++){
				updSpatialTransform()[i] = *axes[rotationIndices[i]];
			}
			updSpatialTransform().constructIndependentAxes(nRotations, 0);
			// Add Translations from old list then pad with default ones, 
            // make sure no singularity.
			for (int i=0; i<nTranslations; i++){
				updSpatialTransform()[i+3] = *axes[translationIndices[i]];
//.........这里部分代码省略.........

/** Compute and return the distance errors between all model marker and observations. */
void InverseKinematicsSolver::computeCurrentSquaredMarkerErrors(SimTK::Array_<double> &markerErrors)
{
    markerErrors.resize(_markerAssemblyCondition->getNumMarkers());
    for(unsigned int i=0; i<markerErrors.size(); i++)
        markerErrors[i] = _markerAssemblyCondition->findCurrentMarkerErrorSquared(SimTK::Markers::MarkerIx(i));
}

/** Compute and return the spatial locations of all markers in ground. */
void InverseKinematicsSolver::computeCurrentMarkerLocations(SimTK::Array_<SimTK::Vec3> &markerLocations)
{
    markerLocations.resize(_markerAssemblyCondition->getNumMarkers());
    for(unsigned int i=0; i<markerLocations.size(); i++)
        markerLocations[i] = _markerAssemblyCondition->findCurrentMarkerLocation(SimTK::Markers::MarkerIx(i));
}

int main()
{
    int itc = 0;
    SimTK::Array_<std::string> failures;

    try { ++itc;
        testInverseKinematicsSolverWithOrientations(); 
    }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testInverseKinematicsSolverWithOrientations");
    }

    try {
        ++itc;
        testInverseKinematicsSolverWithEulerAnglesFromFile();
    }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testInverseKinematicsSolverWithEulerAnglesFromFile");
    }

    try {
        ++itc;
        testMarkerWeightAssignments("subject01_Setup_InverseKinematics.xml");
    }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testMarkerWeightAssignments");
    }

    Storage  standard("std_subject01_walk1_ik.mot");
    try {
        InverseKinematicsTool ik1("subject01_Setup_InverseKinematics.xml");
        ik1.run();
        Storage result1(ik1.getOutputMotionFileName());
        CHECK_STORAGE_AGAINST_STANDARD(result1, standard, 
            std::vector<double>(24, 0.2), __FILE__, __LINE__, 
            "testInverseKinematicsGait2354 failed");
        cout << "testInverseKinematicsGait2354 passed" << endl;
    }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testInverseKinematicsGait2354");
    }

    try {
        InverseKinematicsTool ik2("subject01_Setup_InverseKinematics_NoModel.xml");
        Model mdl("subject01_simbody.osim");
        mdl.initSystem();
        ik2.setModel(mdl);
        ik2.run();
        Storage result2(ik2.getOutputMotionFileName());
        CHECK_STORAGE_AGAINST_STANDARD(result2, standard, 
            std::vector<double>(24, 0.2), __FILE__, __LINE__, 
            "testInverseKinematicsGait2354 GUI workflow failed");
        cout << "testInverseKinematicsGait2354 GUI workflow passed" << endl;
    }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testInverseKinematicsGait2354_GUI_workflow");
    }

    try {
        InverseKinematicsTool ik3("constraintTest_setup_ik.xml");
        ik3.run();
        cout << "testInverseKinematicsConstraintTest passed" << endl;
    }
    catch (const std::exception& e) {
        cout << e.what() << endl;
        failures.push_back("testInverseKinematicsConstraintTest");
    }

    if (!failures.empty()) {
        cout << "Done, with " << failures.size() << " failure(s) out of ";
        cout << itc << " test cases." << endl;
        cout << "Failure(s): " << failures << endl;
        return 1;
    }

    cout << "Done. All cases passed." << endl;
    return 0;
}