C++ PathPoint::getdPointdQ方法代码示例

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

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