Motion.cpp

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