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;
}
