C++ TaskInfo::getTime方法代码示例

void
Contact::output(const TaskInfo& taskInfo)
{
  if (nonZeroIntersection(taskInfo.getSampleTimeSet(),
                          SampleTime::PerTimestep)) {
    Log(Model, Debug) << "Contact::output(): \"" << getName()
                      << "\" computing ground plane below" << endl;
    getGround(taskInfo.getTime());
  }

  // Transform the plane equation to the local frame.
  Plane lp = mMountFrame->planeFromRef(mGroundVal.plane);
  
  // Get the intersection length.
  real_type compressLength = lp.getDist(Vector3::zeros());
  
  // Don't bother if we do not intersect the ground.
  if (compressLength < 0) {
    setForce(Vector6::zeros());
    return;
  }
  
  // The velocity of the ground patch in the current frame.
  Vector6 groundVel(mMountFrame->rotFromRef(mGroundVal.vel.getAngular()),
                    mMountFrame->rotFromRef(mGroundVal.vel.getLinear()));
  groundVel -= mMountFrame->getRefVel();
  // Now get the relative velocity of the ground wrt the contact point
  Vector3 relVel = - groundVel.getLinear();

  // The velocity perpandicular to the plane.
  // Positive when the contact spring is compressed,
  // negative when decompressed.
  real_type compressVel = - lp.scalarProjectToNormal(relVel);
  
  // The in plane velocity.
  Vector3 sVel = lp.projectToPlane(relVel);
  
  // Get the plane normal force.
  real_type normForce = computeNormalForce(compressLength, compressVel);
  // The normal force cannot get negative here.
  normForce = max(static_cast<real_type>(0), normForce);
  
  // Get the friction force.
  Vector3 fricForce = computeFrictionForce(normForce, sVel, lp.getNormal(),
                                           mGroundVal.friction);
  
  // The resulting force is the sum of both.
  // The minus sign is because of the direction of the surface normal.
  Vector3 force = fricForce - normForce*lp.getNormal();
  
  // We don't have an angular moment.
  setForce(Vector6(Vector3::zeros(), force));
}

void
Launchbar::output(const TaskInfo& taskInfo)
{
  if (nonZeroIntersection(taskInfo.getSampleTimeSet(),
                          SampleTime::PerTimestep)) {
    Log(Model, Debug) << "Launchbar::output(): \"" << getName()
                      << "\" computing ground plane below" << std::endl;
    getGround(taskInfo.getTime());
  }

  // The launchbar angle
  mAngle = computeCurrentLaunchbarAngle();

  // Ok, apply the tension at the launchbar and have the holdback
  if (mState != Mounted && mState != Launching) {
    setForce(Vector6::zeros());
    return;
  }

  // Query the catapult, write the result into the attached frame
  // ... yes this function works through sideffects ... :-/
  real_type catLen;
  if (!computeCatFrame(taskInfo.getTime(), catLen)) {
    setForce(Vector6::zeros());
    return;
  }

  // The catapult direction vector
  Vector3 catPos0 = mCatFrame->posToParent(Vector3::zeros());
  Vector3 catDir = mCatFrame->rotToParent(Vector3::unit(0));

  // The launchbar's tip position
  Vector3 lbTip(cos(mAngle)*mLength, 0, -sin(mAngle)*mLength);
  
  // The tension applied over the launchbar
  // allways pulls the aircraft back to the cat ...
  // project the launchbar tip to the catpult line ...
  Vector3 lbTipOnCat = catPos0 + dot(lbTip - catPos0, catDir)*catDir;
  Vector6 force = Vector6::zeros();
  if (mState == Mounted) {
    force.setLinear(mLaunchForce/5*normalize(lbTipOnCat));
  } else {
    force.setLinear(mLaunchForce*normalize(lbTipOnCat));
  }
  
  if (mState == Mounted) {
    // the position of the holdback's deck mount
    Vector3 hbDeckMount = mCatFrame->posToParent(mPosOnCat*Vector3::unit(0));

    // ok, for now the holback is a stiff spring, will model that different
    // when loop closure contranits are availble ...
    Vector3 hbDir = mHoldbackMount - hbDeckMount;
    real_type hbLen = norm(hbDir);
    if (mHoldbackLength < hbLen) {
      Vector3 hbForce = (2*mLaunchForce*(mHoldbackLength - hbLen)/hbLen)*hbDir;
      force += forceFrom(mHoldbackMount, hbForce);
    }

    // Some damping force, just at the position the launchbar applies its force
    force += mLaunchForce/2*mCatFrame->motionToParent(mCatFrame->getRelVel());
  }
  
  setForce(force);
}