C++ PathPoint类代码示例

void OpenSimContext::setLocation(PathPoint& mp, int i, double d) {
    mp.setLocation(*_configState, i, d);
  _configState->invalidateAll(SimTK::Stage::Position);
  _model->getMultibodySystem().realize(*_configState, SimTK::Stage::Position);
  mp.getPath()->updateGeometry(*_configState);
    return;
}

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

/**
 * Calculate the wrapping of one path segment over one wrap object.
 *
 * @param aPoint1 The first patth point
 * @param aPoint2 The second path point
 * @param aPathWrap An object holding the parameters for this path/wrap-object pairing
 * @param aWrapResult The result of the wrapping (tangent points, etc.)
 * @return The status, as a WrapAction enum
 */
int WrapObject::wrapPathSegment(const SimTK::State& s, PathPoint& aPoint1, PathPoint& aPoint2,
										  const PathWrap& aPathWrap, WrapResult& aWrapResult) const
{
   int return_code = noWrap;
	bool p_flag;
	Vec3 pt1(0.0);
	Vec3 pt2(0.0);

	// Convert the path points from the frames of the bodies they are attached
	// to to the frame of the wrap object's body
	_model->getSimbodyEngine().transformPosition(s, aPoint1.getBody(), aPoint1.getLocation(), getBody(), pt1);

	_model->getSimbodyEngine().transformPosition(s, aPoint2.getBody(), aPoint2.getLocation(), getBody(), pt2);

	// Convert the path points from the frame of the wrap object's body
	// into the frame of the wrap object
	pt1 = _pose.shiftBaseStationToFrame(pt1);
	pt2 = _pose.shiftBaseStationToFrame(pt2);

	return_code = wrapLine(s, pt1, pt2, aPathWrap, aWrapResult, p_flag);

   if (p_flag == true && return_code > 0) {
		// Convert the tangent points from the frame of the wrap object to the
		// frame of the wrap object's body
		aWrapResult.r1 = _pose.shiftFrameStationToBase(aWrapResult.r1);
		aWrapResult.r2 = _pose.shiftFrameStationToBase(aWrapResult.r2);

		// Convert the surface points (between the tangent points) from the frame of
		// the wrap object to the frame of the wrap object's body
		for (int i = 0; i < aWrapResult.wrap_pts.getSize(); i++)
			aWrapResult.wrap_pts.get(i) = _pose.shiftFrameStationToBase(aWrapResult.wrap_pts.get(i));
   }

   return return_code;
}

DBOOL PathListData::Copy(HOBJECT hPathPoint)
{
	CServerDE* pServerDE = BaseClass::GetServerDE();
	if (!hPathPoint || !pServerDE) return DFALSE;

    // Get the Object Position
	pServerDE->GetObjectPos(hPathPoint, &m_vPos);
	char* pName = pServerDE->GetObjectName(hPathPoint);
	if (pName)
	{
		m_hstrName = pServerDE->CreateString(pName);
	}
    
    // Get the Objects action
    PathPoint *pPathPoint = (PathPoint*)pServerDE->HandleToObject(hPathPoint);

    HSTRING hstrAction = pPathPoint->GetActionTarget();
	if (hstrAction)
	{
		m_hstrActionTarget = pServerDE->CopyString(hstrAction);
	}

    hstrAction = pPathPoint->GetActionMessage();
	if (hstrAction)
	{
		m_hstrActionMessage = pServerDE->CopyString(hstrAction);
	}

	return DTRUE;
}

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

PathPoint PathPoint::withChangedPointType (const Path::Iterator::PathElementType newType,
                                           const Rectangle<int>& parentArea) const
{
    PathPoint p (*this);

    if (newType != p.type)
    {
        int oldNumPoints = getNumPoints();
        p.type = newType;
        int numPoints = p.getNumPoints();

        if (numPoints != oldNumPoints)
        {
            double lastX, lastY;
            double x, y, w, h;

            p.pos [numPoints - 1] = p.pos [oldNumPoints - 1];
            p.pos [numPoints - 1].getRectangleDouble (x, y, w, h, parentArea, owner->getDocument()->getComponentLayout());

            const int index = owner->points.indexOf (this);

            if (PathPoint* lastPoint = owner->points [index - 1])
            {
                lastPoint->pos [lastPoint->getNumPoints() - 1]
                            .getRectangleDouble (lastX, lastY, w, h, parentArea, owner->getDocument()->getComponentLayout());
            }
            else
            {
                jassertfalse;
                lastX = x;
                lastY = y;
            }

            for (int i = 0; i < numPoints - 1; ++i)
            {
                p.pos[i] = p.pos [numPoints - 1];

                p.pos[i].updateFrom (lastX + (x - lastX) * (i + 1) / numPoints,
                                     lastY + (y - lastY) * (i + 1) / numPoints,
                                     w, h,
                                     parentArea,
                                     owner->getDocument()->getComponentLayout());
            }
        }
    }

    return p;
}

PathPoint* PathPoint::makePathPointOfType(PathPoint* aPoint, const string& aNewTypeName)
{
    PathPoint* newPoint = NULL;
    cout << "PathPoint::makePathPointOfType()" << endl;
    if (aPoint != NULL) {
        Object* newObject = Object::newInstanceOfType(aNewTypeName);
        if (newObject) {
            newPoint = dynamic_cast<PathPoint*>(newObject);
            if (newPoint) {
                // Copy the contents from aPoint.
                newPoint->init(*aPoint);
            }
        }
    }

    return newPoint;
}

void BezierController::MakeSegment(float time, 
                                   float inc,
                                   const PathPoint& p1,
                                   const PathPoint& p2,
                                   const PathPoint& p3,
                                   const PathPoint& p4)
{
   osg::Vec3 pos, tangent;

   for (int i = 0; i < 3; ++i)
   {
      pos[i] = (BlendFunction(inc,0) * p1.GetPosition()[i]) +
               (BlendFunction(inc,1) * p2.GetPosition()[i]) +
               (BlendFunction(inc,2) * p3.GetPosition()[i]) +
               (BlendFunction(inc,3) * p4.GetPosition()[i]);

      //tangent[i] = (TangentFunction(inc,1) * p2.GetPosition()[i]) + (TangentFunction(inc,2) * p3.GetPosition()[i]) + (TangentFunction(inc,3) * p4.GetPosition()[i]);
   }

   osg::Quat quat;
   quat.slerp(1.0 - inc, p4.GetOrientation(), p1.GetOrientation());

   PathData pd;
   pd.mTime = time;
   pd.mPoint.SetPosition(pos);
   pd.mPoint.SetOrientation(quat);

   mPath.push_back(pd);
}

// 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;
    PathPoint* start;
    PathPoint* end;
    const OpenSim::PhysicalFrame* startBody;
    const OpenSim::PhysicalFrame* endBody;
    const Array<PathPoint*>& currentPath = getCurrentPath(s);

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

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

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

            // Find the positions of start and end in the inertial frame.
            //engine.getPosition(s, start->getBody(), start->getLocation(), posStart);
            posStart = start->getLocationInGround(s);
            
            //engine.getPosition(s, end->getBody(), 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);
        }
    }
}

void PaintElementPath::setCurrentBounds (const Rectangle<int>& b,
                                         const Rectangle<int>& parentArea,
                                         const bool /*undoable*/)
{
    Rectangle<int> newBounds (b);
    newBounds.setSize (jmax (1, newBounds.getWidth()),
                       jmax (1, newBounds.getHeight()));

    const Rectangle<int> current (getCurrentBounds (parentArea));

    if (newBounds != current)
    {
        const int borderSize = getBorderSize();

        const int dx = newBounds.getX() - current.getX();
        const int dy = newBounds.getY() - current.getY();

        const double scaleStartX = current.getX() + borderSize;
        const double scaleStartY = current.getY() + borderSize;
        const double scaleX = (newBounds.getWidth() - borderSize * 2) / (double) (current.getWidth() - borderSize * 2);
        const double scaleY = (newBounds.getHeight() - borderSize * 2) / (double) (current.getHeight() - borderSize * 2);

        for (int i = 0; i < points.size(); ++i)
        {
            PathPoint* const destPoint = points.getUnchecked(i);
            PathPoint p (*destPoint);

            for (int j = p.getNumPoints(); --j >= 0;)
                rescalePoint (p.pos[j], dx, dy,
                              scaleX, scaleY,
                              scaleStartX, scaleStartY,
                              parentArea);

            perform (new ChangePointAction (destPoint, i, p), "Move path");
        }
    }
}

PathPoint Path::update(Vector3<float> refPos, Vector3<float> playerPos)
{
  float D_val;
  PathPoint current = getCurrent();
  PathPoint previous = getPrevious();
  float diffX = refPos.x - current.getPosition().x;
  float diffY = refPos.y - current.getPosition().y;
  float diffZ = refPos.z - current.getPosition().z;
  float playerDistFromPlane = 0;
  float firstDistFromPlane = 0;
  Vector3<float> vect1 (current.getPosition().x - previous.getPosition().x,
			current.getPosition().y - previous.getPosition().y,
			current.getPosition().z - previous.getPosition().z);

  Vector3<float> vect2 (current.getPosition().x + current.getUp().x,
			current.getPosition().y + current.getUp().y,
			current.getPosition().z + current.getUp().z);

  Vector3<float> normal;
  float distance = sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ);
  PathPoint firstChoice = getAt(current.getFirstID());

  if (distance < RANGE_CHECK) {
    if (current.getNumberOfIDs() == 1) {

      setChoice(current.getFirstID());
      return getCurrent();
    }

    normal = vect1.Cross(vect2).Normalized();
    D_val = (current.getPosition().x * normal.x + current.getPosition().y 
	     * normal.y + current.getPosition().z * normal.z) * -1.0f;
    
    playerDistFromPlane = (normal.x * playerPos.x) + 
       (normal.y * playerPos.y) + (normal.z * playerPos.z) + D_val;
    
    if (abs(playerDistFromPlane) < MID_BUFFER_WIDTH && 
	current.getNumberOfIDs() == 3) {
      setChoice(current.getThirdID());
      return getCurrent();
    }
    
    firstDistFromPlane = normal.x * firstChoice.getPosition().x 
       + normal.y * firstChoice.getPosition().y 
       + normal.z * firstChoice.getPosition().z + D_val;

    if (playerDistFromPlane / abs(playerDistFromPlane) == 
	firstDistFromPlane / abs(firstDistFromPlane)) {
      setChoice(current.getFirstID());
      return getCurrent();
    }
    else {
      setChoice(current.getSecondID());
      return getCurrent();
    }
  }
  return getCurrent();
}

bool OpenSimContext::isActivePathPoint(PathPoint& mp) {
  return mp.isActive(*_configState);
};

void OpenSimContext::setBody(PathPoint& pathPoint, Body&  newBody) {
   pathPoint.changeBodyPreserveLocation(*_configState, newBody);
   this->recreateSystemAfterSystemExists();
   realizeVelocity();
}

/* 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
{
    PathPoint* start = NULL;
    PathPoint* end = NULL;
    const SimTK::MobilizedBody* bo = NULL;
    const SimTK::MobilizedBody* bf = NULL;
    const Array<PathPoint*>& 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->getBody().getMobilizedBody();
        bf = &end->getBody().getMobilizedBody();

        if (bo != bf) {
            // Find the positions of start and end in the inertial frame.
            po = start->getLocationInGround(s);
            pf = end->getLocationInGround(s);

            // Form a vector from start to end, in the inertial frame.
            dir = (pf - po);

            // 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(), 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(), -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());

//.........这里部分代码省略.........