vrpn_HapticPosition TexturePlane::projectPointOrthogonalToStaticPlane(vrpn_HapticPosition pnt) const
{
double y = computeHeight(pnt[0], pnt[2]);
vrpn_HapticPosition projection = pnt;
projection[1] = y;
return projection;
}
void Segmentation::createPlane(std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &clusters)
{
_planes.clear();
for (int v = 0 ; v < clusters.size(); v++)
{
Plane plane;
//pcl::transformPointCloud(*clusters[v], *clusters[v], transformXY);
plane.setPlaneCloud(clusters[v]);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr concaveHull(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::ConcaveHull<pcl::PointXYZRGBA> chull;
chull.setInputCloud (clusters[v]);
chull.setAlpha (0.1);
chull.reconstruct (*concaveHull);
//vectorHull.push_back(concaveHull);
plane.setHull(concaveHull);
_planes.push_back(plane);
}
//pcl::PointCloud<pcl::sPointXYZRGBA>::Ptr gr(new pcl::PointCloud<pcl::PointXYZRGBA>);
//gr = _ground.getPlaneCloud();
//pcl::transformPointCloud(*_mainCloud, *_mainCloud, transformXY);
//pcl::transformPointCloud(*gr, *gr, transformXY);
//_ground.setPlaneCloud(gr);
if(!first)
{
computeTransformation();
first = true;
}
computeHeight();
}
int ToolTipWidget::computeHeight(const QModelIndex &index) const
{
int s = rowHeight(index);
for (int i = 0; i < model()->rowCount(index); ++i)
s += computeHeight(model()->index(i, 0, index));
return s;
}
// for texture:
double TexturePlane::computeHeight(double x, double z) const
{
double r_sq = x*x + z*z;
double r = sqrt(r_sq);
double height = computeHeight(r);
return height;
}
Q_SLOT void ToolTipWidget::computeSize()
{
int columns = 0;
for (int i = 0; i < 3; ++i) {
resizeColumnToContents(i);
columns += sizeHintForColumn(i);
}
int rows = computeHeight(QModelIndex());
m_size = QSize(columns + 5, rows + 5);
setMinimumSize(m_size);
setMaximumSize(m_size);
}
// returns what a single wavelength looks like (height in mm as a function
// of distance from texture origin)
void TexturePlane::getTextureShape(int nsamples, float *surface) const
{
double radius = 0;
double incr = texWL/(float)(nsamples-1);
int i;
if (texWN == 0) {
surface[0] = (float)computeHeight(0);
for (i = 1; i < nsamples; i++){
surface[i] = surface[0];
}
}
else {
for (i = 0; i < nsamples; i++){
// (radius, height)
surface[i] = (float)computeHeight(radius);
radius += incr;
}
}
}
void RectangleProcessor::processRectangle(Rectangle input, int inputTarget)
{
// Variable Initializations
inputRect = input;
target = inputTarget;
// Process Rectangle
computeElevation();
computeProportionalDistance();
computeConstantsDistance();
computeHeight();
computeHorizontalDistance();
computeAzimuth();
//fixHeight();
computeTilt();
computeAspectRatio();
computeLogAspectRatios();
}
/*
IMPORTANT: This function assumes that the plane equation is
y = 0 for simplicity of computation
the position is expected to be in a coordinate system such that
the plane has this equation.
Therefore, to simulate other planes, the position in world coordinates
must be transformed correctly
*/
vrpn_HapticPosition TexturePlane::computeSCPfromGradient(vrpn_HapticPosition currentPos) const
{
double pos_x, pos_y, pos_z, pos_r; // position in local coordinates
double scp_h; // scp in local coordinates
// first, translate position into texture coordinates
pos_x = (currentPos[0]);
pos_y = currentPos[1];
pos_z = (currentPos[2]);
pos_r = sqrt(pos_x*pos_x + pos_z*pos_z);
// the first time we contact
if (!inContact){
return currentPos;
}
vrpn_HapticPosition newSCP;
// get height of surface at contact point
scp_h = computeHeight(pos_r);
// return current position if we are not touching the surface
if (scp_h < pos_y)
newSCP = currentPos;
else {
vrpn_HapticVector normal = computeNormal(pos_x, pos_z);
normal.normalize();
double delta_y = scp_h - pos_y;
// compute scp in untranslated coordinates
newSCP[0] = pos_x + normal[0]*(delta_y/normal[1]);
newSCP[1] = scp_h;
newSCP[2] = pos_z + normal[2]*(delta_y/normal[1]);
}
return newSCP;
}
Cylinder::Cylinder(unsigned int id,
Eigen::Vector3f origin,
Eigen::Vector3f sym_axis,
double radius,
std::vector<std::vector<Eigen::Vector3f> >& contours_3d,
std::vector<bool> holes,
std::vector<float> color)
: Polygon()
{
//Cylinder();
id_ = id;
sym_axis_ = sym_axis;
r_ = radius;
holes_ = holes;
color_ = color;
if (sym_axis_[2] < 0 )
{
sym_axis_ = sym_axis_ * -1;
}
computePose(origin, contours_3d);
setContours3D(contours_3d);
computeHeight();
}
Cylinder::Cylinder(unsigned int id,
Eigen::Vector3f origin,
Eigen::Vector3f sym_axis,
double radius,
std::vector<pcl::PointCloud<pcl::PointXYZ> >& contours_3d,
std::vector<bool> holes,
std::vector<float> color)
: Polygon()
{
std::vector<std::vector<Eigen::Vector3f> > contours_eigen;
for(unsigned int i = 0; i < contours_3d.size(); i++)
{
std::vector<Eigen::Vector3f> c_eigen;
for(unsigned int j = 0; j < contours_3d[i].size(); j++)
{
Eigen::Vector3f pt = contours_3d[i].points[j].getVector3fMap();
c_eigen.push_back(pt);
}
contours_eigen.push_back(c_eigen);
}
id_ = id;
sym_axis_ = sym_axis;
r_ = radius;
//std::cout << "origin " << origin << std::endl;
//std::cout << "r " << r_ << std::endl;
holes_ = holes;
color_ = color;
if (sym_axis_[2] < 0 )
{
sym_axis_ = sym_axis_ * -1;
}
computePose(origin, contours_eigen);
setContours3D(contours_eigen);
computeHeight();
//Cylinder(id, origin, sym_axis, radius, contours_eigen, holes, color);
}
void
MppTabBar :: paintEvent(QPaintEvent *event)
{
int ht = computeHeight(width());
if (ht != height())
doRepaintEnqueue();
QPainter paint(this);
int w = width();
int h = height();
int x_off = 0;
int y_off;
int n;
int r;
paint.setRenderHints(QPainter::Antialiasing, 1);
paint.setFont(font());
QColor grey(192,192,192);
QColor light(128,128,128);
QColor white(255,255,255);
QColor black(0,0,0);
paint.setPen(QPen(grey, 0));
paint.setBrush(grey);
paint.drawRoundedRect(QRect(0,0,w,h), 4, 4);
for (r = n = 0; n != ntabs; n++) {
int dw = computeWidth(n);
if (x_off != 0 && x_off + dw >= w) {
x_off = 0;
r++;
}
y_off = r * basic_size * 2;
if (isVisible(tabs[n].w)) {
paint.setPen(QPen(black, 0));
paint.setBrush(black);
} else {
paint.setPen(QPen(light, 0));
paint.setBrush(light);
}
if (tabs[n].flags & FLAG_LEFT) {
QPoint temp[3] = {
QPoint(x_off, y_off + basic_size),
QPoint(x_off + basic_size, y_off + (basic_size / 4)),
QPoint(x_off + basic_size, y_off + (basic_size * 2) - (basic_size / 4) - 1)
};
paint.drawPolygon(temp, 3);
} else if (tabs[n].flags & FLAG_RIGHT) {
QPoint temp[3] = {
QPoint(x_off + dw - basic_size, y_off + (basic_size / 4)),
QPoint(x_off + dw, y_off + basic_size),
QPoint(x_off + dw - basic_size, y_off + (basic_size * 2) - (basic_size / 4) - 1)
};
paint.drawPolygon(temp, 3);
}
tabs[n].area = QRect(x_off + (basic_size / 2), y_off + (basic_size / 4),
dw - basic_size, (basic_size * 2) - (basic_size / 2));
if (isVisible(tabs[n].w)) {
QRect area(x_off + basic_size, y_off + (basic_size / 4),
dw - (2*basic_size), (basic_size * 2) - (basic_size / 2));
paint.setPen(QPen(white, 0));
paint.setBrush(white);
paint.drawRect(area);
}
paint.setPen(QPen(black, 0));
paint.setBrush(black);
paint.drawText(QRect(x_off + basic_size + (basic_size / 4),
y_off + (basic_size / 2),
dw - (2 * basic_size) - (basic_size / 2), basic_size),
Qt::TextSingleLine | Qt::AlignCenter, tabs[n].name);
x_off += dw;
}
if (x_off != 0) {
x_off = 0;
r++;
}
for (n = 0; n != nwidgets; n++) {
int dw = (widgets[n].pWidget ? 4 : 3) * basic_size;
if (x_off != 0 && x_off + dw >= w) {
x_off = 0;
r++;
}
y_off = r * basic_size * 2;
x_off += dw;
if (widgets[n].pWidget == 0)
continue;
widgets[n].area = QRect(x_off - dw, y_off,
4 * basic_size, 2 * basic_size);
widgets[n].pWidget->setGeometry(widgets[n].area);
QWidget::eventFilter(widgets[n].pWidget, event);
}
y_off = (r + 1) * basic_size * 2;
}
