计算几何
2020-12-09 本文已影响0人
hehehehe
import numpy as np
import math
import time
from datetime import datetime
import Tools_geoTools as geotool
from shapely.geometry import Point,LineString
EPS = 1E-10
def vecCross(vec1 : list,vec2 : list):
"""
a.x * b.y - a.y * b.x;
"""
return vec1[0] * vec2[1] - vec1[1] * vec2[0]
def vecDot(vec1 : list,vec2 : list):
"""
a.x * b.x + a.y * b.y
"""
return vec1[0] * vec2[0] + vec1[1] * vec2[1]
def vecNorm(vec : list):
"""
a.x * a.x + a.y * a.y
"""
return vec[0] * vec[0] + vec[1] * vec[1]
def vecLen(vec : list):
"""
a.x * a.x + a.y * a.y1//
"""
return math.sqrt(vecNorm(vec))
def getVec(p1 : list, p2 : list):
"""
拿到向量p1p2
"""
return p2[0] - p1[0], p2[1] - p1[1]
def getAngle(p1 : list, p2 : list , p3 : list, p4 : list) -> float:
"""
计算向量p1p2和p3p4角度
"""
vec1 = getVec(p1,p2)
vec2 = getVec(p3,p4)
return math.acos(vecDot(vec1,vec2)/vecLen(vec1)/vecLen(vec2))
# def vecLen(p1 : list, p2 : list) -> float:
# """
# 计算向量p1p2的模
# """
# vec = getVec(p1,p2)
# return np.linalg.norm(vec)
def vecNormal(p1 : list, p2 : list) -> tuple:
"""
计算向量p1p2的法向量
"""
vec = getVec(p1,p2)
length = vecLen(p1,p2)
return (-vec[1]/length,vec[0]/length)
def pointLineDistance(p : list, line_a : list, line_b : list) -> float:
"""
计算p和向量line_aline_b的距离
"""
v = getVec(line_a,p)
u = getVec(line_a,line_b)
return math.fabs(vecCross(v,u))/vecLen(getVec(line_a,line_b))
def pointSegmentDistance(p : list, line_a : list, line_b : list) -> float:
"""
计算p和线段line_aline_b的距离
"""
if(vecDot(getVec(line_a,line_b),getVec(line_a,p)) < 0.0):
return vecLen(getVec(line_a,p))
if(vecDot(getVec(line_b,line_a),getVec(line_b,p)) < 0.0):
return vecLen(getVec(line_b,p))
return pointLineDistance(p,line_a,line_b)
def getFootPoint(point, line_p1, line_p2):
"""
@point, line_p1, line_p2 : [x, y, z]
"""
x0 = point[0]
y0 = point[1]
z0 = point[2]
x1 = line_p1[0]
y1 = line_p1[1]
z1 = line_p1[2]
x2 = line_p2[0]
y2 = line_p2[1]
z2 = line_p2[2]
k = -((x1 - x0) * (x2 - x1) + (y1 - y0) * (y2 - y1) + (z1 - z0) * (z2 - z1)) / \
((x2 - x1) ** 2 + (y2 - y1) ** 2 + (z2 - z1) ** 2)*1.0
xn = k * (x2 - x1) + x1
yn = k * (y2 - y1) + y1
zn = k * (z2 - z1) + z1
return (xn, yn, zn)
def project(point, line_p1, line_p2):
"""
点point在线段line_p1, line_p2上的投影
"""
base = getVec(line_p1,line_p2)
r = vecDot(getVec(line_p1,point), base) / vecNorm(getVec(line_p1,line_p2))
return np.array(line_p1) + np.array(base) * r
def symmetrical(point, line_p1, line_p2):
"""
计算point和线段line_aline_b的对称点
"""
return np.array(point) + (getFootPoint2(point, line_p1, line_p2) - np.array(point)) * 2.0
def isParallel(p1 : list, p2 : list , p3 : list, p4 : list):
"""
判断向量p1p2和p3p4是否平行
"""
vec1 = getVec(p1,p2)
vec2 = getVec(p3,p4)
return np.cross(vec1,vec2) - 0 < EPS
def isOrthogonal(p1 : list, p2 : list , p3 : list, p4 : list):
"""
判断向量p1p2和p3p4是垂直
"""
vec1 = getVec(p1,p2)
vec2 = getVec(p3,p4)
return np.dot(vec1,vec2) - 0 < EPS
def clockWise(point, line_p1, line_p2):
vec1 = getVec(point,line_p1)
vec2 = getVec(point,line_p2)
"""
判断向量之间的位置关系
"""
if(np.cross(vec1,vec2) > EPS):
return 1
if(np.cross(vec1,vec2) < -EPS):
return -1
if(np.dot(vec1,vec2) < -EPS):
return 0
if(vecLen(point,line_p1) < vecLen(point,line_p2)):
return -2
if(vecLen(point,line_p1) > vecLen(point,line_p2)):
return 2
return 123
def pointSegmentClockWise(point, line_p1, line_p2):
vec1 = getVec(point,line_p1)
vec2 = getVec(point,line_p2)
"""
判断点和线段之间的位置关系
"""
if (vecLen(point,line_p1) < EPS):
return "位于p1"
if (vecLen(point,line_p2) < EPS):
return "位于p2"
if(np.cross(vec1,vec2) > EPS):
return "在左边"
if(np.cross(vec1,vec2) < -EPS):
return "在右边"
if(np.dot(vec1,vec2) < -EPS):
return "在线段上"
if(vecLen(point,line_p1) < vecLen(point,line_p2)):
return "在线后"
if(vecLen(point,line_p1) > vecLen(point,line_p2)):
return "在线前"
return 123
def pointSegmentClockWiseOnly(point, line_p1, line_p2):
vec1 = getVec(point,line_p1)
vec2 = getVec(point,line_p2)
"""
仅用于判断左右位置关系
用于判断线段是否相交
"""
if(np.cross(vec1,vec2) > EPS):
return 1
if(np.cross(vec1,vec2) < -EPS):
return -1
return 0
def segmentIntersect(p1 : list, p2 : list , p3 : list, p4 : list):
"""
判断线段是否相交
这里必须and了下 要使的p1和p3都满足
"""
bool1 = pointSegmentClockWiseOnly(p1,p2,p3) * pointSegmentClockWiseOnly(p1,p2,p4) <= 0
bool2 = pointSegmentClockWiseOnly(p3,p2,p1) * pointSegmentClockWiseOnly(p3,p4,p2) <= 0
return bool1 and bool2
def segmentDistance(p1 : list, p2 : list , p3 : list, p4 : list):
if segmentIntersect(p1,p2,p3,p4):
return 0.0
return np.min([np.min([pointSegmentDistance(p1,p3,p4),pointSegmentDistance(p2,p3,p4)]),
np.min([pointSegmentDistance(p3,p1,p2),pointSegmentDistance(p4,p1,p2)])])
def segmentCrossPoint(p1 : list, p2 : list , p3 : list, p4 : list):
if not segmentIntersect(p1,p2,p3,p4):
return None
base = getVec(p3,p4)
d1 = np.linalg.norm(np.cross(base,getVec(p3,p1)))
d2 = np.linalg.norm(np.cross(base,getVec(p3,p2)))
t =d1 / (d1+d2)
return p1 + getVec(p1,p2) * t
def lineCrossPoint(p1 : list, p2 : list , p3 : list, p4 : list):
v= getVec(p1,p2)
w= getVec(p3,p4)
u= getVec(p3,p1)
t= np.cross(w, u) / np.cross(v, w);
return p1 + v * t;
def lineCrossPointAdvance(p1 : list, p2 : list , p3 : list, p4 : list):
a1 = p1[1] - p2[1];
b1 = p2[0] - p1[0];
c1 = np.cross(p1,p2);
a2 = p3[1] - p4[1];
b2 = p4[0] - p3[0];
c2 = np.cross(p3,p4);
d = a1 * b2 - a2 * b1;
x = (b1 * c2 - b2 * c1) / d
y = (c1 * a2 - c2 * a1) / d
return [x, y];
def twoPointLineFunction(p1 : list, p2 : list):
'''
计算经过两点的直线方程,y= ax + b
:param f_coords:
:param l_coords:
:return:
'''
try:
gradient = (p2[1] - p1[1]) / (p2[0] - p1[0])
bias = p1[1] - gradient * p1[0]
except ZeroDivisionError:
gradient = None
bias = p1[0]
return gradient, bias
def pointRotate(p1 : list,po : list,alpha : float) -> tuple:
"""
返回点p以点o为圆心逆时针旋转alpha(单位：弧度)后所在的位置
"""
vec = getVec(po,p1)
x = vec[0] * math.cos(alpha) - vec[1] * math.sin(alpha) + po[0]
y = vec[1] * math.cos(alpha) + vec[0] * math.sin(alpha) + po[1]
return x,y
def pointOnSegment(p1 : list,line_p1 : list,line_p2 : list) -> bool:
"""
判断点p是否在线段l上
条件：(p在线段l所在的直线上) && (点p在以线段l为对角线的矩形内)
return( (multiply(l.e,p,l.s)==0) &&
( ( (p.x-l.s.x)*(p.x-l.e.x)<=0 )&&( (p.y-l.s.y)*(p.y-l.e.y)<=0 ) ) );
"""
vec1 = getVec(p1,line_p1)
vec2 = getVec(p1,line_p2)
isInLine = vecCross(vec1,vec2) > -EPS or vecCross(vec1,vec2) < EPS
isInRectangle = ((p1[0] - line_p2[0]) * (p1[0] - line_p1[0]) <= 0) and \
((p1[1] - line_p2[1]) * (p1[1] - line_p1[1]) <=0)
return isInLine and isInRectangle
def pointOnSegment2(p1 : list,line_p1 : list,line_p2 : list) -> bool:
"""
判断点p是否在线段l上
条件：(p在线段l所在的直线上) && (点p在以线段l为对角线的矩形内)
return( (multiply(l.e,p,l.s)==0) &&
( ( (p.x-l.s.x)*(p.x-l.e.x)<=0 )&&( (p.y-l.s.y)*(p.y-l.e.y)<=0 ) ) );
"""
vec1 = getVec(p1,line_p1)
vec2 = getVec(p1,line_p2)
isInLine = math.fabs(vecCross(vec1,vec2)) < EPS
isInRectangle = vecDot(vec1,vec2) <= 0
return isInLine and isInRectangle
def getLineByTwoPoints(p1 : list,p2 : list):
"""
根据已知两点坐标，求过这两点的直线解析方程： a*x+b*y+c = 0 (a >= 0)
"""
sign = 1;
a = p2[1] - p1[1]
if a < 0 :
sign = -1
a = -a
b = sign * (p1[0] - p2[0])
c = sign * (p1[1] * p2[0] - p1[0] * p2[1])
return a,b,c
if __name__ == '__main__':
# print(getAngle([0,0],[1,0],[0,0],[1,1]) * 180 / 3.14)
# print(vecNormal([0,0],[10,0]))
# print(pointLineDistance([2,-1],[0,2],[2,0]))
# p1,p2,p3,p4 = [1,0],[0,1],[0,2],[2,0]
# print(pointSegmentDistance([1,0],[0,1],[0,2]))
# print(getFootPoint([1,-1,0],[1,0,0],[0,1,0]))
# print(getFootPoint2([0,0],[1,0],[0,10]))
# print(isParallel([0,0],[1,0],[0,0],[0,10]))
# print(isOrthogonal([0,0],[1,0],[0,0],[0,10]))
# print(symmetrical([0,0],[1,0],[0,1]))
# print(clockWise([0,1],[0,0],[0,1]))
# print(segmentIntersect([0,0],[1.1,1.8],[0,2],[2,0]))
# print(segmentDistance([0,0],[1,1],[1,0],[2,0]))
# print(segmentCrossPoint([0,0],[1.1,1.8],[0,2],[2,0]))
# start1 = time.perf_counter()
# # print(pointSegmentDistance([1,0],[0,1],[0,2]))
# print(pointSegmentDistance([0.5,0.5],[0,2],[2,0]))
# end1 = time.perf_counter()
# print(end1-start1)
# start2 = time.perf_counter()
# print(geotool.distance_point2segment((0.5,0.5),((0,2),(2,0))))
# end2 = time.perf_counter()
# print(str(end2-start2))
# start3 = time.perf_counter()
# line = LineString([(0,2),(2,0)])
# point = Point(0.5,0.5)
# print(point.distance(line))
# end3 = time.perf_counter()
# print(end3-start3)
# print(point_in_2segment((1,1),((2,0),(0,1))))
# print(lineCrossPointAdvance([0,0],[1.1,1.8],[0,2],[2,0]))
# print(intersection_2line([],[]))
# print(twoPointLineFunction([0,0],[1,2]))
# print(pointRotate([1,1],[0,0],45 * 3.14/180))
# start3 = time.perf_counter()
# print(pointOnSegment2([1.99,1.998],[0,0],[2,2]))
# start4 = time.perf_counter()
# print(start4-start3)
print(getLineByTwoPoints([1,1],[2,4]))
# print(pointSegmentClockWise([0,1.1],[0,0],[0,1]))
# print(pointSegmentClockWise([0,1],[0,0],[0,1]))
# print(pointSegmentClockWise([0,0.8],[0,0],[0,1]))
# print(pointSegmentClockWise([0,0.2],[0,0],[0,1]))
# print(pointSegmentClockWise([0,0],[0,0],[0,1]))
# print(pointSegmentClockWise([0,-0.1],[0,0],[0,1]))
# print("============")
# print(pointSegmentClockWise([1,1.1],[0,0],[0,1]))
# print(pointSegmentClockWise([1,1],[0,0],[0,1]))
# print(pointSegmentClockWise([1,0.8],[0,0],[0,1]))
# print(pointSegmentClockWise([1,0.2],[0,0],[0,1]))
# print(pointSegmentClockWise([1,0],[0,0],[0,1]))
# print(pointSegmentClockWise([1,-0.1],[0,0],[0,1]))
# print("============")
# print(pointSegmentClockWise([-1,1.1],[0,0],[0,1]))
# print(pointSegmentClockWise([-1,1],[0,0],[0,1]))
# print(pointSegmentClockWise([-1,0.8],[0,0],[0,1]))
# print(pointSegmentClockWise([-1,0.2],[0,0],[0,1]))
# print(pointSegmentClockWise([-1,0],[0,0],[0,1]))
# print(pointSegmentClockWise([-1,-0.1],[0,0],[0,1]))
# print("============")
# print(pointSegmentClockWise([0.5,0.5],[2,0],[0,2]))
# print(pointSegmentClockWise([1.5,1.5],[2,0],[0,2]))
# print(segmentIntersect([0,0],[1,1.5],[1,0],[1,1]))
# print(segmentIntersect([0,0],[1,1],[1,0],[1,1]))
# print(segmentIntersect([0,0],[1,0.9],[1,0],[1,1]))
# print(segmentIntersect([0,0],[1,0],[1,0],[1,1]))
# print(segmentIntersect([0,0],[1,-0.1],[1,0],[1,1]))
# print("============")
# print(segmentIntersect([0,0],[0.5,0.5],[1,0],[1,1]))
# print(segmentIntersect([0,0],[1,1],[1,0],[1,0.5]))
# print(segmentIntersect([0,0],[0.5,0.5],[1,0],[1,1]))
# print(segmentIntersect([0,0],[0.5,0.5],[0,2],[2,0]))
# print(np.cross([0,1],[-1,1]))
