//
|
// 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 <cctype>
|
#include <iosfwd>
|
#include <iomanip>
|
#include <sstream>
|
#include <vector>
|
|
#include "../jni_log.h"
|
|
using namespace std;
|
|
const double EPSILON = 1e-6;
|
const double EPSILON2 = 1e-3;
|
|
bool isEqual(double a, double b)
|
{
|
return (fabs(a - b) <= EPSILON);
|
}
|
|
bool isEqual2(double a, double b)
|
{
|
return (fabs(a - b) <= EPSILON2);
|
}
|
|
double toRadians(double degree)
|
{
|
return (degree * M_PI) / 180.0;
|
}
|
|
double toDegree(double radians)
|
{
|
return (radians * 180.0) / M_PI;
|
}
|
|
void MAKE_LINE(Line &a, PointF &b, PointF &c)
|
{
|
a.p1 = b;
|
a.p2 = c;
|
}
|
|
/*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->p1.X + bl->p2.X) - bp->X;
|
point->Y = (bl->p1.Y + bl->p2.Y) - bp->Y;
|
}
|
|
void CleanPolygon(Polygon *polygon)
|
{
|
if (polygon->point != NULL) {
|
free(polygon->point);
|
polygon->point = NULL;
|
}
|
polygon->num = 0;
|
}
|
|
relation_t 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_t 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_t 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.p1.X = polygon->point[index].X;
|
line2.p1.Y = polygon->point[index].Y;
|
line2.p2.X = polygon->point[index2].X;
|
line2.p2.Y = polygon->point[index2].Y;
|
|
// LOGD("line1(%d %d - %d %d) line2(%d %d - %d %d)", line.p1.X, line.p1.Y, line.p2.X, line.p2.Y,
|
// line2.p1.X, line2.p1.Y, line2.p2.X, line2.p2.Y);
|
|
relation_t 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.p1.X;
|
point2.Y = line.p1.Y;
|
|
return tangent ? GM_Tangent : IntersectionOf(point2, polygon);
|
}
|
|
relation_t 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.p1.X = polygon->point[0].X;
|
line2.p1.Y = polygon->point[0].Y;
|
line2.p2.X = polygon->point[1].X;
|
line2.p2.Y = 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_t IntersectionOf(PointF point, Line line)
|
{
|
double bottomY = fmin(line.p1.Y, line.p2.Y);
|
double topY = fmax(line.p1.Y, line.p2.Y);
|
bool heightIsRight = point.Y >= bottomY &&
|
point.Y <= topY;
|
//Vertical line, slope is divideByZero error!
|
if (isEqual(line.p1.X, line.p2.X)) {
|
if (isEqual(point.X, line.p1.X) && heightIsRight) {
|
return GM_Tangent;
|
} else {
|
return GM_None;
|
}
|
}
|
|
double slope = (line.p2.X - line.p1.X) / (line.p2.Y - line.p1.Y);
|
bool onLine = isEqual(line.p1.Y - point.Y, slope * (line.p1.X - point.X));
|
|
if (onLine && heightIsRight) {
|
return GM_Tangent;
|
} else {
|
return GM_None;
|
}
|
}
|
|
relation_t IntersectionOf(Line line1, Line line2)
|
{
|
// Fail if either line segment is zero-length.
|
if ((isEqual(line1.p1.X, line1.p2.X) && isEqual(line1.p1.Y, line1.p2.Y)) || (isEqual(line2.p1.X, line2.p2.X) && isEqual(line2.p1.Y, line2.p2.Y)))
|
return GM_None;
|
|
if ((isEqual(line1.p1.X, line2.p1.X) && isEqual(line1.p1.Y, line2.p1.Y)) || (isEqual(line1.p2.X, line2.p1.X) && isEqual(line1.p2.Y, line2.p1.Y)))
|
return GM_Intersection;
|
|
if ((isEqual(line1.p1.X, line2.p2.X) && isEqual(line1.p1.Y, line2.p2.Y)) || (isEqual(line1.p2.X, line2.p2.X) && isEqual(line1.p2.Y, line2.p2.Y)))
|
return GM_Intersection;
|
|
// (1) Translate the system so that point A is on the origin.
|
line1.p2.X -= line1.p1.X; line1.p2.Y -= line1.p1.Y;
|
line2.p1.X -= line1.p1.X; line2.p1.Y -= line1.p1.Y;
|
line2.p2.X -= line1.p1.X; line2.p2.Y -= line1.p1.Y;
|
|
// Discover the length of segment A-B.
|
double distAB = sqrt(line1.p2.X * line1.p2.X + line1.p2.Y * line1.p2.Y);
|
|
// (2) Rotate the system so that point B is on the positive X axis.
|
double theCos = line1.p2.X / distAB;
|
double theSin = line1.p2.Y / distAB;
|
double newX = line2.p1.X * theCos + line2.p1.Y * theSin;
|
|
line2.p1.Y = line2.p1.Y * theCos - line2.p1.X * theSin;
|
line2.p1.X = newX;
|
newX = line2.p2.X * theCos + line2.p2.Y * theSin;
|
line2.p2.Y = line2.p2.Y * theCos - line2.p2.X * theSin;
|
line2.p2.X = newX;
|
|
// Fail if segment C-D doesn't cross line A-B.
|
if ((line2.p1.Y < 0 && line2.p2.Y < 0) || (line2.p1.Y >= 0 && line2.p2.Y >= 0)) {
|
return GM_None;
|
}
|
|
// (3) Discover the position of the intersection point along line A-B.
|
double posAB = line2.p2.X + (line2.p1.X - line2.p2.X) * line2.p2.Y / (line2.p2.Y - line2.p1.Y);
|
|
// 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.p1.X)*(point.X-line.p1.X) + (point.Y-line.p1.Y)*(point.Y-line.p1.Y));
|
// float b = sqrt((point.X-line.p2.X)*(point.X-line.p2.X) + (point.Y-line.p2.Y)*(point.Y-line.p2.Y));
|
double c = sqrt((line.p1.X-line.p2.X)*(line.p1.X-line.p2.X) + (line.p1.Y-line.p2.Y)*(line.p1.Y-line.p2.Y));
|
|
// float p = (a+b+c)/2;
|
|
// dis = 2 * sqrt(p*(p-a)*(p-b)*(p-c)) / c;
|
|
return fabs(point.X*line.p1.Y + point.Y*line.p2.X + line.p1.X*line.p2.Y - line.p2.X*line.p1.Y - line.p1.X*point.Y - point.X*line.p2.Y) / c;
|
}
|
|
/*********************************************************
|
* p1----------->p2 线端和Y轴的夹角(顺时针)
|
* @param p1
|
* @param p2
|
* @return yaw
|
*/
|
double YawOf(PointF p1, PointF p2)
|
{
|
double deg = 0.0;
|
|
if (fabs(p2.Y - p1.Y) <= GLB_EPSILON) {
|
if (p2.X > p1.X) {
|
deg = 90;
|
} else {
|
deg = 270;
|
}
|
} else if (fabs(p2.X - p1.X) <= GLB_EPSILON) {
|
if (p2.Y > p1.Y) {
|
deg = 0;
|
} else {
|
deg = 180;
|
}
|
} else {
|
deg = atan(fabs(p2.X - p1.X) /
|
fabs(p2.Y - p1.Y));
|
|
deg = toDegree(deg);
|
|
if (p2.X > p1.X &&
|
p2.Y > p1.Y) {
|
|
} else if (p2.X < p1.X &&
|
p2.Y > p1.Y) {
|
deg = 360 - deg;
|
} else if (p2.X < p1.X &&
|
p2.Y < p1.Y) {
|
deg = 180 + deg;
|
} else if (p2.X > p1.X &&
|
p2.Y < p1.Y) {
|
deg = 180 - deg;
|
}
|
}
|
|
return deg;
|
}
|
|
double YawOf(Line &line)
|
{
|
PointF p1 = {
|
.X = line.p1.X,
|
.Y = line.p1.Y
|
};
|
|
PointF p2 = {
|
.X = line.p2.X,
|
.Y = line.p2.Y
|
};
|
|
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)
|
{
|
relational_position_t rel = IntersectionOfLine(p1, p2, p3);
|
|
switch (rel) {
|
case REL_POS_LEFT:
|
case REL_POS_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 == REL_POS_LEFT) ? deg : (360-deg);
|
}
|
case REL_POS_REAR:
|
return 180;
|
default:
|
return 0;
|
};
|
|
return 0;
|
}
|
|
/************************************************
|
* dest相对于base的逆时针夹角
|
* @param base
|
* @param dest
|
* @return
|
*/
|
double AngleOfTowLine(Line base, Line dest)
|
{
|
PointF p1 = {.X = base.p1.X, .Y = base.p1.Y};
|
PointF p2 = {.X = base.p2.X, .Y = base.p2.Y};
|
PointF p3 = {.X = dest.p2.X + (base.p1.X - dest.p1.X), .Y = dest.p2.Y + (base.p1.Y - dest.p1.Y)};
|
|
return AngleOf(p1, p2, p3);
|
}
|
|
double DeltaYaw(double yaw1, double yaw2)
|
{
|
double deltaAng;
|
|
if (fabs(yaw1 - yaw2) > 180.0) {
|
deltaAng = 360.0 - fabs(yaw1 - yaw2);
|
} else {
|
deltaAng = fabs(yaw1 - yaw2);
|
}
|
|
return deltaAng;
|
}
|
|
/**********************************************************
|
* base 和 dest的第二点重合时形成的夹角
|
* @param base
|
* @param dest
|
* @return
|
*/
|
double CalculateAngle(Line base, Line dest)
|
{
|
double angle = 0;
|
|
double dx = base.p2.X - dest.p2.X;
|
double dy = base.p2.Y - dest.p2.Y;
|
|
dest.p1.X += dx;
|
dest.p1.Y += dy;
|
|
double c2 = pow((dest.p1.X - base.p1.X), 2) + pow((dest.p1.Y - base.p1.Y), 2);
|
double a2 = pow((base.p1.X - base.p2.X), 2) + pow((base.p1.Y - base.p2.Y), 2);
|
double b2 = pow((dest.p1.X - base.p2.X), 2) + pow((dest.p1.Y - base.p2.Y), 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;
|
}
|
|
PointF centerOfTwoPoint(PointF p1, PointF p2) {
|
PointF center;
|
|
center.X = (p1.X + p2.X) / 2;
|
center.Y = (p1.Y + p2.Y) / 2;
|
|
return center;
|
}
|
|
// 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
|
*/
|
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 REL_POS_ON;
|
else if (fb > 0)
|
return REL_POS_FRONT;
|
else
|
return REL_POS_REAR;
|
} else if (lr > 0) {
|
return REL_POS_LEFT;
|
} else {
|
return REL_POS_RIGHT;
|
}
|
}
|
|
// 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back
|
relational_position_t IntersectionOfLine(PointF p, Line line)
|
{
|
PointF p1, p2;
|
|
p1.X = line.p1.X;
|
p1.Y = line.p1.Y;
|
p2.X = line.p2.X;
|
p2.Y = line.p2.Y;
|
|
return IntersectionOfLine(p1, p2, p);
|
}
|
|
/***************************************************************
|
* 得到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;
|
}
|
|
/**************************************************************
|
* p3于 p1---p2构成直线
|
* @param p1
|
* @param p2
|
* @param p3
|
* @return
|
*/
|
bool VerticalPointOnLine(PointF point, Line line)
|
{
|
PointF p1, p2;
|
|
p1.X = line.p1.X;
|
p1.Y = line.p1.Y;
|
|
p2.X = line.p2.X;
|
p2.Y = line.p2.Y;
|
|
PointF pv = GetVerticalPoint(p1, p2, point);
|
|
if (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) ||
|
(pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) {
|
return true;
|
}
|
return false;
|
}
|
|
bool VerticalPointOnLine(PointF point, Line line, PointF &vp)
|
{
|
PointF p1, p2;
|
|
p1.X = line.p1.X;
|
p1.Y = line.p1.Y;
|
|
p2.X = line.p2.X;
|
p2.Y = line.p2.Y;
|
|
PointF pv = GetVerticalPoint(p1, p2, point);
|
vp = pv;
|
|
if (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) ||
|
(pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) {
|
return true;
|
}
|
return false;
|
}
|
|
/****************************************************************
|
* p3
|
* | 'length'
|
* |
|
* p1------------------>p2
|
* |
|
* | 'R'
|
* p3
|
* @param p1
|
* @param p2
|
* @param length
|
* @param dir
|
* @return
|
*/
|
PointF Calc3Point(PointF p1, PointF p2, double length, char dir)
|
{
|
PointF p3;
|
|
dir = toupper(dir);
|
|
if (dir != 'L' && dir != 'R')
|
dir = 'L';
|
|
if (isEqual(p1.X, p2.X)) {
|
p3.Y = p2.Y;
|
if (p2.Y > p1.Y) {
|
p3.X = p2.X + ((dir == 'L') ? -length : length);
|
} else {
|
p3.X = p2.X + ((dir=='L') ? length : -length);
|
}
|
return p3;
|
}
|
if (isEqual(p1.Y, p2.Y)) {
|
p3.X = p2.X;
|
if (p2.X > p1.X) {
|
p3.Y = p2.Y + ((dir == 'L') ? length : -length);
|
} else {
|
p3.Y = p2.Y + ((dir == 'L') ? -length : length);
|
}
|
return p3;
|
}
|
|
double k = (p2.Y - p1.Y) / (p2.X - p1.X);
|
double b = p1.Y - k*p1.X;
|
|
double A = 1 + pow(k, 2);
|
double B = 2*k*(b - p2.Y) - 2*p2.X;
|
double C = pow(b - p2.Y, 2) + pow(p2.X, 2) - pow(length, 2);
|
|
double x3, y3;
|
|
|
if (p1.X < p2.X) {
|
x3 = (- B - sqrt(pow(B, 2) - 4*A*C))/(2*A);
|
} else {
|
x3 = (- B + sqrt(pow(B, 2) - 4*A*C))/(2*A);
|
}
|
y3 = k * x3 + b;
|
|
p3.X = x3;
|
p3.Y = y3;
|
|
p3 = rotatePoint(p3, p2, (dir == 'L') ? 270 : 90);
|
|
return p3;
|
}
|
|
/*******************************************************
|
* ori点在yaw方向上延长的距离
|
* @param ori
|
* @param length
|
* @param yaw
|
* @return
|
*/
|
PointF PointExtend(PointF ori, double length, double yaw)
|
{
|
PointF ext;
|
|
ext.X = ori.X + length * sin(toRadians(yaw));
|
ext.Y = ori.Y + length * cos(toRadians(yaw));
|
|
return ext;
|
}
|
|
bool IsSamePoint(PointF p1, PointF p2)
|
{
|
return isEqual(p1.X, p2.X) && isEqual(p1.Y, p2.Y);
|
}
|
|
double AvgYaw(std::vector<double> &angles)
|
{
|
double x = 0, y = 0;
|
double deg = 0;
|
|
for (int i = 0; i < angles.size(); ++i) {
|
x += sin(toRadians(angles[i]));
|
y += cos(toRadians(angles[i]));
|
}
|
|
if (fabs(y) <= EPSILON) {
|
if (x > EPSILON) {
|
deg = 90;
|
} else {
|
deg = 270;
|
}
|
}
|
else if (fabs(x) <= EPSILON) {
|
if (y > EPSILON) {
|
deg = 0;
|
} else {
|
deg = 180;
|
}
|
} else {
|
deg = toDegree(atan(fabs(x / y)));
|
|
if (x < -EPSILON && y > EPSILON) {
|
deg = 360 - deg;
|
}
|
else if (x < -EPSILON && y < -EPSILON) {
|
deg = 180 + deg;
|
}
|
else if (x > EPSILON && y < -EPSILON) {
|
deg = 180 - deg;
|
}
|
}
|
|
return deg;
|
}
|
|
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;
|
}
|
