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(); }