예제 #1
0
bool XMLDocument::isElementEqual(SimTK::Xml::Element& elt1, SimTK::Xml::Element& elt2, double toleranceForDoubles)
{
    SimTK::String s1,s2;
    elt1.writeToString(s1);
    elt2.writeToString(s2);
    SimTK::Xml::attribute_iterator att1 = elt1.attribute_begin();
    SimTK::Xml::attribute_iterator att2 = elt2.attribute_begin();
    // Handle different # attributes
    if ( (att1 == elt1.attribute_end() && att2 != elt2.attribute_end()) ||
         (att1 != elt1.attribute_end() && att2 == elt2.attribute_end()) ){
            cout << "Number of attributes is different, element " << elt1.getElementTag() << endl;
            return false;
    }
    bool equal =true;
    // Same size attributes including none
    for(att1 = elt1.attribute_begin(); att1 != elt1.attribute_end() && equal; att1++, att2++){
        equal = (att1->getName() == att2->getName());
        equal = equal && (att1->getValue() == att2->getValue());
        if (!equal) {
            cout << "Attribute " << att1->getName() << " is different " << att1->getValue() << 
            "vs." << att2->getValue() << endl;
        }
    }
    if (!equal) return false;

    // Attributes match now children
    SimTK::Array_<SimTK::Xml::Element> elts1 = elt1.getAllElements();
    SimTK::Array_<SimTK::Xml::Element> elts2 = elt2.getAllElements();
    if (elts1.size() != elts2.size()){
        cout << "Different number of children for Element " << elt1.getElementTag() << endl;
        equal = false;
    }
    if (!equal) return false;
    // Recursively compare Elements unless Value Elements in that case do direct compare
    for(unsigned it = 0; it < elts1.size() && equal; it++){
        SimTK::String elt1Tag = elts1[it].getElementTag();
        cout << "Compare " << elt1Tag << endl;
        SimTK::Xml::element_iterator elt2_iter = elt2.element_begin(elt1Tag);
        if (elt2_iter==elt2.element_end()){
            cout << "Element " << elt1Tag << " was not found in reference document" << endl;
            equal = false;
            break;
        }
        bool value1 = elts1[it].isValueElement();
        bool value2 = elt2_iter->isValueElement();
        equal = (value1 == value2);
        if (!equal){ 
            cout << elts1[it].getElementTag() << " is different. One is Value Element the other isn't" << endl;  
            return false; 
        }
        if (value1){
            // We should check if this's a double or array of doubles in that case we can getValueAs<double> anbd
            try {
                SimTK::Array_<double> v1, v2;
                elts1[it].getValueAs(v1);
                elt2_iter->getValueAs(v2);
                for(unsigned ix=0; ix<v1.size() && equal; ix++)
                    equal = (std::fabs(v1[ix]-v2[ix]) < toleranceForDoubles);
            }
            catch(...){
                equal = (elts1[it].getValue() == elt2_iter->getValue());
            }
        }
        else    // recur
            equal = isElementEqual(elts1[it], elts2[it], toleranceForDoubles);
        if (!equal){ 
            cout << elts1[it].getElementTag() << " is different" << endl;  
            SimTK::String pad;
            elts1[it].writeToString(pad);
            cout << pad << endl;
            cout << "------------------- vs. ------" << endl;
            elts2[it].writeToString(pad);
            cout << pad << endl;
            return equal; 
        }
    }

    return equal;
}
예제 #2
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]];
			}
			updSpatialTransform().constructIndependentAxes(nTranslations, 3);
		}
	}

    // Delegate to superclass now.
	Super::updateFromXMLNode(aNode, versionNumber);

	const CoordinateSet& coordinateSet = get_CoordinateSet();

	// Fix coordinate type post deserialization
	// Fix coordinate type post deserialization
	for (int i=0; i<coordinateSet.getSize(); i++){
		OpenSim::Coordinate& nextCoord = coordinateSet.get(i);
		// Find TransformAxis for the coordinate and use it to set Coordinate's motionType
		for(int axisIndex=0; axisIndex<6; axisIndex++){
			const TransformAxis& nextAxis = getSpatialTransform()[axisIndex];
			const Property<std::string>& coordNames = nextAxis.getCoordinateNames();
			if (coordNames.findIndex(nextCoord.getName())!=-1){
				coordinateSet.get(i).setMotionType((axisIndex>2)? 
						Coordinate::Translational : Coordinate::Rotational);
				break;
			}
		}
	}
	// Axes should be independent otherwise Simbody throws an exception in addToSystem
	double tol = 1e-5;
    // Verify that none of the rotation axes are colinear
	const std::vector<SimTK::Vec3> axes=getSpatialTransform().getAxes();
	for(int startIndex=0; startIndex<=3; startIndex+=3){
        if(((axes[startIndex+0]%axes[startIndex+1]).norm() < tol)||
			((axes[startIndex+0]%axes[startIndex+2]).norm() < tol)||
			((axes[startIndex+1]%axes[startIndex+2]).norm() < tol)){
				throw(Exception("CustomJoint " + getName() + 
                    " has colinear axes and so is not well-defined."
                    " Please fix and retry loading.."));
		}
	}
    updProperty_SpatialTransform().setValueIsDefault(false);
}