void SliderComponent::onValueChanged()
{
// update suffix textcache
if(mFont)
{
std::stringstream ss;
ss << std::fixed;
ss.precision(0);
ss << mValue;
ss << mSuffix;
const std::string val = ss.str();
ss.str("");
ss.clear();
ss << std::fixed;
ss.precision(0);
ss << mMax;
ss << mSuffix;
const std::string max = ss.str();
Eigen::Vector2f textSize = mFont->sizeText(max);
mValueCache = std::shared_ptr<TextCache>(mFont->buildTextCache(val, mSize.x() - textSize.x(), (mSize.y() - textSize.y()) / 2, 0x777777FF));
mValueCache->metrics.size[0] = textSize.x(); // fudge the width
}
// update knob position/size
mKnob.setResize(0, mSize.y() * 0.7f);
float lineLength = mSize.x() - mKnob.getSize().x() - (mValueCache ? mValueCache->metrics.size.x() + 4 : 0);
mKnob.setPosition(((mValue + mMin) / mMax) * lineLength + mKnob.getSize().x()/2, mSize.y() / 2);
}
// Returns a list of points on the convex hull in counter-clockwise order.
// Note: the last point in the returned list is the same as the first one.
std::vector<int> convex_hull(std::vector<int> idxs, std::function<Eigen::Vector2f(int)> & getPoint){
int n = idxs.size(), k = 0;
std::vector<int> hull(2*n);
// Sort points lexicographically
sort(idxs.begin(), idxs.end(), [&getPoint](int a_, int b_){
Eigen::Vector2f a = getPoint(a_);
Eigen::Vector2f b = getPoint(b_);
return a.x() < b.x() || (a.x() == b.x() && a.y() < b.y());
});
// Build lower hull
for (int i = 0; i < n; ++i) {
while (k >= 2 && cross(getPoint(hull[k-2]), getPoint(hull[k-1]), getPoint(idxs[i])) <= 0) k--;
hull[k++] = idxs[i];
}
// Build upper hull
for (int i = n-2, t = k+1; i >= 0; i--) {
while (k >= t && cross(getPoint(hull[k-2]), getPoint(hull[k-1]), getPoint(idxs[i])) <= 0) k--;
hull[k++] = idxs[i];
}
hull.resize(k);
return hull;
}
void TextEditComponent::onCursorChanged()
{
if(isMultiline())
{
Eigen::Vector2f textSize = mFont->getWrappedTextCursorOffset(mText, getTextAreaSize().x(), mCursor);
if(mScrollOffset.y() + getTextAreaSize().y() < textSize.y() + mFont->getHeight()) //need to scroll down?
{
mScrollOffset[1] = textSize.y() - getTextAreaSize().y() + mFont->getHeight();
}else if(mScrollOffset.y() > textSize.y()) //need to scroll up?
{
mScrollOffset[1] = textSize.y();
}
}else{
Eigen::Vector2f cursorPos = mFont->sizeText(mText.substr(0, mCursor));
if(mScrollOffset.x() + getTextAreaSize().x() < cursorPos.x())
{
mScrollOffset[0] = cursorPos.x() - getTextAreaSize().x();
}else if(mScrollOffset.x() > cursorPos.x())
{
mScrollOffset[0] = cursorPos.x();
}
}
}
//this could probably be optimized
//draws text and ensures it's never longer than xLen
void Font::drawWrappedText(std::string text, const Eigen::Vector2f& offset, float xLen, unsigned int color)
{
float y = offset.y();
std::string line, word, temp;
Eigen::Vector2f textSize;
size_t space, newline;
while(text.length() > 0 || !line.empty()) //while there's text or we still have text to render
{
space = text.find(' ', 0);
if(space == std::string::npos)
space = text.length() - 1;
word = text.substr(0, space + 1);
//check if the next word contains a newline
newline = word.find('\n', 0);
if(newline != std::string::npos)
{
word = word.substr(0, newline);
text.erase(0, newline + 1);
}else{
text.erase(0, space + 1);
}
temp = line + word;
textSize = sizeText(temp);
//if we're on the last word and it'll fit on the line, just add it to the line
if((textSize.x() <= xLen && text.length() == 0) || newline != std::string::npos)
{
line = temp;
word = "";
}
//if the next line will be too long or we're on the last of the text, render it
if(textSize.x() > xLen || text.length() == 0 || newline != std::string::npos)
{
//render line now
if(textSize.x() > 0) //make sure it's not blank
drawText(line, Eigen::Vector2f(offset.x(), y), color);
//increment y by height and some extra padding for the next line
y += textSize.y() + 4;
//move the word we skipped to the next line
line = word;
}else{
//there's still space, continue building the line
line = temp;
}
}
}
/**
* Checks if point is inside rectangle. Rectangle coordinate parameters are
* ordered in this check!
*
* param [in] p point
* param [in] r0 lower left corner of rectange
* param [in] r1 upper right corner of rectange
*
* returns true if point was inside or at the edge of rectangle,
* false otherwise
*/
static inline bool pointInsideOrderedRectange(const Eigen::Vector2f & p, const Eigen::Vector2f & r0, const Eigen::Vector2f & r1)
{
if(r0.x() <= p.x() && p.x() <= r1.x() &&
r0.y() <= p.y() && p.y() <= r1.y())
{
return true;
}
return false;
}
void MainWindow::onCalculateButtonPress()
{
arcr.setLinePoint(0,
ui->line0x0SpinBox->value(), ui->line0y0SpinBox->value(),
ui->line0x1SpinBox->value(), ui->line0y1SpinBox->value(),
ui->line0x2SpinBox->value(), ui->line0y2SpinBox->value());
arcr.setLinePoint(1,
ui->line1x0SpinBox->value(), ui->line1y0SpinBox->value(),
ui->line1x1SpinBox->value(), ui->line1y1SpinBox->value(),
ui->line1x2SpinBox->value(), ui->line1y2SpinBox->value());
arcr.computeHMatrix();
Eigen::Vector3f img(inputImage.width(), inputImage.height(), 1.0f);
float xmin = 0;
float xmax = 0;
float ymin = 0;
float ymax = 0;
arcr.computImageSize(0, 0, inputImage.width(), inputImage.height(), xmin, xmax, ymin, ymax);
float aspect = (xmax - xmin) / (ymax - ymin);
outputImage = QImage(inputImage.width(), inputImage.width() / aspect, inputImage.format());
outputImage.fill(qRgb(0, 0, 0));
std::cout << "Output size: " << outputImage.width() << ", " << outputImage.height() << std::endl;
float dx = (xmax - xmin) / float(outputImage.width());
float dy = (ymax - ymin) / float(outputImage.height());
std::cout << std::fixed << "dx, dy: " << dx << ", " << dy << std::endl;
for (int x = 0; x < outputImage.width(); ++x)
{
for (int y = 0; y < outputImage.height(); ++y)
{
Eigen::Vector3f px(x, y, 1);
float tx = 0.0f;
float ty = 0.0f;
Eigen::Vector2f t = arcr.multiplyPointMatrixInverse(xmin + x * dx, ymin + y * dy);
if (t.x() > -1 && t.y() > -1
&& t.x() < inputImage.width()
&& t.y() < inputImage.height())
{
QRgb rgb = bilinearInterpol(inputImage, t.x(), t.y(), dx, dy);
outputImage.setPixel(x, y, rgb);
}
}
}
ui->outputRadioButton->setChecked(true);
update();
}
void Font::drawCenteredText(std::string text, float xOffset, float y, unsigned int color)
{
Eigen::Vector2f pos = sizeText(text);
pos[0] = (Renderer::getScreenWidth() - pos.x());
pos[0] = (pos.x() / 2) + (xOffset / 2);
pos[1] = y;
drawText(text, pos, color);
}
double Terrain::GetHeight(Eigen::Vector2f xy) const{
Cube* temp_cube;
Cylinder* temp_cylinder;
double x = xy.x();
double z = xy.y();
//check if x y lands on a surface object
for(int i = 0; i < m_surface_objects.size(); i++){
switch (m_surface_objects[i]->m_type)
{
case TypeCube:
temp_cube = dynamic_cast<Cube*>(m_surface_objects[i]);
if(x < temp_cube->m_Center[0] + temp_cube->m_Size[0]*0.5
&& x > temp_cube->m_Center[0] - temp_cube->m_Size[0]*0.5
&& z < temp_cube->m_Center[2] + temp_cube->m_Size[2]*0.5
&& z > temp_cube->m_Center[2] - temp_cube->m_Size[2]*0.5)
return temp_cube->m_Size[1];
break;
case TypeCylinder:
temp_cylinder = dynamic_cast<Cylinder*>(m_surface_objects[i]);
if(x < temp_cylinder->m_Center[0] + temp_cylinder->m_Size[0]*0.5
&&x > temp_cylinder->m_Center[0] - temp_cylinder->m_Size[0]*0.5
&&z < temp_cylinder->m_Center[2] + temp_cylinder->m_Size[2]*0.5
&&z > temp_cylinder->m_Center[0] - temp_cylinder->m_Size[2]*0.5)
return sqrt(0.25*temp_cylinder->m_Size[1]*temp_cylinder->m_Size[1] - (x - temp_cylinder->m_Center[0])*(x - temp_cylinder->m_Center[0]));
break;
default:
break;
}
}
int idx, idz;
idx = (xy.x() + 0.5*m_size_x)/m_dx;
idz = (xy.y() + 0.5*m_size_z)/m_dz;
//interpolation
double upleft, upright, downleft, downright;
downleft = m_height_data[idx*(m_res_z+1) + idz];
downright = m_height_data[(idx+1)*(m_res_z+1) + idz];
upleft = m_height_data[idx*(m_res_z+1) + idz+1];
upright = m_height_data[(idx+1)*(m_res_z+1) + idz+1];
double alpha, beta;
alpha = 1 - (xy.x() + 0.5*m_size_x - idx*m_dx)/m_dx;
beta = 1 - (xy.y() + 0.5*m_size_z - idz*m_dz)/m_dz;
return alpha*beta*downleft + (1-alpha)*beta*downright + (1-beta)*alpha*upleft + (1-beta)*(1-alpha)*upright;
}
bool CreditScene::init() {
Graphics::getInstance()->fadeIn(0.6f);
_isInit = true;
auto playerInput = Factory::get()->loadEntityFromFile("playerInput");
playerInput->init();
Scene::setPlayerInput(playerInput);
_credits = Factory::get()->loadEntityFromFile("credits");
_credits->init();
auto PosCmp = _credits->GET_CMP(PositionComponent);
PosCmp->setPosition(Graphics::getInstance()->getScreenSize().cast<float>() * 0.5);
auto SpCmp = _credits->GET_CMP(SpriteCmp);
// 1.5 1.5 looks good
SpCmp->rescale(2.0f, 2.0f);
Eigen::Vector2f pos = Graphics::getInstance()->getScreenSize().cast<float>() * 0.75;
pos.x() = Graphics::getInstance()->getScreenSize().cast<float>().x() * 0.88;
Entity* returnButton = Factory::get()->loadEntityFromFile("buttonBack");
returnButton->init();
std::function<void()> optionFunc = [this](){ free(); Director::get()->pushScene(new MenuScene()); };
returnButton->GET_CMP(ButtonCmp)->setCallback(optionFunc);
PositionComponent* posCmp = returnButton->GET_CMP(PositionComponent);
pos.y() += 200.0f;
posCmp->setPosition(pos);
_buttons.push_back(returnButton);
_cursor = _buttons.begin();
_brighter();
return _isInit;
}
Terrain::Terrain(Eigen::Vector2f a_size, Eigen::Vector2i a_res, int n_hill, TerrainType a_type){
m_type = TypeTerrain;
m_frictness = 10;
InitBase(a_size.x(), a_size.y(), a_res.x(), a_res.y(), n_hill, a_type);
InitDraw();
}
void NinePatchComponent::fitTo(Eigen::Vector2f size, Eigen::Vector3f position, Eigen::Vector2f padding)
{
size += padding;
position[0] -= padding.x() / 2;
position[1] -= padding.y() / 2;
setSize(size + Eigen::Vector2f(getCornerSize().x() * 2, getCornerSize().y() * 2));
setPosition(-getCornerSize().x() + position.x(), -getCornerSize().y() + position.y());
}
/**
* Convert from image plane to pixel coordinates
* @param camera_coordinate The 2D coordinate in camera image plane
* @return image_coordinate The 2D coordinate in the image space
*/
Eigen::Vector2i Camera::image_plane_to_pixel( const Eigen::Vector2f & camera_coordinate ) const {
using namespace Eigen;
Vector3f cam_h { camera_coordinate.x(), camera_coordinate.y(), 1.0f };
Vector3f image_h = m_k * cam_h;
Eigen::Vector2i image_coordinate;
image_coordinate.x( ) = std::round( image_h.x() );
image_coordinate.y( ) = std::round( image_h.y() );
return image_coordinate;
}
void VideoComponent::applyTheme(const std::shared_ptr<ThemeData>& theme, const std::string& view, const std::string& element, unsigned int properties)
{
using namespace ThemeFlags;
const ThemeData::ThemeElement* elem = theme->getElement(view, element, "video");
if(!elem)
{
return;
}
Eigen::Vector2f scale = getParent() ? getParent()->getSize() : Eigen::Vector2f((float)Renderer::getScreenWidth(), (float)Renderer::getScreenHeight());
if ((properties & POSITION) && elem->has("pos"))
{
Eigen::Vector2f denormalized = elem->get<Eigen::Vector2f>("pos").cwiseProduct(scale);
setPosition(Eigen::Vector3f(denormalized.x(), denormalized.y(), 0));
mStaticImage.setPosition(Eigen::Vector3f(denormalized.x(), denormalized.y(), 0));
}
if(properties & ThemeFlags::SIZE)
{
if(elem->has("size"))
setResize(elem->get<Eigen::Vector2f>("size").cwiseProduct(scale));
else if(elem->has("maxSize"))
setMaxSize(elem->get<Eigen::Vector2f>("maxSize").cwiseProduct(scale));
}
// position + size also implies origin
if (((properties & ORIGIN) || ((properties & POSITION) && (properties & ThemeFlags::SIZE))) && elem->has("origin"))
setOrigin(elem->get<Eigen::Vector2f>("origin"));
if(elem->has("default"))
mConfig.defaultVideoPath = elem->get<std::string>("default");
if((properties & ThemeFlags::DELAY) && elem->has("delay"))
mConfig.startDelay = (unsigned)(elem->get<float>("delay") * 1000.0f);
if (elem->has("showSnapshotNoVideo"))
mConfig.showSnapshotNoVideo = elem->get<bool>("showSnapshotNoVideo");
if (elem->has("showSnapshotDelay"))
mConfig.showSnapshotDelay = elem->get<bool>("showSnapshotDelay");
}
void TextComponent::onTextChanged()
{
calculateExtent();
if(!mFont || mText.empty())
{
mTextCache.reset();
return;
}
std::string text = mUppercase ? strToUpper(mText) : mText;
std::shared_ptr<Font> f = mFont;
const bool isMultiline = (mSize.y() == 0 || mSize.y() > f->getHeight()*1.2f);
bool addAbbrev = false;
if(!isMultiline)
{
size_t newline = text.find('\n');
text = text.substr(0, newline); // single line of text - stop at the first newline since it'll mess everything up
addAbbrev = newline != std::string::npos;
}
Eigen::Vector2f size = f->sizeText(text);
if(!isMultiline && mSize.x() && text.size() && (size.x() > mSize.x() || addAbbrev))
{
// abbreviate text
const std::string abbrev = "...";
Eigen::Vector2f abbrevSize = f->sizeText(abbrev);
while(text.size() && size.x() + abbrevSize.x() > mSize.x())
{
size_t newSize = Font::getPrevCursor(text, text.size());
text.erase(newSize, text.size() - newSize);
size = f->sizeText(text);
}
text.append(abbrev);
mTextCache = std::shared_ptr<TextCache>(f->buildTextCache(text, Eigen::Vector2f(0, 0), (mColor >> 8 << 8) | mOpacity, mSize.x(), mAlignment, mLineSpacing));
}else{
Eigen::Vector3f Terrain::GetNormal(Eigen::Vector2f xy) const{
//COULD HAVE BUG HERE
int idx, idz;
idx = (xy.x() + 0.5*m_size_x)/m_dx;
idz = (xy.y() + 0.5*m_size_z)/m_dz;
//interpolation
Eigen::Vector3f upleft, upright, downleft, downright;
downleft = m_normal_data[idx*(m_res_z+1) + idz];
downright = m_normal_data[(idx+1)*(m_res_z+1) + idz];
upleft = m_normal_data[idx*(m_res_z+1) + idz+1];
upright = m_normal_data[(idx+1)*(m_res_z+1) + idz+1];
double alpha, beta;
alpha = 1 - (xy.x() + 0.5*m_size_x - idx*m_dx)/m_dx;
beta = 1 - (xy.y() + 0.5*m_size_z - idz*m_dz)/m_dz;
return alpha*beta*downleft + (1-alpha)*beta*downright + (1-beta)*alpha*upleft + (1-beta)*(1-alpha)*upright;
}
void draw(MapWriterInterface *interface)
{
if (!initialized_) return;
hector_worldmodel_msgs::GetObjectModel data;
if (!service_client_.call(data)) {
ROS_ERROR_NAMED(name_, "Cannot draw victims, service %s failed", service_client_.getService().c_str());
return;
}
std::string team_name;
std::string mission_name;
nh_.getParamCached("/team", team_name);
nh_.getParamCached("/mission", mission_name);
boost::posix_time::ptime now = ros::Time::now().toBoost();
boost::gregorian::date now_date(now.date());
boost::posix_time::time_duration now_time(now.time_of_day().hours(), now.time_of_day().minutes(), now.time_of_day().seconds(), 0);
std::ofstream description_file((interface->getBasePathAndFileName() + "_qr.csv").c_str());
if (description_file.is_open()) {
if (!team_name.empty()) description_file << team_name << std::endl;
description_file << now_date << "; " << now_time << std::endl;
if (!mission_name.empty()) description_file << mission_name << std::endl;
description_file << std::endl;
}
int counter = 0;
for(hector_worldmodel_msgs::ObjectModel::_objects_type::const_iterator it = data.response.model.objects.begin(); it != data.response.model.objects.end(); ++it) {
const hector_worldmodel_msgs::Object& object = *it;
if (!class_id_.empty() && object.info.class_id != class_id_) continue;
if (!draw_all_objects_ && object.state.state != hector_worldmodel_msgs::ObjectState::CONFIRMED) continue;
if (object.state.state == hector_worldmodel_msgs::ObjectState::DISCARDED) continue;
Eigen::Vector2f coords;
coords.x() = object.pose.pose.position.x;
coords.y() = object.pose.pose.position.y;
interface->drawObjectOfInterest(coords, boost::lexical_cast<std::string>(++counter), MapWriterInterface::Color(10,10,240));
if (description_file.is_open()) {
boost::posix_time::time_duration time_of_day(object.header.stamp.toBoost().time_of_day());
boost::posix_time::time_duration time(time_of_day.hours(), time_of_day.minutes(), time_of_day.seconds(), 0);
description_file << counter << ";" << time << ";" << object.info.object_id << ";" << object.pose.pose.position.x << ";" << object.pose.pose.position.y << ";" << object.pose.pose.position.z << std::endl;
}
}
description_file.close();
}
void ComponentGrid::updateSeparators()
{
mLines.clear();
bool drawAll = Settings::getInstance()->getBool("DebugGrid");
Eigen::Vector2f pos;
Eigen::Vector2f size;
for(auto it = mCells.begin(); it != mCells.end(); it++)
{
if(!it->border && !drawAll)
continue;
// find component position + size
pos << 0, 0;
size << 0, 0;
for(int x = 0; x < it->pos.x(); x++)
pos[0] += getColWidth(x);
for(int y = 0; y < it->pos.y(); y++)
pos[1] += getRowHeight(y);
for(int x = it->pos.x(); x < it->pos.x() + it->dim.x(); x++)
size[0] += getColWidth(x);
for(int y = it->pos.y(); y < it->pos.y() + it->dim.y(); y++)
size[1] += getRowHeight(y);
if(it->border & BORDER_TOP || drawAll)
{
mLines.push_back(Vert(pos.x(), pos.y()));
mLines.push_back(Vert(pos.x() + size.x(), pos.y()));
}
if(it->border & BORDER_BOTTOM || drawAll)
{
mLines.push_back(Vert(pos.x(), pos.y() + size.y()));
mLines.push_back(Vert(pos.x() + size.x(), mLines.back().y));
}
if(it->border & BORDER_LEFT || drawAll)
{
mLines.push_back(Vert(pos.x(), pos.y()));
mLines.push_back(Vert(pos.x(), pos.y() + size.y()));
}
if(it->border & BORDER_RIGHT || drawAll)
{
mLines.push_back(Vert(pos.x() + size.x(), pos.y()));
mLines.push_back(Vert(mLines.back().x, pos.y() + size.y()));
}
}
mLineColors.reserve(mLines.size());
Renderer::buildGLColorArray((GLubyte*)mLineColors.data(), 0xC6C7C6FF, mLines.size());
}
void HectorMappingRos::setServiceGetMapData(nav_msgs::GetMap::Response& map_, const hectorslam::GridMap& gridMap)
{
Eigen::Vector2f mapOrigin (gridMap.getWorldCoords(Eigen::Vector2f::Zero()));
mapOrigin.array() -= gridMap.getCellLength()*0.5f;
map_.map.info.origin.position.x = mapOrigin.x();
map_.map.info.origin.position.y = mapOrigin.y();
map_.map.info.origin.orientation.w = 1.0;
map_.map.info.resolution = gridMap.getCellLength();
map_.map.info.width = gridMap.getSizeX();
map_.map.info.height = gridMap.getSizeY();
map_.map.header.frame_id = p_map_frame_;
map_.map.data.resize(map_.map.info.width * map_.map.info.height);
}
void TextEditComponent::render(const Eigen::Affine3f& parentTrans)
{
Eigen::Affine3f trans = getTransform() * parentTrans;
renderChildren(trans);
// text + cursor rendering
// offset into our "text area" (padding)
trans.translation() += Eigen::Vector3f(getTextAreaPos().x(), getTextAreaPos().y(), 0);
Eigen::Vector2i clipPos((int)trans.translation().x(), (int)trans.translation().y());
Eigen::Vector3f dimScaled = trans * Eigen::Vector3f(getTextAreaSize().x(), getTextAreaSize().y(), 0); // use "text area" size for clipping
Eigen::Vector2i clipDim((int)dimScaled.x() - trans.translation().x(), (int)dimScaled.y() - trans.translation().y());
Renderer::pushClipRect(clipPos, clipDim);
trans.translate(Eigen::Vector3f(-mScrollOffset.x(), -mScrollOffset.y(), 0));
trans = roundMatrix(trans);
Renderer::setMatrix(trans);
if(mTextCache)
{
mFont->renderTextCache(mTextCache.get());
}
// pop the clip early to allow the cursor to be drawn outside of the "text area"
Renderer::popClipRect();
// draw cursor
if(mEditing)
{
Eigen::Vector2f cursorPos;
if(isMultiline())
{
cursorPos = mFont->getWrappedTextCursorOffset(mText, getTextAreaSize().x(), mCursor);
}else{
cursorPos = mFont->sizeText(mText.substr(0, mCursor));
cursorPos[1] = 0;
}
float cursorHeight = mFont->getHeight() * 0.8f;
Renderer::drawRect(cursorPos.x(), cursorPos.y() + (mFont->getHeight() - cursorHeight) / 2, 2.0f, cursorHeight, 0x000000FF);
}
}
//the worst algorithm ever written
//breaks up a normal string with newlines to make it fit xLen
std::string Font::wrapText(std::string text, float xLen)
{
std::string out;
std::string line, word, temp;
size_t space;
Eigen::Vector2f textSize;
while(text.length() > 0) //while there's text or we still have text to render
{
space = text.find_first_of(" \t\n");
if(space == std::string::npos)
space = text.length() - 1;
word = text.substr(0, space + 1);
text.erase(0, space + 1);
temp = line + word;
textSize = sizeText(temp);
// if the word will fit on the line, add it to our line, and continue
if(textSize.x() <= xLen)
{
line = temp;
continue;
}else{
// the next word won't fit, so break here
out += line + '\n';
line = word;
}
}
// whatever's left should fit
out += line;
return out;
}
void draw(MapWriterInterface *interface)
{
if (!initialized_) return;
hector_worldmodel_msgs::GetObjectModel data;
if (!service_client_.call(data)) {
ROS_ERROR_NAMED(name_, "Cannot draw victims, service %s failed", service_client_.getService().c_str());
return;
}
int counter = 0;
for(hector_worldmodel_msgs::ObjectModel::_objects_type::const_iterator it = data.response.model.objects.begin(); it != data.response.model.objects.end(); ++it) {
const hector_worldmodel_msgs::Object& object = *it;
if (!draw_all_objects_ && object.state.state != hector_worldmodel_msgs::ObjectState::CONFIRMED) continue;
if (!class_id_.empty() && object.info.class_id != class_id_) continue;
Eigen::Vector2f coords;
coords.x() = object.pose.pose.position.x;
coords.y() = object.pose.pose.position.y;
interface->drawObjectOfInterest(coords, boost::lexical_cast<std::string>(++counter), MapWriterInterface::Color(240,10,10));
}
}
static inline float distance(const Eigen::Vector2f & v0, const Eigen::Vector2f & v1)
{
return sqrt(
(v0.x() - v1.x()) * (v0.x() - v1.x()) +
(v0.y() - v1.y()) * (v0.y() - v1.y()));
}
void PathCmp::search (Eigen::Vector2f targetPos_) {
_level = LogicSystem::getInstance()->getLevel();
for (auto it = _openList.begin(); it != _openList.end(); ++ it) {
if (*it != nullptr)
delete *it;
}
_openList.clear();
for (auto it = _closedList.begin(); it != _closedList.end(); ++ it) {
if (*it != nullptr)
delete *it;
}
_closedList.clear();
_shortestPath.clear();
auto posCmp = getEntity()->GET_CMP(PositionComponent);
_srcIdx = _level->posToTileIndex(posCmp->getPosition());
_tarIdx = _level->posToTileIndex(targetPos_);
if (_srcIdx == _tarIdx)
return;
if (_level->isTileObstacle(_tarIdx.x(), _tarIdx.y()))
return;
PathNode* srcNode = new PathNode(_srcIdx);
calcCostH(srcNode);
_openList.push_back(srcNode);
do {
PathNode* curNode = _openList[0];
_closedList.push_back(curNode);
_openList.erase(_openList.begin());
if (curNode->getIndex() == _tarIdx) {
PathNode* walkNode = curNode;
_shortestPath.clear();
do {
Eigen::Vector2f pos;
pos.x() = _level->tileIndexXToPosX(walkNode->getIndex().x());
pos.y() = _level->tileIndexYToPosY(walkNode->getIndex().y());
_shortestPath.push_front(pos);
walkNode = walkNode->getParent();
} while (walkNode);
for (auto it = _openList.begin(); it != _openList.end(); ++ it) {
if (*it != nullptr)
delete *it;
}
_openList.clear();
for (auto it = _closedList.begin(); it != _closedList.end(); ++ it) {
if (*it != nullptr)
delete *it;
}
_closedList.clear();
break;
}
putAdjacentTiles(curNode);
for (auto it = _adjacentTiles.begin(); it != _adjacentTiles.end(); ++ it) {
bool isInClosedList = false;
for (int i = 0; i < _closedList.size(); ++ i)
if ((*it) == _closedList[i]->getIndex())
isInClosedList = true;
if (isInClosedList) continue;
auto node = new PathNode(*it);
int costGToAdd = calcCostG(node, curNode);
std::vector<PathNode *>::iterator opit = _openList.begin();
while (opit != _openList.end()) {
if ((*opit)->getIndex() == node->getIndex())
break;
++ opit;
}
// already on openlist
if (opit!= _openList.end()) {
delete node;
node = (*opit);
if (curNode->getCostG() + costGToAdd < node->getCostG())
node->setCostG(curNode->getCostG() + costGToAdd);
} else {
node->setParent(curNode);
node->setCostG(curNode->getCostG() + costGToAdd);
calcCostH(node);
addOpenList(node);
}
}
} while (_openList.size() > 0);
}
D2D1_POINT_2F PointConversion(const Eigen::Vector2f& p) {
D2D1_POINT_2F rv;
rv.x = p.x();
rv.y = p.y();
return rv;
}
/** Sets normal from gradient */
void setNormalFromGradient(const Eigen::Vector2f& g) {
const float gx = g.x();
const float gy = g.y();
const float scl = Danvil::MoreMath::FastInverseSqrt(gx*gx + gy*gy + 1.0f);
setNormal(Eigen::Vector3f(scl*gx, scl*gy, -scl));
}
void __AGE Button::update(float dt)
{
Entity::update(dt);
Input& input = Input::instance();
if( transformationDirty )
{
// make sure the text passes the z test
textEntity->setZ(z()+1e-3);
computeClickBoundingBox();
}
// update state
switch( state )
{
case 0:
case 1:
{
for( int id = 0; id < MOUSE_POINTERS; ++id )
{
Eigen::Vector2f pos;
input.mouseXY(pos.x(), pos.y(),id);
bool inside_bb = isInside(pos, clickBoundingBoxMin, clickBoundingBoxMax);
if( inside_bb )
{
if( input.mouseJustPressed(0,0,id) )
{
state = 2;
mouse_id = id;
}
else
{
#ifdef __ANDROID__
state = 0; // no hover on touch devices
#else
state = 1; // hover
#endif
}
break;
}
else
state = 0;
}
}
break;
case 2:
{
Eigen::Vector2f pos;
input.mouseXY(pos.x(), pos.y(),mouse_id);
if( input.mouseJustReleased(0,0,mouse_id) )
{
bool inside_bb = isInside(pos, clickBoundingBoxMin, clickBoundingBoxMax);
if( inside_bb )
{
_triggeredCallback(dt);
}
state = 0;
}
}
break;
}
}
void glsl_program::set_uniform(unsigned loc, const Eigen::Vector2f& value) const
{
if (used_program != this)
throw runtime_error("glsl_program::set_uniform, program not bound!");
glUniform2f(loc, value.x(), value.y());
}
static float cross(const Eigen::Vector2f & a, const Eigen::Vector2f & b)
{
return a.x() * b.y() - a.y() * b.x();
}
static Eigen::Vector2f normal(const Eigen::Vector2f & line)
{
return Eigen::Vector2f(line.y(), -line.x());
}
/**
* param [in] point point
* param [in] p1 line segments start
* param [in] p0 line segments end
* param [out] closest closest point on segment for the point
* returns distance from point to the closest point
*
* Returns minimum distance between line segment vw and point p.
* returns the closest point on the line segment.
*/
static float pointDistanceToLine(const Eigen::Vector2f & point, const Eigen::Vector2f & p1, const Eigen::Vector2f & p2, Eigen::Vector2f & closest)
{
float dx = p2.x() - p1.x();
float dy = p2.y() - p1.y();
if((dx == 0) && (dy == 0))
{
// Line is a point!
closest = p1;
dx = point.x() - p1.x();
dy = point.y() - p1.y();
}
else
{
// Calculate the t that minimizes the distance
float t = ((point.x() - p1.x()) * dx + (point.y() - p1.y()) * dy) / (dx * dx + dy * dy);
// See if this represents one of the segment's end points or
// a point in the middle.
if(t < 0)
{
closest = Eigen::Vector2f(p1.x(), p1.y());
dx = point.x() - p1.x();
dy = point.y() - p1.y();
}
else if(t > 1)
{
closest = Eigen::Vector2f(p2.x(), p2.y());
dx = point.x() - p2.x();
dy = point.y() - p2.y();
}
else
{
closest = Eigen::Vector2f(p1.x() + t * dx, p1.y() + t * dy);
dx = point.x() - closest.x();
dy = point.y() - closest.y();
}
}
return sqrt(dx * dx + dy * dy);
}