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.
On the first glance your solution is not working with every case. You can just simply use the appropriate vector arithmetics:
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.