//
|
// Created by YY on 2019/4/30.
|
//
|
|
#include "defs.h"
|
#include "Geometry.h"
|
#include <stdbool.h>
|
#include <stdint.h>
|
#include <cmath>
|
#include <jni.h>
|
#include <malloc.h>
|
#include <initializer_list>
|
|
#include "jni_log.h"
|
|
using namespace std;
|
|
const double EPSILON = 1e-6;
|
const double EPSILON2 = 1e-3;
|
|
inline bool isEqual(double a, double b)
|
{
|
return (fabs(a - b) <= EPSILON);
|
}
|
|
inline bool isEqual2(double a, double b)
|
{
|
return (fabs(a - b) <= EPSILON2);
|
}
|
|
inline double toRadians(double degree)
|
{
|
return (degree * M_PI) / 180.0;
|
}
|
|
inline double toDegree(double radians)
|
{
|
return (radians * 180.0) / M_PI;
|
}
|
|
void MakeLine(Line *line, const PointF *p1, const PointF *p2)
|
{
|
line->X1 = p1->X;
|
line->Y1 = p1->Y;
|
line->X2 = p2->X;
|
line->Y2 = p2->Y;
|
}
|
|
void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set)
|
{
|
int n = 0;
|
|
polygon->point = (PointF *)malloc(point_set.size() * sizeof(PointF));
|
|
for (auto ptr : point_set) {
|
polygon->point[n++] = ptr;
|
}
|
polygon->num = n;
|
}
|
|
void MakeHidePoint(PointF *point, const PointF *bp, const Line *bl)
|
{
|
point->X = (bl->X1 + bl->X2) - bp->X;
|
point->Y = (bl->Y1 + bl->Y2) - bp->Y;
|
}
|
|
void CleanPolygon(Polygon *polygon)
|
{
|
if (polygon->point != NULL) {
|
free(polygon->point);
|
polygon->point = NULL;
|
}
|
polygon->num = 0;
|
}
|
|
Relation IntersectionOf(const Polygon *polygon1, const Polygon *polygon2)
|
{
|
bool inside = false, outside = false, tangent = false;
|
|
if (polygon1->num == 0 || polygon2->num == 0) {
|
return GM_None;
|
}
|
|
for (int idx = 0; idx < polygon1->num; ++idx) {
|
Relation relation = IntersectionOf(polygon1->point[idx], polygon2);
|
|
if (relation == GM_Containment) {
|
inside = true;
|
} else if (relation == GM_None) {
|
outside = true;
|
} else {
|
tangent = true;
|
}
|
|
if (inside && outside) {
|
break;
|
}
|
}
|
|
if (inside && outside) {
|
return GM_Intersection;
|
} else if (tangent) {
|
return GM_Tangent;
|
} else if (inside) {
|
return GM_Containment;
|
}
|
return GM_None;
|
}
|
|
Relation IntersectionOf(Line line, const Polygon *polygon)
|
{
|
if (polygon->num == 0) {
|
return GM_None;
|
}
|
|
if (polygon->num == 1) {
|
return IntersectionOf(polygon->point[0], line);
|
}
|
|
bool tangent = false;
|
for (int index = 0; index < polygon->num; index++) {
|
int index2 = (index + 1) % polygon->num;
|
Line line2;
|
|
line2.X1 = polygon->point[index].X;
|
line2.Y1 = polygon->point[index].Y;
|
line2.X2 = polygon->point[index2].X;
|
line2.Y2 = polygon->point[index2].Y;
|
|
// 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);
|
|
// LOGD("relation = %d", relation);
|
|
if (relation == GM_Intersection) {
|
return relation;
|
}
|
if (relation == GM_Tangent) {
|
tangent = true;
|
}
|
}
|
|
PointF point2;
|
|
point2.X = line.X1;
|
point2.Y = line.Y1;
|
|
return tangent ? GM_Tangent : IntersectionOf(point2, polygon);
|
}
|
|
Relation IntersectionOf(PointF point, const Polygon *polygon)
|
{
|
switch (polygon->num)
|
{
|
case 0:
|
return GM_None;
|
case 1:
|
if (isEqual(polygon->point[0].X, point.X) &&
|
isEqual(polygon->point[0].Y, point.Y)) {
|
return GM_Tangent;
|
}
|
else {
|
return GM_None;
|
}
|
case 2: {
|
Line line2;
|
|
line2.X1 = polygon->point[0].X;
|
line2.Y1 = polygon->point[0].Y;
|
line2.X2 = polygon->point[1].X;
|
line2.Y2 = polygon->point[1].Y;
|
return IntersectionOf(point, line2);
|
}
|
default:
|
break;
|
}
|
|
int counter = 0;
|
int i;
|
PointF p1;
|
int n = polygon->num;
|
|
p1.X = polygon->point[0].X;
|
p1.Y = polygon->point[0].Y;
|
|
if (isEqual(point.X, p1.X) && isEqual(point.Y, p1.Y)) {
|
return GM_Tangent;
|
}
|
|
for (i = 1; i <= n; i++) {
|
PointF p2;
|
|
p2.X = polygon->point[i % n].X;
|
p2.Y = polygon->point[i % n].Y;
|
|
if (isEqual(point.X, p2.X) && isEqual(point.Y, p2.Y)) {
|
return GM_Tangent;
|
}
|
|
if (point.Y > fmin(p1.Y, p2.Y)) {
|
if (point.Y <= fmax(p1.Y, p2.Y)) {
|
if (point.X <= fmax(p1.X, p2.X)) {
|
if (p1.Y != p2.Y) {
|
double xinters = (point.Y - p1.Y) * (p2.X - p1.X) / (p2.Y - p1.Y) + p1.X;
|
|
if (isEqual(p1.X, p2.X) || point.X <= xinters)
|
counter++;
|
}
|
}
|
}
|
}
|
p1 = p2;
|
}
|
|
return (counter % 2 == 1) ? GM_Containment : GM_None;
|
}
|
|
Relation IntersectionOf(PointF point, Line line)
|
{
|
double bottomY = fmin(line.Y1, line.Y2);
|
double topY = fmax(line.Y1, line.Y2);
|
bool heightIsRight = point.Y >= bottomY &&
|
point.Y <= topY;
|
//Vertical line, slope is divideByZero error!
|
if (isEqual(line.X1, line.X2)) {
|
if (isEqual(point.X, line.X1) && heightIsRight) {
|
return GM_Tangent;
|
} else {
|
return GM_None;
|
}
|
}
|
|
double slope = (line.X2 - line.X1) / (line.Y2 - line.Y1);
|
bool onLine = isEqual(line.Y1 - point.Y, slope * (line.X1 - point.X));
|
|
if (onLine && heightIsRight) {
|
return GM_Tangent;
|
} else {
|
return GM_None;
|
}
|
}
|
|
Relation 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)))
|
return GM_None;
|
|
if ((isEqual(line1.X1, line2.X1) && isEqual(line1.Y1, line2.Y1)) || (isEqual(line1.X2, line2.X1) && isEqual(line1.Y2, line2.Y1)))
|
return GM_Intersection;
|
|
if ((isEqual(line1.X1, line2.X2) && isEqual(line1.Y1, line2.Y2)) || (isEqual(line1.X2, line2.X2) && isEqual(line1.Y2, line2.Y2)))
|
return GM_Intersection;
|
|
// (1) Translate the system so that point A is on the origin.
|
line1.X2 -= line1.X1; line1.Y2 -= line1.Y1;
|
line2.X1 -= line1.X1; line2.Y1 -= line1.Y1;
|
line2.X2 -= line1.X1; line2.Y2 -= line1.Y1;
|
|
// Discover the length of segment A-B.
|
double distAB = sqrt(line1.X2 * line1.X2 + line1.Y2 * line1.Y2);
|
|
// (2) Rotate the system so that point B is on the positive X axis.
|
double theCos = line1.X2 / distAB;
|
double theSin = line1.Y2 / distAB;
|
double newX = line2.X1 * theCos + line2.Y1 * theSin;
|
|
line2.Y1 = line2.Y1 * theCos - line2.X1 * theSin;
|
line2.X1 = newX;
|
newX = line2.X2 * theCos + line2.Y2 * theSin;
|
line2.Y2 = line2.Y2 * theCos - line2.X2 * theSin;
|
line2.X2 = newX;
|
|
// Fail if segment C-D doesn't cross line A-B.
|
if ((line2.Y1 < 0 && line2.Y2 < 0) || (line2.Y1 >= 0 && line2.Y2 >= 0)) {
|
return GM_None;
|
}
|
|
// (3) Discover the position of the intersection point along line A-B.
|
double posAB = line2.X2 + (line2.X1 - line2.X2) * line2.Y2 / (line2.Y2 - line2.Y1);
|
|
// Fail if segment C-D crosses line A-B outside of segment A-B.
|
if (posAB < 0 || posAB > distAB) {
|
return GM_None;
|
}
|
// (4) Apply the discovered position to line A-B in the original coordinate system.
|
return GM_Intersection;
|
}
|
|
double DistanceOf(PointF point1, PointF point2)
|
{
|
return sqrt((point1.X-point2.X)*(point1.X-point2.X) + (point1.Y-point2.Y)*(point1.Y-point2.Y));
|
}
|
|
double DistanceOf(PointF point, Line line)
|
{
|
// float a = sqrt((point.X-line.X1)*(point.X-line.X1) + (point.Y-line.Y1)*(point.Y-line.Y1));
|
// float b = sqrt((point.X-line.X2)*(point.X-line.X2) + (point.Y-line.Y2)*(point.Y-line.Y2));
|
double c = sqrt((line.X1-line.X2)*(line.X1-line.X2) + (line.Y1-line.Y2)*(line.Y1-line.Y2));
|
|
// float p = (a+b+c)/2;
|
|
// dis = 2 * sqrt(p*(p-a)*(p-b)*(p-c)) / c;
|
|
return fabs(point.X*line.Y1 + point.Y*line.X2 + line.X1*line.Y2 - line.X2*line.Y1 - line.X1*point.Y - point.X*line.Y2) / c;
|
}
|
|
/*********************************************************
|
* p2----------->p1 线端和Y轴的夹角
|
* @param p1
|
* @param p2
|
* @return yaw
|
*/
|
double YawOf(PointF p1, PointF p2)
|
{
|
double deg = 0.0;
|
|
if (fabs(p1.Y - p2.Y) <= GLB_EPSILON) {
|
if (p1.X > p2.X) {
|
deg = 90;
|
} else {
|
deg = 270;
|
}
|
} else if (fabs(p1.X - p2.X) <= GLB_EPSILON) {
|
if (p1.Y > p2.Y) {
|
deg = 0;
|
} else {
|
deg = 180;
|
}
|
} else {
|
deg = atan(fabs(p1.X - p2.X) /
|
fabs(p1.Y - p2.Y));
|
|
deg = toDegree(deg);
|
|
if (p1.X > p2.X &&
|
p1.Y > p2.Y) {
|
|
} else if (p1.X < p2.X &&
|
p1.Y > p2.Y) {
|
deg = 360 - deg;
|
} else if (p1.X < p2.X &&
|
p1.Y < p2.Y) {
|
deg = 180 + deg;
|
} else if (p1.X > p2.X &&
|
p1.Y < p2.Y) {
|
deg = 180 - deg;
|
}
|
}
|
|
return deg;
|
}
|
|
/**********************************************************
|
* base 和 dest的第二点重合时形成的夹角
|
* @param base
|
* @param dest
|
* @return
|
*/
|
double CalculateAngle(Line base, Line dest)
|
{
|
double angle = 0;
|
|
double dx = base.X2 - dest.X2;
|
double dy = base.Y2 - dest.Y2;
|
|
dest.X1 += dx;
|
dest.Y1 += dy;
|
|
double c2 = pow((dest.X1 - base.X1), 2) + pow((dest.Y1 - base.Y1), 2);
|
double a2 = pow((base.X1 - base.X2), 2) + pow((base.Y1 - base.Y2), 2);
|
double b2 = pow((dest.X1 - base.X2), 2) + pow((dest.Y1 - base.Y2), 2);
|
|
angle = acos((a2 + b2 - c2) / (2 * sqrt(a2) * sqrt(b2)));
|
|
return toDegree(angle);
|
}
|
|
PointF rotatePoint(PointF oldPoint, PointF centre, double degree) {
|
PointF newPoint;
|
newPoint.X = (oldPoint.X-centre.X)*cos(toRadians(degree)) - (oldPoint.Y-centre.Y)*sin(toRadians(degree)) + centre.X;
|
newPoint.Y = (oldPoint.X-centre.X)*sin(toRadians(degree)) + (oldPoint.Y-centre.Y)*cos(toRadians(degree)) + centre.Y;
|
return newPoint;
|
}
|
|
// All in
|
bool InsidePolygon(const Polygon *t1, const Polygon *t2) {
|
for (int i = 0; i < t1->num; ++i) {
|
if (IntersectionOf(t1->point[i], t2) != GM_Containment) {
|
return false;
|
}
|
}
|
return true;
|
}
|
|
// Any part inside
|
bool PartInsidePolygon(const Polygon *t1, const Polygon *t2) {
|
for (int i = 0; i < t1->num; ++i) {
|
if (IntersectionOf(t1->point[i], t2) == GM_Containment) {
|
return true;
|
}
|
}
|
return false;
|
}
|
|
// All out
|
bool OutsidePolygon(const Polygon *t1, const Polygon *t2) {
|
for (int i = 0; i < t1->num; ++i) {
|
if (IntersectionOf(t1->point[i], t2) != GM_None) {
|
return false;
|
}
|
}
|
return true;
|
}
|
|
/***************************************************************
|
* @brief p3位于由p1--------->p2构成的射线，左侧还是右侧，同向，反向
|
* @param p1
|
* @param p2
|
* @param p3
|
* @return 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back
|
*/
|
int 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) <= EPSILON) {
|
double fb = (p2.X-p1.X)*(p3.X-p1.X) + (p2.Y-p1.Y)*(p3.Y-p1.Y);
|
if (fabs(fb) <= EPSILON)
|
return 0;
|
else if (fb > 0)
|
return 2;
|
else
|
return -2;
|
} else if (lr > 0) {
|
return 1;
|
} else {
|
return -1;
|
}
|
}
|
|
/***************************************************************
|
* 得到p3于p1,p2组成的直线上的垂点
|
* @param p1
|
* @param p2
|
* @param p3
|
* @return
|
*/
|
PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3)
|
{
|
PointF p4;
|
|
if (isEqual2(p1.X, p2.X)) {
|
p4.Y = p3.Y;
|
p4.X = p1.X;
|
return p4;
|
}
|
if (isEqual2(p1.Y, p2.Y)) {
|
p4.X = p3.X;
|
p4.Y = p1.Y;
|
return p4;
|
}
|
|
double k = (p2.Y - p1.Y) / (p2.X - p1.X);
|
double b = p1.Y - k * p1.X;
|
|
double k2 = (p2.X - p1.X) / (p1.Y - p2.Y);
|
double b2 = p3.Y - p3.X * k2;
|
|
p4.X = (b2 - b) / (k - k2);
|
p4.Y = k2 * p4.X + b2;
|
|
return p4;
|
}
|
