Question

我给了一个以xyz为起点的箭头，以abc为方向（不是箭头尖，而是一个以坐标系原点为原点的方向矢量）和一个点p。我想使箭头始终远离该点，但是我不确定我的解决方案是否正确。我的天真做法是这样的：

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;
    }
  }
}

vectorXd在前3个数组元素中保存xyz，在后3个数组元素中保存abc。

Answer 1

鉴于arrow的位置和point的位置，只需减去以下即可获得指向point的方向：

arrow.tail<3>() = arrow.head<3> - point.getVector3fMap();

您可能希望对方向进行标准化，在这种情况下，可以使用Eigen的Block operations for vectors来获取abc：

arrow.tail<3>().normalize();

Answer 2

乍一看，您的解决方案并非适用于所有情况。您可以简单地使用适当的矢量算法：

将abc（方向定义为从[0，0，0]开始的向量）分配给3d向量
通过从POINT中减去箭头原点来计算3d向量
计算两个向量之间的角度

示例：

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;

如果大于Pi/2，则箭头会移开。

如何使箭头远离某个点？

2 个答案: