// Function: dumpMemory
// Description: Outputs the contents of the YESS little-endian memory
// WORDSPERLINE four-byte words per line. A * is displayed
// at the end of a line if each line in memory after that
// up to the next ine displayed is identical to the * line.
// Params: none
// Returns: none
// Modifies: none
void dumpMemory()
{
int address = 0;
int prevLine[WORDSPERLINE];
int currLine[WORDSPERLINE];
int star = 0;
buildLine(prevLine, address);
dumpLine(prevLine, address);
for (address=WORDSPERLINE; address < MEMSIZE; address+=WORDSPERLINE)
{
buildLine(currLine, address);
if (isEqual(prevLine, currLine))
{
if (!star)
{
printf("*\n");
star = 1;
}
} else
{
printf("\n");
dumpLine(currLine, address);
star = 0;
}
copy(prevLine, currLine);
}
printf("\n");
}
void buildLine(int point, double *coordsX, double *coordsY, int coords_n, int *lineAssign, int *n, double *vals, int *endIndex) {
int buildRadius = 50; // Adjacent indexed points that are closer than X mm square are added to the current line
(*n)++; // Increment number of points in this line
lineAssign[point] = 1; // Mark point as added
//printf("Building ... ");
if (point < endIndex[0]) { // Update endpoints based on point index - sensor sweeps in one direction so points are ordered
endIndex[0] = point;
}
if (point > endIndex[1]) {
endIndex[1] = point;
}
vals[0] += coordsX[point];
vals[1] += coordsY[point];
vals[2] += coordsX[point] * coordsX[point];
vals[3] += coordsX[point] * coordsY[point];
//printf("%d %lf %lf\n", point, coordsX[point], coordsY[point]);
if (point < 1 || point >= coords_n - 1) return; // Return if reached the start or end of sensor's frame
// Add next point if within buildRadius (square) and not already added
if ((lineAssign[point+1] == 0) && (fabs(coordsX[point+1] - coordsX[point]) <= buildRadius) && (fabs(coordsY[point+1] - coordsY[point]) <= buildRadius)) {
buildLine(point+1, coordsX, coordsY, coords_n, lineAssign, n, vals, endIndex);
}
// Add previous point if within buildRadius (square) and not already added
if ((lineAssign[point-1] == 0) && (fabs(coordsX[point-1] - coordsX[point]) <= buildRadius) && (fabs(coordsY[point-1] - coordsY[point]) <= buildRadius)) {
buildLine(point-1, coordsX, coordsY, coords_n, lineAssign, n, vals, endIndex);
}
}
/*!
Build a vpMbtDistanceLine thanks to two points corresponding to the extremities.
\param _p1 : The first extremity.
\param _p2 : The second extremity.
*/
void
vpMbtDistanceLine::buildFrom(vpPoint &_p1, vpPoint &_p2)
{
line = new vpLine ;
poly.setNbPoint(2);
poly.addPoint(0, _p1);
poly.addPoint(1, _p2);
p1 = &poly.p[0];
p2 = &poly.p[1];
vpColVector V1(3);
vpColVector V2(3);
vpColVector V3(3);
vpColVector V4(3);
V1[0] = p1->get_oX();
V1[1] = p1->get_oY();
V1[2] = p1->get_oZ();
V2[0] = p2->get_oX();
V2[1] = p2->get_oY();
V2[2] = p2->get_oZ();
//if((V1-V2).sumSquare()!=0)
if(std::fabs((V1-V2).sumSquare()) > std::numeric_limits<double>::epsilon())
{
{
V3[0]=double(rand()%1000)/100;
V3[1]=double(rand()%1000)/100;
V3[2]=double(rand()%1000)/100;
vpColVector v_tmp1,v_tmp2;
v_tmp1 = V2-V1;
v_tmp2 = V3-V1;
V4=vpColVector::cross(v_tmp1,v_tmp2);
}
vpPoint P3;
P3.setWorldCoordinates(V3[0],V3[1],V3[2]);
vpPoint P4;
P4.setWorldCoordinates(V4[0],V4[1],V4[2]);
buildLine(*p1,*p2, P3,P4, *line) ;
}
else
{
vpPoint P3;
P3.setWorldCoordinates(V1[0],V1[1],V1[2]);
vpPoint P4;
P4.setWorldCoordinates(V2[0],V2[1],V2[2]);
buildLine(*p1,*p2,P3,P4,*line) ;
}
}
int analyze(int startPoint, double *coordsX, double *coordsY, int coords_n, int *lineAssignPtr, int *n, double *l, double *a, double *b, double *endPoints) {
int minPointCount = 50; // Minimum number of points to define a valid wall line (avoids detecting obstacles as walls)
int endIndex[2] = {700, -1}; // Store the indexes of the start and end points of the built line
if (startPoint == 0) return 1;
double vals[4] = {0};
*n = 0;
buildLine(startPoint, coordsX, coordsY, coords_n, lineAssignPtr, n, vals, endIndex);
*b = ((*n * vals[3]) - (vals[0] * vals[1])) / ((*n * vals[2]) - (vals[0] * vals[0]));
*a = (vals[1] - (*b * vals[0])) / *n;
// Check validity, is it a reasonable wall? Must be less than 5 meters from sensor and less than 90 degrees off in slope
int check = !isnan(*a) && !isnan(*b) && (*a != 0.0) && (*b != 0.0) && (*n > 5) && (fabs(*a) < 5000.0) && (fabs(*b) < 10);
if (!check) return 1; // Return error if not good line
double bottom = (pow(*b, 2) + 1);
double X1 = ((coordsY[endIndex[0]]* (*b)) + coordsX[endIndex[0]] - (*a * (*b))) / bottom;
double Y1 = ((*b * ((coordsY[endIndex[0]] * (*b)) + coordsX[endIndex[0]])) + (*a)) / bottom;
double X2 = ((coordsY[endIndex[1]]* (*b)) + coordsX[endIndex[1]] - (*a * (*b))) / bottom;
double Y2 = ((*b * ((coordsY[endIndex[1]] * (*b)) + coordsX[endIndex[1]])) + (*a)) / bottom;
endPoints[0] = X1;
endPoints[1] = X2;
*l = sqrt(pow(X2 - X1, 2) + pow(Y2 - Y1, 2));
return 0;
}
void DebugDrawer::drawLine(
const Ogre::Vector3& start,
const Ogre::Vector3& end,
const Ogre::ColourValue& colour)
{
buildLine(start, end, colour);
}
void PolylineStyleBuilder<V>::addMesh(const Line& _line, const Parameters& _params) {
m_builder.cap = _params.fill.cap;
m_builder.join = _params.fill.join;
m_builder.miterLimit = _params.fill.miterLimit;
m_builder.keepTileEdges = _params.keepTileEdges;
if (_params.lineOn) { buildLine(_line, _params.fill, m_meshData[0]); }
if (!_params.outlineOn) { return; }
if (!_params.lineOn ||
_params.stroke.cap != _params.fill.cap ||
_params.stroke.join != _params.fill.join ||
_params.stroke.miterLimit != _params.fill.miterLimit) {
// need to re-triangulate with different cap and/or join
m_builder.cap = _params.stroke.cap;
m_builder.join = _params.stroke.join;
m_builder.miterLimit = _params.stroke.miterLimit;
buildLine(_line, _params.stroke, m_meshData[1]);
} else {
auto& fill = m_meshData[0];
auto& stroke = m_meshData[1];
// reuse indices from original line, overriding color and width
size_t nIndices = fill.offsets.back().first;
size_t nVertices = fill.offsets.back().second;
stroke.offsets.emplace_back(nIndices, nVertices);
auto indicesIt = fill.indices.end() - nIndices;
stroke.indices.insert(stroke.indices.end(),
indicesIt,
fill.indices.end());
auto vertexIt = fill.vertices.end() - nVertices;
glm::vec2 width = _params.stroke.width;
GLuint abgr = _params.stroke.color;
short order = _params.stroke.height[1];
for (; vertexIt != fill.vertices.end(); ++vertexIt) {
stroke.vertices.emplace_back(*vertexIt, order, width, abgr);
}
}
}
void Style::addData(TileData& _data, MapTile& _tile, const MapProjection& _mapProjection) {
onBeginBuildTile(_tile);
std::shared_ptr<VboMesh> mesh(newMesh());
for (auto& layer : _data.layers) {
// Skip any layers that this style doesn't have a rule for
auto it = m_layers.begin();
while (it != m_layers.end() && it->first != layer.name) { ++it; }
if (it == m_layers.end()) { continue; }
// Loop over all features
for (auto& feature : layer.features) {
/*
* TODO: do filter evaluation for each feature for sublayer!
* construct a unique ID for a the set of filters matched
* use this ID pass to the style's parseStyleParams method to construct styleParam cache
* NOTE: for the time being use layerName as ID for cache
*/
feature.props.numericProps["zoom"] = _tile.getID().z;
switch (feature.geometryType) {
case GeometryType::POINTS:
// Build points
for (auto& point : feature.points) {
buildPoint(point, parseStyleParams(it->first, it->second), feature.props, *mesh);
}
break;
case GeometryType::LINES:
// Build lines
for (auto& line : feature.lines) {
buildLine(line, parseStyleParams(it->first, it->second), feature.props, *mesh);
}
break;
case GeometryType::POLYGONS:
// Build polygons
for (auto& polygon : feature.polygons) {
buildPolygon(polygon, parseStyleParams(it->first, it->second), feature.props, *mesh);
}
break;
default:
break;
}
}
}
onEndBuildTile(_tile, mesh);
if (mesh->numVertices() == 0) {
mesh.reset();
} else {
mesh->compileVertexBuffer();
_tile.addGeometry(*this, mesh);
}
}
void updateStats(telemetry::OpenConfigData *data)
{
// Should we even process this element sample ?
if (data->kv_size() < 2) {
return;
}
// Get the timestamp
uint64_t ts = data->timestamp();
const telemetry::KeyValue &kv = data->kv(0);
if (kv.key() == "__timestamp__") {
ts = kv.uint_value();
}
// Get the interface name
const telemetry::KeyValue &kv1 = data->kv(1);
if (kv1.key() != "__prefix__") {
return;
}
// Find the interface name
std::string port_name = kv1.str_value();
std::string ifname;
size_t pos = port_name.find("=", 0);
if (pos != std::string::npos) {
// Found matching "="
ifname = port_name.substr(pos+1);
size_t pos = ifname.find("]", 0);
if (pos != std::string::npos) {
ifname.erase(pos);
}
} else {
ifname = "XXXX";
}
// Build a line
std::string resource("interface");
std::string sysname = "sysname=" + data->system_id();
std::string name("ifname=");
buildLine(resource, sysname, name + ifname, ts);
}
void helper(vector<string> &words, int start, int L, vector<string> &result)
{
int spaceCount = 0;
string tmp = "";
tmp += words[0];
for(int i = 0; i<words.size() - 1; ++i)
{
if(tmp.size() + words[i+1].size() >= L)
{
buildLine(tmp, spaceCount, L);
result.push_back(tmp);
tmp = words[i+1];
spaceCount = 0;
continue;
}
else
{
tmp = tmp + " " + words[i+1];
spaceCount++;
}
}
padSpace(tmp, L);
result.push_back(tmp);
}
void Style::buildFeature(Tile& _tile, const Feature& _feat, const DrawRule& _rule) const {
if (!checkRule(_rule)) { return; }
bool visible;
if (_rule.get(StyleParamKey::visible, visible) && !visible) {
return;
}
auto& mesh = _tile.getMesh(*this);
if (!mesh) {
mesh.reset(newMesh());
}
switch (_feat.geometryType) {
case GeometryType::points:
for (auto& point : _feat.points) {
buildPoint(point, _rule, _feat.props, *mesh, _tile);
}
break;
case GeometryType::lines:
for (auto& line : _feat.lines) {
buildLine(line, _rule, _feat.props, *mesh, _tile);
}
break;
case GeometryType::polygons:
for (auto& polygon : _feat.polygons) {
buildPolygon(polygon, _rule, _feat.props, *mesh, _tile);
}
break;
default:
break;
}
}
/*
* growSensor: this functions main purpose is, in fact, to build the
* potential intersection lines
*/
void growSensor(long range, int sensor, double growthFactor, Line trap[3]){
double theta0,theta1,thetaf1,thetaf2,thetab1,thetab2,temp0,temp1,grow,range_grow;
long front,back,osensor;
Robo_Point front1,front2,back1,back2;
osensor=sensor;
//order the sensor to make the angle calc work
sensor=sensor-15;
//find the angles
theta0=(5*M_PI/16)+sensor*(M_PI/8);
theta1=(7*M_PI/16)+sensor*(M_PI/8);
//increase value by the radius(so we've got a point) Maxwell: why?
range += RADIUS;
//generate the growth factors
grow = RADIUS*growthFactor;
range_grow = RANGE_GROW*RADIUS;
//grow the sensors
front = (long)( range - grow + 0.5);
if(front < 0){
front = 0;
}
back = (long)( range + range_grow + 0.5);
//temporary values. the math works out.
temp0=atan((
(front*sin(M_PI/16.0)+grow)
/(front*cos(M_PI/16.0))));
temp1=atan((
(back*sin(M_PI/16.0)+grow)
/(back*cos(M_PI/16.0))));
//find the final angles
thetaf1=((theta0+M_PI/16)-temp0);
thetaf2=((theta0+M_PI/16)+temp0);
thetab1=((theta0+M_PI/16)-temp1);
thetab2=((theta0+M_PI/16)+temp1);
front1=polar2Cart(thetaf1,front);
front2=polar2Cart(thetaf2,front);
back1=polar2Cart(thetab1,back);
back2=polar2Cart(thetab2,back);
//build lines from the cartesian coordinates
trap[0]=buildLine(front1,front2);
trap[1]=buildLine(front1,back1);
trap[2]=buildLine(front2,back2);
// if(sensor==0){
// sprintf(buffer,"Sensor Y value: %f",trap[0].pointB.y);
// GCM_log2(buffer,GDEBUG,false);
//}
return;
}
// analize of the formula
DWPOINT* MathDraw::preLoadFormula(char* cFSTR, int iLong)
{
int lf=0,inner=0,inner_end=0, prefrmd=0;
int ib=0,ki=0,pwi=0,InrStrC=0;
int non_sens=0;
DWPOINT* preRes;
preRes=NULL; // set in null
if(cFSTR=="") return NULL; // return if null
// if string is number, then buld point for line
if (StringAnalizer::isStrNumeric(cFSTR,iLong)) preRes = buildLine(atof(cFSTR),POINT_COUNT);
// do multiplication
else if (StringAnalizer::isMltpl(cFSTR,iLong)) preRes = buildMltpl(cFSTR,iLong,POINT_COUNT);
// do division
else if (StringAnalizer::isDiv(cFSTR,iLong)) preRes = buildDiv(cFSTR,iLong,POINT_COUNT);
// do substraction
else if (StringAnalizer::isMinus(cFSTR,iLong)) preRes = buildMinus(cFSTR,iLong,POINT_COUNT);
// do powering
else if (StringAnalizer::isPow(cFSTR,iLong)) preRes = buildPow(cFSTR,iLong,POINT_COUNT);
// do addition
else if (StringAnalizer::isPlus(cFSTR,iLong)) preRes = buildPlus(cFSTR,iLong,POINT_COUNT);
else
{
// if none was returned, continue
for (int i=0; i<iLong; i++)
{
char tmp[100];
char inrbuf[100];
tmp[ib++]=cFSTR[i];
if (tmp[ib-1]=='(') // reached '(' - next data may be func argument of...
{
tmp[ib-1]='\0';
if(non_sens==0)
{
if(tmp[0]=='s' && tmp[1]=='i' && tmp[2]=='n') lf=1; //...sin
if(tmp[0]=='c' && tmp[1]=='o' && tmp[2]=='s') lf=2; //...cos
if(tmp[0]=='t' && tmp[1]=='a' && tmp[2]=='n') lf=3; //...tan
}
inner++;
non_sens++;
ib=0;
}
if (tmp[ib-1]==')') // reached ')'; end of the argument
{
tmp[ib-1]='\0';
inner--;
ib=0;
non_sens--;
inner_end=1;
}
if (!inner && tmp[ib-1]=='x') // func Х()
{
lf=4;
ib=0;
}
if(inner) // if inside "(..)" ...
{
inrbuf[InrStrC++]=cFSTR[i+1]; // ...write-up content
}
if(!inner && inner_end) // exited from "(..)"
{
inrbuf[InrStrC-1]='\0';
// send content recursively
preRes=preLoadFormula(inrbuf, InrStrC);
inner_end=0;
}
}
}
switch (lf) // select correct function
{
case 1:
return buildSin(preRes,POINT_COUNT);
case 2:
return buildCos(preRes,POINT_COUNT);
case 3:
return buildTan(preRes,POINT_COUNT);
case 4:
return buildX(POINT_COUNT);
default:
return preRes; // or return as is
}
}
