Motion.cpp

00001 /*
00002  * Motion.cpp
00003  * Copyright (c) 2017, ZHAW
00004  * All rights reserved.
00005  *
00006  *  Created on: 02.02.2017
00007  *      Author: Marcel Honegger
00008  */
00009 
00010 #include <cmath>
00011 #include <algorithm>
00012 #include "Motion.h"
00013 
00014 using namespace std;
00015 
00016 const float Motion::DEFAULT_LIMIT = 1.0f;       // default value for limits
00017 const float Motion::MINIMUM_LIMIT = 1.0e-6f;    // smallest value allowed for limits
00018 
00019 /**
00020  * Creates a <code>Motion</code> object.
00021  * The values for position, velocity and acceleration are set to 0.
00022  */
00023 Motion::Motion() {
00024     
00025     position = 0.0;
00026     velocity = 0.0f;
00027     acceleration = 0.0f;
00028     
00029     profileVelocity = DEFAULT_LIMIT;
00030     profileAcceleration = DEFAULT_LIMIT;
00031     profileDeceleration = DEFAULT_LIMIT;
00032 }
00033 
00034 /**
00035  * Creates a <code>Motion</code> object with given values for position and velocity.
00036  * @param position the initial position value of this motion, given in [m] or [rad].
00037  * @param velocity the initial velocity value of this motion, given in [m/s] or [rad/s].
00038  */
00039 Motion::Motion(double position, float velocity) {
00040     
00041     this->position = position;
00042     this->velocity = velocity;
00043     this->acceleration = 0.0f;
00044     
00045     profileVelocity = DEFAULT_LIMIT;
00046     profileAcceleration = DEFAULT_LIMIT;
00047     profileDeceleration = DEFAULT_LIMIT;
00048 }
00049 
00050 /**
00051  * Creates a <code>Motion</code> object with given values for position, velocity and acceleration.
00052  * @param position the initial position value of this motion, given in [m] or [rad].
00053  * @param velocity the initial velocity value of this motion, given in [m/s] or [rad/s].
00054  * @param acceleration the initial acceleration value of this motion, given in [m/s&sup2;] or [rad/s&sup2;].
00055  */
00056 Motion::Motion(double position, float velocity, float acceleration) {
00057     
00058     this->position = position;
00059     this->velocity = velocity;
00060     this->acceleration = acceleration;
00061     
00062     profileVelocity = DEFAULT_LIMIT;
00063     profileAcceleration = DEFAULT_LIMIT;
00064     profileDeceleration = DEFAULT_LIMIT;
00065 }
00066 
00067 /**
00068  * Creates a <code>Motion</code> object with given values for position, velocity and acceleration.
00069  * @param position the initial position value of this motion, given in [m] or [rad].
00070  * @param velocity the initial velocity value of this motion, given in [m/s] or [rad/s].
00071  * @param profileVelocity the limit of the velocity.
00072  * @param profileAcceleration the limit of the acceleration.
00073  * @param profileDeceleration equal to profileAcceleration.
00074  */
00075 Motion::Motion(double position, float velocity, float profileVelocity, float profileAcceleration) {
00076 
00077   this->position      = position;
00078   this->velocity      = velocity;
00079   this->acceleration  = 0.0f;
00080 
00081   this->profileVelocity     = profileVelocity;
00082   this->profileAcceleration = profileAcceleration;
00083   this->profileDeceleration = profileAcceleration;
00084 }
00085 
00086 /**
00087  * Creates a <code>Motion</code> object with given values for position and velocity.
00088  * @param motion another <code>Motion</code> object to copy the values from.
00089  */
00090 Motion::Motion(const Motion& motion) {
00091     
00092     position = motion.position;
00093     velocity = motion.velocity;
00094     acceleration = motion.acceleration;
00095     
00096     profileVelocity = motion.profileVelocity;
00097     profileAcceleration = motion.profileAcceleration;
00098     profileDeceleration = motion.profileDeceleration;
00099 }
00100 
00101 Motion::~Motion() {}
00102 
00103 /**
00104  * Sets the values for position and velocity.
00105  * @param position the desired position value of this motion, given in [m] or [rad].
00106  * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s].
00107  */
00108 void Motion::set(double position, float velocity) {
00109     
00110     this->position = position;
00111     this->velocity = velocity;
00112 }
00113 
00114 /**
00115  * Sets the values for position, velocity and acceleration.
00116  * @param position the desired position value of this motion, given in [m] or [rad].
00117  * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s].
00118  * @param acceleration the desired acceleration value of this motion, given in [m/s&sup2;] or [rad/s&sup2;].
00119  */
00120 void Motion::set(double position, float velocity, float acceleration) {
00121     
00122     this->position = position;
00123     this->velocity = velocity;
00124     this->acceleration = acceleration;
00125 }
00126 
00127 /**
00128  * Sets the values for position, velocity and acceleration.
00129  * @param motion another <code>Motion</code> object to copy the values from.
00130  */
00131 void Motion::set(const Motion& motion) {
00132     
00133     position = motion.position;
00134     velocity = motion.velocity;
00135     acceleration = motion.acceleration;
00136 }
00137 
00138 /**
00139  * Sets the position value.
00140  * @param position the desired position value of this motion, given in [m] or [rad].
00141  */
00142 void Motion::setPosition(double position) {
00143     
00144     this->position = position;
00145 }
00146 
00147 /**
00148  * Gets the position value.
00149  * @return the position value of this motion, given in [m] or [rad].
00150  */
00151 double Motion::getPosition() {
00152     
00153     return position;
00154 }
00155 
00156 /**
00157  * Sets the velocity value.
00158  * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s].
00159  */
00160 void Motion::setVelocity(float velocity) {
00161     
00162     this->velocity = velocity;
00163 }
00164 
00165 /**
00166  * Gets the velocity value.
00167  * @return the velocity value of this motion, given in [m/s] or [rad/s].
00168  */
00169 float Motion::getVelocity() {
00170     
00171     return velocity;
00172 }
00173 
00174 /**
00175  * Sets the acceleration value.
00176  * @param acceleration the desired acceleration value of this motion, given in [m/s&sup2;] or [rad/s&sup2;].
00177  */
00178 void Motion::setAcceleration(float acceleration) {
00179     
00180     this->acceleration = acceleration;
00181 }
00182 
00183 /**
00184  * Gets the acceleration value.
00185  * @return the acceleration value of this motion, given in [m/s&sup2;] or [rad/s&sup2;].
00186  */
00187 float Motion::getAcceleration() {
00188     
00189     return acceleration;
00190 }
00191 
00192 /**
00193  * Sets the limit for the velocity value.
00194  * @param profileVelocity the limit of the velocity.
00195  */
00196 void Motion::setProfileVelocity(float profileVelocity) {
00197     
00198     if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
00199 }
00200 
00201 /**
00202  * Sets the limit for the acceleration value.
00203  * @param profileAcceleration the limit of the acceleration.
00204  */
00205 void Motion::setProfileAcceleration(float profileAcceleration) {
00206     
00207     if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
00208 }
00209 
00210 /**
00211  * Sets the limit for the deceleration value.
00212  * @param profileDeceleration the limit of the deceleration.
00213  */
00214 void Motion::setProfileDeceleration(float profileDeceleration) {
00215     
00216     if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
00217 }
00218 
00219 /**
00220  * Sets the limits for velocity, acceleration and deceleration values.
00221  * @param profileVelocity the limit of the velocity.
00222  * @param profileAcceleration the limit of the acceleration.
00223  * @param profileDeceleration the limit of the deceleration.
00224  */
00225 void Motion::setLimits(float profileVelocity, float profileAcceleration, float profileDeceleration) {
00226     
00227     if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
00228     if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
00229     if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
00230 }
00231 
00232 /**
00233  * Gets the time needed to move to a given target position.
00234  * @param targetPosition the desired target position given in [m] or [rad].
00235  * @return the time to move to the target position, given in [s].
00236  */
00237 float Motion::getTimeToPosition(double targetPosition) {
00238     
00239     // calculate position, when velocity is reduced to zero
00240     
00241     double stopPosition = (velocity > 0.0f) ? position+static_cast<double>(velocity*velocity/profileDeceleration*0.5f) : position-static_cast<double>(velocity*velocity/profileDeceleration*0.5f);
00242     
00243     if (targetPosition > stopPosition) { // positive velocity required
00244         
00245         if (velocity > profileVelocity) { // slow down to profile velocity first
00246             
00247             float t1 = (velocity-profileVelocity)/profileDeceleration;
00248             float t2 = static_cast<float>(targetPosition-stopPosition)/profileVelocity;
00249             float t3 = profileVelocity/profileDeceleration;
00250             
00251             return t1+t2+t3;
00252             
00253         } else if (velocity > 0.0f) { // speed up to profile velocity
00254             
00255             float t1 = (profileVelocity-velocity)/profileAcceleration;
00256             float t3 = profileVelocity/profileDeceleration;
00257             float t2 = (static_cast<float>(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
00258             
00259             if (t2 < 0.0f) {
00260                 float maxVelocity = sqrt((2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
00261                 t1 = (maxVelocity-velocity)/profileAcceleration;
00262                 t2 = 0.0f;
00263                 t3 = maxVelocity/profileDeceleration;
00264             }
00265             
00266             return t1+t2+t3;
00267             
00268         } else { // slow down to zero first, and then speed up to profile velocity
00269             
00270             float t1 = -velocity/profileDeceleration;
00271             float t2 = profileVelocity/profileAcceleration;
00272             float t4 = profileVelocity/profileDeceleration;
00273             float t3 = (static_cast<float>(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
00274             
00275             if (t3 < 0.0f) {
00276                 float maxVelocity = sqrt((2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
00277                 t2 = maxVelocity/profileAcceleration;
00278                 t3 = 0.0f;
00279                 t4 = maxVelocity/profileDeceleration;
00280             }
00281             
00282             return t1+t2+t3+t4;
00283         }
00284         
00285     } else { // negative velocity required
00286         
00287         if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
00288             
00289             float t1 = (-profileVelocity-velocity)/profileDeceleration;
00290             float t2 = static_cast<float>(stopPosition-targetPosition)/profileVelocity;
00291             float t3 = profileVelocity/profileDeceleration;
00292             
00293             return t1+t2+t3;
00294             
00295         } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
00296             
00297             float t1 = (velocity+profileVelocity)/profileAcceleration;
00298             float t3 = profileVelocity/profileDeceleration;
00299             float t2 = (static_cast<float>(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
00300             
00301             if (t2 < 0.0f) {
00302                 float minVelocity = -sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
00303                 t1 = (velocity-minVelocity)/profileAcceleration;
00304                 t2 = 0.0f;
00305                 t3 = -minVelocity/profileDeceleration;
00306             }
00307             
00308             return t1+t2+t3;
00309             
00310         } else { // slow down to zero first, and then speed up to (negative) profile velocity
00311             
00312             float t1 = velocity/profileDeceleration;
00313             float t2 = profileVelocity/profileAcceleration;
00314             float t4 = profileVelocity/profileDeceleration;
00315             float t3 = (-static_cast<float>(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
00316             
00317             if (t3 < 0.0f) {
00318                 float minVelocity = -sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
00319                 t2 = -minVelocity/profileAcceleration;
00320                 t3 = 0.0f;
00321                 t4 = -minVelocity/profileDeceleration;
00322             }
00323             
00324             return t1+t2+t3+t4;
00325         }
00326     }
00327 }
00328 
00329 /**
00330  * Gets the distance moved until the velocity reaches zero.
00331  * @return the distance to the stop position.
00332  */
00333 double Motion::getDistanceToStop() {
00334     
00335     return static_cast<double>(velocity*velocity/profileDeceleration*0.5f);
00336 }
00337 
00338 /**
00339  * Increments the current motion towards a given target velocity.
00340  * @param targetVelocity the desired target velocity given in [m/s] or [rad/s].
00341  * @param period the time period to increment the motion values for, given in [s].
00342  */
00343 void Motion::incrementToVelocity(float targetVelocity, float period) {
00344     
00345     if (targetVelocity < -profileVelocity) targetVelocity = -profileVelocity;
00346     else if (targetVelocity > profileVelocity) targetVelocity = profileVelocity;
00347     
00348     if (targetVelocity > 0.0f) {
00349         
00350         if (velocity > targetVelocity) { // slow down to target velocity
00351             
00352             float t1 = (velocity-targetVelocity)/profileDeceleration;
00353             
00354             if (t1 > period) {
00355                 position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period);
00356                 velocity += -profileDeceleration*period;
00357                 acceleration = -profileDeceleration;
00358             } else {
00359                 position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
00360                 velocity += -profileDeceleration*t1;
00361                 position += static_cast<double>(velocity*(period-t1));
00362                 acceleration = 0.0f;
00363             }
00364             
00365         } else if (velocity > 0.0f) { // speed up to target velocity
00366             
00367             float t1 = (targetVelocity-velocity)/profileAcceleration;
00368             
00369             if (t1 > period) {
00370                 position += static_cast<double>((velocity+profileAcceleration*0.5f*period)*period);
00371                 velocity += profileAcceleration*period;
00372                 acceleration = profileAcceleration;
00373             } else {
00374                 position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1);
00375                 velocity += profileAcceleration*t1;
00376                 position += static_cast<double>(velocity*(period-t1));
00377                 acceleration = 0.0f;
00378             }
00379             
00380         } else { // slow down to zero first, and then speed up to target velocity
00381             
00382             float t1 = -velocity/profileDeceleration;
00383             float t2 = targetVelocity/profileAcceleration;
00384             
00385             if (t1 > period) {
00386                 position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period);
00387                 velocity += profileDeceleration*period;
00388                 acceleration = profileDeceleration;
00389             } else if (t1+t2 > period) {
00390                 position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
00391                 velocity += profileDeceleration*t1;
00392                 position += static_cast<double>((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
00393                 velocity += profileAcceleration*(period-t1);
00394                 acceleration = profileAcceleration;
00395             } else {
00396                 position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
00397                 velocity += profileDeceleration*t1;
00398                 position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2);
00399                 velocity += profileAcceleration*t2;
00400                 position += static_cast<double>(velocity*(period-t1-t2));
00401                 acceleration = 0.0f;
00402             }
00403         }
00404         
00405     } else {
00406         
00407         if (velocity < targetVelocity) { // slow down to (negative) target velocity
00408             
00409             float t1 = (targetVelocity-velocity)/profileDeceleration;
00410             
00411             if (t1 > period) {
00412                 position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period);
00413                 velocity += profileDeceleration*period;
00414                 acceleration = profileDeceleration;
00415             } else {
00416                 position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
00417                 velocity += profileDeceleration*t1;
00418                 position += static_cast<double>(velocity*(period-t1));
00419                 acceleration = 0.0f;
00420             }
00421             
00422         } else if (velocity < 0.0f) { // speed up to (negative) target velocity
00423             
00424             float t1 = (velocity-targetVelocity)/profileAcceleration;
00425             
00426             if (t1 > period) {
00427                 position += static_cast<double>((velocity-profileAcceleration*0.5f*period)*period);
00428                 velocity += -profileAcceleration*period;
00429                 acceleration = -profileAcceleration;
00430             } else {
00431                 position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1);
00432                 velocity += -profileAcceleration*t1;
00433                 position += static_cast<double>(velocity*(period-t1));
00434                 acceleration = 0.0f;
00435             }
00436             
00437         } else { // slow down to zero first, and then speed up to (negative) target velocity
00438             
00439             float t1 = velocity/profileDeceleration;
00440             float t2 = -targetVelocity/profileAcceleration;
00441             
00442             if (t1 > period) {
00443                 position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period);
00444                 velocity += -profileDeceleration*period;
00445                 acceleration = -profileDeceleration;
00446             } else if (t1+t2 > period) {
00447                 position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
00448                 velocity += -profileDeceleration*t1;
00449                 position += static_cast<double>((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
00450                 velocity += -profileAcceleration*(period-t1);
00451                 acceleration = -profileAcceleration;
00452             } else {
00453                 position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
00454                 velocity += -profileDeceleration*t1;
00455                 position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2);
00456                 velocity += -profileAcceleration*t2;
00457                 position += static_cast<double>(velocity*(period-t1-t2));
00458                 acceleration = 0.0f;
00459             }
00460         }
00461     }
00462 }
00463 
00464 /**
00465  * Increments the current motion towards a given target position.
00466  * @param targetPosition the desired target position given in [m] or [rad].
00467  * @param period the time period to increment the motion values for, given in [s].
00468  */
00469 void Motion::incrementToPosition(double targetPosition, float period) {
00470     
00471     // calculate position, when velocity is reduced to zero
00472     
00473     double stopPosition = (velocity > 0.0f) ? position+static_cast<double>(velocity*velocity/profileDeceleration*0.5f) : position-static_cast<double>(velocity*velocity/profileDeceleration*0.5f);
00474     
00475     if (targetPosition > stopPosition) { // positive velocity required
00476         
00477         if (velocity > profileVelocity) { // slow down to profile velocity first
00478             
00479             float t1 = (velocity-profileVelocity)/profileDeceleration;
00480             float t2 = static_cast<float>(targetPosition-stopPosition)/profileVelocity;
00481             float t3 = profileVelocity/profileDeceleration;
00482             
00483             if (t1 > period) {
00484                 position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period);
00485                 velocity += -profileDeceleration*period;
00486                 acceleration = -profileDeceleration;
00487             } else if (t1+t2 > period) {
00488                 position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
00489                 velocity += -profileDeceleration*t1;
00490                 position += static_cast<double>(velocity*(period-t1));
00491                 acceleration = 0.0f;
00492             } else if (t1+t2+t3 > period) {
00493                 position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
00494                 velocity += -profileDeceleration*t1;
00495                 position += static_cast<double>(velocity*t2);
00496                 position += static_cast<double>((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
00497                 velocity += -profileDeceleration*(period-t1-t2);
00498                 acceleration = -profileDeceleration;
00499             } else {
00500                 position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
00501                 velocity += -profileDeceleration*t1;
00502                 position += static_cast<double>(velocity*t2);
00503                 position += static_cast<double>((velocity-profileDeceleration*0.5f*t3)*t3);
00504                 velocity += -profileDeceleration*t3;
00505                 acceleration = 0.0f;
00506             }
00507             
00508         } else if (velocity > 0.0f) { // speed up to profile velocity
00509             
00510             float t1 = (profileVelocity-velocity)/profileAcceleration;
00511             float t3 = profileVelocity/profileDeceleration;
00512             float t2 = (static_cast<float>(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
00513             
00514             if (t2 < 0.0f) {
00515                 float maxVelocity = sqrt((2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
00516                 t1 = (maxVelocity-velocity)/profileAcceleration;
00517                 t2 = 0.0f;
00518                 t3 = maxVelocity/profileDeceleration;
00519             }
00520             
00521             if (t1 > period) {
00522                 position += static_cast<double>((velocity+profileAcceleration*0.5f*period)*period);
00523                 velocity += profileAcceleration*period;
00524                 acceleration = profileAcceleration;
00525             } else if (t1+t2 > period) {
00526                 position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1);
00527                 velocity += profileAcceleration*t1;
00528                 position += static_cast<double>(velocity*(period-t1));
00529                 acceleration = 0.0f;
00530             } else if (t1+t2+t3 > period) {
00531                 position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1);
00532                 velocity += profileAcceleration*t1;
00533                 position += static_cast<double>(velocity*t2);
00534                 position += static_cast<double>((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
00535                 velocity += -profileDeceleration*(period-t1-t2);
00536                 acceleration = -profileDeceleration;
00537             } else {
00538                 position += static_cast<double>((velocity+profileAcceleration*0.5f*t1)*t1);
00539                 velocity += profileAcceleration*t1;
00540                 position += static_cast<double>(velocity*t2);
00541                 position += static_cast<double>((velocity-profileDeceleration*0.5f*t3)*t3);
00542                 velocity += -profileDeceleration*t3;
00543                 acceleration = 0.0f;
00544             }
00545             
00546         } else { // slow down to zero first, and then speed up to profile velocity
00547             
00548             float t1 = -velocity/profileDeceleration;
00549             float t2 = profileVelocity/profileAcceleration;
00550             float t4 = profileVelocity/profileDeceleration;
00551             float t3 = (static_cast<float>(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
00552             
00553             if (t3 < 0.0f) {
00554                 float maxVelocity = sqrt((2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
00555                 t2 = maxVelocity/profileAcceleration;
00556                 t3 = 0.0f;
00557                 t4 = maxVelocity/profileDeceleration;
00558             }
00559             
00560             if (t1 > period) {
00561                 position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period);
00562                 velocity += profileDeceleration*period;
00563                 acceleration = profileDeceleration;
00564             } else if (t1+t2 > period) {
00565                 position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
00566                 velocity += profileDeceleration*t1;
00567                 position += static_cast<double>((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
00568                 velocity += profileAcceleration*(period-t1);
00569                 acceleration = profileAcceleration;
00570             } else if (t1+t2+t3 > period) {
00571                 position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
00572                 velocity += profileDeceleration*t1;
00573                 position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2);
00574                 velocity += profileAcceleration*t2;
00575                 position += static_cast<double>(velocity*(period-t1-t2));
00576                 acceleration = 0.0f;
00577             } else if (t1+t2+t3+t4 > period) {
00578                 position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
00579                 velocity += profileDeceleration*t1;
00580                 position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2);
00581                 velocity += profileAcceleration*t2;
00582                 position += static_cast<double>(velocity*t3);
00583                 position += static_cast<double>((velocity-profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
00584                 velocity += -profileDeceleration*(period-t1-t2-t3);
00585                 acceleration = -profileDeceleration;
00586             } else {
00587                 position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
00588                 velocity += profileDeceleration*t1;
00589                 position += static_cast<double>((velocity+profileAcceleration*0.5f*t2)*t2);
00590                 velocity += profileAcceleration*t2;
00591                 position += static_cast<double>(velocity*t3);
00592                 position += static_cast<double>((velocity-profileDeceleration*0.5f*t4)*t4);
00593                 velocity += -profileDeceleration*t4;
00594                 acceleration = 0.0f;
00595             }
00596         }
00597         
00598     } else { // negative velocity required
00599         
00600         if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
00601             
00602             float t1 = (-profileVelocity-velocity)/profileDeceleration;
00603             float t2 = static_cast<float>(stopPosition-targetPosition)/profileVelocity;
00604             float t3 = profileVelocity/profileDeceleration;
00605             
00606             if (t1 > period) {
00607                 position += static_cast<double>((velocity+profileDeceleration*0.5f*period)*period);
00608                 velocity += profileDeceleration*period;
00609                 acceleration = profileDeceleration;
00610             } else if (t1+t2 > period) {
00611                 position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
00612                 velocity += profileDeceleration*t1;
00613                 position += static_cast<double>(velocity*(period-t1));
00614                 acceleration = 0.0f;
00615             } else if (t1+t2+t3 > period) {
00616                 position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
00617                 velocity += profileDeceleration*t1;
00618                 position += static_cast<double>(velocity*t2);
00619                 position += static_cast<double>((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
00620                 velocity += profileDeceleration*(period-t1-t2);
00621                 acceleration = profileDeceleration;
00622             } else {
00623                 position += static_cast<double>((velocity+profileDeceleration*0.5f*t1)*t1);
00624                 velocity += profileDeceleration*t1;
00625                 position += static_cast<double>(velocity*t2);
00626                 position += static_cast<double>((velocity+profileDeceleration*0.5f*t3)*t3);
00627                 velocity += profileDeceleration*t3;
00628                 acceleration = 0.0f;
00629             }
00630             
00631         } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
00632             
00633             float t1 = (velocity+profileVelocity)/profileAcceleration;
00634             float t3 = profileVelocity/profileDeceleration;
00635             float t2 = (static_cast<float>(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
00636             
00637             if (t2 < 0.0f) {
00638                 float minVelocity = -sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
00639                 t1 = (velocity-minVelocity)/profileAcceleration;
00640                 t2 = 0.0f;
00641                 t3 = -minVelocity/profileDeceleration;
00642             }
00643             
00644             if (t1 > period) {
00645                 position += static_cast<double>((velocity-profileAcceleration*0.5f*period)*period);
00646                 velocity += -profileAcceleration*period;
00647                 acceleration = -profileAcceleration;
00648             } else if (t1+t2 > period) {
00649                 position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1);
00650                 velocity += -profileAcceleration*t1;
00651                 position += static_cast<double>(velocity*(period-t1));
00652                 acceleration = 0.0f;
00653             } else if (t1+t2+t3 > period) {
00654                 position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1);
00655                 velocity += -profileAcceleration*t1;
00656                 position += static_cast<double>(velocity*t2);
00657                 position += static_cast<double>((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
00658                 velocity += profileDeceleration*(period-t1-t2);
00659                 acceleration = profileDeceleration;
00660             } else {
00661                 position += static_cast<double>((velocity-profileAcceleration*0.5f*t1)*t1);
00662                 velocity += -profileAcceleration*t1;
00663                 position += static_cast<double>(velocity*t2);
00664                 position += static_cast<double>((velocity+profileDeceleration*0.5f*t3)*t3);
00665                 velocity += profileDeceleration*t3;
00666                 acceleration = 0.0f;
00667             }
00668             
00669         } else { // slow down to zero first, and then speed up to (negative) profile velocity
00670             
00671             float t1 = velocity/profileDeceleration;
00672             float t2 = profileVelocity/profileAcceleration;
00673             float t4 = profileVelocity/profileDeceleration;
00674             float t3 = (-static_cast<float>(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
00675             
00676             if (t3 < 0.0f) {
00677                 float minVelocity = -sqrt((-2.0f*static_cast<float>(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
00678                 t2 = -minVelocity/profileAcceleration;
00679                 t3 = 0.0f;
00680                 t4 = -minVelocity/profileDeceleration;
00681             }
00682             
00683             if (t1 > period) {
00684                 position += static_cast<double>((velocity-profileDeceleration*0.5f*period)*period);
00685                 velocity += -profileDeceleration*period;
00686                 acceleration = -profileDeceleration;
00687             } else if (t1+t2 > period) {
00688                 position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
00689                 velocity += -profileDeceleration*t1;
00690                 position += static_cast<double>((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
00691                 velocity += -profileAcceleration*(period-t1);
00692                 acceleration = -profileAcceleration;
00693             } else if (t1+t2+t3 > period) {
00694                 position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
00695                 velocity += -profileDeceleration*t1;
00696                 position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2);
00697                 velocity += -profileAcceleration*t2;
00698                 position += static_cast<double>(velocity*(period-t1-t2));
00699                 acceleration = 0.0f;
00700             } else if (t1+t2+t3+t4 > period) {
00701                 position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
00702                 velocity += -profileDeceleration*t1;
00703                 position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2);
00704                 velocity += -profileAcceleration*t2;
00705                 position += static_cast<double>(velocity*t3);
00706                 position += static_cast<double>((velocity+profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
00707                 velocity += profileDeceleration*(period-t1-t2-t3);
00708                 acceleration = profileDeceleration;
00709             } else {
00710                 position += static_cast<double>((velocity-profileDeceleration*0.5f*t1)*t1);
00711                 velocity += -profileDeceleration*t1;
00712                 position += static_cast<double>((velocity-profileAcceleration*0.5f*t2)*t2);
00713                 velocity += -profileAcceleration*t2;
00714                 position += static_cast<double>(velocity*t3);
00715                 position += static_cast<double>((velocity+profileDeceleration*0.5f*t4)*t4);
00716                 velocity += profileDeceleration*t4;
00717                 acceleration = 0.0f;
00718             }
00719         }
00720     }
00721 }