bool MissionControlSpeedFilter::advance(double dt) {
const double maxHeadingVel = maximumBaseTwistInHeadingFrame_.getTranslationalVelocity().x();
filteredVelocities_[0].update(unfilteredBaseTwistInHeadingFrame_.getTranslationalVelocity().x());
double headingVel = filteredVelocities_[0].val();
// boundToRange(&headingVel, -maxHeadingVel, maxHeadingVel);
headingVel = mapInRange(headingVel, -1.0, 1.0, -maxHeadingVel, maxHeadingVel);
const double maxLateralVel = maximumBaseTwistInHeadingFrame_.getTranslationalVelocity().y();
filteredVelocities_[1].update(unfilteredBaseTwistInHeadingFrame_.getTranslationalVelocity().y());
double lateralVel = filteredVelocities_[1].val();
// boundToRange(&lateralVel, -maxLateralVel, maxLateralVel);
lateralVel = mapInRange(lateralVel, -1.0, 1.0, -maxLateralVel, maxLateralVel);
LinearVelocity linearVelocity(headingVel, lateralVel, 0.0);
const double maxTurningVel = maximumBaseTwistInHeadingFrame_.getRotationalVelocity().z();
filteredVelocities_[2].update(unfilteredBaseTwistInHeadingFrame_.getRotationalVelocity().z());
double turningVel = filteredVelocities_[2].val();
//boundToRange(&turningVel, -maxTurningVel, maxTurningVel);
turningVel = mapInRange(turningVel, -1.0, 1.0, -maxTurningVel, maxTurningVel);
LocalAngularVelocity angularVelocity(0.0, 0.0, turningVel);
baseTwistInHeadingFrame_ = Twist(linearVelocity, angularVelocity);
/* -------- Position -------- */
// desiredPositionOffsetInWorldFrame_.z() = 0.5*(maxHeight-minHeight)*(joyStick->getVertical()-minHeight) + maxHeight;
boundToRange(&desiredPositionOffsetInWorldFrame_.x(), minimalDesiredPositionOffsetInWorldFrame_.x(), maximalDesiredPositionOffsetInWorldFrame_.x());
boundToRange(&desiredPositionOffsetInWorldFrame_.y(), minimalDesiredPositionOffsetInWorldFrame_.y(), maximalDesiredPositionOffsetInWorldFrame_.y());
boundToRange(&desiredPositionOffsetInWorldFrame_.z(), minimalDesiredPositionOffsetInWorldFrame_.z(), maximalDesiredPositionOffsetInWorldFrame_.z());
/* -------- Orientation -------- */
EulerAnglesZyx desEulerAnglesZyx(desiredOrientationHeadingToBase_);
desEulerAnglesZyx.setUnique();
EulerAnglesZyx minEulerAnglesZyx(minimalOrientationHeadingToBase_);
minEulerAnglesZyx.setUnique();
EulerAnglesZyx maxEulerAnglesZyx(maximalOrientationHeadingToBase_);
maxEulerAnglesZyx.setUnique();
double value = desEulerAnglesZyx.roll();
boundToRange(&value, minEulerAnglesZyx.roll(), maxEulerAnglesZyx.roll());
desEulerAnglesZyx.setRoll(value);
value = desEulerAnglesZyx.pitch();
boundToRange(&value, minEulerAnglesZyx.pitch(), maxEulerAnglesZyx.pitch());
desEulerAnglesZyx.setPitch(value);
value = desEulerAnglesZyx.yaw();
boundToRange(&value, minEulerAnglesZyx.yaw(), maxEulerAnglesZyx.yaw());
desEulerAnglesZyx.setYaw(value);
desiredOrientationHeadingToBase_(desEulerAnglesZyx.getUnique());
return true;
}
void Box2DBody::setLinearVelocity(const QPointF &velocity)
{
if (linearVelocity() == velocity)
return;
mBodyDef.linearVelocity = invertY(velocity);
if (mBody)
mBody->SetLinearVelocity(mBodyDef.linearVelocity);
emit linearVelocityChanged();
}
//------------------------------------------------------------------------------
//!
bool
LineAnimator::animate( float delta, ParticleEntity& e, ParticleRNG& /*rng*/ )
{
delta = CGM::min( delta, 1.0f/16.0f ); // Don't fall below 16fps.
ParticleData& data = e.data();
for( auto cur = data.iter(); cur(); cur.next() )
{
Vec3f& pos = cur.position();
Vec3f& vel = cur.linearVelocity();
float spd2 = vel.sqrLength();
if( spd2 > 1e-6f )
{
// Only care if the particle moves.
Vec3f dir = _dst - pos;
float len = dir.length();
float spd = CGM::sqrt( spd2 );
float dist = len - (spd*delta); // Distance after the timestep.
if( dist <= _dtd )
{
// Will be close enough to distanceToDie, or overshoot.
data.removeParticle( cur );
continue;
}
vel = dir * (spd/len); // Rescale to honor original speed.
pos += vel * delta;
}
else
{
// Non-moving particles will never reach the destination.
StdErr << "Non-moving particles expired." << nl;
data.removeParticle( cur );
}
}
//StdErr << "Once animated:" << nl;
//data.printInfo();
//if( !data.empty() && false )
//{
// ParticleData::Iterator it = data.iter();
// StdErr << "last"
// << " pos=" << it.position()
// << " size=" << it.size()
// << " col=" << it.color()
// << " nrg=" << it.energy()
// << " vel=" << it.linearVelocity()
// << nl;
//}
////getchar();
return false;
}
void Box2DBody::setLinearVelocity(const QPointF &_linearVelocity)
{
if (linearVelocity() == _linearVelocity)
return;
b2Vec2 point(_linearVelocity.x() / scaleRatio,
-_linearVelocity.y() / scaleRatio);
if (mBody)
mBody->SetLinearVelocity(point);
else
mBodyDef.linearVelocity = point;
emit linearVelocityChanged();
}
