|
|
|
|
@ -1,12 +1,11 @@
|
|
|
|
|
import { Vector3 } from 'three';
|
|
|
|
|
import { arrayRemoveDuplicateBySort } from '../Common/ArrayExt';
|
|
|
|
|
import { getDeterminantFor3V } from '../Common/CurveUtils';
|
|
|
|
|
import { Arc } from '../DatabaseServices/Arc';
|
|
|
|
|
import { Circle } from '../DatabaseServices/Circle';
|
|
|
|
|
import { Curve } from '../DatabaseServices/Curve';
|
|
|
|
|
import { Line } from '../DatabaseServices/Line';
|
|
|
|
|
import { Polyline } from '../DatabaseServices/Polyline';
|
|
|
|
|
import { comparePoint, equalv3, equaln, midPoint } from '../Geometry/GeUtils';
|
|
|
|
|
import { comparePoint, equaln, equalv3, midPoint } from '../Geometry/GeUtils';
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
@ -217,101 +216,67 @@ export function IntersectLAndLFor2D(p1: Vector3, p2: Vector3, p3: Vector3, p4: V
|
|
|
|
|
|
|
|
|
|
return pt;
|
|
|
|
|
}
|
|
|
|
|
export function IntersectLAndLFor3D(p1: Vector3, p2: Vector3, p3: Vector3, p4: Vector3)
|
|
|
|
|
{
|
|
|
|
|
let x12 = p1.x - p2.x;
|
|
|
|
|
let x43 = p4.x - p3.x;
|
|
|
|
|
let x42 = p4.x - p2.x;
|
|
|
|
|
let y12 = p1.y - p2.y;
|
|
|
|
|
let y43 = p4.y - p3.y;
|
|
|
|
|
let y42 = p4.y - p2.y;
|
|
|
|
|
let z12 = p1.z - p2.z;
|
|
|
|
|
let z43 = p4.z - p3.z;
|
|
|
|
|
let z42 = p4.z - p2.z;
|
|
|
|
|
|
|
|
|
|
let v1 = p2.clone().sub(p1).normalize();
|
|
|
|
|
let v2 = p4.clone().sub(p3).normalize();
|
|
|
|
|
let w = p3.clone().sub(p1).normalize();
|
|
|
|
|
|
|
|
|
|
if (equaln(Math.abs(v1.dot(v2)), 1, 1e-6) && (equalv3(w, new Vector3()) || equaln(Math.abs(v1.dot(w)), 1, 1e-6))) //平行共线
|
|
|
|
|
return undefined;
|
|
|
|
|
else if (equaln(Math.abs(v1.dot(v2)), 1, 1e-6)) //平行不共线
|
|
|
|
|
return undefined;
|
|
|
|
|
else if (equaln(getDeterminantFor3V(v1, v2, w), 0, 0.01))
|
|
|
|
|
{
|
|
|
|
|
let t: number;
|
|
|
|
|
if (x12 * y43 - y12 * x43)
|
|
|
|
|
{
|
|
|
|
|
t = (x42 * y43 - x43 * y42) / (x12 * y43 - y12 * x43);
|
|
|
|
|
}
|
|
|
|
|
else if (x12 * z43 - x43 * z12)
|
|
|
|
|
{
|
|
|
|
|
t = (x42 * z43 - x43 * z42) / (x12 * z43 - x43 * z12);
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
|
|
|
|
|
export function IntersectLine3AndLine3(p1: Vector3, p2: Vector3, p3: Vector3, p4: Vector3, epsilon = 1e-6)
|
|
|
|
|
{
|
|
|
|
|
t = (y42 * z43 - y43 * z42) / (y12 * z43 - y43 * z12);
|
|
|
|
|
}
|
|
|
|
|
return new Vector3(x12 * t + p2.x, y12 * t + p2.y, z12 * t + p2.z);
|
|
|
|
|
let pts = ShortestLine3AndLine3(p1, p2, p3, p4);
|
|
|
|
|
if (pts) return pts[0];
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
export function IntersectLAndLFor3D1(p1: Vector3, p2: Vector3, p3: Vector3, p4: Vector3)
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* 三维中两行之间最短的直线
|
|
|
|
|
* ref:https://stackoverflow.com/questions/2316490/the-algorithm-to-find-the-point-of-intersection-of-two-3d-line-segment
|
|
|
|
|
* ref:http://paulbourke.net/geometry/pointlineplane/
|
|
|
|
|
* ref:http://paulbourke.net/geometry/pointlineplane/calclineline.cs
|
|
|
|
|
*
|
|
|
|
|
* @export
|
|
|
|
|
* @param {Vector3} p1 l1.start
|
|
|
|
|
* @param {Vector3} p2 l1.end
|
|
|
|
|
* @param {Vector3} p3 l2.start
|
|
|
|
|
* @param {Vector3} p4 l2.end
|
|
|
|
|
* @returns 交点集合
|
|
|
|
|
*/
|
|
|
|
|
function ShortestLine3AndLine3(p1: Vector3, p2: Vector3, p3: Vector3, p4: Vector3, epsilon = 1e-6)
|
|
|
|
|
{
|
|
|
|
|
let x12 = p1.x - p2.x;
|
|
|
|
|
let x43 = p4.x - p3.x;
|
|
|
|
|
let x42 = p4.x - p2.x;
|
|
|
|
|
let y12 = p1.y - p2.y;
|
|
|
|
|
let y43 = p4.y - p3.y;
|
|
|
|
|
let y42 = p4.y - p2.y;
|
|
|
|
|
let z12 = p1.z - p2.z;
|
|
|
|
|
let z43 = p4.z - p3.z;
|
|
|
|
|
let z42 = p4.z - p2.z;
|
|
|
|
|
let p43 = p4.clone().sub(p3);
|
|
|
|
|
if (p43.lengthSq() < epsilon)
|
|
|
|
|
return;
|
|
|
|
|
let p21 = p2.clone().sub(p1);
|
|
|
|
|
if (p21.lengthSq() < epsilon)
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
|
|
let pts: Vector3[] = [];
|
|
|
|
|
let v1 = p2.clone().sub(p1).normalize();
|
|
|
|
|
let v2 = p4.clone().sub(p3).normalize();
|
|
|
|
|
let w = p3.clone().sub(p1).normalize();
|
|
|
|
|
let p13 = p1.clone().sub(p3);
|
|
|
|
|
|
|
|
|
|
if (equaln(Math.abs(v1.dot(v2)), 1, 1e-6) && equaln(Math.abs(v1.dot(w)), 1, 1e-6)) //平行共线
|
|
|
|
|
{
|
|
|
|
|
let tmpPts = [p1, p2, p3, p4];
|
|
|
|
|
tmpPts.sort(comparePoint('xyz'));
|
|
|
|
|
if (equalv3(tmpPts[1], p3) || equalv3(tmpPts[1], p4))
|
|
|
|
|
{
|
|
|
|
|
pts = [tmpPts[1], tmpPts[2]];
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
pts = [midPoint(tmpPts[1], tmpPts[2])]
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
else if (equaln(Math.abs(v1.dot(v2)), 1, 1e-6)) //平行不共线
|
|
|
|
|
{
|
|
|
|
|
return [];
|
|
|
|
|
}
|
|
|
|
|
else if (equaln(getDeterminantFor3V(v1, v2, w), 0, 0.01))
|
|
|
|
|
{
|
|
|
|
|
let t: number;
|
|
|
|
|
if (x12 * y43 - y12 * x43)
|
|
|
|
|
{
|
|
|
|
|
t = (x42 * y43 - x43 * y42) / (x12 * y43 - y12 * x43);
|
|
|
|
|
}
|
|
|
|
|
else if (x12 * z43 - x43 * z12)
|
|
|
|
|
{
|
|
|
|
|
t = (x42 * z43 - x43 * z42) / (x12 * z43 - x43 * z12);
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
t = (y42 * z43 - y43 * z42) / (y12 * z43 - y43 * z12);
|
|
|
|
|
}
|
|
|
|
|
pts = [new Vector3(x12 * t + p2.x, y12 * t + p2.y, z12 * t + p2.z)]
|
|
|
|
|
}
|
|
|
|
|
return pts;
|
|
|
|
|
let d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
|
|
|
|
|
let d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
|
|
|
|
|
let d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
|
|
|
|
|
let d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
|
|
|
|
|
let d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;
|
|
|
|
|
|
|
|
|
|
let denom = d2121 * d4343 - d4321 * d4321;
|
|
|
|
|
if (Math.abs(denom) < epsilon)
|
|
|
|
|
return;
|
|
|
|
|
let numer = d1343 * d4321 - d1321 * d4343;
|
|
|
|
|
|
|
|
|
|
let mua = numer / denom;
|
|
|
|
|
let mub = (d1343 + d4321 * (mua)) / d4343;
|
|
|
|
|
|
|
|
|
|
let resultSegmentPoint1 = new Vector3();
|
|
|
|
|
resultSegmentPoint1.x = p1.x + mua * p21.x;
|
|
|
|
|
resultSegmentPoint1.y = p1.y + mua * p21.y;
|
|
|
|
|
resultSegmentPoint1.z = p1.z + mua * p21.z;
|
|
|
|
|
let resultSegmentPoint2 = new Vector3();
|
|
|
|
|
resultSegmentPoint2.x = p3.x + mub * p43.x;
|
|
|
|
|
resultSegmentPoint2.y = p3.y + mub * p43.y;
|
|
|
|
|
resultSegmentPoint2.z = p3.z + mub * p43.z;
|
|
|
|
|
|
|
|
|
|
return [resultSegmentPoint1, resultSegmentPoint2];
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
//直线和直线
|
|
|
|
|
export function IntersectLineAndLine(l1: Line, l2: Line, extType: IntersectOption)
|
|
|
|
|
{
|
|
|
|
|
let pt = IntersectLAndLFor3D(l1.StartPoint, l1.EndPoint, l2.StartPoint, l2.EndPoint);
|
|
|
|
|
let pt = IntersectLine3AndLine3(l1.StartPoint, l1.EndPoint, l2.StartPoint, l2.EndPoint);
|
|
|
|
|
|
|
|
|
|
return pt ? CheckPointOnCurve([pt], l1, l2, extType) : [];
|
|
|
|
|
}
|
|
|
|
|
|
ChenX
6 years ago