/*
* 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 aBody The body to attach the point to.
* @return Pointer to the newly created path point.
*/
PathPoint* GeometryPath::
addPathPoint(const SimTK::State& s, int aIndex, PhysicalFrame& aBody)
{
PathPoint* newPoint = new PathPoint();
newPoint->setBody(aBody);
Vec3& location = newPoint->getLocation();
placeNewPathPoint(s, location, aIndex, aBody);
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;
}
PathPoint* GeometryPath::
appendNewPathPoint(const std::string& proposedName,
PhysicalFrame& aBody, const SimTK::Vec3& aPositionOnBody)
{
PathPoint* newPoint = new PathPoint();
newPoint->setBody(aBody);
newPoint->setName(proposedName);
newPoint->set_location(aPositionOnBody);
upd_PathPointSet().adoptAndAppend(newPoint);
return newPoint;
}