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);
}
void
Launchbar::update(const TaskInfo& taskInfo)
{
  real_type unlagedAngleCommand = mUpAngle;

  // Query the catapult, write the result into the attached frame
  // ... yes this function works through sideffects ... :-/
  real_type catLen;
  if (!computeCatFrame(taskInfo.getTime(), catLen)) {
    mState = Unmounted;
    unlagedAngleCommand = mUpAngle;
  } else {
    // Ok, here we know that the catapult frame contains some sensible values

    // The catapult direction vector
    Vector3 catPos0 = mCatFrame->posToParent(Vector3::zeros());
    Vector3 catDir = mCatFrame->rotToParent(Vector3::unit(0));
    
    real_type angle = computeCurrentLaunchbarAngle();
    // The launchbar's tip position
    Vector3 lbTip(cos(angle)*mLength, 0, -sin(angle)*mLength);
    
    // Compute the distance from the launchbar tip to the catapult
    real_type dist = norm(lbTip-catPos0-dot(catDir, lbTip - catPos0)*catDir);
    
    if (mState == Unmounted) {
      bool tryMount = mTryMountPort.getRealValue();
      if (tryMount) {
        unlagedAngleCommand = mDownAngle;
        
        // can only mount if we are near enough
        if (dist < 0.1) {
          // Now compute a reference position on the catapult line which
          // will be used as the reference for the holdback
          
          // compute the nearest point on the catapult line to the holdback
          // mount
          Vector3 hbNearest = catPos0
            + dot(mHoldbackMount - catPos0, catDir)*catDir;
          
          // Find the distance backwards from that point matching
          // the holdback length
          real_type sqrCatx = mHoldbackLength*mHoldbackLength
            - dot(hbNearest, hbNearest);
          /// There is something wrong if the holdback mount is too far
          if (0 <= sqrCatx) {
            Vector3 hbDeckMount = hbNearest - sqrt(sqrCatx)*catDir;
            
            // The reference position
            mPosOnCat = dot(catDir, hbDeckMount - catPos0);
            //           if (mPosOnCat < 30)
            
            // Ok, survived, mounted now ...
            mState = Mounted;
          }
        }
      } else
        unlagedAngleCommand = mUpAngle;
      
    } else if (mState == Mounted) {
      if (mLaunchCommandPort.getRealValue())
        mState = Launching;
      
      if (dist > 1)
        mState = Unmounted;
      
      unlagedAngleCommand = mDownAngle;
    } else {
      // Launching
      if (dist > 1)
        mState = Unmounted;
      
      Vector3 catPos1 = mCatFrame->posToParent(catLen*Vector3::unit(0));
      if (dot(catPos1, catDir) < 0)
        mState = Unmounted;
      
      unlagedAngleCommand = mDownAngle;
    }
  }

  real_type angleError = unlagedAngleCommand - mAngleCommand;
  angleError = sign(angleError)*min(mAngularVelocity, 40*fabs(angleError));
  /// FIXME: isPerTimestep sample times do not contain the step size ...
  /// hardwire that ATM
  mAngleCommand += 1/120.0*angleError;
}