void BaseWidget::setPosition(const dReal *position)
{
if(! dBodyIsEnabled(body)) return;
//Update position
int xPos = ABSOLUTE(position[0]);
int yPos = ABSOLUTE(position[1]);
// Update Qt's position
qDebug("Position: %i, %i", xPos, yPos);
move(xPos, yPos);
}
int main (void)
{
char ch;
int hours, minutes, closest;
int time[8], i;
for (;;) {
printf("\nEnter a 24-hour time(xx:xx): ");
scanf("%d:%d", &hours, &minutes);
minutes = hours*60 + minutes;
for (i = 0; i < 8; i++)
time[i] = ABSOLUTE(travel[i].departure - minutes);
printf("Closest departure time is ");
if (hours < 8 | hours >= 21) {
if (time[0] < time[7]) printf("8:00 a.m., arriving at 10:16 a.m.\n");
else printf("9:45 p.m., arriving at 11:58 p.m.\n");
}
if (hours >= 8 && hours < 9) {
if (time[0] < time[1]) printf("8:00 a.m., arriving at 10:16 a.m.\n");
else printf("9:43 a.m., arriving at 11:52 a.m.\n");
}
if (hours >= 9 && hours < 11) {
if (time[1] < time[2]) printf("9:43 a.m., arriving at 11:52 a.m.\n");
else printf("11:19 a.m., arriving at 1:31 p.m.\n");
}
if (hours >= 11 && hours < 12) {
if (time[2] < time[3]) printf("11:19 a.m., arriving at 1:31 p.m.\n");
else printf("12:47 p.m., arriving at 3:00 p.m.\n");
}
if (hours >= 12 && hours < 14) {
if (time[3] < time[4]) printf("12:47 p.m., arriving at 3:00 p.m.\n");
else printf("2:00 p.m., arriving at 4:08 p.m.\n");
}
if (hours >= 14 && hours < 15) {
if (time[4] < time[5]) printf("2:00 p.m., arriving at 4:08 p.m.\n");
else printf("3:45 p.m., arriving at 5:55 p.m.\n");
}
if (hours >= 15 && hours < 19) {
if (time[5] < time[6]) printf("3:45 p.m., arriving at 5:55 p.m.\n");
else printf("7:00 p.m., arriving at 9:20 p.m.\n");
}
if (hours >= 19 && hours < 21) {
if (time[6] < time[7]) printf("7:00 p.m., arriving at 9:20 p.m.\n");
else printf("9:45 p.m., arriving at 11:58 p.m.\n");
}
printf("\nEnter 'q' if you want to quit: ");
while (getchar() != '\n')
;
ch = getchar();
if ( ch == 'q') break;
}
return 0;
}
void SkillSet::trajectory1D(const float dist,
const float startVel,
const float finalVel,
const float frameRate,
const float maxAcc,
const float maxVel,
float& trajAcc,
float& trajTime)
{
if (dist < 0.5 * (startVel + finalVel)*ABSOLUTE(finalVel - startVel) / maxAcc)
{
// Path not feasible given the constraints
trajTime = INFINITY;
return;
}
// Computation when bot can reach max velocity
float accTime = (maxVel - startVel) / maxAcc;
float decTime = (maxVel - finalVel) / maxAcc;
float velTime = (dist / maxVel) - (accTime / 2) * (1 + startVel / maxVel) - (decTime / 2) * (1 + finalVel / maxVel);
// Computation when bot cannot reach max velocity
if (velTime < 0)
{
velTime = 0;
float discr = 2 * (startVel * startVel + finalVel * finalVel) + 4 * maxAcc * dist;
assert(discr >= 0);
float sqrtDiscr = sqrt(discr);
accTime = (-2 * startVel + sqrtDiscr) / (2 * maxAcc);
decTime = (-2 * finalVel + sqrtDiscr) / (2 * maxAcc);
assert(accTime >= 0 || decTime >= 0); // Both acceleration and deceleration times cannot be negative
if (accTime < 0)
{
accTime = 0;
decTime = 2 * dist / (startVel + finalVel);
}
if (decTime < 0)
{
decTime = 0;
accTime = 2 * dist / (startVel + finalVel);
}
}
} // trajectory1D
T MedianAbsoluteDeviation(T *xd, int nd, T& M, bool save_flag=true)
throw(Exception)
{
int i;
T mad, *save;
if(!xd || nd < 2) {
Exception e("Invalid input");
GPSTK_THROW(e);
}
// store data in a temporary array
if(save_flag) {
save = new T[nd];
if(!save) {
Exception e("Could not allocate temporary array");
GPSTK_THROW(e);
}
for(i=0; i<nd; i++) save[i]=xd[i];
}
// get the median (don't care if xd gets sorted...)
M = Median(xd, nd, false);
// compute xd=abs(xd-M)
for(i=0; i<nd; i++) xd[i] = ABSOLUTE(xd[i]-M);
// sort xd in ascending order
QSort(xd, nd);
// find median and normalize to get mad
mad = Median(xd, nd, false) / T(RobustTuningE);
// restore original data from temporary
if(save_flag) {
for(i=0; i<nd; i++) xd[i]=save[i];
delete[] save;
}
return mad;
} // end MedianAbsoluteDeviation
T MEstimate(const T *xd, int nd, const T& M, const T& MAD, T *w=NULL)
throw(Exception)
{
try {
T tv, m, mold, sum, sumw, *wt, weight, *t;
T tol=0.000001;
int i, n, N=10; // N is the max number of iterations
if(!xd || nd < 2) {
Exception e("Invalid input");
GPSTK_THROW(e);
}
tv = T(RobustTuningT)*MAD;
n = 0;
m = M;
do {
mold = m;
n++;
sum = sumw = T();
for(i=0; i<nd; i++) {
if(w) wt=&w[i];
else wt=&weight;
*wt = T(1);
if(xd[i]<m-tv) *wt = -tv/(xd[i]-m);
else if(xd[i]>m+tv) *wt = tv/(xd[i]-m);
sumw += (*wt);
sum += (*wt)*xd[i];
}
m = sum / sumw;
} while(T(ABSOLUTE((m-mold)/m)) > tol && n < N);
return m;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
} // end MEstimate
