void Joint::scale(const ScaleSet& scaleSet) { SimTK::Vec3 parentFactors(1.0); SimTK::Vec3 bodyFactors(1.0); // SCALING TO DO WITH THE PARENT BODY ----- // Joint kinematics are scaled by the scale factors for the // parent body, so get those body's factors const string& parentName = getParentBody().getName(); const string& bodyName = getChildBody().getName(); // Get scale factors bool found_p = false; bool found_b = false; for (int i=0; i<scaleSet.getSize(); i++) { Scale& scale = scaleSet.get(i); if (!found_p & scale.getSegmentName() == parentName) { scale.getScaleFactors(parentFactors); found_p = true; } if (!found_b & scale.getSegmentName() == bodyName) { scale.getScaleFactors(bodyFactors); found_b = true; } if(found_p & found_b) break; } SimTK::Vec3& location = upd_location_in_child(); SimTK::Vec3& locationInParent = upd_location_in_parent(); for(int i=0; i<3; i++){ locationInParent[i]*= parentFactors[i]; location[i]*= bodyFactors[i]; } }
/** * Scale a joint based on XYZ scale factors for the bodies. * * @param aScaleSet Set of XYZ scale factors for the bodies. * @todo Need to scale transforms appropriately, given an arbitrary axis. */ void EllipsoidJoint::scale(const ScaleSet& aScaleSet) { Vec3 scaleFactors(1.0); // Joint knows how to scale locations of the joint in parent and on the body Super::scale(aScaleSet); // SCALING TO DO WITH THE PARENT BODY ----- // Joint kinematics are scaled by the scale factors for the // parent body, so get those body's factors const string& parentName = getParentFrame().getName(); for (int i=0; i<aScaleSet.getSize(); i++) { Scale& scale = aScaleSet.get(i); if (scale.getSegmentName()==parentName) { scale.getScaleFactors(scaleFactors); break; } } SimTK::Vec3& ellipsoidRadii = upd_radii_x_y_z(); for(int i=0; i<3; i++){ // Scale the size of the mobilizer ellipsoidRadii[i] *= scaleFactors[i]; } }
/** * Apply a scale factor to a scale set, according to the elements of * the Measurement's _bodyScaleSet. * * @param aFactor the scale factor to apply * @param aScaleSet the set of scale factors to modify */ void Measurement::applyScaleFactor(double aFactor, ScaleSet& aScaleSet) { for (int i = 0; i < _bodyScaleSet.getSize(); i++) { const string& bodyName = _bodyScaleSet[i].getName(); for (int j = 0; j < aScaleSet.getSize(); j++) { if (aScaleSet[j].getSegmentName() == bodyName) { const Array<std::string>& axisNames = _bodyScaleSet[i].getAxisNames(); Vec3 factors(1.0); aScaleSet[j].getScaleFactors(factors); for (int k = 0; k < axisNames.getSize(); k++) { if (axisNames[k] == "x" || axisNames[k] == "X") factors[0] = aFactor; else if (axisNames[k] == "y" || axisNames[k] == "Y") factors[1] = aFactor; else if (axisNames[k] == "z" || axisNames[k] == "Z") factors[2] = aFactor; } aScaleSet[j].setScaleFactors(factors); } } } }
/* * Scale the path based on XYZ scale factors for each body. * * @param aScaleSet XYZ scale factors for the bodies. * @return Whether path was successfully scaled or not. */ void GeometryPath::scale(const SimTK::State& s, const ScaleSet& aScaleSet) { for (int i = 0; i < get_PathPointSet().getSize(); i++) { const string& bodyName = get_PathPointSet().get(i).getBodyName(); for (int j = 0; j < aScaleSet.getSize(); j++) { Scale& aScale = aScaleSet.get(j); if (bodyName == aScale.getSegmentName()) { Vec3 scaleFactors(1.0); aScale.getScaleFactors(scaleFactors); upd_PathPointSet().get(i).scale(scaleFactors); } } } }
/** * Scale the coordinate coupler constraint according to the mobilized body that * the dependent coordinate belongs too. The scale factor is determined by * dotting the coordinate axis with that of the translation. Rotations are NOT * scaled. * * @param Scaleset */ void CoordinateCouplerConstraint::scale(const ScaleSet& aScaleSet) { Coordinate& depCoordinate = _model->updCoordinateSet().get(get_dependent_coordinate_name()); // Only scale if the dependent coordinate is a translation if (depCoordinate.getMotionType() == Coordinate::Translational) { // Constraint scale factor double scaleFactor = 1.0; // Get appropriate scale factors from parent body Vec3 bodyScaleFactors(1.0); const string& parentName = depCoordinate._joint->getParentName(); // Cycle through the scale set to get the appropriate factors for (int i=0; i<aScaleSet.getSize(); i++) { Scale& scale = aScaleSet.get(i); if (scale.getSegmentName()==parentName) { scale.getScaleFactors(bodyScaleFactors); break; } } // Assume uniform scaling unless proven otherwise scaleFactor = bodyScaleFactors[0]; // We can handle non-uniform scaling along transform axes of custom joints ONLY at this time const Joint *joint = dynamic_cast<const Joint*>(depCoordinate._joint.get()); // Simplifies things if we have uniform scaling so check first // TODO: Non-uniform scaling below has not been exercised! - ASeth if(joint) { if (bodyScaleFactors[0] != bodyScaleFactors[1] || bodyScaleFactors[0] != bodyScaleFactors[2] ) { // Get the coordinate axis defined on the parent body Vec3 xyzEuler; joint->getOrientationInParent(xyzEuler); Rotation orientInParent(BodyRotationSequence,xyzEuler[0],XAxis,xyzEuler[1],YAxis,xyzEuler[2],ZAxis); Vec3 axis; throw(Exception("Non-uniform scaling of CoordinateCoupler constraints not implemented.")); } } // scale the user-defined OpenSim set_scale_factor(get_scale_factor() * scaleFactor); } }
/** * Scale a joint based on XYZ scale factors for the bodies. * * @param aScaleSet Set of XYZ scale factors for the bodies. * @todo Need to scale transforms appropriately, given an arbitrary axis. */ void CustomJoint::scale(const ScaleSet& aScaleSet) { Vec3 scaleFactors(1.0); // Joint knows how to scale locations of the joint in parent and on the body Super::scale(aScaleSet); // SCALING TO DO WITH THE PARENT BODY ----- // Joint kinematics are scaled by the scale factors for the // parent body, so get those body's factors const string& parentName = getParentBody().getName(); for (int i=0; i<aScaleSet.getSize(); i++) { Scale& scale = aScaleSet.get(i); if (scale.getSegmentName()==parentName) { scale.getScaleFactors(scaleFactors); break; } } updSpatialTransform().scale(scaleFactors); }
/** * Scale marker set by a set of scale factors */ void MarkerSet::scale(const ScaleSet& scaleSet) { Vec3 scaleFactors(1.0); for (int i = 0; i < getSize(); i++) { Marker& nextMarker = get(i); const string& refFrameName = nextMarker.getFrameName(); //assert(refBodyName); bool found = false; for (int j = 0; j < scaleSet.getSize() && !found; j++) { Scale& nextScale = scaleSet.get(j); if (nextScale.getSegmentName() == refFrameName) { found = true; nextScale.getScaleFactors(scaleFactors); nextMarker.scale(scaleFactors); } } } }
bool compareStdScaleToComputed(const ScaleSet& std, const ScaleSet& comp) { for (int i = 0; i < std.getSize(); ++i) { const Scale& scaleStd = std[i]; int fix = -1; //find corresponding scale factor by segment name for (int j = 0 ; j < comp.getSize(); ++j) { if (comp[j].getSegmentName() == scaleStd.getSegmentName()) { fix = j; break; } } if (fix < 0) { cout << "Computed ScaleSet does not contain factors for "; cout << std[i].getSegmentName() << "." << endl; return false; } if (!(scaleStd == comp[fix])) { return false; } } return true; }
/** * This method scales a model based on user-specified XYZ body scale factors * and/or a set of marker-to-marker distance measurements. * * @param aModel the model to scale. * @param aSubjectMass the final mass of the model after scaling. * @return Whether the scaling process was successful or not. */ bool ModelScaler::processModel(Model* aModel, const string& aPathToSubject, double aSubjectMass) { if (!getApply()) return false; int i; ScaleSet theScaleSet; Vec3 unity(1.0); cout << endl << "Step 2: Scaling generic model" << endl; ComponentList<PhysicalFrame> segments = aModel->getComponentList<PhysicalFrame>(); ComponentList<PhysicalFrame>::const_iterator it = segments.begin(); /* Make a scale set with a Scale for each physical frame. * Initialize all factors to 1.0. */ for (; it != segments.end(); ++it) { Scale* segmentScale = new Scale(); segmentScale->setSegmentName(it->getName()); segmentScale->setScaleFactors(unity); segmentScale->setApply(true); theScaleSet.adoptAndAppend(segmentScale); } SimTK::State& s = aModel->initSystem(); aModel->getMultibodySystem().realize(s, SimTK::Stage::Position); try { /* Make adjustments to theScaleSet, in the user-specified order. */ for (i = 0; i < _scalingOrder.getSize(); i++) { /* For measurements, measure the distance between a pair of markers * in the model, and in the static pose. The latter divided by the * former is the scale factor. Put that scale factor in theScaleSet, * using the body/axis names specified in the measurement to * determine in what place[s] to put the factor. */ if (_scalingOrder[i] == "measurements") { /* Load the static pose marker file, and convert units. */ MarkerData *markerData = 0; if(!_markerFileName.empty() && _markerFileName!=PropertyStr::getDefaultStr()) { markerData = new MarkerData(aPathToSubject + _markerFileName); markerData->convertToUnits(aModel->getLengthUnits()); } /* Now take and apply the measurements. */ for (int j = 0; j < _measurementSet.getSize(); j++) { if (_measurementSet.get(j).getApply()) { if(!markerData) throw Exception("ModelScaler.processModel: ERROR- "+_markerFileNameProp.getName()+ " not set but measurements are used",__FILE__,__LINE__); double scaleFactor = computeMeasurementScaleFactor(s,*aModel, *markerData, _measurementSet.get(j)); if (!SimTK::isNaN(scaleFactor)) _measurementSet.get(j).applyScaleFactor(scaleFactor, theScaleSet); else cout << "___WARNING___: " << _measurementSet.get(j).getName() << " measurement not used to scale " << aModel->getName() << endl; } } } /* For manual scales, just copy the XYZ scale factors from * the manual scale into theScaleSet. */ else if (_scalingOrder[i] == "manualScale") { for (int j = 0; j < _scaleSet.getSize(); j++) { if (_scaleSet[j].getApply()) { const string& bodyName = _scaleSet[j].getSegmentName(); Vec3 factors(1.0); _scaleSet[j].getScaleFactors(factors); for (int k = 0; k < theScaleSet.getSize(); k++) { if (theScaleSet[k].getSegmentName() == bodyName) theScaleSet[k].setScaleFactors(factors); } } } } else { throw Exception("ModelScaler: ERR- Unrecognized string '"+_scalingOrder[i]+"' in "+_scalingOrderProp.getName()+" property (expecting 'measurements' or 'manualScale').",__FILE__,__LINE__); } } /* Now scale the model. */ aModel->scale(s, theScaleSet, aSubjectMass, _preserveMassDist); if(_printResultFiles) { std::string savedCwd = IO::getCwd(); IO::chDir(aPathToSubject); if (!_outputModelFileNameProp.getValueIsDefault()) { if (aModel->print(_outputModelFileName)) cout << "Wrote model file " << _outputModelFileName << " from model " << aModel->getName() << endl; } if (!_outputScaleFileNameProp.getValueIsDefault()) { if (theScaleSet.print(_outputScaleFileName)) cout << "Wrote scale file " << _outputScaleFileName << " for model " << aModel->getName() << endl; } IO::chDir(savedCwd); } } catch (const Exception& x) { x.print(cout); return false; } return true; }