void AngleVariation::checkParentBranchIntersection(QPointF* parentBounds)
{
QLineF sideLine1 = QLineF(parentBounds[0], parentBounds[3]);
QLineF sideLine2 = QLineF(parentBounds[1], parentBounds[2]);
float tempSide1 = findDistance(branchLine.p1(), closestPointOnLine(sideLine1.p1(), sideLine1.p2(), branchLine.p1()));
float tempSide2 = findDistance(branchLine.p1(), closestPointOnLine(sideLine2.p1(), sideLine2.p2(), branchLine.p1()));
QLineF chosenLine;
if (tempSide1 < tempSide2)
chosenLine = sideLine1;
else
chosenLine = sideLine2;
QPointF point1 = calcVarExtremePoint(chosenLine, branchLine.p1(), length);
QPointF point2 = findAdjacentPoint(branchLine.p1(), point1);
QPointF control = calcVarExtremePoint(branchLine, branchLine.p1(), length);
if (findDistance(origPoint, control) >= findDistance(point1, control))
{
origPoint = point1;
variationPoint = calculateOpposingPoint(origPoint);
}
if (findDistance(origPoint, control) >= findDistance(point2, control))
{
origPoint = point2;
variationPoint = calculateOpposingPoint(origPoint);
}
}
IC void closestPointOnTriangle(Fvector& result, const Fvector* V, const Fvector& p) {
Fvector Rab; closestPointOnLine(Rab, V[0], V[1], p); float dAB = p.distance_to_sqr(Rab);
Fvector Rbc; closestPointOnLine(Rbc, V[1], V[2], p); float dBC = p.distance_to_sqr(Rbc);
Fvector Rca; closestPointOnLine(Rca, V[2], V[0], p); float dCA = p.distance_to_sqr(Rca);
float min = dAB;
result.set(Rab);
if (dBC < min) {
min = dBC;
result.set(Rbc);
}
if (dCA < min)
result.set(Rca);
}
bool FloorManager::handleClosestPoint(int &setX, int &setY, int &setPoly) {
int gotX = 320, gotY = 200, gotPoly = -1, i, j, xTest1, yTest1, xTest2, yTest2, closestX, closestY, oldJ, currentDistance = 0xFFFFF, thisDistance;
for (i = 0; i < _currentFloor->numPolygons; i++) {
oldJ = _currentFloor->polygon[i].numVertices - 1;
for (j = 0; j < _currentFloor->polygon[i].numVertices; j++) {
xTest1 = _currentFloor->vertex[_currentFloor->polygon[i].vertexID[j]].x;
yTest1 = _currentFloor->vertex[_currentFloor->polygon[i].vertexID[j]].y;
xTest2 = _currentFloor->vertex[_currentFloor->polygon[i].vertexID[oldJ]].x;
yTest2 = _currentFloor->vertex[_currentFloor->polygon[i].vertexID[oldJ]].y;
closestPointOnLine(closestX, closestY, xTest1, yTest1, xTest2, yTest2, setX, setY);
xTest1 = setX - closestX;
yTest1 = setY - closestY;
thisDistance = xTest1 * xTest1 + yTest1 * yTest1;
if (thisDistance < currentDistance) {
currentDistance = thisDistance;
gotX = closestX;
gotY = closestY;
gotPoly = i;
}
oldJ = j;
}
}
if (gotPoly == -1)
return false;
setX = gotX;
setY = gotY;
setPoly = gotPoly;
return true;
}
QPointF AngleVariation::calculateOpposingPoint(QPointF point1)
{
// Find closest point on line
QPointF tempMidPoint = closestPointOnLine(branchLine.p1(), branchLine.p2(), point1);
// Use closest point as midpoint to find opposing point
QPointF adjacentPoint = findAdjacentPoint(tempMidPoint, point1);
// Return point
return adjacentPoint;
}
/* MathStuff::distanceToLineFast
* Returns the shortest 'distance' between the point at [x,y] and the
* line from [x1,y1] to [x2,y2]. The distance returned isn't the
* real distance, but can be used to find the 'closest' line to the
* point
*******************************************************************/
double MathStuff::distanceToLineFast(double x, double y, double x1, double y1, double x2, double y2)
{
// Calculate intersection point
fpoint2_t i = closestPointOnLine(x, y, x1, y1, x2, y2);
// Return fast distance between intersection and point
// which is the shortest distance to the line
return (i.x-x)*(i.x-x) + (i.y-y)*(i.y-y);
}
/* MathStuff::distanceToLineFast
* Returns the shortest 'distance' between the given point and line.
* The distance returned isn't the real distance, but can be used to
* find the 'closest' line to the point
*******************************************************************/
double MathStuff::distanceToLineFast(fpoint2_t point, fseg2_t line)
{
// Calculate intersection point
fpoint2_t i = closestPointOnLine(point, line);
// Return fast distance between intersection and point
// which is the shortest distance to the line
return (i.x-point.x)*(i.x-point.x) + (i.y-point.y)*(i.y-point.y);
}
/* MathStuff::distanceToLine
* Returns the shortest distance between the point at [x,y] and the
* line from [x1,y1] to [x2,y2]
*******************************************************************/
double MathStuff::distanceToLine(fpoint2_t point, fseg2_t line)
{
// Calculate intersection point
fpoint2_t i = closestPointOnLine(point, line);
// Return distance between intersection and point
// which is the shortest distance to the line
return MathStuff::distance(i, point);
}
bool handleClosestPoint (int & setX, int & setY, int & setPoly) {
int gotX = 320, gotY = 200, gotPoly = -1, i, j, xTest1, yTest1,
xTest2, yTest2, closestX, closestY, oldJ, currentDistance = 0xFFFFF,
thisDistance;
// FILE * dbug = fopen ("debug_closest.txt", "at");
// fprintf (dbug, "\nGetting closest point to %i, %i\n", setX, setY);
for (i = 0; i < currentFloor -> numPolygons; i ++) {
oldJ = currentFloor -> polygon[i].numVertices - 1;
for (j = 0; j < currentFloor -> polygon[i].numVertices; j ++) {
// fprintf (dbug, "Polygon %i, line %i... ", i, j);
xTest1 = currentFloor -> vertex[currentFloor -> polygon[i].vertexID[j]].x;
yTest1 = currentFloor -> vertex[currentFloor -> polygon[i].vertexID[j]].y;
xTest2 = currentFloor -> vertex[currentFloor -> polygon[i].vertexID[oldJ]].x;
yTest2 = currentFloor -> vertex[currentFloor -> polygon[i].vertexID[oldJ]].y;
closestPointOnLine (closestX, closestY, xTest1, yTest1, xTest2, yTest2, setX, setY);
// fprintf (dbug, "closest point is %i, %i... ", closestX, closestY);
xTest1 = setX - closestX;
yTest1 = setY - closestY;
thisDistance = xTest1 * xTest1 + yTest1 * yTest1;
// fprintf (dbug, "Distance squared %i\n", thisDistance);
if (thisDistance < currentDistance) {
// fprintf (dbug, "** We have a new winner! **\n");
currentDistance = thisDistance;
gotX = closestX;
gotY = closestY;
gotPoly = i;
}
oldJ = j;
}
}
// fclose (dbug);
if (gotPoly == -1) return false;
setX = gotX;
setY = gotY;
setPoly = gotPoly;
return true;
}
void PathHandler::scanCb(const sensor_msgs::LaserScan &scan)
{
if (mFollowState != FOLLOW_STATE_BUSY || updateCurrentPosition() == false || getPathSize() == 0)
return;
uint32_t size = (scan.angle_max - scan.angle_min) / scan.angle_increment;
for (uint32_t i = 0; i < size; i++)
{
if (scan.ranges[i] >= scan.range_max)
continue;
geometry_msgs::PointStamped tmp, p;
tmp.header.frame_id = "/base_link";
tmp.header.stamp = ros::Time(0);
tmp.point.x = scan.ranges[i] * cosf(scan.angle_min + scan.angle_increment * i);
tmp.point.y = scan.ranges[i] * sinf(scan.angle_min + scan.angle_increment * i);
try
{
mTransformListener.transformPoint("/map", tmp, p);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
return;
}
for (uint32_t j = mCurrentPathIndex; j < getPathSize() - 1; j++)
{
geometry_msgs::Point closest = closestPointOnLine(p.point, mPath[j].position, mPath[j+1].position);
if (distanceBetweenPoints(p.point, closest) <= ROBOT_RADIUS)
{
ROS_INFO("Detected obstacle on path, resending goal. Distance: %f", distanceBetweenPoints(p.point, closest));
resendGoal();
return;
}
}
}
}
Vector Physics::closestPointOnLine(Vector lineStart, Vector lineEnd, Vector point) {
return closestPointOnLine(lineStart.x, lineStart.y, lineEnd.x, lineEnd.y, point.x, point.y);
}
/**
* Updates path logic.
*/
void PathHandler::updatePath()
{
geometry_msgs::Twist command;
publishState();
// no path? nothing to handle
if (getPathSize() == 0)
return;
// update our position if possible.
if (updateCurrentPosition() == false)
{
ROS_ERROR("Failed to update robot position.");
return;
}
if (mCurrentPathIndex < getPathSize() - 1) // we are moving along a line segment in the path
{
geometry_msgs::Point robotPos;
robotPos.x = mRobotPosition.getOrigin().x();
robotPos.y = mRobotPosition.getOrigin().y();
geometry_msgs::Point closestOnPath = closestPointOnLine(robotPos, mPath[mCurrentPathIndex].position, mPath[mCurrentPathIndex + 1].position);
//ROS_INFO("robotPos[%lf, %lf], closestOnPath[%lf, %lf]", robotPos.x, robotPos.y, closestOnPath.x, closestOnPath.y);
if (distanceBetweenPoints(closestOnPath, robotPos) > mResetDistanceTolerance)
{
ROS_INFO("Drove too far away from path, re-sending goal.");
mFollowState = FOLLOW_STATE_IDLE;
resendGoal();
clearPath();
return;
}
geometry_msgs::Point pointOnPath = getPointOnPathWithDist(closestOnPath, mLookForwardDistance);
//ROS_INFO("closestOnPath[%lf, %lf], pointOnPath[%lf, %lf]", closestOnPath.x, closestOnPath.y, pointOnPath.x, pointOnPath.y);
double angle = angleTo(pointOnPath);
/*double robotYaw = tf::getYaw(mRobotPosition.getRotation()); // only used for printing
ROS_INFO("Follow state: %d Robot pos: (%lf, %lf, %lf). Target pos: (%lf, %lf, %lf). RobotYaw: %lf. FocusYaw: %lf", mFollowState, mRobotPosition.getOrigin().getX(),
mRobotPosition.getOrigin().getY(), mRobotPosition.getOrigin().getZ(), pointOnPath.x,
pointOnPath.y, pointOnPath.z, robotYaw, angle);*/
//ROS_INFO("Angle to path: %f", angle);
command.linear.x = getScaledLinearSpeed(angle);
command.angular.z = getScaledAngularSpeed(angle);
if (distanceBetweenPoints(closestOnPath, mPath[mCurrentPathIndex + 1].position) < mDistanceTolerance)
{
//ROS_INFO("Moving to next line segment on path.");
mCurrentPathIndex++;
}
publishLocalPath(command.linear.x * 3, angle);
}
else // only rotate for final yaw
{
double yaw = tf::getYaw(mPath[getPathSize() - 1].orientation);
btVector3 orientation = btVector3(cos(yaw), sin(yaw), 0.0).rotate(btVector3(0,0,1), -tf::getYaw(mRobotPosition.getRotation()));
double angle = atan2(orientation.getY(), orientation.getX());
//ROS_INFO("Angle to final orientation: %f", angle);
if (fabs(angle) > mFinalYawTolerance)
command.angular.z = getScaledAngularSpeed(angle, true);
else
{
// path is done!
mFollowState = FOLLOW_STATE_FINISHED;
clearPath();
}
publishLocalPath(1.0, angle);
}
mCommandPub.publish(command);
}
// return closest point on triangle to the given point, and the distance
// betweeen them.
// If distance is greater than max_dist then skip the calculations and just return the approximate distance (anything greater than max_dist).
double mesh_core::closestPointOnTriangle(
const Eigen::Vector3d& tri_a,
const Eigen::Vector3d& tri_b,
const Eigen::Vector3d& tri_c,
const Eigen::Vector3d& point,
Eigen::Vector3d& closest_point,
double max_dist)
{
Eigen::Vector3d ab = tri_b - tri_a;
Eigen::Vector3d ac = tri_c - tri_a;
Eigen::Vector3d n = ab.cross(ac);
Eigen::Vector3d norm = n.normalized();
Eigen::Vector3d ap = point - tri_a;
double dist = norm.dot(ap);
double abs_dist = std::abs(dist);
if (abs_dist >= max_dist)
return abs_dist;
closest_point = point - dist * norm;
if (acorn_closest_debug)
{
logInform(" CDB: tri_a (%8.4f %8.4f %8.4f)",
tri_a.x(),
tri_a.y(),
tri_a.z());
logInform(" CDB: tri_b (%8.4f %8.4f %8.4f)",
tri_b.x(),
tri_b.y(),
tri_b.z());
logInform(" CDB: tri_c (%8.4f %8.4f %8.4f)",
tri_c.x(),
tri_c.y(),
tri_c.z());
logInform(" CDB: point (%8.4f %8.4f %8.4f)",
point.x(),
point.y(),
point.z());
logInform(" CDB: intersect (%8.4f %8.4f %8.4f)",
closest_point.x(),
closest_point.y(),
closest_point.z());
}
Eigen::Vector3d ab_norm = ab.cross(norm);
if (acorn_closest_debug)
{
logInform(" CDB: ab_norm (%8.4f %8.4f %8.4f) dp=%8.4f >? da=%8.4f",
ab_norm.x(),
ab_norm.y(),
ab_norm.z(),
ab_norm.dot(point),
ab_norm.dot(tri_a));
}
if (ab_norm.dot(point) > ab_norm.dot(tri_a))
{
if (acorn_closest_debug)
{
logInform(" CDB: beyond ab");
}
return closestPointOnLine(tri_a, tri_b, point, closest_point);
}
Eigen::Vector3d ac_norm = ac.cross(norm);
if (acorn_closest_debug)
{
logInform(" CDB: ac_norm (%8.4f %8.4f %8.4f) dp=%8.4f <? da=%8.4f",
ac_norm.x(),
ac_norm.y(),
ac_norm.z(),
ac_norm.dot(point),
ac_norm.dot(tri_a));
}
if (ac_norm.dot(point) < ac_norm.dot(tri_a))
{
if (acorn_closest_debug)
{
logInform(" CDB: beyond ac");
}
return closestPointOnLine(tri_a, tri_c, point, closest_point);
}
Eigen::Vector3d bc = tri_c - tri_b;
Eigen::Vector3d bc_norm = bc.cross(norm);
if (acorn_closest_debug)
{
logInform(" CDB: ab_norm (%8.4f %8.4f %8.4f) dp=%8.4f >? db=%8.4f",
bc_norm.x(),
bc_norm.y(),
bc_norm.z(),
bc_norm.dot(point),
bc_norm.dot(tri_b));
}
if (bc_norm.dot(point) > bc_norm.dot(tri_b))
{
if (acorn_closest_debug)
{
logInform(" CDB: beyond bc");
}
return closestPointOnLine(tri_b, tri_c, point, closest_point);
}
return abs_dist;
}
