bool UProbPoly::paintRobot(UImage * img, int refX, int refY, double cellSize)
{
bool res = (img != NULL);
UPosition mp;
CvPoint mpt[4];
CvScalar rgb = CV_RGB(255, 128, 128);
UPose pose = poseOrg - poseNow;
//
if (res)
{ // The robot is 65 cm long and 45 cm wheel distance
mp = pose.getPoseToMap(UPosition::position(0.0, 0.225, 0.0)); // left wheel
mpt[0].x = refX + roundi(mp.x / cellSize);
mpt[0].y = refY - roundi(mp.y / cellSize);
mp = pose.getPoseToMap(UPosition::position(0.0, -0.225, 0.0)); // right wheel
mpt[1].x = refX + roundi(mp.x / cellSize);
mpt[1].y = refY - roundi(mp.y / cellSize);
mp = pose.getPoseToMap(UPosition::position(0.65, 0.0, 0.0)); // front wheel
mpt[2].x = refX + roundi(mp.x / cellSize);
mpt[2].y = refY - roundi(mp.y / cellSize);
mp = pose.getPoseToMap(UPosition::position(-0.25, 0.0, 0.0)); // back edge
mpt[3].x = refX + roundi(mp.x / cellSize);
mpt[3].y = refY - roundi(mp.y / cellSize);
// paint robot triangle (arrow)
cvLine(img->cvArr(), mpt[0], mpt[1], rgb, 1, 8, 0);
cvLine(img->cvArr(), mpt[1], mpt[2], rgb, 1, 8, 0);
cvLine(img->cvArr(), mpt[2], mpt[0], rgb, 1, 8, 0);
cvLine(img->cvArr(), mpt[2], mpt[3], rgb, 1, 8, 0);
}
//
return res;
}
// Convert percent pan to midi pan (with no scale conversion)
uint8_t ConvertPercentPanValToStdMidiVal(double percent)
{
uint8_t midiPan = roundi(percent * 126.0);
if (midiPan != 0) {
midiPan++;
}
return midiPan;
}
UXYoffset UCamRad::getRadialU2DOffsetInt(int x, int y)
{ // Gets radial error offset according to
// calibration information in the camera.
// Follows the guidelines in Image Analysis,
// Vision and Computer Graphics DTU/IMM publication
// by Jens Michael Carstensen Lyngby 2001, page 75.
//
UXYoffset oxy; // offset x and y
float dx, dy; // distance from center
//
getRadialU2DOffset(x, y, &dx, &dy);
//
oxy.dx = roundi(dx * resultDecimalFactor);
oxy.dy = roundi(dy * resultDecimalFactor);
//
return oxy;
}
bool UManArc::getSMRCLcmd(char * buf, int bufCnt, double maxDist)
{
int i, n, m;
double d, dd, da;
char * p1;
bool result = true;
double v;
double dist;
//
// turn is not allowed at zero speed
if (fabs(vel) < 0.2)
v = 0.25;
else
v = vel;
//
if (radius < 3.0)
{
dist = mind(maxDist, getDistance());
snprintf(buf, bufCnt, "turnr %.3f %.3f @v%.2f @a%.3f :($drivendist > %.2f)",
maxd(radius, 0.15), // use at least 15cm turn radius
angle * 180.0 / M_PI, v,
maxd(getMinAcc(), fabs(acc)), dist);
n = strlen(buf);
}
else
{ // it is too unsafe to turn and finish on the implicit angle criteria
// divide into 30 cm steps
m = maxi(1, roundi(fabs(angle) / 0.05)); // about 3 deg steps
d = radius * angle;
dd = d / double(m);
da = angle / double(m);
n = 0;
p1 = buf;
dist = mind(maxDist, getDistance());
for (i = 0; i < m; i++)
{ // one command takes a little less than 60 characters.
// so there need to be at least 60 characters left in buffer
result = ((bufCnt - n) > 60);
if (not result)
{ // no more space for next command
printf("*** UManArc::getSMRCLcmd: no space left (%d left) in buffer (size %d) for next drive command\n", bufCnt, n);
break;
}
// space for next command line
snprintf(p1, bufCnt - n, "turnr %.3f %.3f \"rad\" @v%.2f @a%.3f :($drivendist > %.2f)\n",
radius, da, v, maxd(getMinAcc(), fabs(acc)), dd);
n += strlen(p1);
p1 = &buf[n];
dist -= dd;
if (dist < 0.0)
break;
}
// remove laset '\n'
if (n > 0)
buf[n-1] = '\0';
}
return (n < bufCnt);
}
bool UHokuyo::changeMode(int scanangle, double resolution)
{
const double baseRes = 360.0 / 1024.0;
//
angleResolution = roundi(resolution / baseRes) * baseRes;
modeAngleScan = mini(270,scanangle);
printf("New width=%d res=%g\n", modeAngleScan, angleResolution);
return true;
}
void generate_noise (Vector<int8_t> & noise, float sigma)
{
const float quantization_correction = 1 / 6.0f;
sigma = sqrtf(sqr(sigma) + quantization_correction);
for (size_t i = 0, I = noise.size; i < I; ++i) {
noise[i] = roundi(sigma * random_std());
}
}
bool UResPoly::methodCall(const char * name, const char * paramOrder,
char ** strings, const double * pars,
double * value,
UDataBase ** returnStruct,
int * returnStructCnt)
{
bool result = true;
UPolyItem * pi;
int i;
UPosition pos;
UVariable * var;
bool isOK;
// evaluate standard functions
if ((strcasecmp(name, "addPoint") == 0) and (strcmp(paramOrder, "sdd") == 0))
{
pi = add(strings[0], pars[0], pars[1]);
// return result - if a location is provided
if (value != NULL)
*value = (pi != NULL);
// it is good praktice to set count of returned structures to 0
if (returnStructCnt != NULL)
*returnStructCnt = 0;
}
else if ((strcasecmp(name, "getPoint") == 0) and (strcmp(paramOrder, "sd") == 0))
{
isOK = false;
pi = getItem(strings[0]);
if (pi != NULL)
{
i = roundi(pars[0]);
if (i < pi->getPointsCnt() and i >= 0)
{
isOK = true;
pos = pi->getPoint(i);
}
}
if (not isOK)
pos.clear();
// return result - if a location is provided
if (value != NULL)
*value = isOK;
// it is goot praktice to set count of returned structures to 0
if (returnStructCnt != NULL)
{
if (returnStruct[0]->isA("var"))
{
var = (UVariable*)returnStruct[0];
var->set3D(&pos);
*returnStructCnt = 1;
}
}
}
else
// call name is unknown
result = false;
return result;
}
int UHokuyo::receiveFromDevice(char * start,
int maxLng,
double timeoutSec)
{ // poll wait time
int pollTime = roundi(timeoutSec * 1000.0); // ms
int n, m = maxLng;
struct pollfd sickstruct;
//
sickstruct.fd = hfd;
sickstruct.events = POLLIN;
bool timeout;
int result = 0;
char * bp = start;
//
if (verbose and not laslog.isOpen())
laslog.openLog();
else if (laslog.isOpen() and not verbose)
laslog.closeLog();
// receive a number of data that should include a header
/* get new data */
timeout = false;
while ((m > 0) and not timeout)
{ // Wait for data in up to 100 ms
if (poll(&sickstruct, 1, pollTime) != POLLIN)
// timeout or other error - return
timeout = true;
else
{ /* read up to a full message */
n = read(hfd, bp, m);
if (n == -1)
{ // error
perror("Error in read from serial line");
break;
}
else
{ // allow log
if (result == 0)
{ // first data - timestamp
dataRxTime.now();
}
if ((n > 0) and laslog.isLogOpen())
toLog(bp, n, "<-", dataRxTime);
//
bp[n] = '\0';
m -= n;
result += n;
if (strstr(bp, "\n\n") != NULL)
// a full message is received.
break;
bp = &start[result];
}
}
}
//
return result;
}
char * UHokuyo::makeGetDataString(char * cmdStr, const int cmdStrCnt)
{
int clust;
// full scan for device
int first = 44; // 0 is -135 deg (44 approx -120 deg)
int last = 725; // 768 is +135 deg (725 approx +120 deg)
const double baseRes = 360.0/1024.0;
const int MSL = 12; // minimum string buffer length (exclusive terminating zero)
bool result;
//
result = (cmdStr != NULL) and (cmdStrCnt > MSL);
//
clust = roundi(angleResolution / baseRes);
if (modeAngleScan <= 270)
{ // less than full scan requested - get symmetric values
first = 384 - roundi(double(modeAngleScan) / baseRes / 2.0 - 1);
last = 384 + roundi(double(modeAngleScan) / baseRes / 2.0);
}
if (result)
snprintf(cmdStr, cmdStrCnt, "\nG%03d%03d%02d\n", first, last, clust);
//printf("mode=%d, res=%g, sending:%s", modeAngleScan, angleResolution, s);
return cmdStr;
}
void UFuncPar::update6hMax(double horizon)
{
double mn, mi, mx;
UVariable * rot;
int n;
UTime t0, toTime;
double roll1mMax = 0.0, pitch1mMax = 0.0;
int alarm1mCnt = 0;
//
rot = getVarPool()->getGlobalVariable("imu.rot");
if (rot != NULL)
{
t0 = rot->getUpdTime();
n = rot->getMeanMinMax(0, hoz1min, &mn, &mi, &mx);
if (n > 0 )
roll1mMax = mx - mi;
n = rot->getMeanMinMax(1, hoz1min, &mn, &mi, &mx);
if (n > 0)
pitch1mMax = mx - mi;
}
else
toTime.now();
// latest alarm number
alarm1mCnt = vars.alertFlag->getInt(1);
// make sure to have a valid time
if (not t0.valid)
t0.now();
mi = t0 - hoz1min;
if (mi > 60.0)
{ // one minute has passed - save a new 1min value
vars.roll1min->setDouble(roll1mMax, 0, false, &t0);
vars.pitch1min->setDouble(pitch1mMax, 0, false, &t0);
vars.alarmCnt1min->setDouble(alarm1mCnt, 0, false, &t0);
hoz1min = t0;
}
toTime = t0 - (varp.horizon6h->getInt() * 60.0);
n = vars.roll1min->getMeanMinMax(0, toTime, &mn, &mi, &mx);
if (n > 0)
vars.roll6h->setDouble(fmax(roll1mMax, mx), 0, false, &t0);
n = vars.pitch1min->getMeanMinMax(0, toTime, &mn, &mi, &mx);
if (n > 0)
vars.pitch6h->setDouble(fmax(pitch1mMax, mx), 0, false, &t0);
// find minimum alarm count last 6h period
n = vars.alarmCnt1min->getMeanMinMax(0, toTime, &mn, &mi, &mx);
if (n > 0)
vars.alarmCnt6h->setInt(alarm1mCnt - roundi(mi), 0, false, &t0);
}
bool UResLaserIfObst::methodCall(const char * name, const char * paramOrder,
char ** strings, const double * pars,
double * value,
UDataBase ** returnStruct,
int * returnStructCnt)
{
bool result = true;
int i, n, g, nf;
bool getLocked = false;
UObstacleGroup * og;
// evaluate standard functions
if ((strcasecmp(name, "obstacles") == 0) and (strcasecmp(paramOrder, "ddd") == 0))
{ // lock resource or wait until unlocked
ogLock.lock();
n = roundi(pars[0]);
nf = roundi(pars[1]);
getLocked = roundi(pars[2]) != 0;
if (nf != 0)
nf = 1;
if ((returnStructCnt != NULL) and (returnStruct != NULL))
{
if (n > *returnStructCnt - nf)
n = *returnStructCnt - nf;
if (n > getGroupsCnt())
n = getGroupsCnt();
g = getGroupNewest();
// debug
//printf("UResLaserIfObst::methodCall: regular obst (max%d)", n);
// debug end
for (i = 0; i < n; i++)
{
og = &groups[g];
if (getLocked)
og->lock();
returnStruct[i] = og;
g--;
if (g < 0)
g = MAX_OBST_GRPS - 1;
// debug
//printf(" serial %lu has %d.", og->getSerial(), og->getObstsCnt());
// debug end
}
// debug
//printf("\n");
//printf("UResLaserIfObst::methodCall: fixed obstacle cnt is %d, (obst grps = %d)\n", fixeds.getObstsCnt(), n);
// debug end
if (nf > 0)
{ // fixed obstacles too
if (fixeds.getObstsCnt() > 0 and n > 0)
{ // fixed obstacles are available
returnStruct[n] = &fixeds;
n++;
}
}
*returnStructCnt = n;
}
if (value != NULL)
*value = n;
ogLock.unlock();
}
else
result = false;
return result;
}
// Convert a pan value where 0 = left 0.5 = center and 1 = right to
// 0.1% units where -50% = left 0 = center 50% = right (shared by DLS and SF2)
long ConvertPercentPanTo10thPercentUnits(double percentPan)
{
return roundi(percentPan * 1000) - 500;
}
bool UCamRad::removeRadialErrorPixels(UPixel ps[],
UPixel pd[],
unsigned int height,
unsigned int width,
float pixSize)
{ //
int result = true;
UXYoffset oxy; // offset x and y
UXYoffset * oxyh; // line with ofset values
int decimalFactor = roundi(resultDecimalFactor * pixSize);
int w, h, i;
int rhx, rhy;
double dhx, dhy;
int ix, iy; // pixel offset in Intensity resolution
int r1, r2, c1, c2;
int i1, i2; // intensity (decimal part) from pix 1 and 2
UPixel * pss; // source pixel pointer intensity (Y)
UPixel * pdd; // destination pixel pointer Intensity
UPixel p1, p2, p3, p4, pt, pb, pr; // pixel values
UPixel gray(128,128,128);
bool outside, outsideByOne;
//
// head point in actual resolution
dhx = par.getHx();
dhy = par.getHy();
rhx = int(dhx);
rhy = int(dhx);
// correct all lines
for (h = 0; h < int(height); h++)
{ // get destination
pdd = &pd[h * width];
// get line in offset table
i = absi(int(pixSize * (double(h)- dhy)));
// get pointer to first line of offsets
oxyh = radialOffset[i];
for (w = 0; w < int(width); w++)
{ // get index to offset value on this line
i = absi(int(pixSize * (double(w) - dhx)));
// get offset for this pixel
oxy = oxyh[i];
// corrext for right quadrant - matrix is for lower-right
if (w < rhx)
oxy.dx = -oxy.dx;
if (h < rhy)
oxy.dy = -oxy.dy;
// if not exact right find 4 pixels to interpolate
if ((oxy.dx != 0) or (oxy.dy != 0))
{ // get top left pixel offset
ix = oxy.dx / decimalFactor;
if (oxy.dx < 0) ix--;
iy = oxy.dy / decimalFactor;
if (oxy.dy < 0) iy--;
// limit to edge of image (reuse border pixels)
r1 = h + iy;
r2 = r1 + 1;
c1 = w + ix;
c2 = c1 + 1;
outside = (r1 < 0) or (r2 >= int(height)) or
(c1 < 0) or (c2 >= int(width));
if (outside)
// may be outside by just one pixel
outsideByOne = (((r2 == 0) or (r2 == int(height))) and
(c2 > 0) and (c2 < int(width))) or
(((c2 == 0) or (c2 == int(width))) and
(r2 > 0) and (r2 < int(height)));
else
outsideByOne = false;
//
if (outside and not outsideByOne)
// both outside - use gray
*pdd = gray;
else
{ // not (completely) outside
// get pixels
i = r1 * width + c1;
pss = &ps[i];
p1 = pss[0];
p2 = pss[1];
p3 = pss[width];
p4 = pss[width + 1];
if (outsideByOne)
{ // just outside by one, so adjust index to inside
if (r2 == 0)
{ // top row missing
p1 = p3;
p2 = p4;
}
if (r2 == int(height))
{ // bottom row missing
p3 = p1;
p4 = p2;
}
if (c2 == 0)
{ // left column missing
p1 = p2;
p3 = p4;
}
if (c2 == int(width))
{ // right column missing
p2 = p1;
p4 = p3;
}
}
outside = false;
}
if (not outside)
{ // get index to top-left of pixels in question
// get 4 pixels in question
// get left and right share
i2 = oxy.dx - ix * decimalFactor; // part of pixel in position (ix, iy)
i1 = decimalFactor - i2; // part of (ix+1, iy+1)
// average for top set of pixels
pt.y = (unsigned char)((int(p1.y) * i1 + int(p2.y) * i2)/decimalFactor);
pt.u = (unsigned char)((int(p1.u) * i1 + int(p2.u) * i2)/decimalFactor);
pt.v = (unsigned char)((int(p1.v) * i1 + int(p2.v) * i2)/decimalFactor);
// average for bottom set of pixels
pb.y = (unsigned char)((int(p3.y) * i1 + int(p4.y) * i2)/decimalFactor);
pb.u = (unsigned char)((int(p3.u) * i1 + int(p4.u) * i2)/decimalFactor);
pb.v = (unsigned char)((int(p3.v) * i1 + int(p4.v) * i2)/decimalFactor);
// get shares of top and bottom
i2 = oxy.dy - iy * decimalFactor; // part of pixel in position (ix, iy)
i1 = decimalFactor - i2; // part of (ix, iy+1)
// find result pixel intensity
pr.y = (unsigned char)((int(pt.y) * i1 + int(pb.y) * i2)/decimalFactor);
pr.u = (unsigned char)((int(pt.u) * i1 + int(pb.u) * i2)/decimalFactor);
pr.v = (unsigned char)((int(pt.v) * i1 + int(pb.v) * i2)/decimalFactor);
// implement
*pdd = pr;
}
}
else
{ // no change, so set destination to source
pss = &ps[h * width];
*pdd = pss[w];
}
pdd++;
}
}
return result;
}
int RhoToBin(const float rho) {
return roundi((rho + diaglen/2) * *gvRhoBins / diaglen);
}
int PhiToBin(const float phi) {
return roundi((phi + diaglen/2) * *gvPhiBins / diaglen);
}
bool UManArc::getSMRCLcmd2(char * buf, int bufCnt,
UPoseV * startPose,
bool * first, double firstDistance,
int * lineCnt, int lineCntMax,
double * distSum, double distSumMax,
UTime * t, FILE * logprim)
{
int i;
UPoseV pv, pv2;
double v, d, dist;
int n, m, np;
double segmentSize = 2.0 * firstDistance; // [m]
UManArc ma;
char * p1;
double h;
bool result;
//double dmax;
double breakAt = distSumMax * 0.5;
double ang, ang2, endAng, sa, ca;
const double useTurnRradius = 6.0;
//
if (fabs(vel) < 0.15)
v = 0.15;
else
v = vel;
//
//dmax = distSumMax - *distSum;
if (not first or ((*distSum + getDistance()) > firstDistance))
{ // get command for arc (not skipped)
p1 = buf;
n = 0;
if (radius < useTurnRradius)
{ // use turnr command type
d = getDistance();
np = 4;
if (*distSum < breakAt and d + *distSum >= breakAt)
{
dist = breakAt - *distSum;
// part of turn only, maintaining sign
ang = angle * dist / d;
endAng = startPose->h + ang;
sa = sin(endAng);
ca = cos(endAng);
snprintf(p1, bufCnt - n, "turnr %.3f %.3f \"rad\" @v%.2f : "
"(abs(%.4f - sin($odoth)) + abs(%.4f - cos($odoth)) < 0.1)\n\t\n",
radius, ang, v, sa, ca);
(*distSum) += dist;
(*lineCnt)++;
n += strlen(p1);
p1 = &buf[n];
if (logprim != NULL)
{
ma.setTurnRadius(radius);
ma.setTurnAngle(ang);
pv = ma.getEndPoseV(*startPose);
fprintf(logprim, "%lu.%06lu %d %.3f %.3f %.5f %.2f %.3f %.3f %.5f %.2f\n",
t->getSec(), t->getMicrosec(),
np,
startPose->x, startPose->y, startPose->h, startPose->vel,
pv.x, pv.y, pv.h, v);
np += 100;
}
}
else
{
ang = 0.0;
dist = 0.0;
pv = *startPose;
}
// remaining distance
dist = fmin(distSumMax - *distSum, d - dist);
// remaining part of turn, maintaining sign
ang2 = angle * dist / d;
// end angle to improve stop-condition
endAng = startPose->h + ang + ang2;
sa = sin(endAng);
ca = cos(endAng);
snprintf(p1, bufCnt - n, "turnr %.3f %.3f \"rad\" @v%.2f : "
"(abs(%.4f - sin($odoth)) + abs(%.4f - cos($odoth)) < 0.1)",
radius, ang2, v, sa, ca);
n = strlen(p1);
if (logprim != NULL)
{
ma.setTurnRadius(radius);
ma.setTurnAngle(ang2);
pv2 = ma.getEndPoseV(pv);
fprintf(logprim, "%lu.%06lu %d %.3f %.3f %.5f %.2f %.3f %.3f %.5f %.2f\n",
t->getSec(), t->getMicrosec(),
np,
pv.x, pv.y, pv.h, pv.vel,
pv2.x, pv2.y, pv2.h, v);
}
pv = getEndPoseV(*startPose);
(*distSum) += dist;
*startPose = pv;
(*lineCnt)++;
*first = false;
}
else
{ // segment arc into tangent lines
m = roundi(getDistance() / segmentSize);
ma.setTurnRadius(radius);
ma.setTurnAngle(angle / double(m));
pv2 = *startPose;
pv = ma.getEndPoseV(*startPose);
// debug
// printf("UManArc::getSMRCLcmd2 ma.angle=%g ma.radius=%g",
// ma.angle, ma.radius);
// // debug end
np = 5;
for (i = 0; i < m; i++)
{
h = pv.h * 180.0 / M_PI;
while (h < 0.0)
h += 360.0;
snprintf(p1, bufCnt - n,
"driveon %.3f %.3f %.2f @v%.2f @a%.2f :($targetdist < 0.0) \n",
pv.x, pv.y, h, v, fabs(acc));
n += strlen(p1);
p1 = &buf[n];
(*lineCnt)++;
if (logprim != NULL)
{
fprintf(logprim, "%lu.%06lu %d %.3f %.3f %.5f %.2f %.3f %.3f %.5f %.2f\n",
t->getSec(), t->getMicrosec(),
np,
pv2.x, pv2.y, pv2.h, pv2.vel,
pv.x, pv.y, pv.h, v);
pv2 = pv;
}
d = ma.getDistance();
if ((*distSum < breakAt) and
(d + *distSum) >= breakAt)
{ // mark place for event trigger
snprintf(p1, bufCnt - n, "\t\n");
n += strlen(p1);
p1 = &buf[n];
np += 100;
}
(*distSum) += d;
*startPose = pv;
if (*lineCnt >= lineCntMax)
break;
if (*distSum >= distSumMax)
break;
// advance to next end position
pv = ma.getEndPoseV(pv);
}
// remove last '\n'
if (n > 0)
buf[n-1] = '\0';
*first = false;
}
}
else
{ // too small manoeuvre, or not suited fas first command,
// so just update startpose and distSum
pv = getEndPoseV(*startPose);
(*distSum) += getDistance();
*startPose = pv;
n = 0;
}
result = (n < bufCnt);
return result;
}
int ThetaToBin(const float theta) {
return roundi((M_PI+theta) * *gvThetaBins / M_PI) % *gvThetaBins;
}
bool UResPoly::methodCallV(const char * name, const char * paramOrder,
UVariable * params[],
UDataBase ** returnStruct,
int * returnStructCnt)
{
bool result = true;
UPolyItem * pi;
UPolygon * poly;
UDataBase * db;
int i;
UPosition pos;
UVariable buffer;
UVariable * returnVar = NULL;
bool isOK;
UPose po1, po2;
// evaluate standard functions
if (returnStruct[0]->isA("var"))
{
returnVar = (UVariable*)returnStruct[0];
*returnStructCnt = 1;
}
else
returnVar = &buffer;
//
// test available methods
if ((strcasecmp(name, "addPoint") == 0) and (strcmp(paramOrder, "sc") == 0))
{
pos = params[1]->get3D();
pi = add(params[0]->getValues(), pos.x, pos.y, pos.z);
isOK = pi != NULL;
returnVar->setBool(isOK);
gotNewData();
}
else if ((strcasecmp(name, "del") == 0) and (strcmp(paramOrder, "s") == 0))
{
isOK = del(params[0]->getValues());
returnVar->setBool(isOK);
gotNewData();
}
else if ((strcasecmp(name, "delPoint") == 0) and (strcmp(paramOrder, "sd") == 0))
{
pi = getItem(params[0]->getValues());
isOK = pi != NULL;
if (isOK)
pi->remove(params[1]->getInt());
returnVar->setBool(isOK);
gotNewData();
}
else if ((strcasecmp(name, "getPoint") == 0) and (strcmp(paramOrder, "sd") == 0))
{
isOK = false;
pi = getItem(params[0]->getValues());
if (pi != NULL)
{ // item is found, so get position
i = roundi(params[1]->getInt());
if (i < pi->getPointsCnt() and i >= 0)
{
isOK = true;
pos = pi->getPoint(i);
}
}
if (not isOK)
pos.clear();
returnVar->set3D(&pos);
}
else if ((strcasecmp(name, "isInside") == 0) and (strcmp(paramOrder, "sc") == 0))
{
isOK = false;
pos = params[1]->get3D();
pi = getItem(params[0]->getValues());
if (pi != NULL)
{ // item is found, test
isOK = pi->isInsideConvex(pos.x, pos.y, 0.0);
}
returnVar->setBool(isOK);
}
else if ((strcasecmp(name, "isInside") == 0) and (strcmp(paramOrder, "scc") == 0))
{
isOK = false;
po1 = params[1]->getPose(); // pose of robot
po2 = params[2]->getPose(); // position relative to robot
po1 = po1 + po2;
pi = getItem(params[0]->getValues());
if (pi != NULL)
{ // item is found, test
isOK = pi->isInsideConvex(po1.x, po1.y, 0.0);
}
returnVar->setBool(isOK);
}
else if ((strcasecmp(name, "defined") == 0) and (strcmp(paramOrder, "s") == 0))
{
pi = getItem(params[0]->getValues());
isOK = (pi != NULL);
returnVar->setBool(isOK);
}
else if ((strcasecmp(name, "setRefCoord") == 0) and (strcmp(paramOrder, "sd") == 0))
{
pi = getItem(params[0]->getValues());
isOK = pi != NULL;
if (isOK)
pi->cooSys = params[1]->getInt();
returnVar->setBool(isOK);
}
else if ((strcasecmp(name, "setOpen") == 0) and (strcmp(paramOrder, "s") == 0))
{
pi = getItem(params[0]->getValues());
isOK = pi != NULL;
if (isOK)
pi->setAsPolyline();
returnVar->setBool(isOK);
gotNewData();
}
else if ((strcasecmp(name, "setClosed") == 0) and (strcmp(paramOrder, "s") == 0))
{
pi = getItem(params[0]->getValues());
isOK = pi != NULL;
if (isOK)
pi->setAsPolygon();
returnVar->setBool(isOK);
gotNewData();
}
else if ((strcasecmp(name, "setPolygon") == 0) and
(strcmp(paramOrder, "sc") == 0 or
strcmp(paramOrder, "scd") == 0))
{
isOK = false;
pi = getItem(params[0]->getValues());
db = params[1];
if (db->isAlsoA("polygon"))
poly = (UPolygon*) db;
else
poly = NULL;
isOK = poly != NULL;
if (isOK and pi == NULL)
{ // item is found, so get position
pi = add(params[0]->getValues());
isOK = pi != NULL;
}
if (isOK)
{
isOK = poly->copyTo(pi);
if (strlen(paramOrder) >= 3)
// get also coordinate system reference
pi->cooSys = params[2]->getInt();
pi->setUpdated();
}
returnVar->setBool(isOK);
gotNewData();
}
else
// call name is unknown
result = false;
// set result if just a boolean
if (result)
{ // set return struct value to isOK
if (returnStruct != NULL and returnStructCnt != NULL)
*returnStructCnt = 1;
}
return result;
}
// Takes a percentage amplitude value - that is one using a -20*log10(percent) scale for db attenuation
// and converts it to a standard midi value that uses -40*log10(x/127) for db attenuation
uint8_t ConvertPercentAmpToStdMidiVal(double percent)
{
return roundi(127.0 * sqrt(percent));
}
