/**
* Encapsulates the DynamixelFrictionMotor-specific calculations (PID-controller and motorfriction).
* After setting the current motor angle, this method is tu be used to update the motor.
*/
void DynamixelFrictionMotor::calculateMotorFrictionAndVelocity() {
if(mStepSize == 0.0) {
Core::log("DynamixelFrictionMotor Warning: StepSize was 0.0, prevented division by zero!");
return;
}
if((mCurrentPosition >= 0.0 && mLastPosition >= 0.0) || (mCurrentPosition <= 0.0 && mLastPosition <= 0.0)) {
mVelocity = (mCurrentPosition - mLastPosition);
}
else {
mVelocity = -(mLastPosition + mCurrentPosition);
}
//allow sign changes (rotational joints)
mVelocity = mVelocity / mStepSize;
double calcVel;
double desiredAngle = (mDesiredMotorAngleValue->get() * Math::PI / 180.0);
if(desiredAngle > Math::PI){
calcVel = calculateVelocity(mCurrentPosition, desiredAngle - 2.0 * Math::PI);
}
else if(desiredAngle >= (-1.0 * Math::PI)) {
calcVel = calculateVelocity(mCurrentPosition, desiredAngle);
}
else {
calcVel = calculateVelocity(mCurrentPosition, desiredAngle + 2.0 * Math::PI);
}
if(mMaxVelocity >= 0) {
if(calcVel > 0) {
calcVel = Math::min(calcVel, mMaxVelocity);
}
else {
calcVel = Math::max(calcVel, -mMaxVelocity);
}
}
mCalculatedFriction = calculateFriction(mVelocity);
if(mIncludeNoise->get() == true)
{
calcVel = Math::calculateGaussian(calcVel, mMotorValueNoise);
}
mCurrentVelocityValue->set(calcVel);
mCalculatedVelocity = calcVel;
mLastPosition = mCurrentPosition;
}
void Lifeform::update( float dt ){
sf::Vector2f pos = this->getPosition();
if(!_onGround && !_isFlying)
getTargetVelocity().y += 80.0f;
calculateVelocity();
if(pos.y < getHeight()/2) setPosition(pos.x,getHeight()/2);
if(pos.y > Game::SCREEN_WIDTH - getHeight()/2 ) setPosition(pos.x,Game::SCREEN_WIDTH - getHeight()/2);
if(pos.x < getSprite().getOrigin().x){
_velocity.x = 0;
setPosition(getSprite().getOrigin().x,pos.y);
}else if(pos.x > (Game::SCREEN_WIDTH - getSprite().getOrigin().x)){
_velocity.x = 0;
setPosition((Game::SCREEN_WIDTH - getSprite().getOrigin().x),pos.y);
}
_lastPos = getPosition();
attemptMove( dt );
//BUGGY Collisions \/ \/ \/
//getSprite().move(sf::Vector2f(0,_velocity.y) * dt);
//_onGround = collidesGround();
//if(_onGround)
//setPosition(getPosition().x,_lastPos.y);
}
bool DialView::onTouchEvent(MotionEvent *event) {
if (!isEnabled() || (!inCircle((int) event->getX(), (int) event->getY()) && mTouchState == TOUCH_STATE_RESTING)) {
return false;
}
switch (event->getAction()) {
case MotionEvent::ACTION_DOWN:
{
startTouch(event);
break;
}
case MotionEvent::ACTION_MOVE:
{
if (mTouchState == TOUCH_STATE_CLICK) {
startRotationIfNeeded(event);
}
if (mTouchState == TOUCH_STATE_ROTATE) {
mVelocityTracker->addMovement(event);
rotateChart(event->getX(), event->getY());
}
break;
}
case MotionEvent::ACTION_UP:
{
float velocity = 0;
if (mTouchState == TOUCH_STATE_ROTATE) {
mVelocityTracker->addMovement(event);
mVelocityTracker->computeCurrentVelocity(PIXELS_PER_SECOND);
velocity = calculateVelocity();
}
endTouch(velocity);
break;
}
default:
{
endTouch(0);
break;
}
}
return true;
}
void rosaria_motion::RosariaMotionNode::spin()
{
ROS_INFO("Rosaria Motion Node is up and running!!!");
ros::Rate loop_rate(10.0);
while (nh_.ok())
{
calculateVelocity();
ros::spinOnce();
loop_rate.sleep();
}
}
LinkedTriple::LinkedTriple(StationaryParticle* p0, StationaryParticle* q0, StationaryParticle* r0, int id0)
{
p = p0;
q = q0;
r = r0;
calculateVelocity(p, q, r, v[0], v[1]);
fitness = fitnessMeasure(p, q, r);
id = id0;
prob = 0;
_prob0 = 0;
groupNumber = 0;
}
void WeaponRack::fireLaser()
{
const float spawnDistance = 15.0f;
const float startImpulse = 800.0f; // 200 m/s
Location spawnLocation;
calculateSpawnLocation(spawnLocation, spawnDistance);
v3 startVelocity;
calculateVelocity(startImpulse, startVelocity, false);
warnlog << "STUB: WeaponRack::fireLaser()";
}
void updateSprite(struct SPRITE *s)
{
/* do sprite update, default implementation */
/* keep sprite from moving when touching walls */
if (s->solid) {
if (s->isTouching[LEFT]) {
--s->isTouching[LEFT];
}
if (s->isTouching[RIGHT]) {
--s->isTouching[RIGHT];
}
if (s->isTouching[UP]) {
--s->isTouching[UP];
}
if (s->isTouching[DOWN]) {
--s->isTouching[DOWN];
}
}
/* move sprite, standard physics */
s->SetVX(s, calculateVelocity(s->GetVX(s), s->GetAX(s), s->GetDragX(s), s->GetMaxSpeedX(s)));
s->SetVY(s, calculateVelocity(s->GetVY(s), s->GetAY(s), s->GetDragY(s), s->GetMaxSpeedY(s)));
s->SetX(s, s->GetX(s) + s->GetVX(s));
s->SetY(s, s->GetY(s) + s->GetVY(s));
updateHitbox(&s->box);
/* sprite timer execution */
if (s) {
if (s->timeLeft > 0) {
--s->timeLeft;
} else if (s->timeLeft == 0) {
s->timeLeft = -1;
if (s->timerFunc)
s->timerFunc(s);
}
}
}
int IRArray::readLine(){
int sum=0;
for(int i=0; i<3; i++){
int reading = constrain(analogRead(sensPins[i]), 60, 400);
//int reading = constrain(analogRead(sensPins[i]), low[i], high[i]);
sensorVal[i]=map(reading,60,400,0,127);
sum+=sensorVal[i];
//Serial.println(sum);
}
int vel = 0;
if(sum>LINE_THRESSHOLD) vel = calculateVelocity(sum);
return vel;
}
void JointTrajectoryAction::controlLoop(const std::vector<double>& actualJointAngles,
const std::vector<double>& actualJointVelocities,
const KDL::Trajectory_Composite* trajectoryComposite,
int numberOfJoints,
ros::Time startTime,
std::vector<double>& velocities) {
velocities.clear();
double elapsedTime = ros::Duration(ros::Time::now() - startTime).toSec();
for (int i = 0; i < numberOfJoints; ++i) {
double velocity = calculateVelocity(actualJointAngles[i],
actualJointVelocities[i],
trajectoryComposite[i],
elapsedTime);
velocities.push_back(velocity);
}
}
void QKineticScrollerPrivate::timerEventWhileScrolling()
{
qreal deltaTime = qreal(scrollRelativeTimer.restart()) / 1000;
qreal time = qreal(scrollAbsoluteTimer.elapsed()) / 1000;
// calculate the velocity for the passed interval deltatime.
// using the midpoint of the interval gives a better precision than using just time.
QPointF newVelocity = calculateVelocity(time - deltaTime / 2);
QPointF deltaPos = newVelocity * deltaTime * pixelPerMeter;
// -- move (convert from [m/s] to [pix/frame]
if (!deltaPos.isNull())
setContentPositionHelper(deltaPos);
qKSDebug() << "QKS::timerEventWhileScrolling() -- DeltaPos:" << deltaPos << "- NewVel:" << newVelocity << "- Time:" << time;
if (newVelocity.isNull() ||
(releaseVelocity.isNull() && !scrollToX && !scrollToY && !overshootX && !overshootY))
// if (newVelocity.isNull())
setState(QKineticScroller::StateInactive);
}
void WeaponRack::fireRocket()
{
// No target?
if (mTarget == NULL_HANDLE)
return;
// Out of ammo?
if (mRocketAmmo < 1)
return;
const f32 spawnDistance = 5.0f;
const f32 startImpulse = 50.0f;
--mRocketAmmo;
Location spawnLocation;
calculateSpawnLocation(spawnLocation, spawnDistance);
v3 startVelocity;
calculateVelocity(startImpulse, startVelocity, true);
static size_t count = 0;
++count;
std::stringstream ss;
ss << "Rocket " << count;
Loader::ObjectParameter op;
op.mHandle = generateHandle();
op.mName = ss.str();
op.mType = "Rocket";
op.mLocation = spawnLocation;
op.mLinearVelocity = startVelocity;
emit<SectorEvent>(ID::S_CREATE_GO, op);
emit<GameObjectEvent>(ID::GOE_AUTONAVIGATE, mTarget, op.mHandle);
updateGuiAmmo();
}
void Particle::calculatePhysics() {
calculateAcceleration();
calculateVelocity();
calculatePosition();
}
//Tracking robot direction and velocity vector
bool findRobotVector(void) {
static short lastXpos = 0;
static short lastYpos = 0;
static short lastVelocity = 0;
static short lastBearing = 0;
static short lastObjectDiameter = 0;
static char lastCompass[3] = "00";
//check to see if the object moved (translation in x,y plane)
if( (abs(DetectedObject.xPos-lastXpos) > objectMovedThreshold) || (abs(DetectedObject.yPos-lastYpos) > objectMovedThreshold) ) {
if( (lastXpos > DetectedObject.xPos) && (lastYpos < DetectedObject.yPos) ) { //robot is moving NorthWest
strcpy(DetectedObject.compass,"NW");
}
else if( (lastXpos < DetectedObject.xPos) && (lastYpos < DetectedObject.yPos) ) { //robot is moving NorthEast
strcpy(DetectedObject.compass,"NE");
}
else if( (lastXpos > DetectedObject.xPos) && (lastYpos > DetectedObject.yPos) ) { //robot is moving SouthWest
strcpy(DetectedObject.compass,"SW");
}
else if( (lastXpos < DetectedObject.xPos) && (lastYpos > DetectedObject.yPos) ) { //robot is moving SouthEast
strcpy(DetectedObject.compass,"SE");
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
else if(lastYpos < DetectedObject.yPos) { //robot is moving North
strcpy(DetectedObject.compass,"_N");
}
else if(lastYpos > DetectedObject.yPos) { //robot is moving South
strcpy(DetectedObject.compass,"_S");
}
else if(lastXpos > DetectedObject.xPos) { //robot is moving West
strcpy(DetectedObject.compass,"_W");
}
else if(lastXpos < DetectedObject.xPos) { //robot is moving East
strcpy(DetectedObject.compass,"_E");
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
else { //the robot did not move, so it does not have a velocity vector
//I suggest using the gyro to find the robot orientation while the robot does not move translationally
strcpy(DetectedObject.compass,"00");
DetectedObject.bearing = 0;
DetectedObject.velocity = 0;
DetectedObject.diameter = 0;
return false;
}
if(DetectedObject.compass == lastCompass) { //robot headed in same direction as last measurement
calculateVelocity(DetectedObject.xPos, lastXpos, DetectedObject.yPos, lastYpos);
}
}
//record the last measurements taken in order to calculate the velocity vector the next iteration this function is called
lastXpos = DetectedObject.xPos;
lastYpos = DetectedObject.yPos;;
lastVelocity = DetectedObject.velocity;
strcpy(lastCompass, DetectedObject.compass);
lastBearing = DetectedObject.bearing;
lastObjectDiameter = DetectedObject.diameter;
return true;
}
QMatrix4x4 Exercise13::applyBallTransformation(const int frame)
{
// environment
static const std::vector<float> left = {-0.9f, 0.8f, 0.0f};
static const std::vector<float> right = {0.9f, 0.4f, 0.0f};
static const std::vector<float> bottom = {0.0f, -1.05f, 0.0f};
// ball
static const float r = 0.3f;
static const float d = 0.3f * r;
static const std::vector<float> startPos = {-2.0f, left[1], 0.0f};
static const std::vector<float> endPos = {2.0f, right[1], 0.0f};
static const float ballSpeedX = 0.02f;
static const float angularSpeed = (float const) (-ballSpeedX/(r*2*M_PI)*360);
static bool isBounced = false;
static float x, y, z;
float scaleY = 1, scaleX = 1, scaleZ = 1;
// animation
static const int numFramesPerAnimation = static_cast<int>(4.0f / ballSpeedX);
const int periodicity = frame % numFramesPerAnimation;
// restart animation
if (x >= endPos[0]) {
x = startPos[0];
y = startPos[1];
z = startPos[2];
isBounced = false;
}
if (x <= left[0]) {
// clamp to cliff height
y = left[1] + r;
} else if (x > left[0] && x < right[0]) {
float tolerance = r/3;
if (y - r + tolerance <= bottom[1]) {
isBounced = true;
}
// scale ball
if (y - r <= bottom[1]) {
float delta = (bottom[1] - y - r) / tolerance;
float lerpScale = (1 - delta) * d + delta * d;
scaleX += lerpScale;
scaleY = lerpScale;
scaleZ += lerpScale;
}
// simulate gravitation and bounce
float delta = (x - left[0]) / (right[0] - left[0]);
if (!isBounced) {
int slope = 1500;
float velocityY = calculateVelocity(delta / (slope * ballSpeedX));
y -= velocityY;
} else {
int slope = 1200;
float velocityY = calculateVelocity((1-delta) / (slope * ballSpeedX));
y += velocityY;
}
} else if (x >= right[0]) {
// finish bounce and clamp to cliff height
int slope = 800;
float delta = (x - right[0]) / (endPos[0] - right[0]);
float velocityY = calculateVelocity(delta / (slope * ballSpeedX));
y -= velocityY;
y = (y <= right[1] + r) ? right[1] + r : y;
}
// roll the ball
x += ballSpeedX;
// apply transformations
QMatrix4x4 transform;
transform.setToIdentity();
transform.translate(x, y, z);
transform.scale(scaleX, scaleY, scaleZ);
transform.rotate(angularSpeed * periodicity, 0, 0, 1); // around z-axis
return transform;
}
void Puck::Update(float timeTotal, float timeDelta, XMFLOAT2 currentField) {
calculateVelocity(currentField);
Sprite::Update(timeTotal, timeDelta);
}
void QAbstractKineticScrollerPrivate::handleMove(QMouseEvent *me, QPoint &delta)
{
Q_Q(QAbstractKineticScroller);
if (mode == QAbstractKineticScroller::AccelerationMode) {
// we need delta to be the delta to ipos, not pos in this case
delta = me->globalPos() - ipos;
}
if (axisLockThreshold) {
int dx = qAbs(delta.x());
int dy = qAbs(delta.y());
if (dx || dy) {
bool vertical = (dy > dx);
qreal alpha = qreal(vertical ? dx : dy) / qreal(vertical ? dy : dx);
qKSDebug() << "axis lock: " << alpha << " / " << axisLockThreshold << " - isvertical: " << vertical << " - dx: " << dx << " - dy: " << dy;
if (alpha <= axisLockThreshold) {
if (vertical)
delta.setX(0);
else
delta.setY(0);
}
}
}
switch (mode) {
case QAbstractKineticScroller::PushMode:
// Scroll by the amount of pixels the cursor has moved
// since the last motion event.
scrollUpdate(delta);
pos = me->globalPos();
break;
case QAbstractKineticScroller::AccelerationMode: {
// Set acceleration relative to the initial click
QSize size = q->viewportSize();
qreal signX = 0, signY = 0;
if (delta.x() < 0)
signX = -1;
else if (delta.x() > 0)
signX = 1;
if (delta.y() < 0)
signY = -1;
else if (delta.y() > 0)
signY = 1;
velocity.setX(signX * ((qreal(qAbs(delta.x())) / qreal(size.width()) * (maxVelocity - minVelocity)) + minVelocity));
velocity.setY(signY * ((qreal(qAbs(delta.y())) / qreal(size.height()) * (maxVelocity - minVelocity)) + minVelocity));
break;
}
case QAbstractKineticScroller::AutoMode:
QPointF newVelocity = calculateVelocity(delta, lastTime.elapsed());
QPoint maxPos = q->maximumScrollPosition();
bool alwaysOvershoot = (overshootPolicy == QAbstractKineticScroller::OvershootAlwaysOn);
if (!maxPos.x() && !alwaysOvershoot) {
delta.setX(0);
newVelocity.setX(0);
}
if (!maxPos.y() && !alwaysOvershoot) {
delta.setY(0);
newVelocity.setY(0);
}
velocity = newVelocity;
scrollUpdate(delta);
if (maxPos.x() || alwaysOvershoot)
pos.setX(me->globalPos().x());
if (maxPos.y() || alwaysOvershoot)
pos.setY(me->globalPos().y());
break;
}
}
/*! \internal
Helps when setting the content position.
It will try to move the content by the requested delta but stop in case
when we are coming back from an overshoot or a scrollTo.
It will also indicate a new overshooting condition by the overshootX and oversthootY flags.
In this cases it will reset the velocity variables and other flags.
Also keeps track of the current over-shooting value in overshootPosition.
\deltaPos is the amout of pixels the current content position should be moved
*/
void QKineticScrollerPrivate::setContentPositionHelper(const QPointF &deltaPos)
{
Q_Q(QKineticScroller);
if (state == QKineticScroller::StateDragging && overshootDragResistanceFactor)
overshootPosition /= overshootDragResistanceFactor;
QPointF oldPos = q->contentPosition() + overshootPosition;
QPointF newPos = oldPos + deltaPos;
QPointF maxPos = q->maximumContentPosition();
QPointF oldScrollToDist = scrollToPosition - oldPos;
QPointF newScrollToDist = scrollToPosition - newPos;
qKSDebug() << "QKS::setContentPositionHelper(" << deltaPos << " [pix])";
qKSDebug() << " --> overshoot:" << overshootPosition << "- old pos:" << oldPos << "- new pos:" << newPos;
QPointF oldClampedPos;
oldClampedPos.setX(qBound(qreal(0), oldPos.x(), maxPos.x()));
oldClampedPos.setY(qBound(qreal(0), oldPos.y(), maxPos.y()));
QPointF newClampedPos;
newClampedPos.setX(qBound(qreal(0), newPos.x(), maxPos.x()));
newClampedPos.setY(qBound(qreal(0), newPos.y(), maxPos.y()));
// --- handle overshooting and stop if the coordinate is going back inside the normal area
bool alwaysOvershootX = (hOvershootPolicy == QKineticScroller::OvershootAlwaysOn);
bool alwaysOvershootY = (vOvershootPolicy == QKineticScroller::OvershootAlwaysOn);
bool noOvershootX = (hOvershootPolicy == QKineticScroller::OvershootAlwaysOff) ||
((state == QKineticScroller::StateDragging) && !overshootDragResistanceFactor);
bool noOvershootY = (vOvershootPolicy == QKineticScroller::OvershootAlwaysOff) ||
((state == QKineticScroller::StateDragging) && !overshootDragResistanceFactor);
bool canOvershootX = !noOvershootX && (alwaysOvershootX || maxPos.x());
bool canOvershootY = !noOvershootY && (alwaysOvershootY || maxPos.y());
qreal oldOvershootX = (canOvershootX) ? oldPos.x() - oldClampedPos.x() : 0;
qreal oldOvershootY = (canOvershootY) ? oldPos.y() - oldClampedPos.y() : 0;
qreal newOvershootX = (canOvershootX) ? newPos.x() - newClampedPos.x() : 0;
qreal newOvershootY = (canOvershootY) ? newPos.y() - newClampedPos.y() : 0;
if (state == QKineticScroller::StateDragging && overshootDragResistanceFactor) {
oldOvershootX *= overshootDragResistanceFactor;
oldOvershootY *= overshootDragResistanceFactor;
newOvershootX *= overshootDragResistanceFactor;
newOvershootY *= overshootDragResistanceFactor;
}
// -- stop at the maximum overshoot distance (if set)
if (!overshootMaximumDistance.isNull()) {
newOvershootX = qBound(-overshootMaximumDistance.x() * pixelPerMeter, newOvershootX, overshootMaximumDistance.x() * pixelPerMeter);
newOvershootY = qBound(-overshootMaximumDistance.y() * pixelPerMeter, newOvershootY, overshootMaximumDistance.y() * pixelPerMeter);
}
// --- sanity check for scrollTo in case we can't even scroll that direction
if (!(maxPos.x() || alwaysOvershootX))
scrollToX = false;
if (!(maxPos.y() || alwaysOvershootY))
scrollToY = false;
// --- handle crossing over borders (scrollTo and overshoot)
qKSDebug() << " --> old overshoot Y:" << oldOvershootY << "- new overshoot Y:" << newOvershootY;
// -- x axis
if (scrollToX && qSign(oldScrollToDist.x()) != qSign(newScrollToDist.x())) {
newClampedPos.setX(scrollToPosition.x());
newOvershootX = 0;
releaseVelocity.setX(0);
scrollToX = false;
} else if (oldOvershootX && (qSign(oldOvershootX) != qSign(newOvershootX))) {
newClampedPos.setX((oldOvershootX < 0) ? 0 : maxPos.x());
newOvershootX = 0;
releaseVelocity.setX(0);
overshootVelocity.setX(0);
overshootX = false;
} else if (!oldOvershootX && newOvershootX) {
overshootStartTimeX = qreal(scrollAbsoluteTimer.elapsed()) / 1000;
overshootVelocity.setX(calculateVelocity(overshootStartTimeX).x());
// restrict the overshoot to overshootMaximumDistance
qreal maxOvershootVelocity = overshootMaximumDistance.x() * overshootSpringConstantRoot;
if (overshootMaximumDistance.x() && qAbs(overshootVelocity.x()) > maxOvershootVelocity)
overshootVelocity.setX(maxOvershootVelocity * qSign(newOvershootX));
// -- prevent going into overshoot too far
if (state != QKineticScroller::StateDragging)
newOvershootX = qSign(newOvershootX) * qreal(0.0001);
scrollToX = false;
overshootX = true;
}
// -- y axis
if (scrollToY && qSign(oldScrollToDist.y()) != qSign(newScrollToDist.y())) {
newClampedPos.setY(scrollToPosition.y());
newOvershootY = 0;
releaseVelocity.setY(0);
scrollToY = false;
} else if (oldOvershootY && (qSign(oldOvershootY) != qSign(newOvershootY))) {
newClampedPos.setY((oldOvershootY < 0) ? 0 : maxPos.y());
newOvershootY = 0;
releaseVelocity.setY(0);
overshootVelocity.setY(0);
overshootY = false;
} else if (!oldOvershootY && newOvershootY) {
overshootStartTimeY = qreal(scrollAbsoluteTimer.elapsed()) / 1000;
overshootVelocity.setY(calculateVelocity(overshootStartTimeY).y());
// -- restrict the overshoot to overshootMaximumDistance
qreal maxOvershootVelocity = overshootMaximumDistance.y() * overshootSpringConstantRoot;
if (overshootMaximumDistance.y() && (qAbs(overshootVelocity.y()) > maxOvershootVelocity))
overshootVelocity.setY(maxOvershootVelocity * qSign(newOvershootY));
// -- prevent going into overshoot too far
if (state != QKineticScroller::StateDragging)
newOvershootY = qSign(newOvershootY) * qreal(0.0001);
scrollToY = false;
overshootY = true;
}
overshootPosition.setX(newOvershootX);
overshootPosition.setY(newOvershootY);
q->setContentPosition(newClampedPos, overshootPosition);
if (debugHook)
debugHook(debugHookUser, calculateVelocity(qreal(scrollAbsoluteTimer.elapsed()) / 1000), newClampedPos, overshootPosition);
qKSDebug() << " --> new position:" << newClampedPos << "- new overshoot:" << overshootPosition <<
"- overshoot x/y?:" << overshootX << "/" << overshootY << "- scrollto x/y?:" << scrollToX << "/" << scrollToY;
}
