How to orient arrow away from a point?

Question

I have given an arrow with xyz as starting point with abc as the orientation(not the arrow tip, but a orientation vector with coordinate system origin as the origin) and a point p. I would like to orient the arrow always away from the point, but I am not sure if my solution is correct. My naive approach was this:

void pointArrowAwayFromPoint(Eigen::VectorXd &arrow, pcl::PointXYZRGB point){
  bool invert = false;
  for(int i = 0; i < 3; i++){
    if(arrow[i] < point.data[i] && arrow[i+3] > 0){
      invert = true;
      break;
    }
    else if(arrow[i] > point.data[i] && arrow[i+3] < 0){
      invert = true;
      break;
    }
  }
  if(invert){
    for(int i = 3; i < 6; i++){
      arrow[i] *= -1;
    }
  }
}

The vectorXd holds xyz in the first 3 array elements and abc in the next 3 ones.

Answer 1

On the first glance your solution is not working with every case. You can just simply use the appropriate vector arithmetics:

• assign your abc (which is the orientation defined as a vector starting at [0, 0, 0]) to the 3d vector
• calculate the 3d vector by subtracting your arrow origin from the POINT
• calculate angle between that two vectors

Example:

Eigen::VectorXd arrow(6);
// simple arrow pointing in the direction of X
arrow[3] = 1.0;
Eigen::Vector3d point{-2, 1, 1};
Eigen::Vector3d v1 = arrow.tail<3>();
Eigen::Vector3d v2 = point - arrow.head<3>();
double cosX = v1.dot(v2)/(v1.norm()*v2.norm());
std::cout << "angle between arrow and the point: " << std::acos(cosX) << std::endl;

If it's more than Pi/2, then arrow looks away.