int
common_suffix_length (const char* a, const char* b)
{
int length = 0;
const char* a_iter = last_position(a);
const char* b_iter = last_position(b);
while (a_iter >= a && b_iter >= b)
{
if (*a_iter != *b_iter) { break; }
a_iter--;
b_iter--;
length++;
}
return length;
}
char*
remove_extension (const char* file_name)
{
int length = 0;
int prefix_length = 0;
const char* iter = last_position(file_name);
while (iter > file_name)
{
if (*iter == '.')
{
prefix_length = length;
}
iter--;
length++;
}
if (length - prefix_length > 0)
{
char* file_name_without_extension =
(char*)calloc(length - prefix_length, sizeof(char));
if (file_name_without_extension)
{
strncpy(file_name_without_extension, file_name, length-1);
/* todo: this is a common operation, factor it out */
*(file_name_without_extension + length) = '\0';
}
return file_name_without_extension;
}
return NULL;
}
void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& message )
{
++messages_received_;
if( !validateFloats( *message ))
{
setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
return;
}
setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );
if( last_used_message_ )
{
Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x, last_used_message_->pose.pose.position.y, last_used_message_->pose.pose.position.z);
Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y, message->pose.pose.position.z);
Ogre::Quaternion last_orientation(last_used_message_->pose.pose.orientation.w, last_used_message_->pose.pose.orientation.x, last_used_message_->pose.pose.orientation.y, last_used_message_->pose.pose.orientation.z);
Ogre::Quaternion current_orientation(message->pose.pose.orientation.w, message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z);
if( (last_position - current_position).length() < position_tolerance_property_->getFloat() &&
(last_orientation - current_orientation).normalise() < angle_tolerance_property_->getFloat() )
{
return;
}
}
Arrow* arrow = new Arrow( scene_manager_, scene_node_, 0.8f, 0.05f, 0.2f, 0.2f );
transformArrow( message, arrow );
QColor color = color_property_->getColor();
arrow->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );
float length = length_property_->getFloat();
Ogre::Vector3 scale( length, length, length );
arrow->setScale( scale );
arrows_.push_back( arrow );
last_used_message_ = message;
context_->queueRender();
}
std::list<SimplifySketchTool::SortPoint> SimplifySketchTool::GetPoints( TopoDS_Wire wire, const double deviation )
{
std::list<SortPoint> points;
std::vector<TopoDS_Edge> edges = SortEdges(wire);
SortPoint last_position(0.0, 0.0, 0.0);
for (std::vector<TopoDS_Edge>::size_type i=0; i<edges.size(); i++)
{
const TopoDS_Shape &E = edges[i];
// enum GeomAbs_CurveType
// 0 - GeomAbs_Line
// 1 - GeomAbs_Circle
// 2 - GeomAbs_Ellipse
// 3 - GeomAbs_Hyperbola
// 4 - GeomAbs_Parabola
// 5 - GeomAbs_BezierCurve
// 6 - GeomAbs_BSplineCurve
// 7 - GeomAbs_OtherCurve
BRepAdaptor_Curve curve(TopoDS::Edge(E));
GeomAbs_CurveType curve_type = curve.GetType();
switch(curve_type)
{
case GeomAbs_Line:
// make a line
{
double uStart = curve.FirstParameter();
double uEnd = curve.LastParameter();
gp_Pnt PS;
gp_Vec VS;
curve.D1(uStart, PS, VS);
gp_Pnt PE;
gp_Vec VE;
curve.D1(uEnd, PE, VE);
if (last_position == SortPoint(PS))
{
// We're heading towards the PE point.
SortPoint point(PE);
points.push_back(point);
last_position = point;
} // End if - then
else if (last_position == SortPoint(PE))
{
SortPoint point(PS);
points.push_back(point);
last_position = point;
}
else
{
SortPoint start(PS);
SortPoint end(PE);
if (i < (edges.size()-1))
{
if (! DirectionTowarardsNextEdge( edges[i], edges[i+1] ))
{
// The next edge is closer to this edge's start point. reverse direction
// so that the next movement is better.
SortPoint temp = start;
start = end;
end = temp;
}
}
points.push_back(start);
points.push_back(end);
last_position = end;
}
}
break;
default:
{
// make lots of small lines
double uStart = curve.FirstParameter();
double uEnd = curve.LastParameter();
gp_Pnt PS;
gp_Vec VS;
curve.D1(uStart, PS, VS);
gp_Pnt PE;
gp_Vec VE;
curve.D1(uEnd, PE, VE);
TopoDS_Edge edge(TopoDS::Edge(E));
BRepTools::Clean(edge);
BRepMesh::Mesh(edge, deviation);
TopLoc_Location L;
Handle(Poly_Polygon3D) Polyg = BRep_Tool::Polygon3D(edge, L);
if (!Polyg.IsNull()) {
const TColgp_Array1OfPnt& Points = Polyg->Nodes();
Standard_Integer po;
int i = 0;
std::list<SortPoint> interpolated_points;
for (po = Points.Lower(); po <= Points.Upper(); po++, i++) {
SortPoint p = (Points.Value(po)).Transformed(L);
interpolated_points.push_back(p);
} // End for
// See if we should go from the start to the end or the end to the start.
if (*interpolated_points.rbegin() == last_position)
{
// We need to go from the end to the start. Reverse the point locations to
// make this easier.
interpolated_points.reverse();
} // End if - then
if (*interpolated_points.begin() != last_position)
{
// This curve is not nearby to the last_position. Rapid to the start
// point to start this off.
// We need to move to the start BEFORE machining this line.
SortPoint start(last_position);
SortPoint end(*interpolated_points.begin());
points.push_back(end);
last_position = end;
}
for (std::list<SortPoint>::iterator itPoint = interpolated_points.begin(); itPoint != interpolated_points.end(); itPoint++)
{
if (*itPoint != last_position)
{
points.push_back(*itPoint);
last_position = *itPoint;
} // End if - then
} // End for
} // End if - then
}
break;
} // End switch
}
return(points);
}
