<NME> GeometryPath.cpp
<BEF> /* -------------------------------------------------------------------------- *
 *                         OpenSim:  GeometryPath.cpp                         *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2017 Stanford University and the Authors                *
 * Author(s): Peter Loan, Ajay Seth                                           *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

//=============================================================================
// INCLUDES
//=============================================================================
#include "GeometryPath.h"
#include "ConditionalPathPoint.h"
#include "MovingPathPoint.h"
#include "PointForceDirection.h"
#include <OpenSim/Simulation/Wrap/PathWrap.h>
#include "Model.h"

//=============================================================================
// STATICS
//=============================================================================
using namespace std;
using namespace OpenSim;
using namespace SimTK;
using SimTK::Vec3;

// this is what is stored in a cache variable at runtime. It's a value-type that's safe to
// pass between multiple models, which looks up a point in the component path
class OpenSim::GeometryPath::PathElementLookup {
public:
    static PathElementLookup fromPtr(const OpenSim::PathPointSet& pps,
                                     const OpenSim::PathWrapSet& pws,
                                     const OpenSim::GeometryPath& geomPath,
                                     const OpenSim::AbstractPathPoint* ptr)
    {
        // linearly search each collection to figure out its type + index

        const OpenSim::Component* owner = &ptr->getOwner();

        if (owner == &geomPath) {
            for (int i = 0, len = pps.getSize(); i < len; ++i) {
                if (&pps[i] == ptr) {
                    return PathElementLookup{PointType::InPathPointSet, i};
                }
            }
        }
        else {
            for (int i = 0, len = pws.getSize(); i < len; ++i) {
                if (&pws[i] == owner) {
                    PointType t = ptr == &pws[i].getWrapPoint1() ? PointType::PathWrapPt1 : PointType::PathWrapPt2;
                    return PathElementLookup{t, i};
                }
            }
        }

        OPENSIM_THROW(OpenSim::Exception, "unable to create a PathElementLookup for the given pointer: are you sure it is part of the GeometryPath?");
    }

private:
    enum class PointType : char { InPathPointSet, PathWrapPt1, PathWrapPt2 };

    PathElementLookup(PointType pointType, int index) :
        index_{index},
        pointType_{pointType}
    {
    }
public:
    OpenSim::AbstractPathPoint* toPtr(const OpenSim::PathPointSet& pps,
                                      const OpenSim::PathWrapSet& pws) const
    {
        switch (pointType_) {
        case PointType::InPathPointSet:
            return &pps.get(index_);
        case PointType::PathWrapPt1:
            return const_cast<OpenSim::PathWrapPoint*>(&pws.get(index_).getWrapPoint1());
        case PointType::PathWrapPt2:
            return const_cast<OpenSim::PathWrapPoint*>(&pws.get(index_).getWrapPoint2());
        default:
            OPENSIM_THROW(OpenSim::Exception, "unhandled PointType passed to `toPtr`: this is a developer error");
        }
    }

private:
    int index_ : 24;
    PointType pointType_ : 8;
};

static void PopulatePathPointersCache(
    const OpenSim::PathPointSet& pps,
    const OpenSim::PathWrapSet& pws,
    const std::vector<OpenSim::GeometryPath::PathElementLookup>& lookups,
    OpenSim::Array<OpenSim::AbstractPathPoint*>& out)
{
    out.setSize(static_cast<int>(lookups.size()));
    for (int i = 0, len = static_cast<int>(lookups.size()); i < len; ++i) {
        out[i] = lookups[i].toPtr(pps, pws);
    }
}

static void PopulatePathElementLookup(
    const OpenSim::PathPointSet& pps,
    const OpenSim::PathWrapSet& pws,
    const OpenSim::Array<OpenSim::AbstractPathPoint*>& ptrs,
    const OpenSim::GeometryPath& geomPath,
    std::vector<OpenSim::GeometryPath::PathElementLookup>& out)
{
    out.clear();
    out.reserve(ptrs.getSize());
    for (int i = 0, len = ptrs.getSize(); i < len; ++i) {
        out.push_back(OpenSim::GeometryPath::PathElementLookup::fromPtr(pps, pws, geomPath, ptrs[i]));
    }
}

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================
//_____________________________________________________________________________
/*
 * Default constructor.
 */
GeometryPath::GeometryPath() :
    ModelComponent(),
    _preScaleLength(0.0)
{
    setAuthors("Peter Loan");
    constructProperties();
 }

//_____________________________________________________________________________
/*
* Perform set up functions after model has been deserialized or copied.
*
*/
void GeometryPath::extendFinalizeFromProperties()
{
    Super::extendFinalizeFromProperties();

    for (int i = 0; i < get_PathWrapSet().getSize(); ++i) {
        if (upd_PathWrapSet()[i].getName().empty()) {
            std::stringstream label;
            label << "pathwrap_" << i;
            upd_PathWrapSet()[i].setName(label.str());
        }
    }
}

void GeometryPath::extendConnectToModel(Model& aModel)
{
    Super::extendConnectToModel(aModel);

    OPENSIM_THROW_IF_FRMOBJ(get_PathPointSet().getSize() < 2,
        InvalidPropertyValue,
        getProperty_PathPointSet().getName(),
        "A valid path must be connected to a model by at least two PathPoints.")

    // Name the path points based on the current path
    // (i.e., the set of currently active points is numbered
    // 1, 2, 3, ...).
    namePathPoints(0);
}

//_____________________________________________________________________________
/*
 * Create the SimTK state, discrete and/or cache for this GeometryPath.
 */
 void GeometryPath::extendAddToSystem(SimTK::MultibodySystem& system) const 
{
    Super::extendAddToSystem(system);

    // Allocate cache entries to save the current length and speed(=d/dt length)
    // of the path in the cache. Length depends only on q's so will be valid
    // after Position stage, speed requires u's also so valid at Velocity stage.
    this->_lengthCV = addCacheVariable("length", 0.0, SimTK::Stage::Position);
    this->_speedCV = addCacheVariable("speed", 0.0, SimTK::Stage::Velocity);

    // Cache the set of points currently defining this path.
    this->_currentPathCV = addCacheVariable("current_path", std::vector<PathElementLookup>{}, SimTK::Stage::Position);

    // We consider this cache entry valid any time after it has been created
    // and first marked valid, and we won't ever invalidate it.
    this->_colorCV = addCacheVariable("color", get_Appearance().get_color(), SimTK::Stage::Topology);
}

 void GeometryPath::extendInitStateFromProperties(SimTK::State& s) const
{
    Super::extendInitStateFromProperties(s);
    markCacheVariableValid(s, _colorCV);  // it is OK at its default value
}

//------------------------------------------------------------------------------
//                         GENERATE DECORATIONS
//------------------------------------------------------------------------------
// The GeometryPath takes care of drawing itself here, using information it
// can extract from the supplied state, including position information and
// color information that may have been calculated as late as Stage::Dynamics.
// For example, muscles may want the color to reflect activation level and 
// other path-using components might want to use forces (tension). We will
// ensure that the state has been realized to Stage::Dynamics before looking
// at it. (It is only guaranteed to be at Stage::Position here.)
void GeometryPath::
generateDecorations(bool fixed, const ModelDisplayHints& hints, 
                    const SimTK::State& state, 
                    SimTK::Array_<SimTK::DecorativeGeometry>& appendToThis) const
{        
    // There is no fixed geometry to generate here.
    if (fixed) { return; }

    const Array<AbstractPathPoint*>& pathPoints = getCurrentPath(state);

    assert(pathPoints.size() > 1);

    const AbstractPathPoint* lastPoint = pathPoints[0];
    MobilizedBodyIndex mbix(0);

    Vec3 lastPos = lastPoint->getLocationInGround(state);
    if (hints.get_show_path_points())
        DefaultGeometry::drawPathPoint(mbix, lastPos, getColor(state), appendToThis);

    Vec3 pos;

    for (int i = 1; i < pathPoints.getSize(); ++i) {
        AbstractPathPoint* point = pathPoints[i];
        PathWrapPoint* pwp = dynamic_cast<PathWrapPoint*>(point);

        if (pwp) {
            // A PathWrapPoint provides points on the wrapping surface as Vec3s
            const Array<Vec3>& surfacePoints = pwp->getWrapPath(state);
            // The surface points are expressed w.r.t. the wrap surface's body frame.
            // Transform the surface points into the ground reference frame to draw
            // the surface point as the wrapping portion of the GeometryPath
            const Transform& X_BG = pwp->getParentFrame().getTransformInGround(state);
            // Cycle through each surface point and draw it the Ground frame
            for (int j = 0; j<surfacePoints.getSize(); ++j) {
                // transform the surface point into the Ground reference frame
                pos = X_BG*surfacePoints[j];
                if (hints.get_show_path_points())
                    DefaultGeometry::drawPathPoint(mbix, pos, getColor(state),
                        appendToThis);
                // Line segments will be in ground frame
                appendToThis.push_back(DecorativeLine(lastPos, pos)
                    .setLineThickness(4)
                    .setColor(getColor(state)).setBodyId(0).setIndexOnBody(j));
                lastPos = pos;
            }
        } 
        else { // otherwise a regular PathPoint so just draw its location
            pos = point->getLocationInGround(state);
            if (hints.get_show_path_points())
                DefaultGeometry::drawPathPoint(mbix, pos, getColor(state),
                    appendToThis);
            // Line segments will be in ground frame
            appendToThis.push_back(DecorativeLine(lastPos, pos)
                .setLineThickness(4)
                .setColor(getColor(state)).setBodyId(0).setIndexOnBody(i));
            lastPos = pos;
        }
    }
}

//_____________________________________________________________________________
/*
 * Connect properties to local pointers.
 */
void GeometryPath::constructProperties()
{
    constructProperty_PathPointSet(PathPointSet());

    constructProperty_PathWrapSet(PathWrapSet());
    
    Appearance appearance;
    appearance.set_color(SimTK::Gray);
    constructProperty_Appearance(appearance);
}

//_____________________________________________________________________________
/*
 * Name the path points based on their position in the set. To keep the
 * names up to date, this method should be called every time the path changes.
 *
 * @param aStartingIndex The index of the first path point to name.
 */
void GeometryPath::namePathPoints(int aStartingIndex)
{
    char indx[5];
    for (int i = aStartingIndex; i < get_PathPointSet().getSize(); i++)
    {
        sprintf(indx,"%d",i+1);
        AbstractPathPoint& point = get_PathPointSet().get(i);
        if (point.getName()=="" && hasOwner()) {
            point.setName(getOwner().getName() + "-P" + indx);
        }
    }
}

//_____________________________________________________________________________
/*
 * get the current path of the path
 *
 * @return The array of currently active path points.
 * 
 */
const OpenSim::Array <AbstractPathPoint*> & GeometryPath::
getCurrentPath(const SimTK::State& s)  const
{
    computePath(s);   // compute checks if path needs to be recomputed
    return _currentPathPtrsCache;
}

// get the path as PointForceDirections directions 
// CAUTION: the return points are heap allocated; you must delete them yourself! 
// (TODO: that is really lame)
void GeometryPath::
getPointForceDirections(const SimTK::State& s, 
                        OpenSim::Array<PointForceDirection*> *rPFDs) const
{
    int i;
    AbstractPathPoint* start;
    AbstractPathPoint* end;
    const OpenSim::PhysicalFrame* startBody;
    const OpenSim::PhysicalFrame* endBody;
    const Array<AbstractPathPoint*>& currentPath = getCurrentPath(s);

    int np = currentPath.getSize();
    rPFDs->ensureCapacity(np);
    
    for (i = 0; i < np; i++) {
        PointForceDirection *pfd = 
            new PointForceDirection(currentPath[i]->getLocation(s), 
                                    currentPath[i]->getParentFrame(), Vec3(0));
        rPFDs->append(pfd);
    }

    for (i = 0; i < np-1; i++) {
        start = currentPath[i];
        end = currentPath[i+1];
        startBody = &start->getParentFrame();
        endBody = &end->getParentFrame();

        if (startBody != endBody)
        {
            Vec3 posStart, posEnd;
            Vec3 direction(0);

            // Find the positions of start and end in the inertial frame.
            //engine.getPosition(s, start->getParentFrame(), start->getLocation(), posStart);
            posStart = start->getLocationInGround(s);
            
            //engine.getPosition(s, end->getParentFrame(), end->getLocation(), posEnd);
            posEnd = end->getLocationInGround(s);

            // Form a vector from start to end, in the inertial frame.
            direction = (posEnd - posStart);

            // Check that the two points are not coincident.
            // This can happen due to infeasible wrapping of the path,
            // when the origin or insertion enters the wrapping surface.
            // This is a temporary fix, since the wrap algorithm should
            // return NaN for the points and/or throw an Exception- aseth
            if (direction.norm() < SimTK::SignificantReal){
                direction = direction*SimTK::NaN;
            }
            else{
                direction = direction.normalize();
            }

            // Get resultant direction at each point 
            rPFDs->get(i)->addToDirection(direction);
            rPFDs->get(i+1)->addToDirection(-direction);
        }
    }
}

/* add in the equivalent spatial forces on bodies for an applied tension 
    along the GeometryPath to a set of bodyForces */
void GeometryPath::addInEquivalentForces(const SimTK::State& s,
    const double& tension, 
    SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
    SimTK::Vector& mobilityForces) const
{
    AbstractPathPoint* start = NULL;
    AbstractPathPoint* end = NULL;
    const SimTK::MobilizedBody* bo = NULL;
    const SimTK::MobilizedBody* bf = NULL;
    const Array<AbstractPathPoint*>& currentPath = getCurrentPath(s);
    int np = currentPath.getSize();

    const SimTK::SimbodyMatterSubsystem& matter = 
                                        getModel().getMatterSubsystem();

    // start point, end point,  direction, and force vectors in ground
    Vec3 po(0), pf(0), dir(0), force(0);
    // partial velocity of point in body expressed in ground 
    Vec3 dPodq_G(0), dPfdq_G(0);

    // gen force (torque) due to moving point under tension
    double fo, ff;

    for (int i = 0; i < np-1; ++i) {
        start = currentPath[i];
        end = currentPath[i+1];

        bo = &start->getParentFrame().getMobilizedBody();
        bf = &end->getParentFrame().getMobilizedBody();

        if (bo != bf) {
{
    // Check if the current path needs to recomputed.
    computePath(s);

    // If display path is current do not need to recompute it.
    if (isCacheVariableValid(s, "current_display_path"))
        return;
}

//=============================================================================

            // Check that the two points are not coincident.
            // This can happen due to infeasible wrapping of the path,
            // when the origin or insertion enters the wrapping surface.
            // This is a temporary fix, since the wrap algorithm should
            // return NaN for the points and/or throw an Exception- aseth
            if (dir.norm() < SimTK::SignificantReal){
                dir = dir*SimTK::NaN;
            }
            else{
                dir = dir.normalize();
            }

            force = tension*dir;

            const MovingPathPoint* mppo =
                dynamic_cast<MovingPathPoint *>(start);

            // do the same for the end point of this segment of the path
            const MovingPathPoint* mppf =
                dynamic_cast<MovingPathPoint *>(end);

            // add in the tension point forces to body forces
            if (mppo) {// moving path point location is a function of the state
                // transform of the frame of the point to the base mobilized body
                auto X_BF = mppo->getParentFrame().findTransformInBaseFrame();
                bo->applyForceToBodyPoint(s, X_BF*mppo->getLocation(s), force,
                    bodyForces);
            }
            else {
                // transform of the frame of the point to the base mobilized body
                auto X_BF = start->getParentFrame().findTransformInBaseFrame();
                bo->applyForceToBodyPoint(s, X_BF*start->getLocation(s), force,
                    bodyForces);
            }

            if (mppf) {// moving path point location is a function of the state
                // transform of the frame of the point to the base mobilized body
                auto X_BF = mppf->getParentFrame().findTransformInBaseFrame();
                bf->applyForceToBodyPoint(s, X_BF*mppf->getLocation(s), -force,
                    bodyForces);
            }
            else {
                // transform of the frame of the point to the base mobilized body
                auto X_BF = end->getParentFrame().findTransformInBaseFrame();
                bf->applyForceToBodyPoint(s, X_BF*end->getLocation(s), -force,
                    bodyForces);
            }

            // Now account for the work being done by virtue of the moving
            // path point motion relative to the body it is on
            if(mppo){
                // torque (genforce) contribution due to relative movement 
                // of a via point w.r.t. the body it is connected to.
                dPodq_G = bo->expressVectorInGroundFrame(s, start->getdPointdQ(s));
                fo = ~dPodq_G*force;            

                // get the mobilized body the coordinate is couple to.
                const SimTK::MobilizedBody& mpbod =
                    matter.getMobilizedBody(mppo->getXCoordinate().getBodyIndex());

                // apply the generalized (mobility) force to the coordinate's body
                mpbod.applyOneMobilityForce(s, 
                    mppo->getXCoordinate().getMobilizerQIndex(), 
                    fo, mobilityForces);
            }

            if(mppf){
                dPfdq_G = bf->expressVectorInGroundFrame(s, end->getdPointdQ(s));
                ff = ~dPfdq_G*(-force);

                // get the mobilized body the coordinate is couple to.
                const SimTK::MobilizedBody& mpbod =
                    matter.getMobilizedBody(mppf->getXCoordinate().getBodyIndex());

                mpbod.applyOneMobilityForce(s, 
                    mppf->getXCoordinate().getMobilizerQIndex(), 
                    ff, mobilityForces);
            }
        }       
    }
}

//_____________________________________________________________________________
/*
 * Update the geometric representation of the path.
 * The resulting geometry is maintained at the VisibleObject layer.
 * This function should not be made public. It is called internally
 * by compute() only when the path has changed.
 * 
 */
void GeometryPath::updateGeometry(const SimTK::State& s) const
{
    // Check if the current path needs to recomputed.
    computePath(s);
}

//=============================================================================
// GET
//=============================================================================
//-----------------------------------------------------------------------------
// LENGTH
//-----------------------------------------------------------------------------
//_____________________________________________________________________________
/*
 * Compute the total length of the path.
 *
 * @return Total length of the path.
 */
double GeometryPath::getLength( const SimTK::State& s) const
{
    computePath(s);  // compute checks if path needs to be recomputed
    return getCacheVariableValue(s, _lengthCV);
}

void GeometryPath::setLength( const SimTK::State& s, double length ) const
{
    setCacheVariableValue(s, _lengthCV, length);
}

void GeometryPath::setColor(const SimTK::State& s, const SimTK::Vec3& color) const
{
    setCacheVariableValue(s, _colorCV, color);
}

Vec3 GeometryPath::getColor(const SimTK::State& s) const
{
    return getCacheVariableValue(s, _colorCV);
}

//_____________________________________________________________________________
/*
 * Compute the lengthening speed of the path.
 *
 * @return lengthening speed of the path.
 */
double GeometryPath::getLengtheningSpeed( const SimTK::State& s) const
{
    computeLengtheningSpeed(s);
    return getCacheVariableValue(s, _speedCV);
}
void GeometryPath::setLengtheningSpeed( const SimTK::State& s, double speed ) const
{
    setCacheVariableValue(s, _speedCV, speed);
}

void GeometryPath::setPreScaleLength( const SimTK::State& s, double length ) {
    _preScaleLength = length;
}
double GeometryPath::getPreScaleLength( const SimTK::State& s) const {
    return _preScaleLength;
}

//=============================================================================
// UTILITY
//=============================================================================
//_____________________________________________________________________________
/*
 * Add a new path point, with default location, to the path.
 *
 * @param aIndex The position in the pathPointSet to put the new point in.
 * @param frame The frame to attach the point to.
 * @return Pointer to the newly created path point.
 */
AbstractPathPoint* GeometryPath::
addPathPoint(const SimTK::State& s, int aIndex, const PhysicalFrame& frame)
{
    PathPoint* newPoint = new PathPoint();
    newPoint->setParentFrame(frame);
    Vec3 newLocation(0.0); 
    // Note: placeNewPathPoint() returns a location by reference.
    // It computes a new location according to the index where the new path point 
    // will be inserted along the path(among the other path points).
    placeNewPathPoint(s, newLocation, aIndex, frame);
    // Now set computed new location into the newPoint
    newPoint->setLocation(newLocation);
    upd_PathPointSet().insert(aIndex, newPoint);

    // Rename the path points starting at this new one.
    namePathPoints(aIndex);

    // Update start point and end point in the wrap instances so that they
    // refer to the same path points they did before the new point
    // was added. These indices are 1-based.
    aIndex++;
    for (int i=0; i<get_PathWrapSet().getSize(); i++) {
        int startPoint = get_PathWrapSet().get(i).getStartPoint();
        int endPoint = get_PathWrapSet().get(i).getEndPoint();
        if (startPoint != -1 && aIndex <= startPoint)
            get_PathWrapSet().get(i).setStartPoint(s,startPoint + 1);
        if (endPoint != -1 && aIndex <= endPoint)
            get_PathWrapSet().get(i).setEndPoint(s,endPoint + 1);
    }

    return newPoint;
}

AbstractPathPoint* GeometryPath::
appendNewPathPoint(const std::string& proposedName, 
                   const PhysicalFrame& frame,
                   const SimTK::Vec3& aPositionOnBody)
{
    PathPoint* newPoint = new PathPoint();
    newPoint->setParentFrame(frame);
    newPoint->setName(proposedName);
    newPoint->setLocation(aPositionOnBody);
    upd_PathPointSet().adoptAndAppend(newPoint);

    return newPoint;
}

//_____________________________________________________________________________
/*
 * Determine an appropriate default XYZ location for a new path point.
 * Note that this method is internal and should not be called directly on a new 
 * point as the point is not really added to the path (done in addPathPoint() 
 * instead)
 * @param aOffset The XYZ location to be determined.
 * @param aIndex The position in the pathPointSet to put the new point in.
 * @param frame The body to attach the point to.
 */
void GeometryPath::
placeNewPathPoint(const SimTK::State& s, SimTK::Vec3& aOffset, int aIndex, 
                  const PhysicalFrame& frame)
{
    // The location of the point is determined by moving a 'distance' from 'base' 
    // along a vector from 'start' to 'end.' 'base' is the existing path point 
    // that is in or closest to the index aIndex. 'start' and 'end' are existing
    // path points--which ones depends on where the new point is being added. 
    // 'distance' is 0.5 for points added to the middle of a path (so the point
    // appears halfway between the two adjacent points), and 0.2 for points that
    // are added to either end of the path. If there is only one point in the 
    // path, the new point is put 0.01 units away in all three dimensions.
    if (get_PathPointSet().getSize() > 1) {
        int start, end, base;
        double distance;
        if (aIndex == 0) {
            start = 1;
            end = 0;
            base = end;
            distance = 0.2;
        } else if (aIndex >= get_PathPointSet().getSize()) {
            start = aIndex - 2;
            end = aIndex - 1;
            base = end;
            distance = 0.2;
        } else {
            start = aIndex;
            end = aIndex - 1;
            base = start;
            distance = 0.5;
        }

        const Vec3 startPt = get_PathPointSet()[start].getLocation(s);
        const Vec3 endPt = get_PathPointSet()[end].getLocation(s);
        const Vec3 basePt = get_PathPointSet()[base].getLocation(s);

        Vec3 startPt2 = get_PathPointSet()[start].getParentFrame()
            .findStationLocationInAnotherFrame(s, startPt, frame);

        Vec3 endPt2 = get_PathPointSet()[end].getParentFrame()
            .findStationLocationInAnotherFrame(s, endPt, frame);

        aOffset = basePt + distance * (endPt2 - startPt2);
    } else if (get_PathPointSet().getSize() == 1) {
        aOffset= get_PathPointSet()[0].getLocation(s) + 0.01;
    }
    else {  // first point, do nothing?
    }
}

//_____________________________________________________________________________
/*
 * See if a path point can be deleted. All paths must have at least two
 * active path points to define the path.
 *
 * @param aIndex The index of the point to delete.
 * @return Whether or not the point can be deleted.
 */
bool GeometryPath::canDeletePathPoint( int aIndex)
{
    // A path point can be deleted only if there would remain
    // at least two other fixed points.
    int numOtherFixedPoints = 0;
    for (int i = 0; i < get_PathPointSet().getSize(); i++) {
        if (i != aIndex) {
            if (!(  get_PathPointSet().get(i).getConcreteClassName()
                  ==("ConditionalPathPoint")))
                numOtherFixedPoints++;
        }
    }

    if (numOtherFixedPoints >= 2)
        return true;

    return false;
}

//_____________________________________________________________________________
/*
 * Delete a path point.
 *
 * @param aIndex The index of the point to delete.
 * @return Whether or not the point was deleted.
 */
bool GeometryPath::deletePathPoint(const SimTK::State& s, int aIndex)
{
    if (canDeletePathPoint(aIndex) == false)
        return false;

    upd_PathPointSet().remove(aIndex);

    // rename the path points starting at the deleted position
    namePathPoints(aIndex);

    // Update start point and end point in the wrap instances so that they
    // refer to the same path points they did before the point was
    // deleted. These indices are 1-based. If the point deleted is start
    // point or end point, the path wrap range is made smaller by one point.
    aIndex++;
    for (int i=0; i<get_PathWrapSet().getSize(); i++) {
        int startPoint = get_PathWrapSet().get(i).getStartPoint();
        int endPoint   = get_PathWrapSet().get(i).getEndPoint();

        if (   (startPoint != -1 && aIndex < startPoint) 
            || (startPoint > get_PathPointSet().getSize()))
            get_PathWrapSet().get(i).setStartPoint(s, startPoint - 1);

        if (   endPoint > 1 
            && aIndex <= endPoint 
            && (   (endPoint > startPoint) 
                || (endPoint > get_PathPointSet().getSize())))
            get_PathWrapSet().get(i).setEndPoint(s, endPoint - 1);
    }

    return true;
}

//_____________________________________________________________________________
/*
 * Replace a path point in the set with another point. The new one is made a
 * member of all the same groups as the old one, and is inserted in the same
 * place the old one occupied.
 *
 *  @param aOldPathPoint Path point to remove.
 *  @param aNewPathPoint Path point to add.
 */
bool GeometryPath::
replacePathPoint(const SimTK::State& s, AbstractPathPoint* aOldPathPoint, 
                 AbstractPathPoint* aNewPathPoint) 
{
    if (aOldPathPoint != NULL && aNewPathPoint != NULL) {
        int count = 0;
        int index = get_PathPointSet().getIndex(aOldPathPoint);
        // If you're switching from non-via to via, check to make sure that the
        // path will be left with at least 2 non-via points.
        ConditionalPathPoint* oldVia = 
            dynamic_cast<ConditionalPathPoint*>(aOldPathPoint);
        ConditionalPathPoint* newVia = 
            dynamic_cast<ConditionalPathPoint*>(aNewPathPoint);
        if (oldVia == NULL && newVia != NULL) {
            for (int i=0; i<get_PathPointSet().getSize(); i++) {
                if (i != index) {
                    if (dynamic_cast<ConditionalPathPoint*>
                                        (&get_PathPointSet().get(i)) == NULL)
                        count++;
                }
            }
        } else {
            count = 2;
        }
        if (count >= 2 && index >= 0) {
            upd_PathPointSet().set(index, aNewPathPoint, true);
            //computePath(s);
            return true;
        }
    }
    return false;
}

//_____________________________________________________________________________
/*
 * Create a new wrap instance and add it to the set.
 *
 * @param aWrapObject The wrap object to use in the new wrap instance.
 */
void GeometryPath::addPathWrap(WrapObject& aWrapObject)
{
    PathWrap* newWrap = new PathWrap();
    newWrap->setWrapObject(aWrapObject);
    newWrap->setMethod(PathWrap::hybrid);
    upd_PathWrapSet().adoptAndAppend(newWrap);
    finalizeFromProperties();
}

//_____________________________________________________________________________
/*
 * Move a wrap instance up in the list. Changing the order of wrap instances for
 * a path may affect how the path wraps over the wrap objects.
 *
 * @param aIndex The index of the wrap instance to move up.
 */
void GeometryPath::moveUpPathWrap(const SimTK::State& s, int aIndex)
{
    if (aIndex > 0) {
        // Make sure wrap object is not deleted by remove().
        upd_PathWrapSet().setMemoryOwner(false); 

        PathWrap& wrap = get_PathWrapSet().get(aIndex);
        upd_PathWrapSet().remove(aIndex);
        upd_PathWrapSet().insert(aIndex - 1, &wrap);
        upd_PathWrapSet().setMemoryOwner(true);
    }
}

//_____________________________________________________________________________
/*
 * Move a wrap instance down in the list. Changing the order of wrap instances
 * for a path may affect how the path wraps over the wrap objects.
 *
 * @param aIndex The index of the wrap instance to move down.
 */
void GeometryPath::moveDownPathWrap(const SimTK::State& s, int aIndex)
{
    if (aIndex < get_PathWrapSet().getSize() - 1) {
        // Make sure wrap object is not deleted by remove().
        upd_PathWrapSet().setMemoryOwner(false);

        PathWrap& wrap = get_PathWrapSet().get(aIndex);
        upd_PathWrapSet().remove(aIndex);
        upd_PathWrapSet().insert(aIndex + 1, &wrap);
        upd_PathWrapSet().setMemoryOwner(true);
    }
}

//_____________________________________________________________________________
/*
 * Delete a wrap instance.
 *
 * @param aIndex The index of the wrap instance to delete.
 */
void GeometryPath::deletePathWrap(const SimTK::State& s, int aIndex)
{
    upd_PathWrapSet().remove(aIndex);

}

//==============================================================================
// SCALING
//==============================================================================
void GeometryPath::
extendPreScale(const SimTK::State& s, const ScaleSet& scaleSet)
{
    Super::extendPreScale(s, scaleSet);
    setPreScaleLength(s, getLength(s));
}

void GeometryPath::
extendPostScale(const SimTK::State& s, const ScaleSet& scaleSet)
{
    Super::extendPostScale(s, scaleSet);
    computePath(s);
}

//--------------------------------------------------------------------------
// COMPUTATIONS
//--------------------------------------------------------------------------
//=============================================================================
// PATH, WRAPPING, AND MOMENT ARM
//=============================================================================
//_____________________________________________________________________________
/*
 * Calculate the current path.
 */
void GeometryPath::computePath(const SimTK::State& s) const
{
    if (isCacheVariableValid(s, _currentPathCV)) {
        // even though the cache variable is valid, re-populate the pointers cache
        //
        // this is because although it may be valid *in the state* the
        // model-level pointers cache may be dirty with lookups from other
        // states
        PopulatePathPointersCache(get_PathPointSet(),
                                  get_PathWrapSet(),
                                  getCacheVariableValue(s, _currentPathCV),
                                  _currentPathPtrsCache);
        return;
    }

    // Clear the current path.
    _currentPathPtrsCache.setSize(0);

    // Add the active fixed and moving via points to the path.
    for (int i = 0; i < get_PathPointSet().getSize(); i++) {
        if (get_PathPointSet()[i].isActive(s))
            _currentPathPtrsCache.append(&get_PathPointSet()[i]); // <--- !!!!BAD
    }
  
    // Use the current path so far to check for intersection with wrap objects, 
    // which may add additional points to the path.
    applyWrapObjects(s, _currentPathPtrsCache);
    calcLengthAfterPathComputation(s, _currentPathPtrsCache);

    // the pointers array now contains the "correct" (wrapped) path
    //
    // because the path is state-dependent (different states may wrap differently), the
    // path points (abstract) must be stored in a SimTK cache variable.
    //
    // However, we can't store raw pointers in a cache variable because that may cause
    // aliasing issues in downstream code. E.g. the state may later be used with a
    // copied/moved-from version of the model, rather than the original one, so the
    // pointers may be stale.
    std::vector<PathElementLookup>& lookups = updCacheVariableValue(s, _currentPathCV);
    PopulatePathElementLookup(get_PathPointSet(),
                              get_PathWrapSet(),
                              _currentPathPtrsCache,
                              *this,
                              lookups);
    markCacheVariableValid(s, _currentPathCV);
}

//_____________________________________________________________________________
/*
 * Compute lengthening speed of the path.
 */
void GeometryPath::computeLengtheningSpeed(const SimTK::State& s) const
{
    if (isCacheVariableValid(s, _speedCV)) {
        return;
    }

    const Array<AbstractPathPoint*>& currentPath = getCurrentPath(s);

    double speed = 0.0;
    
    for (int i = 0; i < currentPath.getSize() - 1; i++) {
        speed += currentPath[i]->calcSpeedBetween(s, *currentPath[i+1]);
    }

    setLengtheningSpeed(s, speed);
}

//_____________________________________________________________________________
/*
 * Apply the wrap objects to the current path.
 */
void GeometryPath::
applyWrapObjects(const SimTK::State& s, Array<AbstractPathPoint*>& path) const 
{
    int wrapSetSize = get_PathWrapSet().getSize();
    if (wrapSetSize < 1)
        return;

    WrapResult best_wrap;
    Array<int> result, order;

    result.setSize(wrapSetSize);
    order.setSize(wrapSetSize);

    // Set the initial order to be the order they are listed in the path.
    for (int i = 0; i < wrapSetSize; i++)
        order[i] = i;

    // If there is only one wrap object, calculate the wrapping only once.
    // If there are two or more objects, perform up to 8 iterations where
    // the result from one wrap object is used as the starting point for
    // the next wrap.
    const int maxIterations = wrapSetSize < 2 ? 1 : 8;
    double last_length = SimTK::Infinity;
    for (int kk = 0; kk < maxIterations; kk++)
    {
        for (int i = 0; i < wrapSetSize; i++)
        {
            result[i] = 0;
            PathWrap& ws = get_PathWrapSet().get(order[i]);
            const WrapObject* wo = ws.getWrapObject();
            best_wrap.wrap_pts.setSize(0);
            double min_length_change = SimTK::Infinity;

            // First remove this object's wrapping points from the current path.
            for (int j = 0; j <path.getSize(); j++) {
                if( path.get(j) == &ws.getWrapPoint1()) {
                    path.remove(j); // remove the first wrap point
                    path.remove(j); // remove the second wrap point
                    break;
                }
            }

            if (wo->get_active()) {
                // startPoint and endPoint in wrapStruct represent the 
                // user-defined starting and ending points in the array of path 
                // points that should be considered for wrapping. These indices 
                // take into account via points, whether or not they are active. 
                // Thus they are indices into mp_orig[], not mp[] (also, mp[] 
                // may contain wrapping points from previous wrap objects, which
                // would mess up the starting and ending indices). But the goal 
                // is to find starting and ending indices in mp[] to consider
                // for wrapping over this wrap object. Here is how that is done:

                // 1. startPoint and endPoint are 1-based, so subtract 1 from 
                // them to get indices into get_PathPointSet(). -1 (or any value
                // less than 1) means use the first (or last) point.
                const int wrapStart = (ws.getStartPoint() < 1
                                            ? 0 
                                            : ws.getStartPoint() - 1);
                const int wrapEnd   = (ws.getEndPoint() < 1
                                            ? get_PathPointSet().getSize() - 1 
                                            : ws.getEndPoint() - 1);

                // 2. Scan forward from wrapStart in get_PathPointSet() to find 
                // the first point that is active. Store a pointer to it (smp).
                int jfwd = wrapStart;
                for (; jfwd <= wrapEnd; jfwd++)
                    if (get_PathPointSet().get(jfwd).isActive(s))
                        break;
                if (jfwd > wrapEnd) // there are no active points in the path
                    return;
                const AbstractPathPoint* const smp = &get_PathPointSet().get(jfwd);

                // 3. Scan backwards from wrapEnd in get_PathPointSet() to find 
                // the last point that is active. Store a pointer to it (emp).
                int jrev = wrapEnd;
                for (; jrev >= wrapStart; jrev--)
                    if (get_PathPointSet().get(jrev).isActive(s))
                        break;
                if (jrev < wrapStart) // there are no active points in the path
                    return;
                const AbstractPathPoint* const emp = &get_PathPointSet().get(jrev);

                // 4. Now find the indices of smp and emp in _currentPath.
                int start=-1, end=-1;
                for (int j = 0; j < path.getSize(); j++) {
                    if (path.get(j) == smp)
                        start = j;
                    if (path.get(j) == emp)
                        end = j;
                }
                if (start == -1 || end == -1) // this should never happen
                    return;

                // You now have indices into _currentPath (which is a list of 
                // all currently active points, including wrap points) that 
                // represent the used-defined range of points to consider for 
                // wrapping over this wrap object. Check each path segment in 
                // this range, choosing the best wrap as the one that changes 
                // the path segment length the least:
                for (int pt1 = start; pt1 < end; pt1++)
                {
                    const int pt2 = pt1 + 1;

                    // As long as the two points are not auto wrap points on the
                    // same wrap object, check them for wrapping.
                    if (   path.get(pt1)->getWrapObject() == NULL 
                        || path.get(pt2)->getWrapObject() == NULL 
                        || (   path.get(pt1)->getWrapObject() 
                            != path.get(pt2)->getWrapObject()))
                    {
                        WrapResult wr;
                        wr.startPoint = pt1;
                        wr.endPoint   = pt2;
                        wr.singleWrap = (wrapSetSize==1);
                        result[i] = wo->wrapPathSegment(s, *path.get(pt1), 
                                                        *path.get(pt2), ws, wr);
                        if (result[i] == WrapObject::mandatoryWrap) {
                            // "mandatoryWrap" means the path actually 
                            // intersected the wrap object. In this case, you 
                            // *must* choose this segment as the "best" one for
                            // wrapping. If the path has more than one segment 
                            // that intersects the object, the first one is
                            // taken as the mandatory wrap (this is considered 
                            // an ill-conditioned case).
                            best_wrap = wr;
                            // Store the best wrap in the pathWrap for possible 
                            // use next time.
                            ws.setPreviousWrap(wr);
                            break;
                        }  else if (result[i] == WrapObject::wrapped) {
                            // "wrapped" means the path segment was wrapped over
                            // the object, but you should consider the other 
                            // segments as well to see if one
                            // wraps with a smaller length change.
                            double path_length_change = 
                                calcPathLengthChange(s, *wo, wr, path);
                            if (path_length_change < min_length_change)
                            {
                                best_wrap = wr;
                                // Store the best wrap in the pathWrap for 
                                // possible use next time
                                ws.setPreviousWrap(wr);
                                min_length_change = path_length_change;
                            } else {
                                // The wrap was not shorter than the current 
                                // minimum, so just free the wrap points that 
                                // were allocated.
                                wr.wrap_pts.setSize(0);
                            }
                        } else {
                            // Nothing to do.
                        }
                    }
                }

                // Deallocate previous wrapping points if necessary.
                ws.updWrapPoint2().clearWrapPath(s);

                if (best_wrap.wrap_pts.getSize() == 0) {
                    ws.resetPreviousWrap();
                    ws.updWrapPoint2().clearWrapPath(s);
                } else {
                    // If wrapping did occur, copy wrap info into the PathStruct.
                    ws.updWrapPoint1().clearWrapPath(s);
                    ws.updWrapPoint2().setWrapPath(s, best_wrap.wrap_pts);

                    // In OpenSim, all conversion to/from the wrap object's 
                    // reference frame will be performed inside 
                    // wrapPathSegment(). Thus, all points in this function will
                    // be in their respective body reference frames.
                    // for (j = 0; j < wrapPath.getSize(); j++){
                    //    convert_from_wrap_object_frame(wo, wrapPath.get(j));
                    //    convert(ms->modelnum, wrapPath.get(j), wo->segment, 
                    //            ms->ground_segment);
                    // }

                    ws.updWrapPoint1().setWrapLength(s, 0.0);
                    ws.updWrapPoint2().setWrapLength(s, best_wrap.wrap_path_length);

                    ws.updWrapPoint1().setLocation(s, best_wrap.r1);
                    ws.updWrapPoint2().setLocation(s, best_wrap.r2);

                    // Now insert the two new wrapping points into mp[] array.
                    path.insert(best_wrap.endPoint, &ws.updWrapPoint1());
                    path.insert(best_wrap.endPoint + 1, &ws.updWrapPoint2());
                }
            }
        }

        const double length = calcLengthAfterPathComputation(s, path); 
        if (std::abs(length - last_length) < 0.0005) {
            break;
        } else {
            last_length = length;
        }

        if (kk == 0 && wrapSetSize > 1) {
            // If the first wrap was a no wrap, and the second was a no wrap
            // because a point was inside the object, switch the order of
            // the first two objects and try again.
            if (   result[0] == WrapObject::noWrap 
                && result[1] == WrapObject::insideRadius)
            {
                order[0] = 1;
                order[1] = 0;

                // remove wrap object 0 from the list of path points
                PathWrap& ws = get_PathWrapSet().get(0);
                for (int j = 0; j < path.getSize(); j++) {
                    if (path.get(j) == &ws.updWrapPoint1()) {
                        path.remove(j); // remove the first wrap point
                        path.remove(j); // remove the second wrap point
                        break;
                    }
                }
            }
        }
    }
}

//_____________________________________________________________________________
/*
 * _calc_path_length_change - given the output of a successful path wrap
 * over a wrap object, determine the percent change in length of the
 * path segment incurred by wrapping.
 */
double GeometryPath::
calcPathLengthChange(const SimTK::State& s, const WrapObject& wo, 
                     const WrapResult& wr, const Array<AbstractPathPoint*>& path)  const
{
    const AbstractPathPoint* pt1 = path.get(wr.startPoint);
    const AbstractPathPoint* pt2 = path.get(wr.endPoint);

    double straight_length = pt1->calcDistanceBetween(s, *pt2);

    double wrap_length = pt1->calcDistanceBetween(s, wo.getFrame(), wr.r1);
    wrap_length += wr.wrap_path_length;
    wrap_length += pt2->calcDistanceBetween(s, wo.getFrame(), wr.r2);

    return wrap_length - straight_length; // return absolute diff, not relative
}

//_____________________________________________________________________________
/*
 * Compute the total length of the path. This function
 * assumes that the path has already been updated.
 */
double GeometryPath::
calcLengthAfterPathComputation(const SimTK::State& s, 
                               const Array<AbstractPathPoint*>& currentPath) const
{
    double length = 0.0;
    const AbstractPathPoint* p1 = currentPath[0];
    Vec3 p1InGround = p1->getLocationInGround(s);
    // Transform all points to ground once rather than once per-segment
    for (int i = 1; i < currentPath.getSize() ; i++) {
        const AbstractPathPoint* p2 = currentPath[i];
        Vec3 p2InGround = p2->getLocationInGround(s);
        // If both points are wrap points on the same wrap object, then this
        // path segment wraps over the surface of a wrap object, so just add in 
        // the pre-calculated length.
        if (   p1->getWrapObject() 
            && p2->getWrapObject() 
            && p1->getWrapObject() == p2->getWrapObject()) 
        {
            const PathWrapPoint* smwp = dynamic_cast<const PathWrapPoint*>(p2);
            if (smwp)
                length += smwp->getWrapLength(s);
        } else {
            length += (p1InGround - p2InGround).norm();
        }
        p1 = p2;
        p1InGround = p2InGround;
    }

    setLength(s,length);
    return( length );
}

//_____________________________________________________________________________
/*
 * Compute the path's moment arms for  specified coordinate.
 *
 * @param aCoord, the coordinate
 */   
double GeometryPath::
computeMomentArm(const SimTK::State& s, const Coordinate& aCoord) const
{
    if (!_maSolver)
        const_cast<Self*>(this)->_maSolver.reset(new MomentArmSolver(*_model));

    return _maSolver->solve(s, aCoord,  *this);
}

//_____________________________________________________________________________
// Override default implementation by object to intercept and fix the XML node
// underneath the model to match current version.
void GeometryPath::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)
{
    if (versionNumber < XMLDocument::getLatestVersion()) {
        if (versionNumber < 30516) {
            // Create Appearance node under GeometryPath
            SimTK::Xml::Element appearanceElement("Appearance");
            aNode.appendNode(appearanceElement);
            SimTK::Xml::element_iterator visObjectIter = aNode.element_begin("VisibleObject");
            if (visObjectIter != aNode.element_end()) {
                SimTK::Xml::element_iterator oldPrefIter = visObjectIter->element_begin("display_preference");
                // old display_preference was used only for hide/show other options unsupported
                if (oldPrefIter != visObjectIter->element_end()) {
                    int oldPrefAsInt = 4;
                    oldPrefIter->getValueAs<int>(oldPrefAsInt);
                    if (oldPrefAsInt == 0) { // Hidden => Appearance/Visible
                        SimTK::Xml::Element visibleElement("visible");
                        visibleElement.setValueAs<bool>(false);
                        appearanceElement.insertNodeAfter(appearanceElement.element_end(), visibleElement);
                    }
                }
            }
            // If default_color existed, copy it to Appearance/color
            SimTK::Xml::element_iterator defaultColorIter = aNode.element_begin("default_color");
            if (defaultColorIter != aNode.element_end()) {
                // Move default_color to Appearance/color
                SimTK::Xml::Element colorElement("color");
                const SimTK::String& colorAsString = defaultColorIter->getValue();
                colorElement.setValue(colorAsString);
                appearanceElement.appendNode(colorElement);
            }
        }
    }
    // Call base class now assuming aNode has been corrected for current version
    Super::updateFromXMLNode(aNode, versionNumber);
}

<MSG> Remove last remnant of "display_path" cache variable

<DFF> @@ -421,10 +421,6 @@ void GeometryPath::updateGeometry(const SimTK::State& s) const
 {
     // Check if the current path needs to recomputed.
     computePath(s);
-
-    // If display path is current do not need to recompute it.
-    if (isCacheVariableValid(s, "current_display_path"))
-        return;
 }
 
 //=============================================================================

{'commit': 'diff --git a/OpenSim/Simulation/Model/GeometryPath.cpp b/OpenSim/Simulation/Model/GeometryPath.cpp\nindex fbdc0fd9d..20e7d1fba 100644\n--- a/OpenSim/Simulation/Model/GeometryPath.cpp\n+++ b/OpenSim/Simulation/Model/GeometryPath.cpp\n@@ -421,10 +421,6 @@ void GeometryPath::updateGeometry(const SimTK::State& s) const\n {\n     // Check if the current path needs to recomputed.\n     computePath(s);\n-\n-    // If display path is current do not need to recompute it.\n-    if (isCacheVariableValid(s, "current_display_path"))\n-        return;\n }\n \n //=============================================================================\n', 'commit_subject': 'Remove last remnant of "display_path" cache variable', 'repo_name': 'opensim-org/opensim-core', 'committer': {'name': 'Ajay Seth', 'email': 'b506629e4698a937c5166aa6069ad6008e969ba5@stanford.edu', 'time_sec': '1473982464', 'tz_offset': '-420', 'date': {'seconds': '1473982464'}}, 'commit_tree': 'ecc2b9e551ca3e46974b10a40ec8f5258768dc2e', 'commit_message': 'Remove last remnant of "display_path" cache variable\n', 'language_name': 'C', 'license': 'apache-2.0', 'hunks': '@@ -421,10 +421,6 @@ void GeometryPath::updateGeometry(const SimTK::State& s) const\n {\n     // Check if the current path needs to recomputed.\n     computePath(s);\n-\n-    // If display path is current do not need to recompute it.\n-    if (isCacheVariableValid(s, "current_display_path"))\n-        return;\n }\n \n //=============================================================================\n', 'addition_count': 0, 'deletion_count': 4, 'src_file': 'a/OpenSim/Simulation/Model/GeometryPath.cpp', 'tgt_file': 'b/OpenSim/Simulation/Model/GeometryPath.cpp', 'file_extension': '.cpp', 'before_file': '/* -------------------------------------------------------------------------- *\n *                         OpenSim:  GeometryPath.cpp                         *\n * -------------------------------------------------------------------------- *\n * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *\n * See http://opensim.stanford.edu and the NOTICE file for more information.  *\n * OpenSim is developed at Stanford University and supported by the US        *\n * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *\n * through the Warrior Web program.                                           *\n *                                                                            *\n * Copyright (c) 2005-2017 Stanford University and the Authors                *\n * Author(s): Peter Loan, Ajay Seth                                           *\n *                                                                            *\n * Licensed under the Apache License, Version 2.0 (the "License"); you may    *\n * not use this file except in compliance with the License. You may obtain a  *\n * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *\n *                                                                            *\n * Unless required by applicable law or agreed to in writing, software        *\n * distributed under the License is distributed on an "AS IS" BASIS,          *\n * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *\n * See the License for the specific language governing permissions and        *\n * limitations under the License.                                             *\n * -------------------------------------------------------------------------- */\n\n//=============================================================================\n// INCLUDES\n//=============================================================================\n#include "GeometryPath.h"\n#include "ConditionalPathPoint.h"\n#include "MovingPathPoint.h"\n#include "PointForceDirection.h"\n#include <OpenSim/Simulation/Wrap/PathWrap.h>\n#include "Model.h"\n\n//=============================================================================\n// STATICS\n//=============================================================================\nusing namespace std;\nusing namespace OpenSim;\nusing namespace SimTK;\nusing SimTK::Vec3;\n\n// this is what is stored in a cache variable at runtime. It\'s a value-type that\'s safe to\n// pass between multiple models, which looks up a point in the component path\nclass OpenSim::GeometryPath::PathElementLookup {\npublic:\n    static PathElementLookup fromPtr(const OpenSim::PathPointSet& pps,\n                                     const OpenSim::PathWrapSet& pws,\n                                     const OpenSim::GeometryPath& geomPath,\n                                     const OpenSim::AbstractPathPoint* ptr)\n    {\n        // linearly search each collection to figure out its type + index\n\n        const OpenSim::Component* owner = &ptr->getOwner();\n\n        if (owner == &geomPath) {\n            for (int i = 0, len = pps.getSize(); i < len; ++i) {\n                if (&pps[i] == ptr) {\n                    return PathElementLookup{PointType::InPathPointSet, i};\n                }\n            }\n        }\n        else {\n            for (int i = 0, len = pws.getSize(); i < len; ++i) {\n                if (&pws[i] == owner) {\n                    PointType t = ptr == &pws[i].getWrapPoint1() ? PointType::PathWrapPt1 : PointType::PathWrapPt2;\n                    return PathElementLookup{t, i};\n                }\n            }\n        }\n\n        OPENSIM_THROW(OpenSim::Exception, "unable to create a PathElementLookup for the given pointer: are you sure it is part of the GeometryPath?");\n    }\n\nprivate:\n    enum class PointType : char { InPathPointSet, PathWrapPt1, PathWrapPt2 };\n\n    PathElementLookup(PointType pointType, int index) :\n        index_{index},\n        pointType_{pointType}\n    {\n    }\npublic:\n    OpenSim::AbstractPathPoint* toPtr(const OpenSim::PathPointSet& pps,\n                                      const OpenSim::PathWrapSet& pws) const\n    {\n        switch (pointType_) {\n        case PointType::InPathPointSet:\n            return &pps.get(index_);\n        case PointType::PathWrapPt1:\n            return const_cast<OpenSim::PathWrapPoint*>(&pws.get(index_).getWrapPoint1());\n        case PointType::PathWrapPt2:\n            return const_cast<OpenSim::PathWrapPoint*>(&pws.get(index_).getWrapPoint2());\n        default:\n            OPENSIM_THROW(OpenSim::Exception, "unhandled PointType passed to `toPtr`: this is a developer error");\n        }\n    }\n\nprivate:\n    int index_ : 24;\n    PointType pointType_ : 8;\n};\n\nstatic void PopulatePathPointersCache(\n    const OpenSim::PathPointSet& pps,\n    const OpenSim::PathWrapSet& pws,\n    const std::vector<OpenSim::GeometryPath::PathElementLookup>& lookups,\n    OpenSim::Array<OpenSim::AbstractPathPoint*>& out)\n{\n    out.setSize(static_cast<int>(lookups.size()));\n    for (int i = 0, len = static_cast<int>(lookups.size()); i < len; ++i) {\n        out[i] = lookups[i].toPtr(pps, pws);\n    }\n}\n\nstatic void PopulatePathElementLookup(\n    const OpenSim::PathPointSet& pps,\n    const OpenSim::PathWrapSet& pws,\n    const OpenSim::Array<OpenSim::AbstractPathPoint*>& ptrs,\n    const OpenSim::GeometryPath& geomPath,\n    std::vector<OpenSim::GeometryPath::PathElementLookup>& out)\n{\n    out.clear();\n    out.reserve(ptrs.getSize());\n    for (int i = 0, len = ptrs.getSize(); i < len; ++i) {\n        out.push_back(OpenSim::GeometryPath::PathElementLookup::fromPtr(pps, pws, geomPath, ptrs[i]));\n    }\n}\n\n//=============================================================================\n// CONSTRUCTOR(S) AND DESTRUCTOR\n//=============================================================================\n//_____________________________________________________________________________\n/*\n * Default constructor.\n */\nGeometryPath::GeometryPath() :\n    ModelComponent(),\n    _preScaleLength(0.0)\n{\n    setAuthors("Peter Loan");\n    constructProperties();\n }\n\n//_____________________________________________________________________________\n/*\n* Perform set up functions after model has been deserialized or copied.\n*\n*/\nvoid GeometryPath::extendFinalizeFromProperties()\n{\n    Super::extendFinalizeFromProperties();\n\n    for (int i = 0; i < get_PathWrapSet().getSize(); ++i) {\n        if (upd_PathWrapSet()[i].getName().empty()) {\n            std::stringstream label;\n            label << "pathwrap_" << i;\n            upd_PathWrapSet()[i].setName(label.str());\n        }\n    }\n}\n\nvoid GeometryPath::extendConnectToModel(Model& aModel)\n{\n    Super::extendConnectToModel(aModel);\n\n    OPENSIM_THROW_IF_FRMOBJ(get_PathPointSet().getSize() < 2,\n        InvalidPropertyValue,\n        getProperty_PathPointSet().getName(),\n        "A valid path must be connected to a model by at least two PathPoints.")\n\n    // Name the path points based on the current path\n    // (i.e., the set of currently active points is numbered\n    // 1, 2, 3, ...).\n    namePathPoints(0);\n}\n\n//_____________________________________________________________________________\n/*\n * Create the SimTK state, discrete and/or cache for this GeometryPath.\n */\n void GeometryPath::extendAddToSystem(SimTK::MultibodySystem& system) const \n{\n    Super::extendAddToSystem(system);\n\n    // Allocate cache entries to save the current length and speed(=d/dt length)\n    // of the path in the cache. Length depends only on q\'s so will be valid\n    // after Position stage, speed requires u\'s also so valid at Velocity stage.\n    this->_lengthCV = addCacheVariable("length", 0.0, SimTK::Stage::Position);\n    this->_speedCV = addCacheVariable("speed", 0.0, SimTK::Stage::Velocity);\n\n    // Cache the set of points currently defining this path.\n    this->_currentPathCV = addCacheVariable("current_path", std::vector<PathElementLookup>{}, SimTK::Stage::Position);\n\n    // We consider this cache entry valid any time after it has been created\n    // and first marked valid, and we won\'t ever invalidate it.\n    this->_colorCV = addCacheVariable("color", get_Appearance().get_color(), SimTK::Stage::Topology);\n}\n\n void GeometryPath::extendInitStateFromProperties(SimTK::State& s) const\n{\n    Super::extendInitStateFromProperties(s);\n    markCacheVariableValid(s, _colorCV);  // it is OK at its default value\n}\n\n//------------------------------------------------------------------------------\n//                         GENERATE DECORATIONS\n//------------------------------------------------------------------------------\n// The GeometryPath takes care of drawing itself here, using information it\n// can extract from the supplied state, including position information and\n// color information that may have been calculated as late as Stage::Dynamics.\n// For example, muscles may want the color to reflect activation level and \n// other path-using components might want to use forces (tension). We will\n// ensure that the state has been realized to Stage::Dynamics before looking\n// at it. (It is only guaranteed to be at Stage::Position here.)\nvoid GeometryPath::\ngenerateDecorations(bool fixed, const ModelDisplayHints& hints, \n                    const SimTK::State& state, \n                    SimTK::Array_<SimTK::DecorativeGeometry>& appendToThis) const\n{        \n    // There is no fixed geometry to generate here.\n    if (fixed) { return; }\n\n    const Array<AbstractPathPoint*>& pathPoints = getCurrentPath(state);\n\n    assert(pathPoints.size() > 1);\n\n    const AbstractPathPoint* lastPoint = pathPoints[0];\n    MobilizedBodyIndex mbix(0);\n\n    Vec3 lastPos = lastPoint->getLocationInGround(state);\n    if (hints.get_show_path_points())\n        DefaultGeometry::drawPathPoint(mbix, lastPos, getColor(state), appendToThis);\n\n    Vec3 pos;\n\n    for (int i = 1; i < pathPoints.getSize(); ++i) {\n        AbstractPathPoint* point = pathPoints[i];\n        PathWrapPoint* pwp = dynamic_cast<PathWrapPoint*>(point);\n\n        if (pwp) {\n            // A PathWrapPoint provides points on the wrapping surface as Vec3s\n            const Array<Vec3>& surfacePoints = pwp->getWrapPath(state);\n            // The surface points are expressed w.r.t. the wrap surface\'s body frame.\n            // Transform the surface points into the ground reference frame to draw\n            // the surface point as the wrapping portion of the GeometryPath\n            const Transform& X_BG = pwp->getParentFrame().getTransformInGround(state);\n            // Cycle through each surface point and draw it the Ground frame\n            for (int j = 0; j<surfacePoints.getSize(); ++j) {\n                // transform the surface point into the Ground reference frame\n                pos = X_BG*surfacePoints[j];\n                if (hints.get_show_path_points())\n                    DefaultGeometry::drawPathPoint(mbix, pos, getColor(state),\n                        appendToThis);\n                // Line segments will be in ground frame\n                appendToThis.push_back(DecorativeLine(lastPos, pos)\n                    .setLineThickness(4)\n                    .setColor(getColor(state)).setBodyId(0).setIndexOnBody(j));\n                lastPos = pos;\n            }\n        } \n        else { // otherwise a regular PathPoint so just draw its location\n            pos = point->getLocationInGround(state);\n            if (hints.get_show_path_points())\n                DefaultGeometry::drawPathPoint(mbix, pos, getColor(state),\n                    appendToThis);\n            // Line segments will be in ground frame\n            appendToThis.push_back(DecorativeLine(lastPos, pos)\n                .setLineThickness(4)\n                .setColor(getColor(state)).setBodyId(0).setIndexOnBody(i));\n            lastPos = pos;\n        }\n    }\n}\n\n//_____________________________________________________________________________\n/*\n * Connect properties to local pointers.\n */\nvoid GeometryPath::constructProperties()\n{\n    constructProperty_PathPointSet(PathPointSet());\n\n    constructProperty_PathWrapSet(PathWrapSet());\n    \n    Appearance appearance;\n    appearance.set_color(SimTK::Gray);\n    constructProperty_Appearance(appearance);\n}\n\n//_____________________________________________________________________________\n/*\n * Name the path points based on their position in the set. To keep the\n * names up to date, this method should be called every time the path changes.\n *\n * @param aStartingIndex The index of the first path point to name.\n */\nvoid GeometryPath::namePathPoints(int aStartingIndex)\n{\n    char indx[5];\n    for (int i = aStartingIndex; i < get_PathPointSet().getSize(); i++)\n    {\n        sprintf(indx,"%d",i+1);\n        AbstractPathPoint& point = get_PathPointSet().get(i);\n        if (point.getName()=="" && hasOwner()) {\n            point.setName(getOwner().getName() + "-P" + indx);\n        }\n    }\n}\n\n//_____________________________________________________________________________\n/*\n * get the current path of the path\n *\n * @return The array of currently active path points.\n * \n */\nconst OpenSim::Array <AbstractPathPoint*> & GeometryPath::\ngetCurrentPath(const SimTK::State& s)  const\n{\n    computePath(s);   // compute checks if path needs to be recomputed\n    return _currentPathPtrsCache;\n}\n\n// get the path as PointForceDirections directions \n// CAUTION: the return points are heap allocated; you must delete them yourself! \n// (TODO: that is really lame)\nvoid GeometryPath::\ngetPointForceDirections(const SimTK::State& s, \n                        OpenSim::Array<PointForceDirection*> *rPFDs) const\n{\n    int i;\n    AbstractPathPoint* start;\n    AbstractPathPoint* end;\n    const OpenSim::PhysicalFrame* startBody;\n    const OpenSim::PhysicalFrame* endBody;\n    const Array<AbstractPathPoint*>& currentPath = getCurrentPath(s);\n\n    int np = currentPath.getSize();\n    rPFDs->ensureCapacity(np);\n    \n    for (i = 0; i < np; i++) {\n        PointForceDirection *pfd = \n            new PointForceDirection(currentPath[i]->getLocation(s), \n                                    currentPath[i]->getParentFrame(), Vec3(0));\n        rPFDs->append(pfd);\n    }\n\n    for (i = 0; i < np-1; i++) {\n        start = currentPath[i];\n        end = currentPath[i+1];\n        startBody = &start->getParentFrame();\n        endBody = &end->getParentFrame();\n\n        if (startBody != endBody)\n        {\n            Vec3 posStart, posEnd;\n            Vec3 direction(0);\n\n            // Find the positions of start and end in the inertial frame.\n            //engine.getPosition(s, start->getParentFrame(), start->getLocation(), posStart);\n            posStart = start->getLocationInGround(s);\n            \n            //engine.getPosition(s, end->getParentFrame(), end->getLocation(), posEnd);\n            posEnd = end->getLocationInGround(s);\n\n            // Form a vector from start to end, in the inertial frame.\n            direction = (posEnd - posStart);\n\n            // Check that the two points are not coincident.\n            // This can happen due to infeasible wrapping of the path,\n            // when the origin or insertion enters the wrapping surface.\n            // This is a temporary fix, since the wrap algorithm should\n            // return NaN for the points and/or throw an Exception- aseth\n            if (direction.norm() < SimTK::SignificantReal){\n                direction = direction*SimTK::NaN;\n            }\n            else{\n                direction = direction.normalize();\n            }\n\n            // Get resultant direction at each point \n            rPFDs->get(i)->addToDirection(direction);\n            rPFDs->get(i+1)->addToDirection(-direction);\n        }\n    }\n}\n\n/* add in the equivalent spatial forces on bodies for an applied tension \n    along the GeometryPath to a set of bodyForces */\nvoid GeometryPath::addInEquivalentForces(const SimTK::State& s,\n    const double& tension, \n    SimTK::Vector_<SimTK::SpatialVec>& bodyForces,\n    SimTK::Vector& mobilityForces) const\n{\n    AbstractPathPoint* start = NULL;\n    AbstractPathPoint* end = NULL;\n    const SimTK::MobilizedBody* bo = NULL;\n    const SimTK::MobilizedBody* bf = NULL;\n    const Array<AbstractPathPoint*>& currentPath = getCurrentPath(s);\n    int np = currentPath.getSize();\n\n    const SimTK::SimbodyMatterSubsystem& matter = \n                                        getModel().getMatterSubsystem();\n\n    // start point, end point,  direction, and force vectors in ground\n    Vec3 po(0), pf(0), dir(0), force(0);\n    // partial velocity of point in body expressed in ground \n    Vec3 dPodq_G(0), dPfdq_G(0);\n\n    // gen force (torque) due to moving point under tension\n    double fo, ff;\n\n    for (int i = 0; i < np-1; ++i) {\n        start = currentPath[i];\n        end = currentPath[i+1];\n\n        bo = &start->getParentFrame().getMobilizedBody();\n        bf = &end->getParentFrame().getMobilizedBody();\n\n        if (bo != bf) {\n{\n    // Check if the current path needs to recomputed.\n    computePath(s);\n\n    // If display path is current do not need to recompute it.\n    if (isCacheVariableValid(s, "current_display_path"))\n        return;\n}\n\n//=============================================================================\n\n            // Check that the two points are not coincident.\n            // This can happen due to infeasible wrapping of the path,\n            // when the origin or insertion enters the wrapping surface.\n            // This is a temporary fix, since the wrap algorithm should\n            // return NaN for the points and/or throw an Exception- aseth\n            if (dir.norm() < SimTK::SignificantReal){\n                dir = dir*SimTK::NaN;\n            }\n            else{\n                dir = dir.normalize();\n            }\n\n            force = tension*dir;\n\n            const MovingPathPoint* mppo =\n                dynamic_cast<MovingPathPoint *>(start);\n\n            // do the same for the end point of this segment of the path\n            const MovingPathPoint* mppf =\n                dynamic_cast<MovingPathPoint *>(end);\n\n            // add in the tension point forces to body forces\n            if (mppo) {// moving path point location is a function of the state\n                // transform of the frame of the point to the base mobilized body\n                auto X_BF = mppo->getParentFrame().findTransformInBaseFrame();\n                bo->applyForceToBodyPoint(s, X_BF*mppo->getLocation(s), force,\n                    bodyForces);\n            }\n            else {\n                // transform of the frame of the point to the base mobilized body\n                auto X_BF = start->getParentFrame().findTransformInBaseFrame();\n                bo->applyForceToBodyPoint(s, X_BF*start->getLocation(s), force,\n                    bodyForces);\n            }\n\n            if (mppf) {// moving path point location is a function of the state\n                // transform of the frame of the point to the base mobilized body\n                auto X_BF = mppf->getParentFrame().findTransformInBaseFrame();\n                bf->applyForceToBodyPoint(s, X_BF*mppf->getLocation(s), -force,\n                    bodyForces);\n            }\n            else {\n                // transform of the frame of the point to the base mobilized body\n                auto X_BF = end->getParentFrame().findTransformInBaseFrame();\n                bf->applyForceToBodyPoint(s, X_BF*end->getLocation(s), -force,\n                    bodyForces);\n            }\n\n            // Now account for the work being done by virtue of the moving\n            // path point motion relative to the body it is on\n            if(mppo){\n                // torque (genforce) contribution due to relative movement \n                // of a via point w.r.t. the body it is connected to.\n                dPodq_G = bo->expressVectorInGroundFrame(s, start->getdPointdQ(s));\n                fo = ~dPodq_G*force;            \n\n                // get the mobilized body the coordinate is couple to.\n                const SimTK::MobilizedBody& mpbod =\n                    matter.getMobilizedBody(mppo->getXCoordinate().getBodyIndex());\n\n                // apply the generalized (mobility) force to the coordinate\'s body\n                mpbod.applyOneMobilityForce(s, \n                    mppo->getXCoordinate().getMobilizerQIndex(), \n                    fo, mobilityForces);\n            }\n\n            if(mppf){\n                dPfdq_G = bf->expressVectorInGroundFrame(s, end->getdPointdQ(s));\n                ff = ~dPfdq_G*(-force);\n\n                // get the mobilized body the coordinate is couple to.\n                const SimTK::MobilizedBody& mpbod =\n                    matter.getMobilizedBody(mppf->getXCoordinate().getBodyIndex());\n\n                mpbod.applyOneMobilityForce(s, \n                    mppf->getXCoordinate().getMobilizerQIndex(), \n                    ff, mobilityForces);\n            }\n        }       \n    }\n}\n\n//_____________________________________________________________________________\n/*\n * Update the geometric representation of the path.\n * The resulting geometry is maintained at the VisibleObject layer.\n * This function should not be made public. It is called internally\n * by compute() only when the path has changed.\n * \n */\nvoid GeometryPath::updateGeometry(const SimTK::State& s) const\n{\n    // Check if the current path needs to recomputed.\n    computePath(s);\n}\n\n//=============================================================================\n// GET\n//=============================================================================\n//-----------------------------------------------------------------------------\n// LENGTH\n//-----------------------------------------------------------------------------\n//_____________________________________________________________________________\n/*\n * Compute the total length of the path.\n *\n * @return Total length of the path.\n */\ndouble GeometryPath::getLength( const SimTK::State& s) const\n{\n    computePath(s);  // compute checks if path needs to be recomputed\n    return getCacheVariableValue(s, _lengthCV);\n}\n\nvoid GeometryPath::setLength( const SimTK::State& s, double length ) const\n{\n    setCacheVariableValue(s, _lengthCV, length);\n}\n\nvoid GeometryPath::setColor(const SimTK::State& s, const SimTK::Vec3& color) const\n{\n    setCacheVariableValue(s, _colorCV, color);\n}\n\nVec3 GeometryPath::getColor(const SimTK::State& s) const\n{\n    return getCacheVariableValue(s, _colorCV);\n}\n\n//_____________________________________________________________________________\n/*\n * Compute the lengthening speed of the path.\n *\n * @return lengthening speed of the path.\n */\ndouble GeometryPath::getLengtheningSpeed( const SimTK::State& s) const\n{\n    computeLengtheningSpeed(s);\n    return getCacheVariableValue(s, _speedCV);\n}\nvoid GeometryPath::setLengtheningSpeed( const SimTK::State& s, double speed ) const\n{\n    setCacheVariableValue(s, _speedCV, speed);\n}\n\nvoid GeometryPath::setPreScaleLength( const SimTK::State& s, double length ) {\n    _preScaleLength = length;\n}\ndouble GeometryPath::getPreScaleLength( const SimTK::State& s) const {\n    return _preScaleLength;\n}\n\n//=============================================================================\n// UTILITY\n//=============================================================================\n//_____________________________________________________________________________\n/*\n * Add a new path point, with default location, to the path.\n *\n * @param aIndex The position in the pathPointSet to put the new point in.\n * @param frame The frame to attach the point to.\n * @return Pointer to the newly created path point.\n */\nAbstractPathPoint* GeometryPath::\naddPathPoint(const SimTK::State& s, int aIndex, const PhysicalFrame& frame)\n{\n    PathPoint* newPoint = new PathPoint();\n    newPoint->setParentFrame(frame);\n    Vec3 newLocation(0.0); \n    // Note: placeNewPathPoint() returns a location by reference.\n    // It computes a new location according to the index where the new path point \n    // will be inserted along the path(among the other path points).\n    placeNewPathPoint(s, newLocation, aIndex, frame);\n    // Now set computed new location into the newPoint\n    newPoint->setLocation(newLocation);\n    upd_PathPointSet().insert(aIndex, newPoint);\n\n    // Rename the path points starting at this new one.\n    namePathPoints(aIndex);\n\n    // Update start point and end point in the wrap instances so that they\n    // refer to the same path points they did before the new point\n    // was added. These indices are 1-based.\n    aIndex++;\n    for (int i=0; i<get_PathWrapSet().getSize(); i++) {\n        int startPoint = get_PathWrapSet().get(i).getStartPoint();\n        int endPoint = get_PathWrapSet().get(i).getEndPoint();\n        if (startPoint != -1 && aIndex <= startPoint)\n            get_PathWrapSet().get(i).setStartPoint(s,startPoint + 1);\n        if (endPoint != -1 && aIndex <= endPoint)\n            get_PathWrapSet().get(i).setEndPoint(s,endPoint + 1);\n    }\n\n    return newPoint;\n}\n\nAbstractPathPoint* GeometryPath::\nappendNewPathPoint(const std::string& proposedName, \n                   const PhysicalFrame& frame,\n                   const SimTK::Vec3& aPositionOnBody)\n{\n    PathPoint* newPoint = new PathPoint();\n    newPoint->setParentFrame(frame);\n    newPoint->setName(proposedName);\n    newPoint->setLocation(aPositionOnBody);\n    upd_PathPointSet().adoptAndAppend(newPoint);\n\n    return newPoint;\n}\n\n//_____________________________________________________________________________\n/*\n * Determine an appropriate default XYZ location for a new path point.\n * Note that this method is internal and should not be called directly on a new \n * point as the point is not really added to the path (done in addPathPoint() \n * instead)\n * @param aOffset The XYZ location to be determined.\n * @param aIndex The position in the pathPointSet to put the new point in.\n * @param frame The body to attach the point to.\n */\nvoid GeometryPath::\nplaceNewPathPoint(const SimTK::State& s, SimTK::Vec3& aOffset, int aIndex, \n                  const PhysicalFrame& frame)\n{\n    // The location of the point is determined by moving a \'distance\' from \'base\' \n    // along a vector from \'start\' to \'end.\' \'base\' is the existing path point \n    // that is in or closest to the index aIndex. \'start\' and \'end\' are existing\n    // path points--which ones depends on where the new point is being added. \n    // \'distance\' is 0.5 for points added to the middle of a path (so the point\n    // appears halfway between the two adjacent points), and 0.2 for points that\n    // are added to either end of the path. If there is only one point in the \n    // path, the new point is put 0.01 units away in all three dimensions.\n    if (get_PathPointSet().getSize() > 1) {\n        int start, end, base;\n        double distance;\n        if (aIndex == 0) {\n            start = 1;\n            end = 0;\n            base = end;\n            distance = 0.2;\n        } else if (aIndex >= get_PathPointSet().getSize()) {\n            start = aIndex - 2;\n            end = aIndex - 1;\n            base = end;\n            distance = 0.2;\n        } else {\n            start = aIndex;\n            end = aIndex - 1;\n            base = start;\n            distance = 0.5;\n        }\n\n        const Vec3 startPt = get_PathPointSet()[start].getLocation(s);\n        const Vec3 endPt = get_PathPointSet()[end].getLocation(s);\n        const Vec3 basePt = get_PathPointSet()[base].getLocation(s);\n\n        Vec3 startPt2 = get_PathPointSet()[start].getParentFrame()\n            .findStationLocationInAnotherFrame(s, startPt, frame);\n\n        Vec3 endPt2 = get_PathPointSet()[end].getParentFrame()\n            .findStationLocationInAnotherFrame(s, endPt, frame);\n\n        aOffset = basePt + distance * (endPt2 - startPt2);\n    } else if (get_PathPointSet().getSize() == 1) {\n        aOffset= get_PathPointSet()[0].getLocation(s) + 0.01;\n    }\n    else {  // first point, do nothing?\n    }\n}\n\n//_____________________________________________________________________________\n/*\n * See if a path point can be deleted. All paths must have at least two\n * active path points to define the path.\n *\n * @param aIndex The index of the point to delete.\n * @return Whether or not the point can be deleted.\n */\nbool GeometryPath::canDeletePathPoint( int aIndex)\n{\n    // A path point can be deleted only if there would remain\n    // at least two other fixed points.\n    int numOtherFixedPoints = 0;\n    for (int i = 0; i < get_PathPointSet().getSize(); i++) {\n        if (i != aIndex) {\n            if (!(  get_PathPointSet().get(i).getConcreteClassName()\n                  ==("ConditionalPathPoint")))\n                numOtherFixedPoints++;\n        }\n    }\n\n    if (numOtherFixedPoints >= 2)\n        return true;\n\n    return false;\n}\n\n//_____________________________________________________________________________\n/*\n * Delete a path point.\n *\n * @param aIndex The index of the point to delete.\n * @return Whether or not the point was deleted.\n */\nbool GeometryPath::deletePathPoint(const SimTK::State& s, int aIndex)\n{\n    if (canDeletePathPoint(aIndex) == false)\n        return false;\n\n    upd_PathPointSet().remove(aIndex);\n\n    // rename the path points starting at the deleted position\n    namePathPoints(aIndex);\n\n    // Update start point and end point in the wrap instances so that they\n    // refer to the same path points they did before the point was\n    // deleted. These indices are 1-based. If the point deleted is start\n    // point or end point, the path wrap range is made smaller by one point.\n    aIndex++;\n    for (int i=0; i<get_PathWrapSet().getSize(); i++) {\n        int startPoint = get_PathWrapSet().get(i).getStartPoint();\n        int endPoint   = get_PathWrapSet().get(i).getEndPoint();\n\n        if (   (startPoint != -1 && aIndex < startPoint) \n            || (startPoint > get_PathPointSet().getSize()))\n            get_PathWrapSet().get(i).setStartPoint(s, startPoint - 1);\n\n        if (   endPoint > 1 \n            && aIndex <= endPoint \n            && (   (endPoint > startPoint) \n                || (endPoint > get_PathPointSet().getSize())))\n            get_PathWrapSet().get(i).setEndPoint(s, endPoint - 1);\n    }\n\n    return true;\n}\n\n//_____________________________________________________________________________\n/*\n * Replace a path point in the set with another point. The new one is made a\n * member of all the same groups as the old one, and is inserted in the same\n * place the old one occupied.\n *\n *  @param aOldPathPoint Path point to remove.\n *  @param aNewPathPoint Path point to add.\n */\nbool GeometryPath::\nreplacePathPoint(const SimTK::State& s, AbstractPathPoint* aOldPathPoint, \n                 AbstractPathPoint* aNewPathPoint) \n{\n    if (aOldPathPoint != NULL && aNewPathPoint != NULL) {\n        int count = 0;\n        int index = get_PathPointSet().getIndex(aOldPathPoint);\n        // If you\'re switching from non-via to via, check to make sure that the\n        // path will be left with at least 2 non-via points.\n        ConditionalPathPoint* oldVia = \n            dynamic_cast<ConditionalPathPoint*>(aOldPathPoint);\n        ConditionalPathPoint* newVia = \n            dynamic_cast<ConditionalPathPoint*>(aNewPathPoint);\n        if (oldVia == NULL && newVia != NULL) {\n            for (int i=0; i<get_PathPointSet().getSize(); i++) {\n                if (i != index) {\n                    if (dynamic_cast<ConditionalPathPoint*>\n                                        (&get_PathPointSet().get(i)) == NULL)\n                        count++;\n                }\n            }\n        } else {\n            count = 2;\n        }\n        if (count >= 2 && index >= 0) {\n            upd_PathPointSet().set(index, aNewPathPoint, true);\n            //computePath(s);\n            return true;\n        }\n    }\n    return false;\n}\n\n//_____________________________________________________________________________\n/*\n * Create a new wrap instance and add it to the set.\n *\n * @param aWrapObject The wrap object to use in the new wrap instance.\n */\nvoid GeometryPath::addPathWrap(WrapObject& aWrapObject)\n{\n    PathWrap* newWrap = new PathWrap();\n    newWrap->setWrapObject(aWrapObject);\n    newWrap->setMethod(PathWrap::hybrid);\n    upd_PathWrapSet().adoptAndAppend(newWrap);\n    finalizeFromProperties();\n}\n\n//_____________________________________________________________________________\n/*\n * Move a wrap instance up in the list. Changing the order of wrap instances for\n * a path may affect how the path wraps over the wrap objects.\n *\n * @param aIndex The index of the wrap instance to move up.\n */\nvoid GeometryPath::moveUpPathWrap(const SimTK::State& s, int aIndex)\n{\n    if (aIndex > 0) {\n        // Make sure wrap object is not deleted by remove().\n        upd_PathWrapSet().setMemoryOwner(false); \n\n        PathWrap& wrap = get_PathWrapSet().get(aIndex);\n        upd_PathWrapSet().remove(aIndex);\n        upd_PathWrapSet().insert(aIndex - 1, &wrap);\n        upd_PathWrapSet().setMemoryOwner(true);\n    }\n}\n\n//_____________________________________________________________________________\n/*\n * Move a wrap instance down in the list. Changing the order of wrap instances\n * for a path may affect how the path wraps over the wrap objects.\n *\n * @param aIndex The index of the wrap instance to move down.\n */\nvoid GeometryPath::moveDownPathWrap(const SimTK::State& s, int aIndex)\n{\n    if (aIndex < get_PathWrapSet().getSize() - 1) {\n        // Make sure wrap object is not deleted by remove().\n        upd_PathWrapSet().setMemoryOwner(false);\n\n        PathWrap& wrap = get_PathWrapSet().get(aIndex);\n        upd_PathWrapSet().remove(aIndex);\n        upd_PathWrapSet().insert(aIndex + 1, &wrap);\n        upd_PathWrapSet().setMemoryOwner(true);\n    }\n}\n\n//_____________________________________________________________________________\n/*\n * Delete a wrap instance.\n *\n * @param aIndex The index of the wrap instance to delete.\n */\nvoid GeometryPath::deletePathWrap(const SimTK::State& s, int aIndex)\n{\n    upd_PathWrapSet().remove(aIndex);\n\n}\n\n//==============================================================================\n// SCALING\n//==============================================================================\nvoid GeometryPath::\nextendPreScale(const SimTK::State& s, const ScaleSet& scaleSet)\n{\n    Super::extendPreScale(s, scaleSet);\n    setPreScaleLength(s, getLength(s));\n}\n\nvoid GeometryPath::\nextendPostScale(const SimTK::State& s, const ScaleSet& scaleSet)\n{\n    Super::extendPostScale(s, scaleSet);\n    computePath(s);\n}\n\n//--------------------------------------------------------------------------\n// COMPUTATIONS\n//--------------------------------------------------------------------------\n//=============================================================================\n// PATH, WRAPPING, AND MOMENT ARM\n//=============================================================================\n//_____________________________________________________________________________\n/*\n * Calculate the current path.\n */\nvoid GeometryPath::computePath(const SimTK::State& s) const\n{\n    if (isCacheVariableValid(s, _currentPathCV)) {\n        // even though the cache variable is valid, re-populate the pointers cache\n        //\n        // this is because although it may be valid *in the state* the\n        // model-level pointers cache may be dirty with lookups from other\n        // states\n        PopulatePathPointersCache(get_PathPointSet(),\n                                  get_PathWrapSet(),\n                                  getCacheVariableValue(s, _currentPathCV),\n                                  _currentPathPtrsCache);\n        return;\n    }\n\n    // Clear the current path.\n    _currentPathPtrsCache.setSize(0);\n\n    // Add the active fixed and moving via points to the path.\n    for (int i = 0; i < get_PathPointSet().getSize(); i++) {\n        if (get_PathPointSet()[i].isActive(s))\n            _currentPathPtrsCache.append(&get_PathPointSet()[i]); // <--- !!!!BAD\n    }\n  \n    // Use the current path so far to check for intersection with wrap objects, \n    // which may add additional points to the path.\n    applyWrapObjects(s, _currentPathPtrsCache);\n    calcLengthAfterPathComputation(s, _currentPathPtrsCache);\n\n    // the pointers array now contains the "correct" (wrapped) path\n    //\n    // because the path is state-dependent (different states may wrap differently), the\n    // path points (abstract) must be stored in a SimTK cache variable.\n    //\n    // However, we can\'t store raw pointers in a cache variable because that may cause\n    // aliasing issues in downstream code. E.g. the state may later be used with a\n    // copied/moved-from version of the model, rather than the original one, so the\n    // pointers may be stale.\n    std::vector<PathElementLookup>& lookups = updCacheVariableValue(s, _currentPathCV);\n    PopulatePathElementLookup(get_PathPointSet(),\n                              get_PathWrapSet(),\n                              _currentPathPtrsCache,\n                              *this,\n                              lookups);\n    markCacheVariableValid(s, _currentPathCV);\n}\n\n//_____________________________________________________________________________\n/*\n * Compute lengthening speed of the path.\n */\nvoid GeometryPath::computeLengtheningSpeed(const SimTK::State& s) const\n{\n    if (isCacheVariableValid(s, _speedCV)) {\n        return;\n    }\n\n    const Array<AbstractPathPoint*>& currentPath = getCurrentPath(s);\n\n    double speed = 0.0;\n    \n    for (int i = 0; i < currentPath.getSize() - 1; i++) {\n        speed += currentPath[i]->calcSpeedBetween(s, *currentPath[i+1]);\n    }\n\n    setLengtheningSpeed(s, speed);\n}\n\n//_____________________________________________________________________________\n/*\n * Apply the wrap objects to the current path.\n */\nvoid GeometryPath::\napplyWrapObjects(const SimTK::State& s, Array<AbstractPathPoint*>& path) const \n{\n    int wrapSetSize = get_PathWrapSet().getSize();\n    if (wrapSetSize < 1)\n        return;\n\n    WrapResult best_wrap;\n    Array<int> result, order;\n\n    result.setSize(wrapSetSize);\n    order.setSize(wrapSetSize);\n\n    // Set the initial order to be the order they are listed in the path.\n    for (int i = 0; i < wrapSetSize; i++)\n        order[i] = i;\n\n    // If there is only one wrap object, calculate the wrapping only once.\n    // If there are two or more objects, perform up to 8 iterations where\n    // the result from one wrap object is used as the starting point for\n    // the next wrap.\n    const int maxIterations = wrapSetSize < 2 ? 1 : 8;\n    double last_length = SimTK::Infinity;\n    for (int kk = 0; kk < maxIterations; kk++)\n    {\n        for (int i = 0; i < wrapSetSize; i++)\n        {\n            result[i] = 0;\n            PathWrap& ws = get_PathWrapSet().get(order[i]);\n            const WrapObject* wo = ws.getWrapObject();\n            best_wrap.wrap_pts.setSize(0);\n            double min_length_change = SimTK::Infinity;\n\n            // First remove this object\'s wrapping points from the current path.\n            for (int j = 0; j <path.getSize(); j++) {\n                if( path.get(j) == &ws.getWrapPoint1()) {\n                    path.remove(j); // remove the first wrap point\n                    path.remove(j); // remove the second wrap point\n                    break;\n                }\n            }\n\n            if (wo->get_active()) {\n                // startPoint and endPoint in wrapStruct represent the \n                // user-defined starting and ending points in the array of path \n                // points that should be considered for wrapping. These indices \n                // take into account via points, whether or not they are active. \n                // Thus they are indices into mp_orig[], not mp[] (also, mp[] \n                // may contain wrapping points from previous wrap objects, which\n                // would mess up the starting and ending indices). But the goal \n                // is to find starting and ending indices in mp[] to consider\n                // for wrapping over this wrap object. Here is how that is done:\n\n                // 1. startPoint and endPoint are 1-based, so subtract 1 from \n                // them to get indices into get_PathPointSet(). -1 (or any value\n                // less than 1) means use the first (or last) point.\n                const int wrapStart = (ws.getStartPoint() < 1\n                                            ? 0 \n                                            : ws.getStartPoint() - 1);\n                const int wrapEnd   = (ws.getEndPoint() < 1\n                                            ? get_PathPointSet().getSize() - 1 \n                                            : ws.getEndPoint() - 1);\n\n                // 2. Scan forward from wrapStart in get_PathPointSet() to find \n                // the first point that is active. Store a pointer to it (smp).\n                int jfwd = wrapStart;\n                for (; jfwd <= wrapEnd; jfwd++)\n                    if (get_PathPointSet().get(jfwd).isActive(s))\n                        break;\n                if (jfwd > wrapEnd) // there are no active points in the path\n                    return;\n                const AbstractPathPoint* const smp = &get_PathPointSet().get(jfwd);\n\n                // 3. Scan backwards from wrapEnd in get_PathPointSet() to find \n                // the last point that is active. Store a pointer to it (emp).\n                int jrev = wrapEnd;\n                for (; jrev >= wrapStart; jrev--)\n                    if (get_PathPointSet().get(jrev).isActive(s))\n                        break;\n                if (jrev < wrapStart) // there are no active points in the path\n                    return;\n                const AbstractPathPoint* const emp = &get_PathPointSet().get(jrev);\n\n                // 4. Now find the indices of smp and emp in _currentPath.\n                int start=-1, end=-1;\n                for (int j = 0; j < path.getSize(); j++) {\n                    if (path.get(j) == smp)\n                        start = j;\n                    if (path.get(j) == emp)\n                        end = j;\n                }\n                if (start == -1 || end == -1) // this should never happen\n                    return;\n\n                // You now have indices into _currentPath (which is a list of \n                // all currently active points, including wrap points) that \n                // represent the used-defined range of points to consider for \n                // wrapping over this wrap object. Check each path segment in \n                // this range, choosing the best wrap as the one that changes \n                // the path segment length the least:\n                for (int pt1 = start; pt1 < end; pt1++)\n                {\n                    const int pt2 = pt1 + 1;\n\n                    // As long as the two points are not auto wrap points on the\n                    // same wrap object, check them for wrapping.\n                    if (   path.get(pt1)->getWrapObject() == NULL \n                        || path.get(pt2)->getWrapObject() == NULL \n                        || (   path.get(pt1)->getWrapObject() \n                            != path.get(pt2)->getWrapObject()))\n                    {\n                        WrapResult wr;\n                        wr.startPoint = pt1;\n                        wr.endPoint   = pt2;\n                        wr.singleWrap = (wrapSetSize==1);\n                        result[i] = wo->wrapPathSegment(s, *path.get(pt1), \n                                                        *path.get(pt2), ws, wr);\n                        if (result[i] == WrapObject::mandatoryWrap) {\n                            // "mandatoryWrap" means the path actually \n                            // intersected the wrap object. In this case, you \n                            // *must* choose this segment as the "best" one for\n                            // wrapping. If the path has more than one segment \n                            // that intersects the object, the first one is\n                            // taken as the mandatory wrap (this is considered \n                            // an ill-conditioned case).\n                            best_wrap = wr;\n                            // Store the best wrap in the pathWrap for possible \n                            // use next time.\n                            ws.setPreviousWrap(wr);\n                            break;\n                        }  else if (result[i] == WrapObject::wrapped) {\n                            // "wrapped" means the path segment was wrapped over\n                            // the object, but you should consider the other \n                            // segments as well to see if one\n                            // wraps with a smaller length change.\n                            double path_length_change = \n                                calcPathLengthChange(s, *wo, wr, path);\n                            if (path_length_change < min_length_change)\n                            {\n                                best_wrap = wr;\n                                // Store the best wrap in the pathWrap for \n                                // possible use next time\n                                ws.setPreviousWrap(wr);\n                                min_length_change = path_length_change;\n                            } else {\n                                // The wrap was not shorter than the current \n                                // minimum, so just free the wrap points that \n                                // were allocated.\n                                wr.wrap_pts.setSize(0);\n                            }\n                        } else {\n                            // Nothing to do.\n                        }\n                    }\n                }\n\n                // Deallocate previous wrapping points if necessary.\n                ws.updWrapPoint2().clearWrapPath(s);\n\n                if (best_wrap.wrap_pts.getSize() == 0) {\n                    ws.resetPreviousWrap();\n                    ws.updWrapPoint2().clearWrapPath(s);\n                } else {\n                    // If wrapping did occur, copy wrap info into the PathStruct.\n                    ws.updWrapPoint1().clearWrapPath(s);\n                    ws.updWrapPoint2().setWrapPath(s, best_wrap.wrap_pts);\n\n                    // In OpenSim, all conversion to/from the wrap object\'s \n                    // reference frame will be performed inside \n                    // wrapPathSegment(). Thus, all points in this function will\n                    // be in their respective body reference frames.\n                    // for (j = 0; j < wrapPath.getSize(); j++){\n                    //    convert_from_wrap_object_frame(wo, wrapPath.get(j));\n                    //    convert(ms->modelnum, wrapPath.get(j), wo->segment, \n                    //            ms->ground_segment);\n                    // }\n\n                    ws.updWrapPoint1().setWrapLength(s, 0.0);\n                    ws.updWrapPoint2().setWrapLength(s, best_wrap.wrap_path_length);\n\n                    ws.updWrapPoint1().setLocation(s, best_wrap.r1);\n                    ws.updWrapPoint2().setLocation(s, best_wrap.r2);\n\n                    // Now insert the two new wrapping points into mp[] array.\n                    path.insert(best_wrap.endPoint, &ws.updWrapPoint1());\n                    path.insert(best_wrap.endPoint + 1, &ws.updWrapPoint2());\n                }\n            }\n        }\n\n        const double length = calcLengthAfterPathComputation(s, path); \n        if (std::abs(length - last_length) < 0.0005) {\n            break;\n        } else {\n            last_length = length;\n        }\n\n        if (kk == 0 && wrapSetSize > 1) {\n            // If the first wrap was a no wrap, and the second was a no wrap\n            // because a point was inside the object, switch the order of\n            // the first two objects and try again.\n            if (   result[0] == WrapObject::noWrap \n                && result[1] == WrapObject::insideRadius)\n            {\n                order[0] = 1;\n                order[1] = 0;\n\n                // remove wrap object 0 from the list of path points\n                PathWrap& ws = get_PathWrapSet().get(0);\n                for (int j = 0; j < path.getSize(); j++) {\n                    if (path.get(j) == &ws.updWrapPoint1()) {\n                        path.remove(j); // remove the first wrap point\n                        path.remove(j); // remove the second wrap point\n                        break;\n                    }\n                }\n            }\n        }\n    }\n}\n\n//_____________________________________________________________________________\n/*\n * _calc_path_length_change - given the output of a successful path wrap\n * over a wrap object, determine the percent change in length of the\n * path segment incurred by wrapping.\n */\ndouble GeometryPath::\ncalcPathLengthChange(const SimTK::State& s, const WrapObject& wo, \n                     const WrapResult& wr, const Array<AbstractPathPoint*>& path)  const\n{\n    const AbstractPathPoint* pt1 = path.get(wr.startPoint);\n    const AbstractPathPoint* pt2 = path.get(wr.endPoint);\n\n    double straight_length = pt1->calcDistanceBetween(s, *pt2);\n\n    double wrap_length = pt1->calcDistanceBetween(s, wo.getFrame(), wr.r1);\n    wrap_length += wr.wrap_path_length;\n    wrap_length += pt2->calcDistanceBetween(s, wo.getFrame(), wr.r2);\n\n    return wrap_length - straight_length; // return absolute diff, not relative\n}\n\n//_____________________________________________________________________________\n/*\n * Compute the total length of the path. This function\n * assumes that the path has already been updated.\n */\ndouble GeometryPath::\ncalcLengthAfterPathComputation(const SimTK::State& s, \n                               const Array<AbstractPathPoint*>& currentPath) const\n{\n    double length = 0.0;\n    const AbstractPathPoint* p1 = currentPath[0];\n    Vec3 p1InGround = p1->getLocationInGround(s);\n    // Transform all points to ground once rather than once per-segment\n    for (int i = 1; i < currentPath.getSize() ; i++) {\n        const AbstractPathPoint* p2 = currentPath[i];\n        Vec3 p2InGround = p2->getLocationInGround(s);\n        // If both points are wrap points on the same wrap object, then this\n        // path segment wraps over the surface of a wrap object, so just add in \n        // the pre-calculated length.\n        if (   p1->getWrapObject() \n            && p2->getWrapObject() \n            && p1->getWrapObject() == p2->getWrapObject()) \n        {\n            const PathWrapPoint* smwp = dynamic_cast<const PathWrapPoint*>(p2);\n            if (smwp)\n                length += smwp->getWrapLength(s);\n        } else {\n            length += (p1InGround - p2InGround).norm();\n        }\n        p1 = p2;\n        p1InGround = p2InGround;\n    }\n\n    setLength(s,length);\n    return( length );\n}\n\n//_____________________________________________________________________________\n/*\n * Compute the path\'s moment arms for  specified coordinate.\n *\n * @param aCoord, the coordinate\n */   \ndouble GeometryPath::\ncomputeMomentArm(const SimTK::State& s, const Coordinate& aCoord) const\n{\n    if (!_maSolver)\n        const_cast<Self*>(this)->_maSolver.reset(new MomentArmSolver(*_model));\n\n    return _maSolver->solve(s, aCoord,  *this);\n}\n\n//_____________________________________________________________________________\n// Override default implementation by object to intercept and fix the XML node\n// underneath the model to match current version.\nvoid GeometryPath::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)\n{\n    if (versionNumber < XMLDocument::getLatestVersion()) {\n        if (versionNumber < 30516) {\n            // Create Appearance node under GeometryPath\n            SimTK::Xml::Element appearanceElement("Appearance");\n            aNode.appendNode(appearanceElement);\n            SimTK::Xml::element_iterator visObjectIter = aNode.element_begin("VisibleObject");\n            if (visObjectIter != aNode.element_end()) {\n                SimTK::Xml::element_iterator oldPrefIter = visObjectIter->element_begin("display_preference");\n                // old display_preference was used only for hide/show other options unsupported\n                if (oldPrefIter != visObjectIter->element_end()) {\n                    int oldPrefAsInt = 4;\n                    oldPrefIter->getValueAs<int>(oldPrefAsInt);\n                    if (oldPrefAsInt == 0) { // Hidden => Appearance/Visible\n                        SimTK::Xml::Element visibleElement("visible");\n                        visibleElement.setValueAs<bool>(false);\n                        appearanceElement.insertNodeAfter(appearanceElement.element_end(), visibleElement);\n                    }\n                }\n            }\n            // If default_color existed, copy it to Appearance/color\n            SimTK::Xml::element_iterator defaultColorIter = aNode.element_begin("default_color");\n            if (defaultColorIter != aNode.element_end()) {\n                // Move default_color to Appearance/color\n                SimTK::Xml::Element colorElement("color");\n                const SimTK::String& colorAsString = defaultColorIter->getValue();\n                colorElement.setValue(colorAsString);\n                appearanceElement.appendNode(colorElement);\n            }\n        }\n    }\n    // Call base class now assuming aNode has been corrected for current version\n    Super::updateFromXMLNode(aNode, versionNumber);\n}\n', 'message': 'Remove last remnant of "display_path" cache variable\n', 'author_name': 'Ajay Seth', 'lang': 'cpp'}