scara机器人工作空间里任意方向下的边界点

  关于scara机器人工作空间的绘制详见另外一篇博文：绘制scara机器人工作空间。
  给定工作空间里任意一点$(x_0,y_0)$及运动方向$\vec{s}$ ，怎样确定该点在该方向上的边界点？如下图，过点$(x_0,y_0)$且与$\vec{s}$平行的直线与四个圆有8个交点，$P_2$为所求的边界点。事实上，对于给定工作空间里任意一点$(x_0,y_0)$及运动方向$\vec{s}$，至多与以下4个圆有8个交点，至少有1个交点(即相切)。

  可以这样确定scara机器人工作空间里任意方向下的边界点：
  (1)依次求直线与4个圆的交点。
  (2)筛选掉在$\vec{s}$反方向上的交点。设$\vec{a}$与$\vec{b}$为同一直线上的两向量，若$\vec{a}\cdot \vec{b}>0$，则$\vec{a}$与$\vec{b}$同向，否则反向。如上图，点$P_1$在$\vec{s}$的反方向上，应剔除。
  (3)筛选掉不在当前手系下对应的圆弧段上的点。如下图，直线$QM$交圆于$Q$ ，由于叉乘$z$方向分量$(\overrightarrow{C_{2}M}\times \overrightarrow{M{B}_2}{)}_z\ge 0$ ，则点$M$为边界点，由于叉乘$z$方向分量$(\overrightarrow{C_{2}Q}\times \overrightarrow{Q{B}_2}{)}_z<0$，则点$Q$不是边界点。
  (4)在剩下的交点中，与点$(x_0,y_0)$距离最短的点则为所求边界点。