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);
}
}
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");
}
//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;
}
}
}
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{
/** Compute optimal translation scale.
* @param std::vector<Eigen::Vector4f> vector with 3D points
* @param double median distance of all points
* @param camera pitch angle
* @param camera height
* @return double optimal scale */
double MonoOdometer5::getTranslationScale(std::vector<Eigen::Vector4f> points3D, double median, double pitch, double height)
{
double sigma = median/50.0;
double weight = 1.0/(2.0*sigma*sigma);
// ds stores the height of all points from the ground (?)
std::vector<Eigen::Vector2f> pointsPlane;
std::vector<double> ds;
Eigen::Vector2f inclination;
inclination(0) = cos(-pitch);
inclination(1) = sin(-pitch);
for(int i=0; i<points3D.size(); i++)
{
double d = inclination.transpose() * points3D[i].segment<2>(1);
ds.push_back(d);
}
int bestIndex = 0;
double maxSum = 0.0;
for(int i=0; i<points3D.size(); i++)
{
if(ds[i] > median / param_odometerMotionThreshold_)
{
double sum = 0.0;
for(int j=0; j<points3D.size(); j++)
{
double dist = ds[j] - ds[i];
sum += exp(-dist*dist*weight);
}
if(sum > maxSum)
{
maxSum = sum;
bestIndex = i;
}
}
}
double scale = height / ds[bestIndex];
return scale;
}
/** Get edge closest to a specified point.
* The point must be within an imaginery line segment parallel to
* the edge, that is a line perpendicular to the edge must go
* through the point and a point on the edge line segment.
* @param pos_x X coordinate in global (map) frame of point
* @param pos_y X coordinate in global (map) frame of point
* @return edge closest to the given point, or invalid edge if
* such an edge does not exist.
*/
NavGraphEdge
NavGraph::closest_edge(float pos_x, float pos_y) const
{
float min_dist = std::numeric_limits<float>::max();
NavGraphEdge rv;
Eigen::Vector2f point(pos_x, pos_y);
for (const NavGraphEdge &edge : edges_) {
const Eigen::Vector2f origin(edge.from_node().x(), edge.from_node().y());
const Eigen::Vector2f target(edge.to_node().x(), edge.to_node().y());
const Eigen::Vector2f direction(target - origin);
const Eigen::Vector2f direction_norm = direction.normalized();
const Eigen::Vector2f diff = point - origin;
const float t = direction.dot(diff) / direction.squaredNorm();
if (t >= 0.0 && t <= 1.0) {
// projection of the point onto the edge is within the line segment
float distance = (diff - direction_norm.dot(diff) * direction_norm).norm();
if (distance < min_dist) {
min_dist = distance;
rv = edge;
}
}
}
return rv;
}
/** Check if the edge intersects with another line segment.
* @param x1 X coordinate of first point of line segment to test
* @param y1 Y coordinate of first point of line segment to test
* @param x2 X coordinate of first point of line segment to test
* @param y2 Y coordinate of first point of line segment to test
* @param ip upon returning true contains intersection point,
* not modified is return value is false
* @return true if the edge intersects with the line segment, false otherwise
*/
bool
NavGraphEdge::intersection(float x1, float y1, float x2, float y2,
fawkes::cart_coord_2d_t &ip) const
{
const Eigen::Vector2f e_from(from_node_.x(), from_node_.y());
const Eigen::Vector2f e_to(to_node_.x(), to_node_.y());
const Eigen::Vector2f p1(x1, y1);
const Eigen::Vector2f p2(x2, y2);
const Eigen::Vector2f lip = line_segm_intersection(e_from, e_to, p1, p2);
#if EIGEN_VERSION_AT_LEAST(3,2,0)
if (lip.allFinite()) {
#else
if (workaround::allFinite(lip)) {
#endif
ip.x = lip[0];
ip.y = lip[1];
return true;
} else {
return false;
}
}
/** Check if the edge intersects with another line segment.
* @param x1 X coordinate of first point of line segment to test
* @param y1 Y coordinate of first point of line segment to test
* @param x2 X coordinate of first point of line segment to test
* @param y2 Y coordinate of first point of line segment to test
* @return true if the edge intersects with the line segment, false otherwise
*/
bool
NavGraphEdge::intersects(float x1, float y1, float x2, float y2) const
{
const Eigen::Vector2f e_from(from_node_.x(), from_node_.y());
const Eigen::Vector2f e_to(to_node_.x(), to_node_.y());
const Eigen::Vector2f p1(x1, y1);
const Eigen::Vector2f p2(x2, y2);
return line_segm_intersect(e_from, e_to, p1, p2);
}
} // end of namespace fawkes
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));
}
}
//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;
}
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 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();
}
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;
}
/** Get the point on edge closest to a given point.
* The method determines a line perpendicular to the edge which goes through
* the given point, i.e. the point must be within the imaginary line segment.
* Then the point on the edge which crosses with that perpendicular line
* is returned.
* @param x X coordinate of point to get point on edge for
* @param y Y coordinate of point to get point on edge for
* @return coordinate of point on edge closest to given point
* @throw Exception thrown if the point is out of the line segment and
* no line perpendicular to the edge going through the given point can
* be found.
*/
cart_coord_2d_t
NavGraphEdge::closest_point_on_edge(float x, float y) const
{
const Eigen::Vector2f point(x, y);
const Eigen::Vector2f origin(from_node_.x(), from_node_.y());
const Eigen::Vector2f target(to_node_.x(), to_node_.y());
const Eigen::Vector2f direction(target - origin);
const Eigen::Vector2f direction_norm = direction.normalized();
const Eigen::Vector2f diff = point - origin;
const float t = direction.dot(diff) / direction.squaredNorm();
if (t >= 0.0 && t <= 1.0) {
// projection of the point onto the edge is within the line segment
Eigen::Vector2f point_on_line = origin + direction_norm.dot(diff) * direction_norm;
return cart_coord_2d_t(point_on_line[0], point_on_line[1]);
}
throw Exception("Point (%f,%f) is not on edge %s--%s", x, y,
from_.c_str(), to_.c_str());
}
template <typename PointInT, typename IntensityT> void
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::track (const PointCloudInConstPtr& prev_input,
const PointCloudInConstPtr& input,
const std::vector<FloatImageConstPtr>& prev_pyramid,
const std::vector<FloatImageConstPtr>& pyramid,
const pcl::PointCloud<pcl::PointUV>::ConstPtr& prev_keypoints,
pcl::PointCloud<pcl::PointUV>::Ptr& keypoints,
std::vector<int>& status,
Eigen::Affine3f& motion) const
{
std::vector<Eigen::Array2f, Eigen::aligned_allocator<Eigen::Array2f> > next_pts (prev_keypoints->size ());
Eigen::Array2f half_win ((track_width_-1)*0.5f, (track_height_-1)*0.5f);
pcl::TransformationFromCorrespondences transformation_computer;
const int nb_points = prev_keypoints->size ();
for (int level = nb_levels_ - 1; level >= 0; --level)
{
const FloatImage& prev = *(prev_pyramid[level*3]);
const FloatImage& next = *(pyramid[level*3]);
const FloatImage& grad_x = *(prev_pyramid[level*3+1]);
const FloatImage& grad_y = *(prev_pyramid[level*3+2]);
Eigen::ArrayXXf prev_win (track_height_, track_width_);
Eigen::ArrayXXf grad_x_win (track_height_, track_width_);
Eigen::ArrayXXf grad_y_win (track_height_, track_width_);
float ratio (1./(1 << level));
for (int ptidx = 0; ptidx < nb_points; ptidx++)
{
Eigen::Array2f prev_pt (prev_keypoints->points[ptidx].u * ratio,
prev_keypoints->points[ptidx].v * ratio);
Eigen::Array2f next_pt;
if (level == nb_levels_ -1)
next_pt = prev_pt;
else
next_pt = next_pts[ptidx]*2.f;
next_pts[ptidx] = next_pt;
Eigen::Array2i iprev_point;
prev_pt -= half_win;
iprev_point[0] = floor (prev_pt[0]);
iprev_point[1] = floor (prev_pt[1]);
if (iprev_point[0] < -track_width_ || (uint32_t) iprev_point[0] >= grad_x.width ||
iprev_point[1] < -track_height_ || (uint32_t) iprev_point[1] >= grad_y.height)
{
if (level == 0)
status [ptidx] = -1;
continue;
}
float a = prev_pt[0] - iprev_point[0];
float b = prev_pt[1] - iprev_point[1];
Eigen::Array4f weight;
weight[0] = (1.f - a)*(1.f - b);
weight[1] = a*(1.f - b);
weight[2] = (1.f - a)*b;
weight[3] = 1 - weight[0] - weight[1] - weight[2];
Eigen::Array3f covar = Eigen::Array3f::Zero ();
spatialGradient (prev, grad_x, grad_y, iprev_point, weight, prev_win, grad_x_win, grad_y_win, covar);
float det = covar[0]*covar[2] - covar[1]*covar[1];
float min_eigenvalue = (covar[2] + covar[0] - std::sqrt ((covar[0]-covar[2])*(covar[0]-covar[2]) + 4.f*covar[1]*covar[1]))/2.f;
if (min_eigenvalue < min_eigenvalue_threshold_ || det < std::numeric_limits<float>::epsilon ())
{
status[ptidx] = -2;
continue;
}
det = 1.f/det;
next_pt -= half_win;
Eigen::Array2f prev_delta (0, 0);
for (unsigned int j = 0; j < max_iterations_; j++)
{
Eigen::Array2i inext_pt = next_pt.floor ().cast<int> ();
if (inext_pt[0] < -track_width_ || (uint32_t) inext_pt[0] >= next.width ||
inext_pt[1] < -track_height_ || (uint32_t) inext_pt[1] >= next.height)
{
if (level == 0)
status[ptidx] = -1;
break;
}
a = next_pt[0] - inext_pt[0];
b = next_pt[1] - inext_pt[1];
weight[0] = (1.f - a)*(1.f - b);
weight[1] = a*(1.f - b);
weight[2] = (1.f - a)*b;
weight[3] = 1 - weight[0] - weight[1] - weight[2];
// compute mismatch vector
Eigen::Array2f beta = Eigen::Array2f::Zero ();
mismatchVector (prev_win, grad_x_win, grad_y_win, next, inext_pt, weight, beta);
// optical flow resolution
Eigen::Vector2f delta ((covar[1]*beta[1] - covar[2]*beta[0])*det, (covar[1]*beta[0] - covar[0]*beta[1])*det);
// update position
next_pt[0] += delta[0]; next_pt[1] += delta[1];
next_pts[ptidx] = next_pt + half_win;
if (delta.squaredNorm () <= epsilon_)
break;
if (j > 0 && std::abs (delta[0] + prev_delta[0]) < 0.01 &&
std::abs (delta[1] + prev_delta[1]) < 0.01 )
{
next_pts[ptidx][0] -= delta[0]*0.5f;
next_pts[ptidx][1] -= delta[1]*0.5f;
break;
}
// update delta
prev_delta = delta;
}
// update tracked points
if (level == 0 && !status[ptidx])
{
Eigen::Array2f next_point = next_pts[ptidx] - half_win;
Eigen::Array2i inext_point;
inext_point[0] = floor (next_point[0]);
inext_point[1] = floor (next_point[1]);
if (inext_point[0] < -track_width_ || (uint32_t) inext_point[0] >= next.width ||
inext_point[1] < -track_height_ || (uint32_t) inext_point[1] >= next.height)
{
status[ptidx] = -1;
continue;
}
// insert valid keypoint
pcl::PointUV n;
n.u = next_pts[ptidx][0];
n.v = next_pts[ptidx][1];
keypoints->push_back (n);
// add points pair to compute transformation
inext_point[0] = floor (next_pts[ptidx][0]);
inext_point[1] = floor (next_pts[ptidx][1]);
iprev_point[0] = floor (prev_keypoints->points[ptidx].u);
iprev_point[1] = floor (prev_keypoints->points[ptidx].v);
const PointInT& prev_pt = prev_input->points[iprev_point[1]*prev_input->width + iprev_point[0]];
const PointInT& next_pt = input->points[inext_point[1]*input->width + inext_point[0]];
transformation_computer.add (prev_pt.getVector3fMap (), next_pt.getVector3fMap (), 1.0);
}
}
}
motion = transformation_computer.getTransformation ();
}
int test_eigen(int argc, char *argv[])
{
int rc = 0;
warnx("testing eigen");
{
Eigen::Vector2f v;
Eigen::Vector2f v1(1.0f, 2.0f);
Eigen::Vector2f v2(1.0f, -1.0f);
float data[2] = {1.0f, 2.0f};
TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3);
TEST_OP_VERIFY("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1), v3.isApprox(v1));
TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]);
TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f);
TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1));
VERIFY_OP("Vector2f + Vector2f", v = v + v1, v.isApprox(v1 + v1));
VERIFY_OP("Vector2f - Vector2f", v = v - v1, v.isApprox(v1));
VERIFY_OP("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1));
VERIFY_OP("Vector2f -= Vector2f", v -= v1, v.isApprox(v1));
TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON);
//TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array?
}
{
Eigen::Vector3f v;
Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
Eigen::Vector3f v2(1.0f, -1.0f, 2.0f);
float data[3] = {1.0f, 2.0f, 3.0f};
TEST_OP("Constructor Vector3f()", Eigen::Vector3f v3);
TEST_OP("Constructor Vector3f(Vector3f)", Eigen::Vector3f v3(v1));
TEST_OP("Constructor Vector3f(float[])", Eigen::Vector3f v3(data));
TEST_OP("Constructor Vector3f(float, float, float)", Eigen::Vector3f v3(1.0f, 2.0f, 3.0f));
TEST_OP("Vector3f = Vector3f", v = v1);
TEST_OP("Vector3f + Vector3f", v + v1);
TEST_OP("Vector3f - Vector3f", v - v1);
TEST_OP("Vector3f += Vector3f", v += v1);
TEST_OP("Vector3f -= Vector3f", v -= v1);
TEST_OP("Vector3f * float", v1 * 2.0f);
TEST_OP("Vector3f / float", v1 / 2.0f);
TEST_OP("Vector3f *= float", v1 *= 2.0f);
TEST_OP("Vector3f /= float", v1 /= 2.0f);
TEST_OP("Vector3f dot Vector3f", v.dot(v1));
TEST_OP("Vector3f cross Vector3f", v1.cross(v2));
TEST_OP("Vector3f length", v1.norm());
TEST_OP("Vector3f length squared", v1.squaredNorm());
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
// Need pragma here intead of moving variable out of TEST_OP and just reference because
// TEST_OP measures performance of vector operations.
TEST_OP("Vector<3> element read", volatile float a = v1(0));
TEST_OP("Vector<3> element read direct", volatile float a = v1.data()[0]);
#pragma GCC diagnostic pop
TEST_OP("Vector<3> element write", v1(0) = 1.0f);
TEST_OP("Vector<3> element write direct", v1.data()[0] = 1.0f);
}
{
Eigen::Vector4f v(0.0f, 0.0f, 0.0f, 0.0f);
Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f);
Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f);
Eigen::Vector4f vres;
float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
TEST_OP("Constructor Vector<4>()", Eigen::Vector4f v3);
TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1));
TEST_OP("Constructor Vector<4>(float[])", Eigen::Vector4f v3(data));
TEST_OP("Constructor Vector<4>(float, float, float, float)", Eigen::Vector4f v3(1.0f, 2.0f, 3.0f, 4.0f));
TEST_OP("Vector<4> = Vector<4>", v = v1);
TEST_OP("Vector<4> + Vector<4>", v + v1);
TEST_OP("Vector<4> - Vector<4>", v - v1);
TEST_OP("Vector<4> += Vector<4>", v += v1);
TEST_OP("Vector<4> -= Vector<4>", v -= v1);
TEST_OP("Vector<4> dot Vector<4>", v.dot(v1));
}
{
Eigen::Vector10f v1;
v1.Zero();
float data[10];
TEST_OP("Constructor Vector<10>()", Eigen::Vector10f v3);
TEST_OP("Constructor Vector<10>(Vector<10>)", Eigen::Vector10f v3(v1));
TEST_OP("Constructor Vector<10>(float[])", Eigen::Vector10f v3(data));
}
{
Eigen::Matrix3f m1;
m1.setIdentity();
Eigen::Matrix3f m2;
m2.setIdentity();
Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
TEST_OP("Matrix3f * Vector3f", m1 * v1);
TEST_OP("Matrix3f + Matrix3f", m1 + m2);
TEST_OP("Matrix3f * Matrix3f", m1 * m2);
}
{
Eigen::Matrix<float, 10, 10> m1;
m1.setIdentity();
Eigen::Matrix<float, 10, 10> m2;
m2.setIdentity();
Eigen::Vector10f v1;
v1.Zero();
TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
}
{
warnx("Nonsymmetric matrix operations test");
// test nonsymmetric +, -, +=, -=
const Eigen::Matrix<float, 2, 3> m1_orig =
(Eigen::Matrix<float, 2, 3>() << 1, 2, 3,
4, 5, 6).finished();
Eigen::Matrix<float, 2, 3> m1(m1_orig);
Eigen::Matrix<float, 2, 3> m2;
m2 << 2, 4, 6,
8, 10, 12;
Eigen::Matrix<float, 2, 3> m3;
m3 << 3, 6, 9,
12, 15, 18;
if (m1 + m2 != m3) {
warnx("Matrix<2, 3> + Matrix<2, 3> failed!");
printEigen(m1 + m2);
printf("!=\n");
printEigen(m3);
rc = 1;
}
if (m3 - m2 != m1) {
warnx("Matrix<2, 3> - Matrix<2, 3> failed!");
printEigen(m3 - m2);
printf("!=\n");
printEigen(m1);
rc = 1;
}
m1 += m2;
if (m1 != m3) {
warnx("Matrix<2, 3> += Matrix<2, 3> failed!");
printEigen(m1);
printf("!=\n");
printEigen(m3);
rc = 1;
}
m1 -= m2;
if (m1 != m1_orig) {
warnx("Matrix<2, 3> -= Matrix<2, 3> failed!");
printEigen(m1);
printf("!=\n");
printEigen(m1_orig);
rc = 1;
}
}
/* QUATERNION TESTS CURRENTLY FAILING
{
// test conversion rotation matrix to quaternion and back
Eigen::Matrix3f R_orig;
Eigen::Matrix3f R;
Eigen::Quaternionf q;
float diff = 0.1f;
float tol = 0.00001f;
warnx("Quaternion transformation methods test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R_orig.eulerAngles(roll, pitch, yaw);
q = R_orig; //from_dcm
R = q.toRotationMatrix();
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
if (fabsf(R_orig(i, j) - R(i, j)) > tol) {
warnx("Quaternion method 'from_dcm' or 'toRotationMatrix' outside tolerance!");
rc = 1;
}
}
}
}
}
}
// test against some known values
tol = 0.0001f;
Eigen::Quaternionf q_true = {1.0f, 0.0f, 0.0f, 0.0f};
R_orig.setIdentity();
q = R_orig;
for (size_t i = 0; i < 4; i++) {
if (!q.isApprox(q_true, tol)) {
warnx("Quaternion method 'from_dcm()' outside tolerance!");
rc = 1;
}
}
q_true = quatFromEuler(0.3f, 0.2f, 0.1f);
q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
for (size_t i = 0; i < 4; i++) {
if (!q.isApprox(q_true, tol)) {
warnx("Quaternion method 'eulerAngles()' outside tolerance!");
rc = 1;
}
}
q_true = quatFromEuler(-1.5f, -0.2f, 0.5f);
q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
for (size_t i = 0; i < 4; i++) {
if (!q.isApprox(q_true, tol)) {
warnx("Quaternion method 'eulerAngles()' outside tolerance!");
rc = 1;
}
}
q_true = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
for (size_t i = 0; i < 4; i++) {
if (!q.isApprox(q_true, tol)) {
warnx("Quaternion method 'eulerAngles()' outside tolerance!");
rc = 1;
}
}
}
{
// test quaternion method "rotate" (rotate vector by quaternion)
Eigen::Vector3f vector = {1.0f, 1.0f, 1.0f};
Eigen::Vector3f vector_q;
Eigen::Vector3f vector_r;
Eigen::Quaternionf q;
Eigen::Matrix3f R;
float diff = 0.1f;
float tol = 0.00001f;
warnx("Quaternion vector rotation method test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R.eulerAngles(roll, pitch, yaw);
q = quatFromEuler(roll, pitch, yaw);
vector_r = R * vector;
vector_q = q._transformVector(vector);
for (int i = 0; i < 3; i++) {
if (fabsf(vector_r(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
}
}
}
// test some values calculated with matlab
tol = 0.0001f;
q = quatFromEuler(M_PI_2_F, 0.0f, 0.0f);
vector_q = q._transformVector(vector);
Eigen::Vector3f vector_true = {1.00f, -1.00f, 1.00f};
for (size_t i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q = quatFromEuler(0.3f, 0.2f, 0.1f);
vector_q = q._transformVector(vector);
vector_true = {1.1566, 0.7792, 1.0273};
for (size_t i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q = quatFromEuler(-1.5f, -0.2f, 0.5f);
vector_q = q._transformVector(vector);
vector_true = {0.5095, 1.4956, -0.7096};
for (size_t i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q._transformVector(vector);
vector_true = { -1.3660, 0.3660, 1.0000};
for (size_t i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
}
*/
return rc;
}
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()));
}
template <typename PointT> void
pcl16::approximatePolygon2D (const typename pcl16::PointCloud<PointT>::VectorType &polygon,
typename pcl16::PointCloud<PointT>::VectorType &approx_polygon,
float threshold, bool refine, bool closed)
{
approx_polygon.clear ();
if (polygon.size () < 3)
return;
std::vector<std::pair<unsigned, unsigned> > intervals;
std::pair<unsigned,unsigned> interval (0, 0);
if (closed)
{
float max_distance = .0f;
for (unsigned idx = 1; idx < polygon.size (); ++idx)
{
float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) +
(polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y);
if (distance > max_distance)
{
max_distance = distance;
interval.second = idx;
}
}
for (unsigned idx = 1; idx < polygon.size (); ++idx)
{
float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) +
(polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y);
if (distance > max_distance)
{
max_distance = distance;
interval.first = idx;
}
}
if (max_distance < threshold * threshold)
return;
intervals.push_back (interval);
std::swap (interval.first, interval.second);
intervals.push_back (interval);
}
else
{
interval.first = 0;
interval.second = static_cast<unsigned int> (polygon.size ()) - 1;
intervals.push_back (interval);
}
std::vector<unsigned> result;
// recursively refine
while (!intervals.empty ())
{
std::pair<unsigned, unsigned>& currentInterval = intervals.back ();
float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y;
float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x;
float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x;
float linelen = 1.0f / sqrt (line_x * line_x + line_y * line_y);
line_x *= linelen;
line_y *= linelen;
line_d *= linelen;
float max_distance = 0.0;
unsigned first_index = currentInterval.first + 1;
unsigned max_index = 0;
// => 0-crossing
if (currentInterval.first > currentInterval.second)
{
for (unsigned idx = first_index; idx < polygon.size(); idx++)
{
float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
if (distance > max_distance)
{
max_distance = distance;
max_index = idx;
}
}
first_index = 0;
}
for (unsigned int idx = first_index; idx < currentInterval.second; idx++)
{
float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
if (distance > max_distance)
{
max_distance = distance;
max_index = idx;
}
}
if (max_distance > threshold)
{
std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
currentInterval.second = max_index;
intervals.push_back (interval);
}
else
{
result.push_back (currentInterval.second);
intervals.pop_back ();
}
}
approx_polygon.reserve (result.size ());
if (refine)
{
std::vector<Eigen::Vector3f> lines (result.size ());
std::reverse (result.begin (), result.end ());
for (unsigned rIdx = 0; rIdx < result.size (); ++rIdx)
{
unsigned nIdx = rIdx + 1;
if (nIdx == result.size ())
nIdx = 0;
Eigen::Vector2f centroid = Eigen::Vector2f::Zero ();
Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero ();
unsigned pIdx = result[rIdx];
unsigned num_points = 0;
if (pIdx > result[nIdx])
{
num_points = static_cast<unsigned> (polygon.size ()) - pIdx;
for (; pIdx < polygon.size (); ++pIdx)
{
covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
centroid [0] += polygon [pIdx].x;
centroid [1] += polygon [pIdx].y;
}
pIdx = 0;
}
num_points += result[nIdx] - pIdx;
for (; pIdx < result[nIdx]; ++pIdx)
{
covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
centroid [0] += polygon [pIdx].x;
centroid [1] += polygon [pIdx].y;
}
covariance.coeffRef (2) = covariance.coeff (1);
float norm = 1.0f / float (num_points);
centroid *= norm;
covariance *= norm;
covariance.coeffRef (0) -= centroid [0] * centroid [0];
covariance.coeffRef (1) -= centroid [0] * centroid [1];
covariance.coeffRef (3) -= centroid [1] * centroid [1];
float eval;
Eigen::Vector2f normal;
eigen22 (covariance, eval, normal);
// select the one which is more "parallel" to the original line
Eigen::Vector2f direction;
direction [0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x;
direction [1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y;
direction.normalize ();
if (fabs (direction.dot (normal)) > float(M_SQRT1_2))
{
std::swap (normal [0], normal [1]);
normal [0] = -normal [0];
}
// needs to be on the left side of the edge
if (direction [0] * normal [1] < direction [1] * normal [0])
normal *= -1.0;
lines [rIdx].head<2> () = normal;
lines [rIdx] [2] = -normal.dot (centroid);
}
float threshold2 = threshold * threshold;
for (unsigned rIdx = 0; rIdx < lines.size (); ++rIdx)
{
unsigned nIdx = rIdx + 1;
if (nIdx == result.size ())
nIdx = 0;
Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]);
vertex /= vertex [2];
vertex [2] = 0.0;
PointT point;
// test whether we need another edge since the intersection point is too far away from the original vertex
Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex;
pq [2] = 0.0;
float distance = pq.squaredNorm ();
if (distance > threshold2)
{
// test whether the old point is inside the new polygon or outside
if ((pq [0] * lines [rIdx] [0] + pq [1] * lines [rIdx] [1] < 0.0) &&
(pq [0] * lines [nIdx] [0] + pq [1] * lines [nIdx] [1] < 0.0) )
{
float distance1 = lines [rIdx] [0] * polygon[result[nIdx]].x + lines [rIdx] [1] * polygon[result[nIdx]].y + lines [rIdx] [2];
float distance2 = lines [nIdx] [0] * polygon[result[nIdx]].x + lines [nIdx] [1] * polygon[result[nIdx]].y + lines [nIdx] [2];
point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0];
point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1];
approx_polygon.push_back (point);
vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0];
vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1];
}
}
point.getVector3fMap () = vertex;
approx_polygon.push_back (point);
}
}
else
{
// we have a new polygon in results, but inverted (clockwise <-> counter-clockwise)
for (std::vector<unsigned>::reverse_iterator it = result.rbegin (); it != result.rend (); ++it)
approx_polygon.push_back (polygon [*it]);
}
}
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);
}
// 2D cross product of OA and OB vectors, i.e. z-component of their 3D cross product.
// Returns a positive value, if OAB makes a counter-clockwise turn,
// negative for clockwise turn, and zero if the points are collinear.
float cross(const Eigen::Vector2f O, const Eigen::Vector2f A, const Eigen::Vector2f B) {
return (long)(A.x() - O.x()) * (B.y() - O.y()) - (long)(A.y() - O.y()) * (B.x() - O.x());
}
void derivatives(const StateMatrix& states, StateMatrix& derivs,
const ControlMatrix& ctrls, const SimulationParameters& params,
const Map & map)
{
float cd_a_rho = params.linearDrag; // 0.1 coeff of drag * area * density of fluid
float k_elastic = params.elasticity; // 4000. // spring constant of ships
float rad = 1.; // leave radius 1 - we decided to change map scale instead
const Eigen::VectorXf &masses = params.shipDensities; // order of 1.0 Mass of ships
float spin_drag_ratio = params.rotationalDrag; // 1.8; // spin friction to translation friction
float eps = 1e-5; // Avoid divide by zero special cases
float mu = params.shipFriction; // 0.05; // friction coefficient between ships
float mu_wall = params.wallFriction; //0.25*?wallFriction; // 0.01; // wall friction parameter
float wall_restitution = params.wallRestitution; // 0.5
float ship_restitution = params.shipRestitution; // circa 0.5
float diameter = 2.*rad; // rad(i) + rad(j) for any i, j
float inertia_mass_ratio = 0.25;
float map_grid = rad * 2. + eps; // must be 2*radius + eps
std::unordered_map<std::pair<int, int>, std::vector<uint>,
boost::hash<std::pair<int, int>>> bins;
uint n = states.rows();
Eigen::MatrixXd f = Eigen::MatrixXd::Zero(n, 2);
Eigen::VectorXd trq = Eigen::VectorXd::Zero(n);
// rotationalThrust Order +- 10
// linearThrust Order +100
// mapscale order 10 - thats params.pixelsize
// Accumulate forces and torques into these:
uint collide_checks = 0; // debug count...
for (uint i=0; i<n; i++) {
Eigen::Vector2f pos_i;
pos_i(0) = states(i,0);
pos_i(1) = states(i,1);
Eigen::Vector2f vel_i;
vel_i(0) = states(i,2);
vel_i(1) = states(i,3);
float theta_i = states(i,4);
float w_i = states(i,5);
// 1. Control
float thrusting = ctrls(i, 0);
float turning = ctrls(i, 1);
f(i, 0) = thrusting * params.linearThrust * cos(theta_i);
f(i, 1) = thrusting * params.linearThrust * sin(theta_i);
trq(i) = turning * params.rotationalThrust;
// 2. Drag
f(i, 0) -= cd_a_rho * vel_i(0);
f(i, 1) -= cd_a_rho * vel_i(1);
trq(i) -= spin_drag_ratio*cd_a_rho*w_i*rad*rad; // * abs(w_i)
// 3. Inter-ship collisions against ships of lower index...
// Figure out this ship's hashes: It has 4 in 2 dimensions
std::unordered_set<uint> collision_shortlist;
std::pair<int, int> my_hash;
for (int dx=-1; dx < 2; dx+=2)
for (int dy=-1; dy < 2; dy+=2)
{
float x_mod = pos_i(0) + float(dx)*rad;
float y_mod = pos_i(1) + float(dy)*rad;
my_hash = std::make_pair(int(x_mod / map_grid),
int(y_mod / map_grid));
if (bins.count(my_hash) > 0)
{
// Already exists - shortlist others and add self
std::vector<uint> current_bin = bins.find(my_hash)->second;
// -->first is the key as it returns a key/value pair
for (uint bin_idx: current_bin)
if (bin_idx != i)
collision_shortlist.insert(bin_idx);
current_bin.push_back(i);
}
else
{
// didnt exist - add self, and push into map
std::vector<uint> current_bin;
current_bin.push_back(i);
bins.insert(std::make_pair(my_hash, current_bin));
}
}
for (uint j: collision_shortlist) { // =i+1; j<n; j++) {
collide_checks ++;
// std::cout << "Checking " << i << ", " << j << "\n";
Eigen::Vector2f pos_j;
pos_j(0) = states(j,0);
pos_j(1) = states(j,1);
Eigen::Vector2f vel_j;
vel_j(0) = states(j,2);
vel_j(1) = states(j,3);
float theta_j = states(j,4);
float w_j = states(j,5);
Eigen::Vector2f dP = pos_j - pos_i;
float dist = dP.norm() + eps - diameter;
Eigen::Vector2f dPhat = dP / (dP.norm() + eps);
if (dist < 0) {
// we have a collision interaction
// A. Direct collision: apply linear spring normal force
float f_magnitude = - dist * k_elastic; // dist < =
if ((vel_j - vel_i).dot(pos_j - pos_i) > 0)
f_magnitude *= ship_restitution;
Eigen::Vector2f f_norm = f_magnitude * dPhat;
f(i, 0) -= f_norm(0);
f(i, 1) -= f_norm(1);
f(j, 0) += f_norm(0);
f(j, 1) += f_norm(1);
// B. Surface frictions: approximate spin effects
Eigen::Vector2f perp; // surface tangent pointing +theta direction
perp(0) = -dPhat(1);
perp(1) = dPhat(0);
// relative velocities of surfaces
float v_rel = rad*w_i + rad*w_j + perp.dot(vel_i - vel_j);
float fric = f_magnitude * mu * sigmoid(v_rel);
Eigen::Vector2f f_fric = fric * perp;
f(i, 0) += f_fric(0);
f(i, 1) += f_fric(1);
f(j, 0) -= f_fric(0);
f(j, 1) -= f_fric(1);
trq(i) -= fric * rad;
trq(j) -= fric * rad;
} // end collision
} // end loop 3. opposing ship
// 4. Wall single body collisions
// compute distance to wall and local normals
float wall_dist, norm_x, norm_y;
interpolate_map(pos_i(0), pos_i(1), wall_dist, norm_x, norm_y, map, params);
float dist = wall_dist - rad;
if (dist < 0)
{
/* if (dist < -1.) */
/* assert(false); */
// Spring force
float f_norm_mag = -dist*k_elastic; // dist is negative, f_norm is +ve
if (norm_x*vel_i(0) + norm_y*vel_i(1) > 0)
f_norm_mag *= wall_restitution;
if (dist > -rad*0.25)
{
// not significantly through wall yet
f(i, 0) += f_norm_mag * norm_x;
f(i, 1) += f_norm_mag * norm_y;
}
else
{
// uh-oh - lets just SET normal forces and seriously damp vel
f(i, 0) = f_norm_mag * norm_x;
f(i, 1) = f_norm_mag * norm_y;
f(i, 0) -= 100. * vel_i(0);
f(i, 1) -= 100. * vel_i(1);
}
// Surface friction
Eigen::Vector2f perp; // surface tangent pointing +theta direction
perp(0) = -norm_y;
perp(1) = norm_x;
float v_rel = w_i * rad + vel_i(0)*norm_y - vel_i(1)*norm_x;
float fric = f_norm_mag * mu_wall * sigmoid(v_rel);
f(i, 0) -= fric*norm_y;
f(i, 1) += fric*norm_x;
trq(i) -= fric * rad;
}
} // end loop current ship
// std::cout << "Collision checks:" << collide_checks << "\n";
// Compose the vector of derivatives:
float vmax = 40.0;
for (int i=0; i<n; i++)
{
float vx = states(i,2);
float vy = states(i,3);
float speed = std::sqrt(vx*vx + vy*vy);
if (speed > vmax)
{
vx *= vmax/speed;
vy *= vmax/speed;
}
// x_dot = vx
derivs(i, 0) = vx;
// y_dot = vy
derivs(i, 1) = vy;
// vx_dot = fx / m
float ax = f(i,0)/masses(i);
float ay = f(i,1)/masses(i);
derivs(i, 2) = ax;
// vy_dot = fy / m
derivs(i, 3) = ay;
// theta_dot = omega
derivs(i, 4) = states(i, 5);
// omega_dot = T_r / (inertia_mass_ratio*m)
derivs(i,5) = trq(i) / (inertia_mass_ratio * masses(i));
}
// ux uy vx vy theta omega
// 0 1 2 3 4 5
}
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());
}
/** 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;
}
}
static Eigen::Vector2f normal(const Eigen::Vector2f & line)
{
return Eigen::Vector2f(line.y(), -line.x());
}
D2D1_POINT_2F PointConversion(const Eigen::Vector2f& p) {
D2D1_POINT_2F rv;
rv.x = p.x();
rv.y = p.y();
return rv;
}
/**
* 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);
}
auto match(vfloat2 p, vfloat2 tr_prediction,
F A, F B, GD Ag,
const int winsize,
const float min_ev_th,
const int max_interations,
const float convergence_delta)
{
typedef typename F::value_type V;
int WS = winsize;
int ws = winsize;
int hws = ws/2;
// Gradient matrix
Eigen::Matrix2f G = Eigen::Matrix2f::Zero();
int cpt = 0;
for(int r = -hws; r <= hws; r++)
for(int c = -hws; c <= hws; c++)
{
vfloat2 n = p + vfloat2(r, c);
if (A.has(n.cast<int>()))
{
Eigen::Matrix2f m;
auto g = Ag.linear_interpolate(n);
float gx = g[0];
float gy = g[1];
m <<
gx * gx, gx * gy,
gx * gy, gy * gy;
G += m;
cpt++;
}
}
// Check minimum eigenvalue.
float min_ev = 99999.f;
auto ev = (G / cpt).eigenvalues();
for (int i = 0; i < ev.size(); i++)
if (fabs(ev[i].real()) < min_ev) min_ev = fabs(ev[i].real());
if (min_ev < min_ev_th)
return std::pair<vfloat2, float>(vfloat2(-1,-1), FLT_MAX);
Eigen::Matrix2f G1 = G.inverse();
// Precompute gs and as.
vfloat2 prediction_ = p + tr_prediction;
vfloat2 v = prediction_;
Eigen::Vector2f nk = Eigen::Vector2f::Ones();
char gs_buffer[WS * WS * sizeof(vfloat2)];
vfloat2* gs = (vfloat2*) gs_buffer;
// was: vfloat2 gs[WS * WS];
typedef plus_promotion<V> S;
char as_buffer[WS * WS * sizeof(S)];
S* as = (S*) as_buffer;
// was: S as[WS * WS];
{
for(int i = 0, r = -hws; r <= hws; r++)
{
for(int c = -hws; c <= hws; c++)
{
vfloat2 n = p + vfloat2(r, c);
if (Ag.has(n.cast<int>()))
{
gs[i] = Ag.linear_interpolate(n).template cast<float>();
as[i] = cast<S>(A.linear_interpolate(n));
}
i++;
}
}
}
auto domain = B.domain();// - border(hws + 1);
// Gradient descent
for (int k = 0; k <= max_interations && nk.norm() >= convergence_delta; k++)
{
Eigen::Vector2f bk = Eigen::Vector2f::Zero();
// Temporal difference.
int i = 0;
for(int r = -hws; r <= hws; r++)
{
for(int c = -hws; c <= hws; c++)
{
vfloat2 n = p + vfloat2(r, c);
if (Ag.has(n.cast<int>()))
{
vfloat2 n2 = v + vfloat2(r, c);
auto g = gs[i];
float dt = (cast<float>(as[i]) - cast<float>(B.linear_interpolate(n2)));
bk += Eigen::Vector2f{g[0] * dt, g[1] * dt};
}
i++;
}
}
nk = G1 * bk;
v += vfloat2{nk[0], nk[1]};
if (!domain.has(v.cast<int>()))
return std::pair<vfloat2, float>(vfloat2(0, 0), FLT_MAX);
}
// Compute the SSD.
float err = 0;
for(int r = -hws; r <= hws; r++)
for(int c = -hws; c <= hws; c++)
{
vfloat2 n2 = v + vfloat2(r, c);
int i = (r+hws) * ws + (c+hws);
{
err += fabs(cast<float>(as[i] - cast<S>(B.linear_interpolate(n2))));
cpt++;
}
}
return std::pair<vfloat2, float>(v - p, err / (cpt));
}
void NinePatchComponent::buildVertices()
{
if(mVertices != NULL)
delete[] mVertices;
if(mColors != NULL)
delete[] mColors;
mTexture = TextureResource::get(mPath);
if(mTexture->getSize() == Eigen::Vector2i::Zero())
{
mVertices = NULL;
mColors = NULL;
LOG(LogWarning) << "NinePatchComponent missing texture!";
return;
}
mVertices = new Vertex[6 * 9];
mColors = new GLubyte[6 * 9 * 4];
updateColors();
const Eigen::Vector2f ts = mTexture->getSize().cast<float>();
//coordinates on the image in pixels, top left origin
const Eigen::Vector2f pieceCoords[9] = {
Eigen::Vector2f(0, 0),
Eigen::Vector2f(16, 0),
Eigen::Vector2f(32, 0),
Eigen::Vector2f(0, 16),
Eigen::Vector2f(16, 16),
Eigen::Vector2f(32, 16),
Eigen::Vector2f(0, 32),
Eigen::Vector2f(16, 32),
Eigen::Vector2f(32, 32),
};
const Eigen::Vector2f pieceSizes = getCornerSize();
//corners never stretch, so we calculate a width and height for slices 1, 3, 5, and 7
float borderWidth = mSize.x() - (pieceSizes.x() * 2); //should be pieceSizes[0] and pieceSizes[2]
//if(borderWidth < pieceSizes.x())
// borderWidth = pieceSizes.x();
float borderHeight = mSize.y() - (pieceSizes.y() * 2); //should be pieceSizes[0] and pieceSizes[6]
//if(borderHeight < pieceSizes.y())
// borderHeight = pieceSizes.y();
mVertices[0 * 6].pos = pieceCoords[0]; //top left
mVertices[1 * 6].pos = pieceCoords[1]; //top middle
mVertices[2 * 6].pos = pieceCoords[1] + Eigen::Vector2f(borderWidth, 0); //top right
mVertices[3 * 6].pos = mVertices[0 * 6].pos + Eigen::Vector2f(0, pieceSizes.y()); //mid left
mVertices[4 * 6].pos = mVertices[3 * 6].pos + Eigen::Vector2f(pieceSizes.x(), 0); //mid middle
mVertices[5 * 6].pos = mVertices[4 * 6].pos + Eigen::Vector2f(borderWidth, 0); //mid right
mVertices[6 * 6].pos = mVertices[3 * 6].pos + Eigen::Vector2f(0, borderHeight); //bot left
mVertices[7 * 6].pos = mVertices[6 * 6].pos + Eigen::Vector2f(pieceSizes.x(), 0); //bot middle
mVertices[8 * 6].pos = mVertices[7 * 6].pos + Eigen::Vector2f(borderWidth, 0); //bot right
int v = 0;
for(int slice = 0; slice < 9; slice++)
{
Eigen::Vector2f size;
//corners
if(slice == 0 || slice == 2 || slice == 6 || slice == 8)
size = pieceSizes;
//vertical borders
if(slice == 1 || slice == 7)
size << borderWidth, pieceSizes.y();
//horizontal borders
if(slice == 3 || slice == 5)
size << pieceSizes.x(), borderHeight;
//center
if(slice == 4)
size << borderWidth, borderHeight;
//no resizing will be necessary
//mVertices[v + 0] is already correct
mVertices[v + 1].pos = mVertices[v + 0].pos + size;
mVertices[v + 2].pos << mVertices[v + 0].pos.x(), mVertices[v + 1].pos.y();
mVertices[v + 3].pos << mVertices[v + 1].pos.x(), mVertices[v + 0].pos.y();
mVertices[v + 4].pos = mVertices[v + 1].pos;
mVertices[v + 5].pos = mVertices[v + 0].pos;
//texture coordinates
//the y = (1 - y) is to deal with texture coordinates having a bottom left corner origin vs. verticies having a top left origin
mVertices[v + 0].tex << pieceCoords[slice].x() / ts.x(), 1 - (pieceCoords[slice].y() / ts.y());
mVertices[v + 1].tex << (pieceCoords[slice].x() + pieceSizes.x()) / ts.x(), 1 - ((pieceCoords[slice].y() + pieceSizes.y()) / ts.y());
mVertices[v + 2].tex << mVertices[v + 0].tex.x(), mVertices[v + 1].tex.y();
mVertices[v + 3].tex << mVertices[v + 1].tex.x(), mVertices[v + 0].tex.y();
mVertices[v + 4].tex = mVertices[v + 1].tex;
mVertices[v + 5].tex = mVertices[v + 0].tex;
v += 6;
}
// round vertices
for(int i = 0; i < 6*9; i++)
{
mVertices[i].pos = roundVector(mVertices[i].pos);
}
}
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());
}
static float cross(const Eigen::Vector2f & a, const Eigen::Vector2f & b)
{
return a.x() * b.y() - a.y() * b.x();
}
