visualization_msgs::Marker
makeMarker(const Eigen::Vector3d& a,
const Eigen::Vector3d& b,
const Eigen::Vector3d& c,
const std::string& ns)
{
visualization_msgs::Marker marker;
marker.header.frame_id = "world";
marker.header.stamp = ros::Time::now();
marker.ns = ns;
marker.id = 0;
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.05;
marker.color.a = 1.0;
if (ns == "1")
{
marker.color.r = 1.0;
}
else if (ns == "2")
{
marker.color.g = 1.0;
}
else if (ns == "3")
{
marker.color.b = 1.0;
}
else if (ns == "0")
{
marker.color.g = 1.0;
marker.color.r = 1.0;
}
else
{
marker.color.g = marker.color.b = marker.color.r = 1.0;
}
marker.lifetime = ros::Duration(0);
marker.frame_locked = false;
marker.points.push_back(fromVec(Eigen::Vector3d(0, 0, 0)));
marker.points.push_back(fromVec(a));
marker.points.push_back(fromVec(b));
marker.points.push_back(fromVec(c));
return marker;
}
void PrintAllCombinationsClean(const std::vector<DataType>& arrayNoDup)
{
std::vector<int>::size_type length = arrayNoDup.size();
std::vector<int> fromVec(length, 0);
std::vector<int> toVec(length, 1);
std::vector<int> selectVec(length, 0);
std::vector<int>::size_type curDigit;
do
{
PrintOut(selectVec, arrayNoDup);
for (curDigit=0;
curDigit<length && ++selectVec[curDigit]>toVec[curDigit];
selectVec[curDigit] = fromVec[curDigit], ++curDigit);
} while (curDigit<length);
}
void GLViewer::timerEvent (QTimerEvent *event) {
if (resetTimer) {
killTimer(event->timerId ());
resetTimer = false;
if (enableGravity) {
this->startTimer(200);
} else {
this->startTimer (200);
}
}
QPoint point = mapFromGlobal(QCursor::pos ());
camera()->convertClickToLine(point, orig, dir);
// Find the selectedPoint coordinates, using camera()->pointUnderPixel().
bool found;
selectedPoint = camera()->pointUnderPixel(point, found);
selectedPoint -= 0.01f*dir; // Small offset to make point clearly visible.
if (enableGravity) {
if ((qAbs(m_frame.translation ().z) > velocity) && (m_frame.translation ().z < 0)) {
double liftAmount = fallZOffset(m_frame.translation ().z);
this->oldZValue = m_frame.translation ().z;
if (!m_frame.isManipulated ()) {
if (liftAmount > 0) {
m_frame.translate (0.0f, 0.0f, (float) gravityForce);
velocity = 0 - liftAmount + gravityForce;
}
if (liftAmount < 0) {
velocity += gravityForce;
m_frame.translate (0.0f, 0.0f, (float) velocity);
}
}
} else {
m_frame.setTranslation ( m_frame.translation ().x, m_frame.translation ().y, 0.0f);
this->oldZValue = m_frame.translation ().z;
velocity = (-1 * (velocity - qAbs(m_frame.translation ().z)));
if (qAbs(velocity) > gravityForce) {
m_frame.translate (0.0f, 0.0f, (float) velocity);
} else {
m_frame.setTranslation ( m_frame.translation ().x, m_frame.translation ().y, 0.0f);
velocity = 0;
this->oldZValue = m_frame.translation ().z;
}
}
}
QString msg;
msg.append("[OrientAxis->Viewdir, OrientAngle] Angle: ");
msg.append(QString("%1").arg(AngleBetweenVectors (fromVec(camera()->orientation().axis ()), fromVec(camera()->viewDirection ()))));
msg.append(" , ");
msg.append(QString("%1").arg(camera()->orientation ().angle ()));
msg.append(" | [Cam] Orient Axis [View Dir]: ");
msg.append(stringFromVector3d(fromVec(camera()->orientation ().axis ())));
msg.append(QString(" [%1]").arg(stringFromVector3d(fromVec(camera()->viewDirection ()))));
//camera()->setOrientation (Quaternion(camera()->orientation ().axis (), AngleBetweenVectors (fromVec(selectedPoint), fromVec(orig))));
emit this->sbMsg (msg);
this->update ();
}
