//计算由a,b,c三点组成的向量 abxac的叉积,注:此处只是返回标量(平行四边形面积)并非实际意义上的叉积
static public double Cross(Vector2_DW ptA, Vector2_DW ptB, Vector2_DW ptC)
{
Vector2_DW ab = ptB - ptA; // new Vector2_DW(b.X - a.X, b.Y - a.Y);
Vector2_DW ac = ptC - ptA; // new Vector2_DW(c.X - a.X, c.Y - a.Y);
return(ab.X * ac.Y - ac.X * ab.Y);
}
static public Vector2_DW Max(Vector2_DW lhs, Vector2_DW rhs)
{
double x = Math.Max(lhs.X, rhs.X);
double y = Math.Max(lhs.Y, rhs.Y);
return(new Vector2_DW(x, y));
}
/// <summary>
/// ///计算pos到直线(lineSt, lineEnd)的最短距离
/// </summary>
/// <param name="pos"></param>
/// <param name="lineSt"></param>
/// <param name="lineEnd"></param>
/// <param name="lineIsSegment">是否为线段</param>
/// <returns></returns>
public static double LinePointDistance(Vector2_DW lineSt, Vector2_DW lineEnd, Vector2_DW pos, bool lineIsSegment)
{
double disRet = 0;
if (lineIsSegment == false)
{
disRet = Vector2_DW.Cross(lineSt, lineEnd, pos) / Vector2_DW.Distance(lineSt, lineEnd);
}
else
{
Vector2_DW ab = lineEnd - lineSt;
Vector2_DW bc = pos - lineEnd;
Vector2_DW ac = pos - lineSt;
if (Vector2_DW.Dot(ab, bc) > 0)//pos在lineEnd外侧
{
disRet = Vector2_DW.Distance(pos, lineEnd);
}
else if (Vector2_DW.Dot(ab, ac) > 0)//pos在lineEnd外侧
{
disRet = Vector2_DW.Distance(pos, lineSt);
}
else//pos在lineSt与lineEnd之间
{
disRet = Vector2_DW.Cross(lineSt, lineEnd, pos) / Vector2_DW.Distance(lineSt, lineEnd);
}
}
return(Math.Abs(disRet));
}
public Vector2_DW Scale(Vector2_DW size)
{
Vector2_DW ptRet = new Vector2_DW();
this.X *= size.X;
this.Y *= size.Y;
return(ptRet);
}
static public Vector2_DW operator +(Vector2_DW lhs, Vector2_DW rhs)
{
Vector2_DW result = lhs;
result.X += rhs.X;
result.Y += rhs.Y;
return(result);
}
static public Vector2_DW operator -(Vector2_DW rhs)
{
Vector2_DW result = new Vector2_DW();
result.X = -rhs.X;
result.Y = -rhs.Y;
return(result);
}
static public Vector2_DW operator *(double d, Vector2_DW rhs)
{
Vector2_DW result = new Vector2_DW();
result.X = d * rhs.X;
result.Y = d * rhs.Y;
return(result);
}
static public Vector2_DW operator *(Vector2_DW lhs, double d)
{
Vector2_DW result = new Vector2_DW();
result.X = d * lhs.X;
result.Y = d * lhs.Y;
return(result);
}
//获取直线的最小包围盒(传入的点的坐标为屏幕坐标)
public static Box2D GetLineMinBox(Vector2_DW screenPtSt, Vector2_DW screenPtEnd)
{
Box2D bxRet = new Box2D();
bxRet._min = Vector2_DW.Min(screenPtSt, screenPtEnd);
bxRet._max = Vector2_DW.Max(screenPtSt, screenPtEnd);
return(bxRet);
}
public Vector2_DW Move(Vector2_DW basePt, Vector2_DW desPt)
{
Vector2_DW ptRet = new Vector2_DW();
Vector2_DW vecOffset = desPt - basePt;
ptRet = this + vecOffset;
return(ptRet);
}
public Vector2_DW Rotate(Vector2_DW ptBase, double angleDegree)
{
Vector2_DW ptRet = new Vector2_DW();
Vector2_DW vec = this - ptBase;
double angleRadium = ToolsMath_DW.Angle2Radium(angleDegree);
double xOffset = (vec.X * Math.Cos(angleRadium) - vec.Y * Math.Sin(angleRadium));
double yOffset = (vec.X * Math.Sin(angleRadium) + vec.Y * Math.Cos(angleRadium));
ptRet = ptBase + new Vector2_DW(xOffset, yOffset);
return(ptRet);
}
public Vector2_DW Mirror(Vector2_DW lineSt, Vector2_DW lineEnd)
{
Vector2_DW ptRet = new Vector2_DW();
Vector2_DW vecPer = (lineSt - lineEnd);
Vector2_DW vec = new Vector2_DW(-vecPer.Y, vecPer.X);
Vector2_DW ptTmp = new Vector2_DW(this + vec);
Vector2_DW ptIntersect = new Vector2_DW();
if (ToolsMath_DW.LineXLine(this, ptTmp, lineSt, lineEnd, ref ptIntersect) != LineIntersectType.None)
{
ptRet = ptIntersect + ptIntersect - this;
}
return(ptRet);
}
static public bool PtInBox(Box2D bx, Vector2_DW posScreen)//判断某一点是否在包围盒内
{
if (posScreen.X > bx._min.X &&
posScreen.X < bx._max.X &&
posScreen.Y > bx._min.Y &&
posScreen.Y < bx._max.Y)
{
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// 判断点pt是否在直线上
/// </summary>
/// <param name="pt"></param>
/// <param name="lnSt"></param>
/// <param name="lnEnd"></param>
/// <param name="isSegment">是否为直线段</param>
/// <returns></returns>
public static bool IsPointOnLine(Vector2_DW pt, Vector2_DW lnSt, Vector2_DW lnEnd, bool isSegment)
{
//设点为Q，线段为P1P2 ，判断点Q在该线段上的依据是：( Q - P1 ) × ( P2 - P1 ) = 0 且 Q 在以 P1，P2为对角顶点的矩形内。前者保证Q点在直线P1P2上，后者是保证Q点不在线段P1P2的延长线或反向延长线上
//My Method:设点为Q，线段为P1P2 ，判断点Q在该线段上的依据是：( Q - P1 ) × ( P2 - P1 ) = 0 且 Q 到p1p2的距离==0。前者保证Q点在直线P1P2上，后者是保证Q点不在线段P1P2的延长线或反向延长线上
Vector2_DW vec1 = pt - lnSt;
Vector2_DW vec2 = pt - lnEnd;
if (IsZero(Vector2_DW.Cross(vec1, vec2)) == true)
{
if (IsZero(LinePointDistance(lnSt, lnEnd, pt, isSegment)))
{
return(true);
}
}
return(false);
}
//判断两个矩形是否相交(注:当一个矩形包围另一个矩形时,也认为是相交)
public static bool BoxXBox(Box2D bx1, Box2D bx2)
{
Box2D intersectBx = new Box2D();//如果两个矩形相交,其相交区域必为矩形
intersectBx._min = Vector2_DW.Max(bx1._min, bx2._min);
intersectBx._max = Vector2_DW.Min(bx1._max, bx2._max);
if (intersectBx._min.X > intersectBx._max.X ||
intersectBx._min.Y > intersectBx._max.Y)
{
return(false);
}
else
{
return(true);
}
}
//获取points的最小包围盒和最左端的点
static public Box2D GetMinBox(Vector2_DW[] points, ref Vector2_DW leftMostPt)
{
Box2D bxRet = new Box2D();
int n = points.Length;
double minX = 0;
double minY = 0;
double maxX = 0;
double maxY = 0;
int nLeftMostIndex = 0;
for (int i = 0; i < n; i++)
{
if (0 == i)
{
minX = maxX = points[i].X;
minY = maxY = points[i].Y;
}
else
{
if (points[i].X < minX)
{
minX = points[i].X;
nLeftMostIndex = i;
}
if (points[i].Y < minY)
{
minY = points[i].Y;
}
if (points[i].X > maxX)
{
maxX = points[i].X;
}
if (points[i].Y > minY)
{
maxY = points[i].Y;
}
}
}
bxRet._min = new Vector2_DW(minX, minY);
bxRet._max = new Vector2_DW(maxX, maxY);
leftMostPt = new Vector2_DW(points[nLeftMostIndex].X, points[nLeftMostIndex].Y);
return(bxRet);
}
/// <summary>
/// 判断直线段与矩形是否相交(注:当线段全部在矩形内,也认为相交)
/// 1,线段端点是否在矩形内,在,true
/// 2,线段包围框是否和矩形相交,否,false
/// 3,矩形四个顶点是否在线段两侧,在true,不在,false
/// </summary>
/// <param name="lStScreen"></param>
/// <param name="lEndScreen"></param>
/// <param name="bx"></param>
/// <returns></returns>
public static bool LineSegmenXBox(Vector2_DW lStScreen, Vector2_DW lEndScreen, Box2D bx)
{
if (ToolsMath_DW.PtInBox(bx, lStScreen) &&
ToolsMath_DW.PtInBox(bx, lEndScreen))
{
return(true);
}
Box2D bxLine = ToolsMath_DW.GetLineMinBox(lStScreen, lEndScreen);
if (!ToolsMath_DW.BoxXBox(bx, bxLine))
{
return(false);
}
Vector2_DW vecLn = lStScreen - lEndScreen;
Vector2_DW vBxLeftUpper = bx._min - lEndScreen;
Vector2_DW vBxRightTop = (new Vector2_DW(bx._max.X, bx._min.Y)) - lEndScreen;
Vector2_DW vBxRightBtm = bx._max - lEndScreen;
Vector2_DW vBxLeftBtm = (new Vector2_DW(bx._min.X, bx._max.Y)) - lEndScreen;
double f1 = Vector2_DW.Cross(vecLn, vBxLeftUpper);
double f2 = Vector2_DW.Cross(vecLn, vBxRightTop);
double f3 = Vector2_DW.Cross(vecLn, vBxRightBtm);
double f4 = Vector2_DW.Cross(vecLn, vBxLeftBtm);
if ((f1 >= 0 && f2 >= 0 && f3 >= 0 && f4 >= 0)
||
(f1 <= 0 && f2 <= 0 && f3 <= 0 && f4 <= 0))
{
return(false);
}
else
{
return(true);
}
}
public Vector2_DW(Vector2_DW vecSrc)
{
_x = vecSrc.X;
_y = vecSrc.Y;
}
public static LineIntersectType LineXLine(Vector2_DW p0, Vector2_DW p1, Vector2_DW p2, Vector2_DW p3, ref Vector2_DW xpt)
{
/*
** Get the intersection of the line defined by p0 and p1 with the
** line defined by p2 and p3. Sets xpt to the intersection point
** if return value is non-negative.
**
** Returns:
** None = Lines are paralell or the points are coincide[eg:p0(0, 0), p1(2, 0),p2(1, 0), p3(4, 0)].
** OnBothExtend = Intersection is not on either segment.
** OnFirstLine = Intersection is on the 1st segment (p0-p1) but not the 2nd.
** OnSecondLine = Intersection is on the 2nd segment (p2-p3) but not the 1st.
** OnBothLine = Intersection is on both line segments.
*/
double a1 = p1.Y - p0.Y;
double b1 = p0.X - p1.X;
double c1 = a1 * p0.X + b1 * p0.Y;
double a2 = p3.Y - p2.Y;
double b2 = p2.X - p3.X;
double c2 = a2 * p2.X + b2 * p2.Y;
double denominator = a1 * b2 - a2 * b1;
if (IsZero(denominator))
{
//paralell
return(LineIntersectType.None);
}
else
{
int on1 = 1;
int on2 = 1;
xpt.X = (b2 * c1 - b1 * c2) / denominator;
xpt.Y = (a1 * c2 - a2 * c1) / denominator;
Vector2_DW vecMin = Vector2_DW.Min(p0, p1);
Vector2_DW vecMax = Vector2_DW.Max(p0, p1);
on1 = (vecMin.X <= xpt.X && xpt.X <= vecMax.X &&
vecMin.Y <= xpt.Y && xpt.Y <= vecMax.Y) ? 1 : 0;
vecMin = Vector2_DW.Min(p2, p3);
vecMax = Vector2_DW.Max(p2, p3);
on2 = (vecMin.X <= xpt.X && xpt.X <= vecMax.X &&
vecMin.Y <= xpt.Y && xpt.Y <= vecMax.Y) ? 1 : 0;
int result = on1 + 2 * on2;
if (0 == result)
{
return(LineIntersectType.OnBothExtend);
}
else if (1 == result)
{
return(LineIntersectType.OnFirstLine);
}
else if (2 == result)
{
return(LineIntersectType.OnSecondLine);
}
else// 3==result
{
return(LineIntersectType.OnBothLine);
}
}
}
public static Vector2_DW operator *(Vector2_DW lhs, double d)
{
Vector2_DW result = new Vector2_DW();
result.X = d * lhs.X;
result.Y = d * lhs.Y;
return result;
}
//返回两个向量之间的夹角(角度制)(0-180)
public static double Angle(Vector2_DW from, Vector2_DW to)
{
return Math.Acos(ToolsMath_DW.Clamp(Vector2_DW.Dot(from.Normalize(), to.Normalize()), -1f, 1f)) * 57.29578f;
}
//计算由a,b,c三点组成的向量 abxac的叉积,注:此处只是返回标量(平行四边形面积)并非实际意义上的叉积
public static double Cross(Vector2_DW ptA, Vector2_DW ptB, Vector2_DW ptC)
{
Vector2_DW ab = ptB - ptA;// new Vector2_DW(b.X - a.X, b.Y - a.Y);
Vector2_DW ac = ptC - ptA;// new Vector2_DW(c.X - a.X, c.Y - a.Y);
return (ab.X * ac.Y - ac.X * ab.Y);
}
/// <summary>
/// 判断直线段与矩形是否相交(注:当线段全部在矩形内,也认为相交)
/// 1,线段端点是否在矩形内,在,true
/// 2,线段包围框是否和矩形相交,否,false
/// 3,矩形四个顶点是否在线段两侧,在true,不在,false
/// </summary>
/// <param name="lStScreen"></param>
/// <param name="lEndScreen"></param>
/// <param name="bx"></param>
/// <returns></returns>
public static bool LineSegmenXBox(Vector2_DW lStScreen, Vector2_DW lEndScreen, Box2D bx)
{
if (ToolsMath_DW.PtInBox(bx, lStScreen)
&& ToolsMath_DW.PtInBox(bx, lEndScreen))
{
return true;
}
Box2D bxLine = ToolsMath_DW.GetLineMinBox(lStScreen, lEndScreen);
if (!ToolsMath_DW.BoxXBox(bx, bxLine))
{
return false;
}
Vector2_DW vecLn = lStScreen - lEndScreen;
Vector2_DW vBxLeftUpper = bx._min - lEndScreen;
Vector2_DW vBxRightTop = (new Vector2_DW(bx._max.X, bx._min.Y)) - lEndScreen;
Vector2_DW vBxRightBtm = bx._max - lEndScreen;
Vector2_DW vBxLeftBtm = (new Vector2_DW(bx._min.X, bx._max.Y)) - lEndScreen;
double f1 = Vector2_DW.Cross(vecLn, vBxLeftUpper);
double f2 = Vector2_DW.Cross(vecLn, vBxRightTop);
double f3 = Vector2_DW.Cross(vecLn, vBxRightBtm);
double f4 = Vector2_DW.Cross(vecLn, vBxLeftBtm);
if ((f1 >= 0 && f2 >= 0 && f3 >= 0 && f4 >= 0)
||
(f1 <= 0 && f2 <= 0 && f3 <= 0 && f4 <= 0))
{
return false;
}
else
{
return true;
}
}
/// <summary>
/// ///计算pos到直线(lineSt, lineEnd)的最短距离
/// </summary>
/// <param name="pos"></param>
/// <param name="lineSt"></param>
/// <param name="lineEnd"></param>
/// <param name="lineIsSegment">是否为线段</param>
/// <returns></returns>
public static double LinePointDistance(Vector2_DW lineSt, Vector2_DW lineEnd, Vector2_DW pos, bool lineIsSegment)
{
double disRet = 0;
if (lineIsSegment == false)
{
disRet = Vector2_DW.Cross(lineSt, lineEnd, pos) / Vector2_DW.Distance(lineSt, lineEnd);
}
else
{
Vector2_DW ab = lineEnd - lineSt;
Vector2_DW bc = pos - lineEnd;
Vector2_DW ac = pos - lineSt;
if (Vector2_DW.Dot(ab, bc) > 0)//pos在lineEnd外侧
{
disRet = Vector2_DW.Distance(pos, lineEnd);
}
else if (Vector2_DW.Dot(ab, ac) > 0)//pos在lineEnd外侧
{
disRet = Vector2_DW.Distance(pos, lineSt);
}
else//pos在lineSt与lineEnd之间
{
disRet = Vector2_DW.Cross(lineSt, lineEnd, pos) / Vector2_DW.Distance(lineSt, lineEnd);
}
}
return Math.Abs(disRet);
}
/// <summary>
/// 判断点pt是否在直线上
/// </summary>
/// <param name="pt"></param>
/// <param name="lnSt"></param>
/// <param name="lnEnd"></param>
/// <param name="isSegment">是否为直线段</param>
/// <returns></returns>
public static bool IsPointOnLine(Vector2_DW pt, Vector2_DW lnSt, Vector2_DW lnEnd, bool isSegment)
{
//设点为Q，线段为P1P2 ，判断点Q在该线段上的依据是：( Q - P1 ) × ( P2 - P1 ) = 0 且 Q 在以 P1，P2为对角顶点的矩形内。前者保证Q点在直线P1P2上，后者是保证Q点不在线段P1P2的延长线或反向延长线上
//My Method:设点为Q，线段为P1P2 ，判断点Q在该线段上的依据是：( Q - P1 ) × ( P2 - P1 ) = 0 且 Q 到p1p2的距离==0。前者保证Q点在直线P1P2上，后者是保证Q点不在线段P1P2的延长线或反向延长线上
Vector2_DW vec1 = pt - lnSt;
Vector2_DW vec2 = pt - lnEnd;
if (IsZero(Vector2_DW.Cross(vec1, vec2)) == true)
{
if (IsZero(LinePointDistance(lnSt, lnEnd, pt, isSegment)))
{
return true;
}
}
return false;
}
//获取points的最小包围盒和最左端的点
public static Box2D GetMinBox(Vector2_DW[] points, ref Vector2_DW leftMostPt)
{
Box2D bxRet = new Box2D();
int n = points.Length;
double minX = 0;
double minY = 0;
double maxX = 0;
double maxY = 0;
int nLeftMostIndex = 0;
for (int i = 0; i < n; i++)
{
if (0 == i)
{
minX = maxX = points[i].X;
minY = maxY = points[i].Y;
}
else
{
if (points[i].X < minX)
{
minX = points[i].X;
nLeftMostIndex = i;
}
if (points[i].Y < minY)
{
minY = points[i].Y;
}
if (points[i].X > maxX)
{
maxX = points[i].X;
}
if (points[i].Y > minY)
{
maxY = points[i].Y;
}
}
}
bxRet._min = new Vector2_DW(minX, minY);
bxRet._max = new Vector2_DW(maxX, maxY);
leftMostPt = new Vector2_DW(points[nLeftMostIndex].X, points[nLeftMostIndex].Y);
return bxRet;
}
//获取直线的最小包围盒(传入的点的坐标为屏幕坐标)
public static Box2D GetLineMinBox(Vector2_DW screenPtSt, Vector2_DW screenPtEnd)
{
Box2D bxRet = new Box2D();
bxRet._min = Vector2_DW.Min(screenPtSt, screenPtEnd);
bxRet._max = Vector2_DW.Max(screenPtSt, screenPtEnd);
return bxRet;
}
public Vector2_DW Scale(Vector2_DW size)
{
Vector2_DW ptRet = new Vector2_DW();
this.X *= size.X;
this.Y *= size.Y;
return ptRet;
}
public Vector2_DW Rotate(Vector2_DW ptBase, double angleDegree)
{
Vector2_DW ptRet = new Vector2_DW();
Vector2_DW vec = this - ptBase;
double angleRadium = ToolsMath_DW.Angle2Radium(angleDegree);
double xOffset = (vec.X * Math.Cos(angleRadium) - vec.Y * Math.Sin(angleRadium));
double yOffset = (vec.X * Math.Sin(angleRadium) + vec.Y * Math.Cos(angleRadium));
ptRet = ptBase + new Vector2_DW(xOffset, yOffset);
return ptRet;
}
public Vector2_DW Move(Vector2_DW basePt, Vector2_DW desPt)
{
Vector2_DW ptRet = new Vector2_DW();
Vector2_DW vecOffset = desPt - basePt;
ptRet = this + vecOffset;
return ptRet;
}
static public double Dot(Vector2_DW lhs, Vector2_DW rhs)
{
return(lhs.X * rhs.X + lhs.Y * rhs.Y);
}
/// <summary>
/// 根据导线点计算巷道左右帮的点
/// 前后两个导线点坐标一样的情况未处理，传入的导线点数据需要保证不重复.
/// </summary>
/// <params name="wirePts">导线点实体</params>
/// <params name="verticesLeftBtmRet">out，根据导线点计算出的巷道左帮所有点</params>
/// <params name="verticesRightBtmRet">out，根据导线点计算出的巷道右帮所有点</params>
/// <returns></returns>
public bool calc_left_and_right_vertics(WirePoint[] wirePts, ref Vector3_DW[] verticesLeftBtmRet,
ref Vector3_DW[] verticesRightBtmRet)
{
if (wirePts == null)
{
return false;
}
int nTraversePtCnt = wirePts.Length;
if (nTraversePtCnt < 1)
{
return false;
}
#region 仅含两个导线点
if (nTraversePtCnt == 2)
{
bool bRet = calc_left_and_right_vertics_with2_traver_points(wirePts, ref verticesLeftBtmRet,
ref verticesRightBtmRet);
if (bRet == false)
{
return false;
}
}
#endregion
#region 大于等于三个点
else
{
var lstLeftBtmVertices = new List<Vector3_DW>();
var lstRightBtmVertices = new List<Vector3_DW>();
#region For loop
for (int i = 0; i < nTraversePtCnt - 2; i++)
{
var lwDatasPreTmp = new[]
{
wirePts[i],
wirePts[i + 1]
};
var lwDatasNextTmp = new[]
{
wirePts[i + 1],
wirePts[i + 2]
};
Vector3_DW[] verticesLeftPreTmp = null;
Vector3_DW[] verticesRightPreTmp = null;
if (false ==
calc_left_and_right_vertics_with2_traver_points(lwDatasPreTmp, ref verticesLeftPreTmp,
ref verticesRightPreTmp))
{
return false;
}
Vector3_DW[] verticesLeftNextTmp = null;
Vector3_DW[] verticesRightNextTmp = null;
if (false ==
calc_left_and_right_vertics_with2_traver_points(lwDatasNextTmp, ref verticesLeftNextTmp,
ref verticesRightNextTmp))
{
return false;
}
var vertexMid2D = new Vector2_DW();
var vertexLeftMid = new Vector3_DW();
var vertexRightMid = new Vector3_DW();
//左邦中间的点
LineIntersectType lit =
ToolsMath_DW.LineXLine(new Vector2_DW(verticesLeftPreTmp[0].X, verticesLeftPreTmp[0].Y),
new Vector2_DW(verticesLeftPreTmp[1].X, verticesLeftPreTmp[1].Y),
new Vector2_DW(verticesLeftNextTmp[0].X, verticesLeftNextTmp[0].Y),
new Vector2_DW(verticesLeftNextTmp[1].X, verticesLeftNextTmp[1].Y), ref vertexMid2D);
if (lit == LineIntersectType.None) //有重复点,可能是这种情况eg:p0(0, 0), p1(2, 0),p2(1, 0), p3(4, 0)
{
vertexLeftMid.X = verticesLeftPreTmp[1].X;
vertexLeftMid.Y = verticesLeftPreTmp[1].Y;
vertexLeftMid.Z = lwDatasPreTmp[1].coordinate_z;
}
else
{
vertexLeftMid.X = vertexMid2D.X;
vertexLeftMid.Y = vertexMid2D.Y;
vertexLeftMid.Z = lwDatasPreTmp[1].coordinate_z;
}
//右邦中间的点
lit = ToolsMath_DW.LineXLine(new Vector2_DW(verticesRightPreTmp[0].X, verticesRightPreTmp[0].Y),
new Vector2_DW(verticesRightPreTmp[1].X, verticesRightPreTmp[1].Y),
new Vector2_DW(verticesRightNextTmp[0].X, verticesRightNextTmp[0].Y),
new Vector2_DW(verticesRightNextTmp[1].X, verticesRightNextTmp[1].Y), ref vertexMid2D);
if (lit == LineIntersectType.None) //有重复点,可能是这种情况eg:p0(0, 0), p1(2, 0),p2(1, 0), p3(4, 0)
{
vertexRightMid.X = verticesRightPreTmp[1].X;
vertexRightMid.Y = verticesRightPreTmp[1].Y;
vertexRightMid.Z = lwDatasPreTmp[1].coordinate_z;
}
else
{
vertexRightMid.X = vertexMid2D.X;
vertexRightMid.Y = vertexMid2D.Y;
vertexRightMid.Z = lwDatasPreTmp[1].coordinate_z;
}
//保存计算出来的点
//第一个顶点
if (i == 0)
{
lstLeftBtmVertices.Add(verticesLeftPreTmp[0]);
lstRightBtmVertices.Add(verticesRightPreTmp[0]);
}
//中间的顶点
lstLeftBtmVertices.Add(vertexLeftMid);
lstRightBtmVertices.Add(vertexRightMid);
//最后一个顶点
if (i == nTraversePtCnt - 3)
{
lstLeftBtmVertices.Add(verticesLeftNextTmp[1]);
lstRightBtmVertices.Add(verticesRightNextTmp[1]);
}
} //end for
#endregion
verticesLeftBtmRet = lstLeftBtmVertices.ToArray();
verticesRightBtmRet = lstRightBtmVertices.ToArray();
}
#endregion
return true;
}
//计算abxac的叉积,注:此处只是返回标量(平行四边形面积)并非实际意义上的叉积
static public double Cross(Vector2_DW vecL, Vector2_DW vecR)
{
return(vecL.X * vecR.Y - vecR.X * vecL.Y);
}
public static Vector2_DW Min(Vector2_DW lhs, Vector2_DW rhs)
{
double x = Math.Min(lhs.X, rhs.X);
double y = Math.Min(lhs.Y, rhs.Y);
return new Vector2_DW(x, y);
}
public static Vector2_DW operator -(Vector2_DW rhs)
{
Vector2_DW result = new Vector2_DW();
result.X = -rhs.X;
result.Y = -rhs.Y;
return result;
}
public static double Dot(Vector2_DW lhs, Vector2_DW rhs)
{
return (lhs.X * rhs.X + lhs.Y * rhs.Y);
}
public static Vector2_DW operator *(double d, Vector2_DW rhs)
{
Vector2_DW result = new Vector2_DW();
result.X = d * rhs.X;
result.Y = d * rhs.Y;
return result;
}
//返回a,b两点之间的距离
public static double Distance(Vector2_DW a, Vector2_DW b)
{
Vector2_DW vec = b - a;
return vec.Magnitude();
}
public Vector2_DW(Vector2_DW vecSrc)
{
_x = vecSrc.X;
_y = vecSrc.Y;
}
//计算abxac的叉积,注:此处只是返回标量(平行四边形面积)并非实际意义上的叉积
public static double Cross(Vector2_DW vecL, Vector2_DW vecR)
{
return (vecL.X * vecR.Y - vecR.X * vecL.Y);
}
//判断某一点是否在包围盒内
public static bool PtInBox(Box2D bx, Vector2_DW posScreen)
{
if (posScreen.X > bx._min.X
&& posScreen.X < bx._max.X
&& posScreen.Y > bx._min.Y
&& posScreen.Y < bx._max.Y)
{
return true;
}
else
{
return false;
}
}
//返回a,b两点之间的距离
static public double Distance(Vector2_DW a, Vector2_DW b)
{
Vector2_DW vec = b - a;
return(vec.Magnitude());
}
public static LineIntersectType LineXLine(Vector2_DW p0, Vector2_DW p1, Vector2_DW p2, Vector2_DW p3, ref Vector2_DW xpt)
{
/*
** Get the intersection of the line defined by p0 and p1 with the
** line defined by p2 and p3. Sets xpt to the intersection point
** if return value is non-negative.
**
** Returns:
** None = Lines are paralell or the points are coincide[eg:p0(0, 0), p1(2, 0),p2(1, 0), p3(4, 0)].
** OnBothExtend = Intersection is not on either segment.
** OnFirstLine = Intersection is on the 1st segment (p0-p1) but not the 2nd.
** OnSecondLine = Intersection is on the 2nd segment (p2-p3) but not the 1st.
** OnBothLine = Intersection is on both line segments.
*/
double a1 = p1.Y - p0.Y;
double b1 = p0.X - p1.X;
double c1 = a1 * p0.X + b1 * p0.Y;
double a2 = p3.Y - p2.Y;
double b2 = p2.X - p3.X;
double c2 = a2 * p2.X + b2 * p2.Y;
double denominator = a1 * b2 - a2 * b1;
if (IsZero(denominator))
{
//paralell
return LineIntersectType.None;
}
else
{
int on1 = 1;
int on2 = 1;
xpt.X = (b2 * c1 - b1 * c2) / denominator;
xpt.Y = (a1 * c2 - a2 * c1) / denominator;
Vector2_DW vecMin = Vector2_DW.Min(p0, p1);
Vector2_DW vecMax = Vector2_DW.Max(p0, p1);
on1 = (vecMin.X <= xpt.X && xpt.X <= vecMax.X &&
vecMin.Y <= xpt.Y && xpt.Y <= vecMax.Y) ? 1 : 0;
vecMin = Vector2_DW.Min(p2, p3);
vecMax = Vector2_DW.Max(p2, p3);
on2 = (vecMin.X <= xpt.X && xpt.X <= vecMax.X &&
vecMin.Y <= xpt.Y && xpt.Y <= vecMax.Y) ? 1 : 0;
int result = on1 + 2 * on2;
if (0 == result)
{
return LineIntersectType.OnBothExtend;
}
else if (1 == result)
{
return LineIntersectType.OnFirstLine;
}
else if (2 == result)
{
return LineIntersectType.OnSecondLine;
}
else// 3==result
{
return LineIntersectType.OnBothLine;
}
}
}
//返回两个向量之间的夹角(角度制)(0-180)
static public double Angle(Vector2_DW from, Vector2_DW to)
{
return(Math.Acos(ToolsMath_DW.Clamp(Vector2_DW.Dot(from.Normalize(), to.Normalize()), -1f, 1f)) * 57.29578f);
}
/// <summary>
/// 计算仅含两个导线点的巷道的左右邦点
/// </summary>
/// <params name="wirePts"></params>
/// <params name="verticesLeftRet"></params>
/// <params name="verticesRightRet"></params>
/// <returns></returns>
private bool calc_left_and_right_vertics_with2_traver_points(WirePoint[] wirePts,
ref Vector3_DW[] verticesLeftRet, ref Vector3_DW[] verticesRightRet)
{
if (wirePts == null)
{
return false;
}
int nTraversePtCnt = wirePts.Length;
if (nTraversePtCnt != 2)
{
return false;
}
//仅含两个导线点
if (nTraversePtCnt == 2)
{
var ptPre = new Vector2_DW(wirePts[0].coordinate_x, wirePts[0].coordinate_y);
var ptNext = new Vector2_DW(wirePts[1].coordinate_x, wirePts[1].coordinate_y);
Vector2_DW vecForwardDir = (ptNext - ptPre).Normalize();
/*根据法线方向判断巷道左右邦.
* 若垂直向量vecPerpendicular与vecForwardDir的叉积所得向量与Y轴夹角小于90度(二者点积大于0),,则认为该垂直向量为左帮方向
* x1*x2 + y1*y2 = 0//垂直
* x1*y2 - x2*y1 = 0//平行
*
*点积 a●b=|a|*|b|*cos(w)=x1*x2+y1*y2=0
*叉积 aXb=|a|*|b|*sin(w)=(x1*z2-z1*y2)x^+(z1*x2-z1*y2)y^+(x1*y2-y1*x2)z^(右手螺旋定则)=0 -->二维的表示平行四边形的面积
*混合积 (aXb)●c = |aXb|*|c|*cos(aXb,c) -->平行六面体的体积
*/
var vecPerpendicularLeft = new Vector2_DW(vecForwardDir.Y, -vecForwardDir.X);
Vector2_DW vecPerpendicularRight = -vecPerpendicularLeft;
Vector3_DW vecNormal = Vector3_DW.Cross(new Vector3_DW(vecForwardDir.X, vecForwardDir.Y, 0),
new Vector3_DW(vecPerpendicularLeft.X, vecPerpendicularLeft.Y, 0));
if (vecNormal.Z < 0)
{
Vector2_DW vecSwap = vecPerpendicularLeft;
vecPerpendicularLeft = vecPerpendicularRight;
vecPerpendicularRight = vecSwap;
}
var leftVertices = new List<Vector3_DW>();
var rightVertices = new List<Vector3_DW>();
Vector2_DW ptLeftPre = ptPre + vecPerpendicularLeft * wirePts[0].left_distance;
Vector2_DW ptLeftNext = ptNext + vecPerpendicularLeft * wirePts[1].left_distance;
Vector2_DW ptRightPre = ptPre + vecPerpendicularRight * wirePts[0].right_distance;
Vector2_DW ptRightNext = ptNext + vecPerpendicularRight * wirePts[1].right_distance;
leftVertices.Add(new Vector3_DW(ptLeftPre.X, ptLeftPre.Y, wirePts[0].coordinate_z));
leftVertices.Add(new Vector3_DW(ptLeftNext.X, ptLeftNext.Y, wirePts[1].coordinate_z));
rightVertices.Add(new Vector3_DW(ptRightPre.X, ptRightPre.Y, wirePts[0].coordinate_z));
rightVertices.Add(new Vector3_DW(ptRightNext.X, ptRightNext.Y, wirePts[1].coordinate_z));
verticesLeftRet = leftVertices.ToArray();
verticesRightRet = rightVertices.ToArray();
}
return true;
}
public Vector2_DW Mirror(Vector2_DW lineSt, Vector2_DW lineEnd)
{
Vector2_DW ptRet = new Vector2_DW();
Vector2_DW vecPer = (lineSt - lineEnd);
Vector2_DW vec = new Vector2_DW(-vecPer.Y, vecPer.X);
Vector2_DW ptTmp = new Vector2_DW(this + vec);
Vector2_DW ptIntersect = new Vector2_DW();
if (ToolsMath_DW.LineXLine(this, ptTmp, lineSt, lineEnd, ref ptIntersect) != LineIntersectType.None)
{
ptRet = ptIntersect + ptIntersect - this;
}
return ptRet;
}
