From 4bd08f0355b6b2cf3c027202d5ad301b4e182953 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 31 三月 2023 17:16:22 +0800
Subject: [PATCH] 科目二修改
---
lib/src/main/cpp/test_common/Geometry.cpp | 148 +++++++++++++++++++++++++++++++++++++++----------
1 files changed, 118 insertions(+), 30 deletions(-)
diff --git a/lib/src/main/cpp/test_common/Geometry.cpp b/lib/src/main/cpp/test_common/Geometry.cpp
index f1fbba9..8fe70d1 100644
--- a/lib/src/main/cpp/test_common/Geometry.cpp
+++ b/lib/src/main/cpp/test_common/Geometry.cpp
@@ -78,7 +78,7 @@
polygon->num = 0;
}
-Relation IntersectionOf(const Polygon *polygon1, const Polygon *polygon2)
+relation_t IntersectionOf(const Polygon *polygon1, const Polygon *polygon2)
{
bool inside = false, outside = false, tangent = false;
@@ -87,7 +87,7 @@
}
for (int idx = 0; idx < polygon1->num; ++idx) {
- Relation relation = IntersectionOf(polygon1->point[idx], polygon2);
+ relation_t relation = IntersectionOf(polygon1->point[idx], polygon2);
if (relation == GM_Containment) {
inside = true;
@@ -112,7 +112,7 @@
return GM_None;
}
-Relation IntersectionOf(Line line, const Polygon *polygon)
+relation_t IntersectionOf(Line line, const Polygon *polygon)
{
if (polygon->num == 0) {
return GM_None;
@@ -135,7 +135,7 @@
// LOGD("line1(%d %d - %d %d) line2(%d %d - %d %d)", line.X1, line.Y1, line.X2, line.Y2,
// line2.X1, line2.Y1, line2.X2, line2.Y2);
- Relation relation = IntersectionOf(line, line2);
+ relation_t relation = IntersectionOf(line, line2);
// LOGD("relation = %d", relation);
@@ -155,7 +155,7 @@
return tangent ? GM_Tangent : IntersectionOf(point2, polygon);
}
-Relation IntersectionOf(PointF point, const Polygon *polygon)
+relation_t IntersectionOf(PointF point, const Polygon *polygon)
{
switch (polygon->num)
{
@@ -222,7 +222,7 @@
return (counter % 2 == 1) ? GM_Containment : GM_None;
}
-Relation IntersectionOf(PointF point, Line line)
+relation_t IntersectionOf(PointF point, Line line)
{
double bottomY = fmin(line.Y1, line.Y2);
double topY = fmax(line.Y1, line.Y2);
@@ -247,7 +247,7 @@
}
}
-Relation IntersectionOf(Line line1, Line line2)
+relation_t IntersectionOf(Line line1, Line line2)
{
// Fail if either line segment is zero-length.
if ((isEqual(line1.X1, line1.X2) && isEqual(line1.Y1, line1.Y2)) || (isEqual(line2.X1, line2.X2) && isEqual(line2.Y1, line2.Y2)))
@@ -313,7 +313,7 @@
}
/*********************************************************
- * p2----------->p1 绾跨鍜孻杞寸殑澶硅
+ * p1----------->p2 绾跨鍜孻杞寸殑澶硅
* @param p1
* @param p2
* @return yaw
@@ -322,40 +322,102 @@
{
double deg = 0.0;
- if (fabs(p1.Y - p2.Y) <= GLB_EPSILON) {
- if (p1.X > p2.X) {
+ if (fabs(p2.Y - p1.Y) <= GLB_EPSILON) {
+ if (p2.X > p1.X) {
deg = 90;
} else {
deg = 270;
}
- } else if (fabs(p1.X - p2.X) <= GLB_EPSILON) {
- if (p1.Y > p2.Y) {
+ } else if (fabs(p2.X - p1.X) <= GLB_EPSILON) {
+ if (p2.Y > p1.Y) {
deg = 0;
} else {
deg = 180;
}
} else {
- deg = atan(fabs(p1.X - p2.X) /
- fabs(p1.Y - p2.Y));
+ deg = atan(fabs(p2.X - p1.X) /
+ fabs(p2.Y - p1.Y));
deg = toDegree(deg);
- if (p1.X > p2.X &&
- p1.Y > p2.Y) {
+ if (p2.X > p1.X &&
+ p2.Y > p1.Y) {
- } else if (p1.X < p2.X &&
- p1.Y > p2.Y) {
+ } else if (p2.X < p1.X &&
+ p2.Y > p1.Y) {
deg = 360 - deg;
- } else if (p1.X < p2.X &&
- p1.Y < p2.Y) {
+ } else if (p2.X < p1.X &&
+ p2.Y < p1.Y) {
deg = 180 + deg;
- } else if (p1.X > p2.X &&
- p1.Y < p2.Y) {
+ } else if (p2.X > p1.X &&
+ p2.Y < p1.Y) {
deg = 180 - deg;
}
}
return deg;
+}
+
+double YawOf(Line &line)
+{
+ PointF p1 = {
+ .X = line.X1,
+ .Y = line.Y1
+ };
+
+ PointF p2 = {
+ .X = line.X2,
+ .Y = line.Y2
+ };
+
+ return YawOf(p1, p2);
+}
+
+/**************************************************
+ * p1----->p3 涓� p1----->p2 涓ゅ皠绾跨殑閫嗘椂閽堝す瑙�
+ * 宸︿晶灏忎簬180锛屽彸渚уぇ浜�180
+ * @param p1
+ * @param p2
+ * @param p3
+ * @return
+ */
+double AngleOf(PointF p1, PointF p2, PointF p3)
+{
+ int rel = IntersectionOfLine(p1, p2, p3);
+
+ switch (rel) {
+ case RELATION_LEFT:
+ case RELATION_RIGHT: {
+ double a = DistanceOf(p2, p3);
+ double b = DistanceOf(p1, p2);
+ double c = DistanceOf(p1, p3);
+
+ double deg = toDegree(acos((pow(b, 2) + pow(c, 2) - pow(a, 2)) / (2 * b * c)));
+
+ return (rel == RELATION_LEFT) ? deg : (360-deg);
+ }
+ case RELATION_BACK:
+ return 180;
+ default:
+ return 0;
+ };
+
+ return 0;
+}
+
+/************************************************
+ * dest鐩稿浜巄ase鐨勯�嗘椂閽堝す瑙�
+ * @param base
+ * @param dest
+ * @return
+ */
+double AngleOfTowLine(Line base, Line dest)
+{
+ PointF p1 = {.X = base.X1, .Y = base.Y1};
+ PointF p2 = {.X = base.X2, .Y = base.Y2};
+ PointF p3 = {.X = dest.X2 + (base.X1 - dest.X1), .Y = dest.Y2 + (base.Y1 - dest.Y1)};
+
+ return AngleOf(p1, p2, p3);
}
double DeltaYaw(double yaw1, double yaw2)
@@ -449,27 +511,27 @@
* @param p3
* @return 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back
*/
-int IntersectionOfLine(PointF p1, PointF p2, PointF p3)
+relational_position_t IntersectionOfLine(PointF p1, PointF p2, PointF p3)
{
double lr = (p1.X-p3.X)*(p2.Y-p3.Y) - (p1.Y-p3.Y)*(p2.X-p3.X);
if (fabs(lr) <= EPSILON2) {
double fb = (p2.X-p1.X)*(p3.X-p1.X) + (p2.Y-p1.Y)*(p3.Y-p1.Y);
if (fabs(fb) <= EPSILON2)
- return 0;
+ return REL_POS_ON;
else if (fb > 0)
- return 2;
+ return REL_POS_FRONT;
else
- return -2;
+ return REL_POS_REAR;
} else if (lr > 0) {
- return 1;
+ return REL_POS_LEFT;
} else {
- return -1;
+ return REL_POS_RIGHT;
}
}
// 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back
-int IntersectionOfLine(PointF p, Line line)
+relational_position_t IntersectionOfLine(PointF p, Line line)
{
PointF p1, p2;
@@ -688,4 +750,30 @@
}
return deg;
-}
\ No newline at end of file
+}
+
+bool Calc3PointCircle(PointF p1, PointF p2, PointF p3, Circle &circle)
+{
+ double a = p1.X - p2.X;
+ double b = p1.Y - p2.Y;
+ double c = p1.X - p3.X;
+ double d = p1.Y - p3.Y;
+ double e = ((pow(p1.X,2) - pow(p2.X,2)) - (pow(p2.Y,2) - pow(p1.Y,2))) / 2;
+ double f = ((pow(p1.X,2) - pow(p3.X,2)) - (pow(p3.Y,2) - pow(p1.Y,2))) / 2;
+
+ // 涓夌偣涓嶈兘鍏辩嚎
+ if (isEqual2(a*d, b*c)) {
+ circle.centre.X = circle.centre.Y = circle.radius = 0;
+ return false;
+ }
+
+ double x0 = (d*e - b*f)/(a*d - b*c);
+ double y0 = (a*f - c*e)/(a*d - b*c);
+ double r = sqrt(pow(x0-p1.X, 2) + pow(y0-p1.Y, 2));
+
+ circle.centre.X = x0;
+ circle.centre.Y = y0;
+ circle.radius = r;
+
+ return true;
+}
--
Gitblit v1.8.0