public static bool IsTriangleInsectTriangle2(Vector2 p1, Vector2 p2, Vector2 p3, Vector2 p4, Vector2 p5, Vector2 p6, ref GeoInsectPointArrayInfo insect)
{
GeoInsectPointArrayInfo tmp = new GeoInsectPointArrayInfo();
List <Vector2> tri = new List <Vector2>();
tri.Add(p1);
tri.Add(p2);
tri.Add(p3);
bool inSect = false;
for (int i = 0; i < 3; ++i)
{
int j = (i + 1) % 3;
inSect = GeoSegmentUtils.IsSegmentInsectOrOverlapTriangle2(tri[i], tri[j], p4, p5, p6, ref tmp) || inSect;
}
if (inSect)
{
insect.mHitGlobalPoint.mPointArray.AddRange(tmp.mHitGlobalPoint.mPointArray);
}
insect.mIsIntersect = insect.mHitGlobalPoint.mPointArray.Count > 0;
if (insect.mIsIntersect)
{
insect.UniquePoint();
}
return(insect.mIsIntersect);
}
public static bool IsTriangleInsectPlaneCircle2(Vector3 p1, Vector3 p2, Vector3 p3, Vector3 center, float r, GeoPlane plane, ref GeoInsectPointArrayInfo insect)
{
Vector3 p11 = plane.TransformToLocal(p1);
Vector3 p21 = plane.TransformToLocal(p2);
Vector3 p31 = plane.TransformToLocal(p3);
Vector3 c1 = plane.TransformToLocal(center);
Vector2 p12 = new Vector2(p11.x, p11.z);
Vector2 p22 = new Vector2(p21.x, p21.z);
Vector2 p32 = new Vector2(p31.x, p31.z);
Vector2 c2 = new Vector2(c1.x, c1.z);
GeoInsectPointArrayInfo temp = new GeoInsectPointArrayInfo();
bool isInsect1 = IsTriangleInsectCircle2(p12, p22, p32, c2, r, ref temp);
if (isInsect1)
{
insect.mIsIntersect = true;
foreach (Vector3 v in temp.mHitGlobalPoint.mPointArray)
{
Vector3 vv = new Vector3(v.x, 0.0f, v.y);
vv = plane.TransformToGlobal(vv);
insect.mHitGlobalPoint.mPointArray.Add(vv);
}
return(true);
}
return(false);
}
public bool IsIntersect(ref GeoRay2 dist, ref GeoInsectPointArrayInfo insect)
{
bool isInsect = GeoRayUtils.IsRayInsectAABB2(dist.mOrigin, dist.mDirection, mAABB2.mMin, mAABB2.mMax, ref insect);
if (isInsect)
{
insect.mHitObject2 = this;
insect.mLength = (GeoUtils.ToVector2(insect.mHitGlobalPoint.mPointArray[0]) - dist.mOrigin).magnitude;
}
return(isInsect);
}
public bool IsIntersect(ref GeoRay3 dist, ref GeoInsectPointArrayInfo insect)
{
GeoInsectPointInfo info = new GeoInsectPointInfo();
bool isInsect = GeoRayUtils.IsRayInsectTriangle3(dist.mOrigin, dist.mDirection, mP1, mP2, mP3, ref info);
if (isInsect)
{
insect.mHitObject2 = this;
insect.mHitGlobalPoint.mPointArray.Add(info.mHitGlobalPoint);
insect.mLength = (info.mHitGlobalPoint - dist.mOrigin).magnitude;
}
return(isInsect);
}
public bool IsIntersect(ref GeoRay2 dist, ref GeoInsectPointArrayInfo insect)
{
GeoInsectPointInfo info = new GeoInsectPointInfo();
bool isInsect = GeoRayUtils.IsRayInsectSegment2(dist.mOrigin, dist.mDirection, mSeg.mP1, mSeg.mP2, ref info);
insect.mIsIntersect = isInsect;
if (isInsect)
{
insect.mHitObject2 = this;
insect.mHitGlobalPoint.mPointArray.Add(info.mHitGlobalPoint);
insect.mLength = (GeoUtils.ToVector2(info.mHitGlobalPoint) - dist.mOrigin).magnitude;
}
return(isInsect);
}
public static bool IsPlaneInsectCircle(Vector3 normal, float d, Vector3 center, float r, GeoPlane plane, ref GeoInsectPointArrayInfo insect)
{
float tmp = Mathf.Abs(Vector3.Dot(plane.mNormal.normalized, normal.normalized));
if (1 - tmp < 1e-5f)
{
return(false); // 平行 或 共面
}
GeoInsectPointArrayInfo tmp1 = new GeoInsectPointArrayInfo();
bool inSect = IsPlaneInsectPlane(normal, d, plane.mNormal, plane.mD, ref tmp1);
if (inSect)
{
Vector3 line1 = tmp1.mHitGlobalPoint.mPointArray[0] + tmp1.mHitGlobalPoint.mPointArray[1];
GeoLineUtils.IsLineInsectCirclePlane2(tmp1.mHitGlobalPoint.mPointArray[0], line1, center, r, plane, ref insect);
}
return(insect.mIsIntersect);
}
public static bool IsTriangleInsectCircle3(Vector3 p1, Vector3 p2, Vector3 p3, Vector3 center, float r, GeoPlane plane, ref GeoInsectPointArrayInfo insect)
{
bool isInsect = GeoPlaneUtils.IsPlaneInsectTriangle(plane.mNormal, plane.mD, p1, p2, p3, ref insect);
if (isInsect)
{
if (insect.mHitGlobalPoint.mPointArray.Count == 3) // 共面， 转化成 2d
{
insect.Clear();
return(IsTriangleInsectPlaneCircle2(p1, p2, p3, center, r, plane, ref insect));
}
else if (insect.mHitGlobalPoint.mPointArray.Count == 1)
{
if (GeoCircleUtils.IsInSphere(center, r, insect.mHitGlobalPoint.mPointArray[0]))
{
return(true);
}
}
else if (insect.mHitGlobalPoint.mPointArray.Count == 2)
{
Vector3 seg1 = plane.TransformToLocal(insect.mHitGlobalPoint.mPointArray[0]);
Vector3 seg2 = plane.TransformToLocal(insect.mHitGlobalPoint.mPointArray[1]);
Vector3 c1 = plane.TransformToLocal(center);
Vector2 p12 = new Vector2(seg1.x, seg1.z);
Vector2 p22 = new Vector2(seg2.x, seg2.z);
Vector2 c2 = new Vector2(c1.x, c1.z);
insect.Clear();
GeoInsectPointArrayInfo temp = new GeoInsectPointArrayInfo();
if (GeoSegmentUtils.IsSegmentInsectCircle2(p12, p22, c2, r, ref temp))
{
insect.mIsIntersect = true;
foreach (Vector3 v in temp.mHitGlobalPoint.mPointArray)
{
Vector3 vv = new Vector3(v.x, 0.0f, v.y);
vv = plane.TransformToGlobal(vv);
insect.mHitGlobalPoint.mPointArray.Add(vv);
}
return(true);
}
}
}
return(false);
}
public static bool IsPlaneInsectAABB2(Vector3 normal, float d, Vector3 min, Vector3 max, GeoPlane plane, ref GeoInsectPointArrayInfo insect)
{
float dot = Vector3.Dot(normal, plane.mNormal);
if (1 - Mathf.Abs(dot) < 1e-5f)
{
return(false);
}
GeoInsectPointArrayInfo tmp = new GeoInsectPointArrayInfo();
bool isInsect = GeoPlaneUtils.IsPlaneInsectPlane(normal, d, plane.mNormal, plane.mD, ref tmp);
if (isInsect)
{
Vector3 l1 = tmp.mHitGlobalPoint.mPointArray[0];
Vector3 l2 = l1 + tmp.mHitGlobalPoint.mPointArray[1];
isInsect = GeoLineUtils.IsLineInsectAABBPlane2(l1, l2, min, max, plane, ref insect);
}
return(insect.mIsIntersect);
}
public bool IsIntersect(ref GeoRay2 dist, ref GeoInsectPointArrayInfo insect)
{
bool isInsect = GeoRayUtils.IsRayInsectTriangle2(dist.mOrigin, dist.mDirection, mP1, mP2, mP3, ref insect);
if (isInsect)
{
insect.mHitObject2 = this;
float min = 1e5f;
foreach (Vector3 v in insect.mHitGlobalPoint.mPointArray)
{
float len = (GeoUtils.ToVector2(v) - dist.mOrigin).magnitude;
if (len < min)
{
min = len;
}
}
insect.mLength = min;
}
return(isInsect);
}
public static bool IsTriangleInsectCircle2(Vector2 p1, Vector2 p2, Vector2 p3, Vector2 center, float r, ref GeoInsectPointArrayInfo insect)
{
GeoInsectPointArrayInfo tmp = new GeoInsectPointArrayInfo();
if (GeoSegmentUtils.IsSegmentInsectCircle2(p1, p2, center, r, ref tmp))
{
insect.mHitGlobalPoint.mPointArray.AddRange(tmp.mHitGlobalPoint.mPointArray);
}
tmp.Clear();
if (GeoSegmentUtils.IsSegmentInsectCircle2(p3, p2, center, r, ref tmp))
{
insect.mHitGlobalPoint.mPointArray.AddRange(tmp.mHitGlobalPoint.mPointArray);
}
tmp.Clear();
if (GeoSegmentUtils.IsSegmentInsectCircle2(p1, p3, center, r, ref tmp))
{
insect.mHitGlobalPoint.mPointArray.AddRange(tmp.mHitGlobalPoint.mPointArray);
}
tmp.Clear();
return(false);
}
// 两三角面 共面的情况
private static bool IsTriangleInsectTrianglePlane2(Vector3 p1, Vector3 p2, Vector3 p3, Vector3 p4, Vector3 p5, Vector3 p6, ref GeoInsectPointArrayInfo insect)
{
GeoPlane plane = GeoPlaneUtils.CreateFromTriangle(p1, p2, p3);
Vector2 p11, p21, p31, p41, p51, p61;
GeoPlaneUtils.PlaneTransformLocalTriangle(p1, p2, p3, plane, out p11, out p21, out p31);
GeoPlaneUtils.PlaneTransformLocalTriangle(p4, p5, p6, plane, out p41, out p51, out p61);
GeoInsectPointArrayInfo tmp = new GeoInsectPointArrayInfo();
bool isInsect = IsTriangleInsectTriangle2(p11, p21, p31, p41, p51, p61, ref tmp);
if (isInsect)
{
foreach (Vector3 v in tmp.mHitGlobalPoint.mPointArray)
{
Vector3 vt = new Vector3(v.x, 0.0f, v.z);
vt = plane.TransformToGlobal(vt);
insect.mHitGlobalPoint.mPointArray.Add(vt);
}
insect.mIsIntersect = insect.mHitGlobalPoint.mPointArray.Count > 0;
}
return(insect.mIsIntersect);
}
public static bool IsCircleInsectCircle3(Vector3 center1, float r1, GeoPlane plane1, Vector3 center2, float r2, GeoPlane plane2, ref GeoInsectPointArrayInfo insect)
{
float dot = Mathf.Abs(Vector3.Dot(plane1.mNormal, plane2.mNormal));
if (1 - dot < 1e-5f)
{
if (GeoPlaneUtils.IsPointOnPlane(plane2.mNormal, plane2.mD, center1)) // 共面
{
return(IsCircleInsectCirclePlane2(center1, r1, center2, r2, plane2, ref insect));
}
return(false);
}
GeoInsectPointArrayInfo tmp = new GeoInsectPointArrayInfo();
bool isInsect = GeoPlaneUtils.IsPlaneInsectCircle(plane1.mNormal, plane1.mD, center2, r2, plane2, ref tmp);
if (isInsect)
{
Vector3 lin1 = tmp.mHitGlobalPoint.mPointArray[0];
Vector3 lin2 = lin1 + tmp.mHitGlobalPoint.mPointArray[1];
GeoLineUtils.IsLineInsectCirclePlane2(lin1, lin2, center1, r1, plane1, ref insect);
}
return(insect.mIsIntersect);
}
public static bool IsAABBInsectCircle3(Vector3 min, Vector3 max, GeoPlane plane1, Vector3 center, float r, GeoPlane plane2, ref GeoInsectPointArrayInfo insect)
{
float dot = Vector3.Dot(plane1.mNormal, plane2.mNormal);
if (1 - Mathf.Abs(dot) < 1e-5f)
{
if (GeoPlaneUtils.IsPointOnPlane(plane2.mNormal, plane2.mD, min))
{
return(IsAABBInsectCirclePlane2(min, max, center, r, plane1, ref insect)); // should use plane1
}
return(false);
}
GeoInsectPointArrayInfo tmp = new GeoInsectPointArrayInfo();
bool isInsect = GeoPlaneUtils.IsPlaneInsectCircle(plane1.mNormal, plane1.mD, center, r, plane2, ref tmp);
if (isInsect)
{
if (tmp.mHitGlobalPoint.mPointArray.Count == 1)
{
Vector3 line1 = tmp.mHitGlobalPoint.mPointArray[0];
if (GeoAABBUtils.IsPointInAABB2Plane(min, max, plane1, ref line1))
{
insect.mIsIntersect = true;
insect.mHitGlobalPoint.mPointArray.Add(line1);
return(true);
}
}
else
{
Vector3 seg1 = tmp.mHitGlobalPoint.mPointArray[0];
Vector3 seg2 = seg1 + tmp.mHitGlobalPoint.mPointArray[1];
return(GeoSegmentUtils.IsSegmentInsectAABB2Plane(seg1, seg2, min, max, plane1, ref insect));
}
}
return(false);
}
public static bool IsTriangleInsectSphere(Vector3 p1, Vector3 p2, Vector3 p3, Vector3 center, float r, ref GeoInsectPointArrayInfo insect)
{
GeoPlane plane = GeoPlaneUtils.CreateFromTriangle(p1, p2, p3);
bool isInsec = GeoPlaneUtils.IsPlaneInsectSphere(plane, center, r, ref insect);
if (isInsec)
{
Vector3 c1 = insect.mHitGlobalPoint.mPointArray[0];
float rad = insect.mHitGlobalPoint.mPointArray[0][0];
insect.Clear();
return(IsTriangleInsectPlaneCircle2(p1, p2, p3, c1, rad, plane, ref insect));
}
return(false);
}
public static bool IsPlaneInsectTriangle(Vector3 normal, float d, Vector3 p1, Vector3 p2, Vector3 p3, ref GeoInsectPointArrayInfo insect)
{
// 414
List <Vector3> tri = new List <Vector3>();
tri.Add(p1);
tri.Add(p2);
tri.Add(p3);
List <Vector3> pPos = new List <Vector3>();
List <Vector3> pOn = new List <Vector3>();
foreach (Vector3 p in tri)
{
bool p1P = IsPointInPositiveHalf(normal, d, p);
if (p1P)
{
pPos.Add(p);
}
else
{
p1P = IsPointOnPlane(normal, d, p);
if (p1P)
{
pOn.Add(p);
}
}
}
if (pPos.Count == 3 && pOn.Count == 0) // 三点都在 一侧
{
return(false);
}
if (pPos.Count == 2 && pOn.Count == 1) // 一点在上，另两点同侧
{
insect.mIsIntersect = true;
insect.mHitGlobalPoint.mPointArray.Add(pOn[0]);
return(true);
}
if (pPos.Count == 1 && pOn.Count == 2)
{
insect.mIsIntersect = true;
insect.mHitGlobalPoint.mPointArray.Add(pOn[0]);
insect.mHitGlobalPoint.mPointArray.Add(pOn[1]);
return(true);
}
if (pPos.Count == 0 && pOn.Count == 3) // 共面
{
insect.mIsIntersect = true;
insect.mHitGlobalPoint.mPointArray.Add(pOn[0]);
insect.mHitGlobalPoint.mPointArray.Add(pOn[1]);
insect.mHitGlobalPoint.mPointArray.Add(pOn[2]);
return(true);
}
insect.mIsIntersect = true;
GeoInsectPointInfo temp = new GeoInsectPointInfo();
for (int i = 0; i < 3; ++i)
{
if (IsPlaneInsectSegment(normal, d, tri[i], tri[(i + 1) % 3], ref temp))
{
insect.mHitGlobalPoint.mPointArray.Add(temp.mHitGlobalPoint);
}
}
return(true);
}
public static bool IsLineInsectTriangle2(Vector2 line1, Vector2 line2, Vector2 p1, Vector2 p2, Vector2 p3, ref GeoInsectPointArrayInfo insect)
{
// 转化为 线 与 线段 相交
List <Vector2> tri = new List <Vector2>();
tri.Add(p1);
tri.Add(p2);
tri.Add(p3);
List <Vector3> ins = new List <Vector3>();
GeoInsectPointInfo info = new GeoInsectPointInfo();
for (int i = 0; i < 3; ++i)
{
bool isSeg = IsLineInsectSegment2(line1, line2, tri[i], tri[(i + 1) % 3], ref info);
if (isSeg)
{
ins.Add(info.mHitGlobalPoint);
}
}
if (ins.Count > 0)
{
insect.mIsIntersect = true;
insect.mHitGlobalPoint.mPointArray = ins;
}
return(insect.mIsIntersect);
}
public static bool IsPlaneInsectPlane(Vector3 normal1, float d1, Vector3 normal2, float d2, ref GeoInsectPointArrayInfo insect)
{
// 410 所有的法向量必须先单位化
Vector3 lineDirection = Vector3.Cross(normal1, normal2);
if (lineDirection.magnitude < 1e-5f) // 平行
{
return(false);
}
float n1n2 = Vector3.Dot(normal1, normal2);
float m = n1n2 * n1n2 - 1;
float a = (-d2 * n1n2 + d1) / m;
float b = (-d1 * n1n2 + d2) / m;
Vector3 p = a * normal1 + b * normal2;
insect.mIsIntersect = true;
insect.mHitGlobalPoint.mPointArray.Add(p); // 直线 上一点
insect.mHitGlobalPoint.mPointArray.Add(lineDirection); // 直线 方向
return(true);
}
/*
* 矩形：R(t1, t2) = C + t1 * e1 + t2 * e2, -w < t1 < w, -h < t2 < h . C 为 矩形 中心点
*
* 作者：张建龙
* 链接：https://www.zhihu.com/question/31763307/answer/53259121
* 来源：知乎
* 轴对齐的矩形对线段的裁剪算法，大致有五种算法，可以解决轴对齐的矩形对线段的裁剪算法。
* 最早由Danny Cohen&Ivan Sutherland提出的一种算法，称为Cohen-Sutherlad裁剪算法（简称CS算法），
* 该算法把平面分成9个区域，根据点的位置确定与矩形的相交边。后来，Cyrus&Beck[1](1978)提出了Cyrus-Beck裁剪算法（简称CB算法），
* 线段用参数方程来表示，计算与矩形的每条边所在的直线的交点，但是该算法的效率相对较低。
* Liang&Barsky[2](1984)尝试对CS算法进行改进，提出了该算法的一种变种，称为Liang-Barsky裁剪算法（简称为LB算法），
* Liang和Barsky[3](1992)进一步对LB算法进行了改进。Sobkow&Pospisil&Yang[4](1987)描述了另外一种线段的裁剪算法，称为快速裁剪算法（简称为SPY算法）。
* Nicholl,Lee&Nicholl[5]对前面四种线段裁剪算法进行了较为深入地分析和比较，并且描述了一种更加高效的算法，称为Nicholl-Lee-Nicholl算法（简称为NLN算法）。
* 对比五种算法的性能，它们并不是在数量级上的差别，只是算法中运算个数的差别，比如NLN算法拥有最少的除法次数。
* CS、LB和SPY在不同机器、不同的数据集上，会显示出不同的算法性能。在算法实现上，NLN和SPY算法较为复杂，CS和LB相对简单。
* 如果不会特别苛求算法的性能，建议选择CS或者LB算法
*
*
* 很多实现并没有 垂直和水平的处理，因为默认相交的几何体都是在
* 第一象限
*/
public static bool IsLineInsectAABB2(Vector2 line1, Vector2 line2, Vector2 min, Vector2 max, ref GeoInsectPointArrayInfo insect)
{
// 转化为 线 与 线段 相交
// 对角线 快速 判断
Vector2 dir = line2 - line1;
if (dir[0] == 0.0f) // 垂直
{
if (line1[0] >= min[0] && line1[0] <= max[0]) // 相交
{
insect.mIsIntersect = true;
insect.mHitGlobalPoint.mPointArray.Add(new Vector3(line1[0], min[1], 0.0f));
insect.mHitGlobalPoint.mPointArray.Add(new Vector3(line1[0], max[1], 0.0f));
return(true);
}
else
{
return(false);
}
}
if (dir[1] == 0.0f) // 平行
{
if (line1[1] >= min[1] && line1[1] <= max[1]) // 相交
{
insect.mIsIntersect = true;
insect.mHitGlobalPoint.mPointArray.Add(new Vector3(min[0], line1[1], 0.0f));
insect.mHitGlobalPoint.mPointArray.Add(new Vector3(max[0], line1[1], 0.0f));
return(true);
}
else
{
return(false);
}
}
float invx = 1.0f / dir[0];
float t1;
float t2;
float dnear;
float dfar;
if (invx > 0)
{
t1 = (min[0] - line1[0]) * invx;
t2 = (max[0] - line1[0]) * invx;
}
else
{
t2 = (min[0] - line1[0]) * invx;
t1 = (max[0] - line1[0]) * invx;
}
dnear = t1;
dfar = t2;
if (dnear > dfar || dfar < 0.0f)
{
return(false);
}
float invy = 1.0f / dir[1];
if (invy > 0)
{
t1 = (min[1] - line1[1]) * invy;
t2 = (max[1] - line1[1]) * invy;
}
else
{
t2 = (min[1] - line1[1]) * invy;
t1 = (max[1] - line1[1]) * invy;
}
if (t1 > dnear)
{
dnear = t1;
}
if (t2 < dfar)
{
dfar = t2;
}
dnear = t1;
dfar = t2;
if (dnear > dfar || dfar < 0.0f)
{
return(false);
}
insect.mIsIntersect = true;
insect.mHitGlobalPoint.mPointArray.Add(line1 + dnear * dir);
if (dnear != dfar)
{
insect.mHitGlobalPoint.mPointArray.Add(line1 + dfar * dir);
}
return(true);
}
public static bool IsLineInsectAABBPlane2(Vector3 line1, Vector3 line2, Vector3 min, Vector3 max, GeoPlane plane, ref GeoInsectPointArrayInfo insect)
{
Vector3 min1 = plane.TransformToLocal(min);
Vector3 max1 = plane.TransformToLocal(max);
Vector3 l1 = plane.TransformToLocal(line1);
Vector3 l2 = plane.TransformToLocal(line2);
bool isInsect = IsLineInsectAABB2(new Vector2(l1.x, l1.z), new Vector2(l2.x, l2.z), new Vector2(min1.x, min1.z), new Vector2(max1.x, max1.z), ref insect);
if (isInsect)
{
insect.mHitGlobalPoint.mPointArray = GeoPlaneUtils.PlaneTransformGlobal(insect.mHitGlobalPoint.mPointArray, plane);
}
return(isInsect);
}
public bool GetIntersection(GeoRay2 ray, ref GeoInsectPointArrayInfo intersection, bool occlusion)
{
if (mFlatTreeList.Count == 0)
{
return(false);
}
intersection.mIsIntersect = false;
intersection.mLength = 999999999.0f;
intersection.mHitObject2 = null;
int closer, other;
BVHTraversal[] todo = new BVHTraversal[64];
todo[0] = new BVHTraversal();
int stackptr = 0;
todo[stackptr].mIndex = 0;
todo[stackptr].mLength = -9999999.0f;
while (stackptr >= 0)
{
int ni = todo[stackptr].mIndex;
float near = todo[stackptr].mLength;
stackptr--;
BVHFlatNode2 node = mFlatTreeList[ni];
// 对叶节点做相交测试
if (node.mRightOffset == 0)
{
bool hit = false;
for (int o = 0; o < node.mLeafCount; ++o)
{
GeoInsectPointArrayInfo current = new GeoInsectPointArrayInfo();
BVHObject2 obj = mBuildPrims[(int)node.mStartIndex + o];
hit = obj.IsIntersect(ref ray, ref current);
if (hit)
{
if (occlusion)
{
intersection = current;
return(true);
}
if (current.mLength < intersection.mLength)
{
intersection = current;
}
}
}
}
else
{
closer = ni + 1;
other = ni + (int)node.mRightOffset;
// 对父结点做测试
GeoInsectPointArrayInfo in1 = new GeoInsectPointArrayInfo();
GeoInsectPointArrayInfo in2 = new GeoInsectPointArrayInfo();
bool hitc0 = GeoRayUtils.IsRayInsectAABB2(ray.mOrigin, ray.mDirection, mFlatTreeList[closer].mBox.mMin, mFlatTreeList[closer].mBox.mMax, ref in1);
bool hitc1 = GeoRayUtils.IsRayInsectAABB2(ray.mOrigin, ray.mDirection, mFlatTreeList[other].mBox.mMin, mFlatTreeList[other].mBox.mMax, ref in2);
if (hitc0 && hitc1)
{
float l0 = (GeoUtils.ToVector2(in1.mHitGlobalPoint[0]) - ray.mOrigin).magnitude;
float l2 = (GeoUtils.ToVector2(in2.mHitGlobalPoint[0]) - ray.mOrigin).magnitude;
if (l2 < l0)
{
float temp = l0;
l0 = l2;
l2 = temp;
int itemp = closer;
closer = other;
other = itemp;
}
todo[++stackptr] = new BVHTraversal(other, l2);
todo[++stackptr] = new BVHTraversal(closer, l0);
}
else if (hitc0)
{
float l0 = (GeoUtils.ToVector2(in1.mHitGlobalPoint[0]) - ray.mOrigin).magnitude;
todo[++stackptr] = new BVHTraversal(closer, l0);
}
else if (hitc1)
{
float l2 = (GeoUtils.ToVector2(in2.mHitGlobalPoint[0]) - ray.mOrigin).magnitude;
todo[++stackptr] = new BVHTraversal(other, l2);
}
}
}
if (intersection.mHitObject2 != null)
{
intersection.mHitGlobalPoint.Clear();
intersection.mHitGlobalPoint.Add(ray.mOrigin + ray.mDirection * intersection.mLength);
}
return(intersection.mHitObject2 != null);
}
public static bool IsLineInsectAABB3(Vector3 line1, Vector3 line2, Vector3 min, Vector3 max, ref GeoInsectPointArrayInfo insect)
{
// 转化为 线 与 线段 相交
// 对角线 快速 判断
Vector3 dir = line2 - line1;
if (dir[0] == 0.0f) // 平行 YOZ
{
if (line1[0] >= min[0] && line1[0] <= max[0]) // 相交 转换到 2d
{
// (line1[0], min[1], min[2]) (line1[0], max[1], max[2]);
Vector2 line11 = new Vector2(line1[1], line1[2]);
Vector2 line21 = new Vector2(line2[1], line2[2]);
Vector2 min1 = new Vector2(min[1], min[2]);
Vector2 max1 = new Vector2(max[1], max[2]);
GeoInsectPointArrayInfo tmp = new GeoInsectPointArrayInfo();
bool isInsect = IsLineInsectAABB2(line11, line21, min1, max1, ref tmp);
if (isInsect)
{
insect.mIsIntersect = true;
foreach (Vector3 v in tmp.mHitGlobalPoint.mPointArray)
{
insect.mHitGlobalPoint.mPointArray.Add(new Vector3(line1[0], v[0], v[1]));
}
}
return(isInsect);
}
}
if (dir[1] == 0.0f) // 平行
{
if (line1[1] >= min[1] && line1[1] <= max[1]) // 相交 转换到 2d
{
// (min[0], line1[1], min[2]) (max[0], line1[1], max[2]);
Vector2 line11 = new Vector2(line1[0], line1[2]);
Vector2 line21 = new Vector2(line2[0], line2[2]);
Vector2 min1 = new Vector2(min[0], min[2]);
Vector2 max1 = new Vector2(max[0], max[2]);
GeoInsectPointArrayInfo tmp = new GeoInsectPointArrayInfo();
bool isInsect = IsLineInsectAABB2(line11, line21, min1, max1, ref tmp);
if (isInsect)
{
insect.mIsIntersect = true;
foreach (Vector3 v in tmp.mHitGlobalPoint.mPointArray)
{
insect.mHitGlobalPoint.mPointArray.Add(new Vector3(v[0], line1[1], v[1]));
}
}
return(isInsect);
}
}
if (dir[2] == 0.0f) // 平行
{
if (line1[2] >= min[2] && line1[2] <= max[2]) // 相交 转换到 2d
{
// (min[0], min[1], line1[2]) (max[0], max[1], line1[2]);
Vector2 line11 = new Vector2(line1[0], line1[1]);
Vector2 line21 = new Vector2(line2[0], line2[1]);
Vector2 min1 = new Vector2(min[0], min[1]);
Vector2 max1 = new Vector2(max[0], max[1]);
GeoInsectPointArrayInfo tmp = new GeoInsectPointArrayInfo();
bool isInsect = IsLineInsectAABB2(line11, line21, min1, max1, ref tmp);
if (isInsect)
{
insect.mIsIntersect = true;
foreach (Vector3 v in tmp.mHitGlobalPoint.mPointArray)
{
insect.mHitGlobalPoint.mPointArray.Add(new Vector3(v[0], v[1], line1[2]));
}
}
return(isInsect);
}
}
float invx = 1.0f / dir[0];
float t1;
float t2;
float dnear;
float dfar;
if (invx > 0)
{
t1 = (min[0] - line1[0]) * invx;
t2 = (max[0] - line1[0]) * invx;
}
else
{
t2 = (min[0] - line1[0]) * invx;
t1 = (max[0] - line1[0]) * invx;
}
dnear = t1;
dfar = t2;
if (dnear > dfar || dfar < 0.0f)
{
return(false);
}
float invy = 1.0f / dir[1];
if (invy > 0)
{
t1 = (min[1] - line1[1]) * invy;
t2 = (max[1] - line1[1]) * invy;
}
else
{
t2 = (min[1] - line1[1]) * invy;
t1 = (max[1] - line1[1]) * invy;
}
if (t1 > dnear)
{
dnear = t1;
}
if (t2 < dfar)
{
dfar = t2;
}
dnear = t1;
dfar = t2;
if (dnear > dfar || dfar < 0.0f)
{
return(false);
}
float invz = 1.0f / dir[2];
if (invz > 0)
{
t1 = (min[2] - line1[2]) * invz;
t2 = (max[2] - line1[2]) * invz;
}
else
{
t2 = (min[2] - line1[2]) * invz;
t1 = (max[2] - line1[2]) * invz;
}
if (t1 > dnear)
{
dnear = t1;
}
if (t2 < dfar)
{
dfar = t2;
}
dnear = t1;
dfar = t2;
if (dnear > dfar || dfar < 0.0f)
{
return(false);
}
insect.mIsIntersect = true;
insect.mHitGlobalPoint.mPointArray.Add(line1 + dnear * dir);
if (dnear != dfar)
{
insect.mHitGlobalPoint.mPointArray.Add(line1 + dfar * dir);
}
return(true);
}
// 三角面
public static bool IsTriangleInsectSegment2(Vector2 p1, Vector2 p2, Vector2 p3, Vector2 seg1, Vector2 seg2, ref GeoInsectPointArrayInfo insect)
{
return(GeoSegmentUtils.IsSegmentInsectTriangle2(seg1, seg2, p1, p2, p3, ref insect));
}
public static bool IsLineInsectSphere(Vector3 line1, Vector3 line2, Vector3 center, float r, ref GeoInsectPointArrayInfo insect)
{
// 389 公式存在问题， 按照代码来
Vector3 direction = line2 - line1;
float a = Vector3.Dot(direction, direction);
Vector3 c1 = line1 - center;
float b = 2 * Vector3.Dot(direction, c1);
float c = Vector3.Dot(c1, c1) - r * r;
float discrm = b * b - 4 * a * c;
if (discrm < 0)
{
return(false);
}
discrm = Mathf.Sqrt(discrm);
float t1 = -b + discrm;
float t2 = -b - discrm;
insect.mIsIntersect = true;
if (discrm == 0)
{
insect.mHitGlobalPoint.mPointArray.Add(line1 + t1 * direction);
}
else
{
insect.mHitGlobalPoint.mPointArray.Add(line1 + t1 * direction);
insect.mHitGlobalPoint.mPointArray.Add(line1 + t2 * direction);
}
return(true);
}
public static bool IsLineInsectCircle2(Vector2 line1, Vector2 line2, Vector2 center, float r, ref GeoInsectPointArrayInfo insect)
{
// 389 公式存在问题， 按照代码来
Vector2 direction = line2 - line1;
float a = Vector2.Dot(direction, direction);
Vector2 c1 = line1 - center;
float b = 2 * Vector2.Dot(direction, c1);
float c = Vector2.Dot(c1, c1) - r * r;
float discrm = b * b - 4 * a * c;
if (discrm < 0)
{
return(false);
}
discrm = Mathf.Sqrt(discrm);
float t1 = -b + discrm;
float t2 = -b - discrm;
insect.mIsIntersect = true;
if (discrm == 0)
{
Vector2 temp = line1 + t1 * direction;
insect.mHitGlobalPoint.mPointArray.Add(new Vector3(temp.x, temp.y, 0.0f));
}
else
{
Vector2 temp = line1 + t1 * direction;
insect.mHitGlobalPoint.mPointArray.Add(new Vector3(temp.x, temp.y, 0.0f));
temp = line1 + t2 * direction;
insect.mHitGlobalPoint.mPointArray.Add(new Vector3(temp.x, temp.y, 0.0f));
}
return(true);
}
public static bool IsTriangleInsectAABB3(Vector3 p1, Vector3 p2, Vector3 p3, Vector3 min, Vector2 max, ref GeoInsectPointArrayInfo insect)
{
return(GeoAABBUtils.IsAABBInsectTriangle3(min, max, p1, p2, p3, ref insect));
}
public static bool IsPlaneInsectAABB3(Vector3 normal, float d, Vector3 min, Vector3 max, ref GeoInsectPointArrayInfo insect)
{
return(false);
}
public static bool IsTriangleInsectTriangle3(Vector3 p1, Vector3 p2, Vector3 p3, Vector3 p4, Vector3 p5, Vector3 p6, ref GeoInsectPointArrayInfo insect)
{
/*
* // 外面进行 剔除 处理
* Vector3 min1 = Vector3.Min(p1, p2);
* min1 = Vector3.Min(min1, p3);
* Vector3 max1 = Vector3.Max(p1, p2);
* max1 = Vector3.Max(max1, p3);
* Vector3 min2 = Vector3.Min(p4, p5);
* min2 = Vector3.Min(min2, p6);
* Vector3 max2 = Vector3.Max(p4, p5);
* max2 = Vector3.Max(max2, p6);
* if (!GeoAABBUtils.IsAABBInsectAABB3(min1, max1, min2, max2))
* {
* return false;
* }
*/
// 417
Vector3 p12 = p2 - p1;
Vector3 p13 = p3 - p1;
Vector3 normal1 = Vector3.Cross(p12, p13);
Vector3 p45 = p5 - p4;
Vector3 p46 = p6 - p4;
Vector3 normal2 = Vector3.Cross(p45, p46);
normal1.Normalize();
normal2.Normalize();
float dot = Vector3.Dot(normal1, normal2);
if (1 - Mathf.Abs(dot) < 1e-5f) // 共面
{
return(IsTriangleInsectTrianglePlane2(p1, p2, p3, p4, p5, p6, ref insect));
}
float d1 = -Vector3.Dot(normal1, p1);
float d2 = -Vector3.Dot(normal2, p4);
GeoInsectPointArrayInfo temp1 = new GeoInsectPointArrayInfo();
if (GeoPlaneUtils.IsPlaneInsectTriangle(normal1, d1, p4, p5, p6, ref temp1))
{
GeoInsectPointArrayInfo temp2 = new GeoInsectPointArrayInfo();
if (GeoPlaneUtils.IsPlaneInsectTriangle(normal2, d2, p1, p2, p3, ref temp2))
{
int count1 = temp1.mHitGlobalPoint.mPointArray.Count;
int count2 = temp2.mHitGlobalPoint.mPointArray.Count;
if (count1 == 1 && count2 == 1)
{
bool isEq = temp1.mHitGlobalPoint.mPointArray[0] == temp2.mHitGlobalPoint.mPointArray[0];
if (isEq)
{
insect.mIsIntersect = true;
insect.mHitGlobalPoint.mPointArray.Add(temp1.mHitGlobalPoint.mPointArray[0]);
return(true);
}
}
else if (count1 == 2 && count2 == 2)
{
Vector3 s1 = temp1.mHitGlobalPoint.mPointArray[0];
Vector3 s2 = temp1.mHitGlobalPoint.mPointArray[1];
Vector3 s3 = temp2.mHitGlobalPoint.mPointArray[0];
Vector3 s4 = temp2.mHitGlobalPoint.mPointArray[1];
GeoSegmentUtils.IsSegmentOverlapSegment3(s1, s2, s3, s4, ref insect);
}
}
}
return(insect.mIsIntersect);
}
public bool TestAABBIntersect(GeoAABB2 aabb)
{
GeoInsectPointArrayInfo insect = new GeoInsectPointArrayInfo();
return(GeoSegmentUtils.IsSegmentInsectAABB2(mSeg.mP1, mSeg.mP2, aabb.mMin, aabb.mMax, ref insect));
}
public static bool IsPlaneInsectSphere(GeoPlane plane, Vector3 center, float r, ref GeoInsectPointArrayInfo insect)
{
Vector3 close = new Vector3();
float dist = PointClosestToPlaneAbs(plane.mNormal, plane.mD, center, ref close);
if (dist > r)
{
return(false);
}
float rad = Mathf.Sqrt(r * r - dist * dist);
insect.mIsIntersect = true;
insect.mHitGlobalPoint.mPointArray.Add(close);
insect.mHitGlobalPoint.mPointArray.Add(Vector3.one * rad); // 相交为一个圆，所在平明为 plane
return(true);
}
public static bool IsTriangleInsectLine2(Vector2 p1, Vector2 p2, Vector2 p3, Vector2 line1, Vector2 line2, ref GeoInsectPointArrayInfo insect)
{
return(GeoLineUtils.IsLineInsectTriangle2(line1, line2, p1, p2, p3, ref insect));
}