C++ 如何使箭头远离一个点？

我已经给出了一个以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;
    }
  }
}

void pointarrowayfrompoint（特征：：向量xd&arrow，pcl：：PointXYZRGB点）{
布尔反转=假；
对于（int i=0；i<3；i++）{
如果（箭头[i]<点数据[i]&箭头[i+3]>0）{
反转=真；
打破
}
else if（箭头[i]>点数据[i]&箭头[i+3]<0）{
反转=真；
打破
}
}
如果（反转）{
对于（int i=3；i<6；i++）{
箭头[i]*=-1；
}
}
}

---

vectorXd在前3个数组元素中保持xyz，在接下来的3个数组元素中保持abc。

给定

箭头的位置和
点的位置，只需减去：

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

arrow.tail（）=arrow.head-point.getVector3fMap（）；

您可能希望将方向规格化，在这种情况下，您可以使用Eigen's仅获得abc：

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

arrow.tail（）.normalize（）；
给定
箭头的位置和
点的位置
，只需减去以下值即可获得指向
点的方向：

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

arrow.tail（）=arrow.head-point.getVector3fMap（）；

您可能希望将方向规格化，在这种情况下，您可以使用Eigen's仅获得abc：

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

arrow.tail（）.normalize（）；
乍一看，您的解决方案并不适用于所有情况。您只需使用适当的向量算法即可：


将abc（定义为从[0,0,0]开始的向量的方向）指定给3d向量
通过从点减去箭头原点来计算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;

Eigen:：VectorXd箭头（6）；
//指向X方向的简单箭头
箭头[3]=1.0；
本征：：向量3D点{-2,1,1}；
Eigen:：Vector3d v1=arrow.tail（）；
Eigen:：Vector3d v2=点箭头.head（）；
双余弦=v1.dot（v2）/（v1.norm（）*v2.norm（））；
std：：cout乍一看，您的解决方案并不适用于所有情况。您只需使用适当的向量算法即可：


将abc（定义为从[0,0,0]开始的向量的方向）指定给3d向量
通过从点减去箭头原点来计算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;

Eigen:：VectorXd箭头（6）；
//指向X方向的简单箭头
箭头[3]=1.0；
本征：：向量3D点{-2,1,1}；
Eigen:：Vector3d v1=arrow.tail（）；
Eigen:：Vector3d v2=点箭头.head（）；
双余弦=v1.dot（v2）/（v1.norm（）*v2.norm（））；
如果箭头比点向量大，这难道不起作用吗？除此之外，我喜欢这种解决方案在任何情况下都应该有效，除非它们在同一点上。你说箭头比点向量大是什么意思？大于？或者更大？我想我真正想问的是，为什么你说这在某些情况下不起作用？请避免手循环，并使用以下块：
arrow.tail（）=arrow.head（）-point.getVector3fMap（）啊！没有意识到PCL积分可以如此轻松地转换。我会做出改变。如果箭头比点向量大，这就行不通了，对吗？除此之外，我喜欢这种解决方案在任何情况下都应该有效，除非它们在同一点上。你说箭头比点向量大是什么意思？大于？或者更大？我想我真正想问的是，为什么你说这在某些情况下不起作用？请避免手循环，并使用以下块：
arrow.tail（）=arrow.head（）-point.getVector3fMap（）啊！没有意识到PCL积分可以如此轻松地转换。我会做出改变。最好使用块：
arrow.head（）
和
arrow.tail（）
@ggael，是的，你完全正确；应用建议更改检查角度是否大于
pi/2
，您只需检查
v1.dot（v2）顺便说一句：
Eigen:：VectorXd箭头（6）不初始化为零。您需要编写，例如：
Eigen:：VectorXd arrow=Eigen:：VectorXd:：Zero（6）或
特征：：矢量xd箭头；箭头。设置零（6）。对于小向量，通常值得在编译时指定大小：
typedef Eigen:：Matrix Vector6d更好地使用块：
arrow.head（）
和
arrow.tail（）
@ggael，是的，你完全正确；应用建议更改检查角度是否大于
pi/2
，您只需检查
v1.dot（v2）顺便说一句：
Eigen:：VectorXd箭头（6）不初始化为零。您需要编写，例如：
Eigen:：VectorXd arrow=Eigen:：VectorXd:：Zero（6）或
特征：：矢量xd箭头；箭头。设置零（6）。对于小向量，通常值得在编译时指定大小：
typedef Eigen:：Matrix Vector6d