void
Tank::update(const TaskInfo& taskInfo)
{
if (!nonZeroIntersection(taskInfo.getSampleTimeSet(),
SampleTime::PerTimestep))
return;
if (!mInputPort.isConnected())
return;
// Modify the tank's content by the requested fluel flow (kg/s)
// FIXME
real_type dt = (*taskInfo.getSampleTimeSet().begin()).getSampleTime();
mNextContent = mContent + dt*mInputPort.getRealValue();
mNextContent = min(mNextContent, mCapacity);
mNextContent = max(mNextContent, real_type(0));
}
void
Tank::output(const TaskInfo& taskInfo)
{
if (!nonZeroIntersection(taskInfo.getSampleTimeSet(),
SampleTime::PerTimestep))
return;
mContent = mNextContent;
setInertia(mContent);
mIsEmpty = (mContent == 0);
}
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);
}