项目背景
项目效果
基本思想
- 五段法生成一组路线
- 起始直线
- 起始圆弧
- 中间直线
- 终点圆弧
- 终点直线
- 实现绕过两个障碍物的功能(S型路径)
- 完整路径由多个五段组成
工程组成
数学库
-
矢量运算
这里仅用到平面矢量,将矢量定义为一个二维数组
- 而有向线段,使用一个起点坐标和矢量作为成员的结构体定义
- 提供的函数接口如下 ```c void VectorReset(double thi);//重置参数数组为0 void VectorCopy(double thi, double tha);//向量拷贝 void VectorPandP(double thi, Point_int32 StartPoint, Point_int32 EndPoint);//向量以点到点格式 void VectorNandA(double* thi, double norm, double argument);//向量以模和辐角格式
bool VectorisZero(double thi);//零向量判断 int VectorgetQuadrants(double thi);//获得向量的所在象限 double VectorgetAngle(double thi, double tha);//计算两向量夹角 double VectorgetArgument(double thi);//计算向量的辐角(与基向量的夹角) double VectorgetLength(double thi);//获得向量长度
double VectorInnerproduct(double thi, double tha);//向量点乘(内积) void VectorReverse(double result, double thi);//向量反向 void VectorgetDirectionVector(doubleresult, Line line);//以向量记录直线方向 void VectorAdd(doubleresult, double thi, double tha);//向量相加 void VectorMul(doubleresult, double Num, double tha);//向量数乘
<a name="Sm6g8"></a>
## 解析几何
- 这里描述了圆、直线的几何关系,方程求解
- 圆通过成员为圆心和半径的结构体定义
- 直线通过成员为直线三参数,斜率的结构体定义
- 提供的函数接口如下
```c
//三点生成一个圆
Circle CreateCircle(Point_int32 point1, Point_int32 point2, Point_int32 point3);
//以点向式生成直线解析式(bx - ay + (ay0 - bx0) = 0) <点(x0, y0), 向(a, b)>
Line CreateLineByPandV(Point_int32 StartPoint, double *Vector);
//以点斜式生成直线解析式(kx - y + (y1 - kx1) = 0)
Line CreateLineByPandK(Point_int32 point, double k, bool isK_NotExist);
//以两点式生成直线解析式
Line CreateLineByPandP(Point_int32 StartPoint, Point_int32 EndPiont);
//生成与已知直线垂直的直线解析式
Line getVerticalLine(Point_int32 point, Line line);
//生成两点连线的中垂线
bool getMidperpendicular(Point_int32 point1, Point_int32 point2, Line* line);
//生成过确定点,针对目标圆的切线
bool getTangentLine(Point_int32 P, Circle C, Line* lines, Point_int32* Points);
//生成两圆的外公切线
bool getExternalCommonTangent(Circle C1, Point_int32* C1_Points, Circle C2,
Point_int32* C2_Points, Line* lines);
//生成两圆的内公切线
bool getInternalCommonTangent(Circle C1, Point_int32* C1_Points, Circle C2,
Point_int32* C2_Points, Line* lines);
//获取两直线的交点
bool GetIntersection(Line line1, Line line2, Point_int32* result);
//生成由一个有向线段确定的一个圆的外切圆
bool getExcircle(Point_int32 P, double *Vec, Circle C, Circle excircle);
//生成角内内切圆(内切圆半径由边点1决定)
bool CreateIncircle(Point_int32 PeakPoint, Point_int32 DecisionPoint,
Point_int32 SidePoint, Circle result);
//获取两点连线的中点
Point_int32 GetMidpoint(Point_int32 point1, Point_int32 point2);
//获得两个向量的逆时针旋向的夹角(向量1逆时针转多少度就能转到向量2的位置)
double getAnticlockAngle(double *StartVec, double *EndVec);
//获得某点到某直线的距离
double getDistanceToLine(Point_int32 point, Line line);
//获得两点间的距离
double GetDistanceToPoint(Point_int32 point1, Point_int32 point2);
//获得顶点与另两点之间的夹角
double GetAngleToLineByPangP(Point_int32 Origin_Point, Point_int32 Point1, Point_int32 Point2);
//获得两直线的夹角
double GetAngleToLineByLandL(Line line1, Line line2, bool isGetSupplementary);
//判断两条射线是否相交
bool isIntersectant(DSegment Radial1, DSegment Radial2);
//判断两个圆是否相同
bool isSameCircle(Circle C1, Circle C2);
//转换值
Point_int32 TransformToInt32(Point_float Point);
轨迹描述
- 这里指生成的轨迹的存放形式
-
轨迹生成
将数学库有效的利用起来,结合设定信息,选择符合要求的路径
- 主要利用的是解析几何中直线与圆弧的相切关系,再做取舍
- 提供的函数接口如下
```c
void PositionConstParamsInit(void);
bool CreateStart_EndLine(struct MoveInLine *Track, DSegment Start, Circle CalcuCircle,
bool CreatMiddleLine(struct MoveInLine *Track,DSegment barrier, bool isPointToCircle, DSegment Otherbarrier)
bool CreatStart_EndCircle(struct MoveInCircle *Track, Point_int32 StartPoint, Point_int32 EndPoint,Circle C1, DSegment Barrier1, struct MoveInLine *Startline,
Circle C2, DSegment Barrier2, struct MoveInLine *Endline,
Point_int32 C2_EndPoint)
bool CreatPointToPointMotionTrack(MoveTrack_PointToPoint *Track,double *SVec, double *EVec, Circle circle)
DSegment Start, DSegment S_barrier, double S_Protected_R,
DSegment End, DSegment E_barrier, double E_Protected_R);
```