static public double MinDistance(Ray3d r, Segment3d s)
{
double rayt, segt;
double dsqr = SquaredDistance(ref r, ref s, out rayt, out segt);
return(Math.Sqrt(dsqr));
}
static public double MinDistanceSegmentParam(Ray3d r, Segment3d s)
{
double rayt, segt;
/*double dsqr = */ SquaredDistance(ref r, ref s, out rayt, out segt);
return(segt);
}
/// <summary>
/// Find point-normal frame at ray-intersection point on mesh, or return false if no hit.
/// Returns interpolated vertex-normal frame if available, otherwise tri-normal frame.
/// </summary>
public static bool RayHitPointFrame(DMesh3 mesh, ISpatial spatial, Ray3d ray, out Frame3f hitPosFrame, bool bForceFaceNormal = false)
{
hitPosFrame = new Frame3f();
int tid = spatial.FindNearestHitTriangle(ray);
if (tid == DMesh3.InvalidID)
{
return(false);
}
var isect = TriangleIntersection(mesh, tid, ray);
if (isect.Result != IntersectionResult.Intersects)
{
return(false);
}
Vector3d surfPt = ray.PointAt(isect.RayParameter);
if (mesh.HasVertexNormals && bForceFaceNormal == false)
{
hitPosFrame = SurfaceFrame(mesh, tid, surfPt); // TODO isect has bary-coords already!!
}
else
{
hitPosFrame = new Frame3f(surfPt, mesh.GetTriNormal(tid));
}
return(true);
}
public bool RayIntersect(Ray3d ray, out Vector3d vHit, out Vector3d vHitNormal)
{
vHit = Vector3d.Zero;
vHitNormal = Vector3d.AxisX;
int tHitID = Spatial.FindNearestHitTriangle(ray);
if (tHitID == DMesh3.InvalidID)
{
return(false);
}
IntrRay3Triangle3 t = MeshQueries.TriangleIntersection(Mesh, tHitID, ray);
vHit = ray.PointAt(t.RayParameter);
if (UseFaceNormal == false && Mesh.HasVertexNormals)
{
vHitNormal = Mesh.GetTriBaryNormal(tHitID, t.TriangleBaryCoords.x, t.TriangleBaryCoords.y, t.TriangleBaryCoords.z);
}
else
{
vHitNormal = Mesh.GetTriNormal(tHitID);
}
return(true);
}
void find_min_distance(ref Ray3d ray, ref double min_dist, ref int min_dist_seg, ref double min_dist_segt, ref double min_dist_rayt, int bi, int iLayerStart, int iLayer)
{
// hit polygon layer, check segments
if (iLayer == 0)
{
int seg_i = 2 * bi;
Segment3d seg_a = Curve.GetSegment(seg_i);
double segt, rayt;
double segdist_sqr = DistRay3Segment3.SquaredDistance(ref ray, ref seg_a, out rayt, out segt);
double segdist = Math.Sqrt(segdist_sqr);
if (segdist <= min_dist)
{
min_dist = segdist;
min_dist_seg = seg_i;
min_dist_segt = segt;
min_dist_rayt = rayt;
}
if ((seg_i + 1) < Curve.SegmentCount)
{
Segment3d seg_b = Curve.GetSegment(seg_i + 1);
segdist_sqr = DistRay3Segment3.SquaredDistance(ref ray, ref seg_b, out rayt, out segt);
segdist = Math.Sqrt(segdist_sqr);
if (segdist <= min_dist)
{
min_dist = segdist;
min_dist_seg = seg_i + 1;
min_dist_segt = segt;
min_dist_rayt = rayt;
}
}
return;
}
// test both boxes and recurse
// TODO: verify that this intersection strategy makes sense?
int prev_layer = iLayer - 1;
int prev_count = layer_counts[prev_layer];
int prev_start = iLayerStart - prev_count;
int prev_a = prev_start + 2 * bi;
bool intersects = IntrRay3Box3.Intersects(ref ray, ref boxes[prev_a], min_dist);
if (intersects)
{
find_min_distance(ref ray, ref min_dist, ref min_dist_seg, ref min_dist_segt, ref min_dist_rayt, 2 * bi, prev_start, prev_layer);
}
if ((2 * bi + 1) >= prev_count)
{
return;
}
int prev_b = prev_a + 1;
bool intersects2 = IntrRay3Box3.Intersects(ref ray, ref boxes[prev_b], min_dist);
if (intersects2)
{
find_min_distance(ref ray, ref min_dist, ref min_dist_seg, ref min_dist_segt, ref min_dist_rayt, 2 * bi + 1, prev_start, prev_layer);
}
}
public bool RayIntersect(Ray3d ray, out Vector3d vHit, out Vector3d vHitNormal)
{
Vector3f rayHit = PlaneFrame.RayPlaneIntersection((Vector3f)ray.Origin, (Vector3f)ray.Direction, NormalAxis);
vHit = rayHit;
vHitNormal = Vector3f.AxisY;
return(rayHit != Vector3f.Invalid);
}
/// <summary>
/// Find min-distance between ray and curve. Pass max_dist if you only care about a certain distance
/// TODO: not 100% sure this is working properly... ?
/// </summary>
public bool FindClosestRayIntersction(Ray3d ray, double radius, out int hitSegment, out double fRayT)
{
int iRoot = boxes.Length - 1;
int iLayer = layers - 1;
hitSegment = -1;
fRayT = double.MaxValue;
find_closest_ray_intersction(ref ray, radius, ref hitSegment, ref fRayT, 0, iRoot, iLayer);
return(hitSegment != -1);
}
public bool RayIntersect(Ray3d ray, out Vector3d vHit, out Vector3d vHitNormal)
{
Ray3d baseRay = MapToBaseF(ray);
if (BaseTarget.RayIntersect(baseRay, out vHit, out vHitNormal))
{
vHit = MapFromBasePosF(vHit);
vHitNormal = MapFromBasePosF(vHitNormal);
return(true);
}
return(false);
}
void find_closest_ray_intersction(ref Ray3d ray, double radius, ref int nearestSegment, ref double nearest_ray_t, int bi, int iLayerStart, int iLayer)
{
// hit polygon layer, check segments
if (iLayer == 0)
{
int seg_i = 2 * bi;
Segment3d seg_a = Curve.GetSegment(seg_i);
double segt, rayt;
double segdist_sqr = DistRay3Segment3.SquaredDistance(ref ray, ref seg_a, out rayt, out segt);
if (segdist_sqr <= radius * radius && rayt < nearest_ray_t)
{
nearestSegment = seg_i;
nearest_ray_t = rayt;
}
if ((seg_i + 1) < Curve.SegmentCount)
{
Segment3d seg_b = Curve.GetSegment(seg_i + 1);
segdist_sqr = DistRay3Segment3.SquaredDistance(ref ray, ref seg_b, out rayt, out segt);
if (segdist_sqr <= radius * radius && rayt < nearest_ray_t)
{
nearestSegment = seg_i + 1;
nearest_ray_t = rayt;
}
}
return;
}
// test both boxes and recurse
// TODO: verify that this intersection strategy makes sense?
int prev_layer = iLayer - 1;
int prev_count = layer_counts[prev_layer];
int prev_start = iLayerStart - prev_count;
int prev_a = prev_start + 2 * bi;
bool intersects = IntrRay3Box3.Intersects(ref ray, ref boxes[prev_a], radius);
if (intersects)
{
find_closest_ray_intersction(ref ray, radius, ref nearestSegment, ref nearest_ray_t, 2 * bi, prev_start, prev_layer);
}
if ((2 * bi + 1) >= prev_count)
{
return;
}
int prev_b = prev_a + 1;
bool intersects2 = IntrRay3Box3.Intersects(ref ray, ref boxes[prev_b], radius);
if (intersects2)
{
find_closest_ray_intersction(ref ray, radius, ref nearestSegment, ref nearest_ray_t, 2 * bi + 1, prev_start, prev_layer);
}
}
public int FindNearestHitTriangle(Ray3d ray, double fMaxDist = double.MaxValue)
{
if (mesh_timestamp != mesh.Timestamp)
{
throw new Exception("DMeshAABBTree3.FindNearestTriangle: mesh has been modified since tree construction");
}
double fNearestT = (fMaxDist < double.MaxValue) ? fMaxDist : double.MaxValue;
int tNearID = DMesh3.InvalidID;
find_hit_triangle(root_index, ref ray, ref fNearestT, ref tNearID);
return(tNearID);
}
/// <summary>
/// convenience function to construct a IntrRay3Triangle3 object for a mesh triangle
/// </summary>
public static IntrRay3Triangle3 TriangleIntersection(DMesh3 mesh, int ti, Ray3d ray)
{
if (!mesh.IsTriangle(ti))
{
return(null);
}
Triangle3d tri = new Triangle3d();
mesh.GetTriVertices(ti, ref tri.V0, ref tri.V1, ref tri.V2);
IntrRay3Triangle3 q = new IntrRay3Triangle3(ray, tri);
q.Find();
return(q);
}
double box_ray_intersect_t(int iBox, Ray3d ray)
{
Vector3d c = box_centers[iBox];
Vector3d e = box_extents[iBox];
AxisAlignedBox3d box = new AxisAlignedBox3d(c - e, c + e);
IntrRay3AxisAlignedBox3 intr = new IntrRay3AxisAlignedBox3(ray, box);
if (intr.Find())
{
return(intr.RayParam0);
}
else
{
return(double.MaxValue);
}
}
double box_ray_intersect_t(int iBox, Ray3d ray)
{
Vector3d c = box_centers[iBox];
Vector3d e = box_extents[iBox];
AxisAlignedBox3d box = new AxisAlignedBox3d(c - e, c + e);
IntrRay3AxisAlignedBox3 intr = new IntrRay3AxisAlignedBox3(ray, box);
if (intr.Find())
{
return(intr.RayParam0);
}
else
{
Debug.Assert(intr.Result != IntersectionResult.InvalidQuery);
return(double.MaxValue);
}
}
/// <summary>
/// Find min-distance between ray and curve. Pass max_dist if you only care about a certain distance
/// TODO: not 100% sure this is working properly... ?
/// </summary>
public double SquaredDistance(Ray3d ray, out int iNearSeg, out double fNearSegT, out double fRayT, double max_dist = double.MaxValue)
{
int iRoot = boxes.Length - 1;
int iLayer = layers - 1;
double min_dist = max_dist;
iNearSeg = -1;
fNearSegT = 0;
fRayT = double.MaxValue;
find_min_distance(ref ray, ref min_dist, ref iNearSeg, ref fNearSegT, ref fRayT, 0, iRoot, iLayer);
if (iNearSeg == -1)
{
return(double.MaxValue);
}
return(min_dist);
}
public int FindNearestHitTriangle(Ray3d ray, double fMaxDist = double.MaxValue)
{
if (mesh_timestamp != mesh.Timestamp)
{
throw new Exception("DMeshAABBTree3.FindNearestHitTriangle: mesh has been modified since tree construction");
}
if (ray.Direction.IsNormalized == false)
{
throw new Exception("DMeshAABBTree3.FindNearestHitTriangle: ray direction is not normalized");
}
// [RMS] note: using float.MaxValue here because we need to use <= to compare box hit
// to fNearestT, and box hit returns double.MaxValue on no-hit. So, if we set
// nearestT to double.MaxValue, then we will test all boxes (!)
double fNearestT = (fMaxDist < double.MaxValue) ? fMaxDist : float.MaxValue;
int tNearID = DMesh3.InvalidID;
find_hit_triangle(root_index, ref ray, ref fNearestT, ref tNearID);
return(tNearID);
}
public static bool FindClosestRayIntersection(ICurve c, double segRadius, Ray3d ray, out double rayT)
{
if (c.Closed)
{
throw new InvalidOperationException("CurveUtils.FindClosestRayIntersection doesn't support closed curves yet");
}
rayT = double.MaxValue;
int nNearSegment = -1;
//double fNearSegT = 0.0;
int N = c.VertexCount;
for (int i = 0; i < N - 1; ++i)
{
DistRay3Segment3 dist = new DistRay3Segment3(ray,
new Segment3d(c.GetVertex(i), c.GetVertex(i + 1)));
// raycast to line bounding-sphere first (is this going ot be faster??)
double fSphereHitT;
bool bHitBoundSphere = RayIntersection.SphereSigned(ray.Origin, ray.Direction,
dist.Segment.Center, dist.Segment.Extent + segRadius, out fSphereHitT);
if (bHitBoundSphere == false)
{
continue;
}
// find ray/seg min-distance and use ray T
double dSqr = dist.GetSquared();
if (dSqr < segRadius * segRadius)
{
if (dist.RayParameter < rayT)
{
rayT = dist.RayParameter;
//fNearSegT = dist.SegmentParameter;
nNearSegment = i;
}
}
}
return(nNearSegment >= 0);
}
public bool Find()
{
if (Result != IntersectionResult.NotComputed)
{
return(Result != IntersectionResult.NoIntersection);
}
Line3d line = new Line3d(ray.Origin, ray.Direction);
IntrLine3Plane3 intr = new IntrLine3Plane3(line, plane);
if (intr.Find())
{
// The line intersects the plane, but possibly at a point that is
// not on the ray.
if (intr.Type == IntersectionType.Line)
{
Result = IntersectionResult.Intersects;
Type = IntersectionType.Line;
Point = intr.Point;
CoincidentRay = new Ray3d(ray.Origin, ray.Direction);
return(true);
}
if (intr.Type == IntersectionType.Point)
{
if (intr.LineParameter >= 0)
{
Type = IntersectionType.Point;
RayParameter = intr.LineParameter;
Point = intr.Point;
return(true);
}
}
}
Result = IntersectionResult.NoIntersection;
Type = IntersectionType.Empty;
return(false);
}
/// <summary>
/// Find intersection of ray with AABB, without having to construct any new classes.
/// Returns ray T-value of first intersection (or double.MaxVlaue on miss)
/// </summary>
public static bool FindRayIntersectT(ref Ray3d ray, ref AxisAlignedBox3d box, out double RayParam)
{
double RayParam0 = 0.0;
double RayParam1 = double.MaxValue;
int Quantity = 0;
Vector3d Point0 = Vector3d.Zero;
Vector3d Point1 = Vector3d.Zero;
IntersectionType Type = IntersectionType.Empty;
IntrLine3AxisAlignedBox3.DoClipping(ref RayParam0, ref RayParam1, ref ray.Origin, ref ray.Direction, ref box,
true, ref Quantity, ref Point0, ref Point1, ref Type);
if (Type != IntersectionType.Empty)
{
RayParam = RayParam0;
return(true);
}
else
{
RayParam = double.MaxValue;
return(false);
}
}
public static int FindHitTriangle_LinearSearch(DMesh3 mesh, Ray3d ray)
{
int tNearestID = DMesh3.InvalidID;
double fNearestT = double.MaxValue;
var tri = new Triangle3d();
foreach (int ti in mesh.TriangleIndices())
{
// [TODO] optimize this
mesh.GetTriVertices(ti, ref tri.V0, ref tri.V1, ref tri.V2);
var ray_tri_hit = new IntrRay3Triangle3(ray, tri);
if (ray_tri_hit.Find())
{
if (ray_tri_hit.RayParameter < fNearestT)
{
fNearestT = ray_tri_hit.RayParameter;
tNearestID = ti;
}
}
}
return(tNearestID);
}
public static bool FindClosestRayIntersection(ISampledCurve3d c, double segRadius, Ray3d ray, out double minRayT)
{
minRayT = double.MaxValue;
int nNearSegment = -1;
int nSegs = c.SegmentCount;
for (int i = 0; i < nSegs; ++i)
{
Segment3d seg = c.GetSegment(i);
// raycast to line bounding-sphere first (is this going ot be faster??)
double fSphereHitT;
bool bHitBoundSphere = RayIntersection.SphereSigned(ref ray.Origin, ref ray.Direction,
ref seg.Center, seg.Extent + segRadius, out fSphereHitT);
if (bHitBoundSphere == false)
{
continue;
}
double rayt, segt;
double dSqr = DistRay3Segment3.SquaredDistance(ref ray, ref seg, out rayt, out segt);
if (dSqr < segRadius * segRadius)
{
if (rayt < minRayT)
{
minRayT = rayt;
nNearSegment = i;
}
}
}
return(nNearSegment >= 0);
}
static public double MinDistanceRay2Param(Ray3d r1, Ray3d r2)
{
return(new DistRay3Ray3(r1, r2).Compute().Ray2Parameter);
}
/// <summary>
/// minimal intersection test, computes ray-t
/// </summary>
public static bool Intersects(ref Ray3d ray, ref Vector3d V0, ref Vector3d V1, ref Vector3d V2, out double rayT)
{
// Compute the offset origin, edges, and normal.
Vector3d diff = ray.Origin - V0;
Vector3d edge1 = V1 - V0;
Vector3d edge2 = V2 - V0;
Vector3d normal = edge1.Cross(ref edge2);
rayT = double.MaxValue;
// Solve Q + t*D = b1*E1 + b2*E2 (Q = kDiff, D = ray direction,
// E1 = kEdge1, E2 = kEdge2, N = Cross(E1,E2)) by
// |Dot(D,N)|*b1 = sign(Dot(D,N))*Dot(D,Cross(Q,E2))
// |Dot(D,N)|*b2 = sign(Dot(D,N))*Dot(D,Cross(E1,Q))
// |Dot(D,N)|*t = -sign(Dot(D,N))*Dot(Q,N)
double DdN = ray.Direction.Dot(ref normal);
double sign;
if (DdN > MathUtil.ZeroTolerance)
{
sign = 1;
}
else if (DdN < -MathUtil.ZeroTolerance)
{
sign = -1;
DdN = -DdN;
}
else
{
// Ray and triangle are parallel, call it a "no intersection"
// even if the ray does intersect.
return(false);
}
Vector3d cross = diff.Cross(ref edge2);
double DdQxE2 = sign * ray.Direction.Dot(ref cross);
if (DdQxE2 >= 0)
{
cross = edge1.Cross(ref diff);
double DdE1xQ = sign * ray.Direction.Dot(ref cross);
if (DdE1xQ >= 0)
{
if (DdQxE2 + DdE1xQ <= DdN)
{
// Line intersects triangle, check if ray does.
double QdN = -sign *diff.Dot(ref normal);
if (QdN >= 0)
{
// Ray intersects triangle.
double inv = (1) / DdN;
rayT = QdN * inv;
return(true);
}
// else: t < 0, no intersection
}
// else: b1+b2 > 1, no intersection
}
// else: b2 < 0, no intersection
}
// else: b1 < 0, no intersection
return(false);
}
void find_hit_triangle(int iBox, ref Ray3d ray, ref double fNearestT, ref int tID)
{
int idx = box_to_index[iBox];
if (idx < triangles_end) // triange-list case, array is [N t1 t2 ... tN]
{
Triangle3d tri = new Triangle3d();
int num_tris = index_list[idx];
for (int i = 1; i <= num_tris; ++i)
{
int ti = index_list[idx + i];
// [TODO] optimize this
mesh.GetTriVertices(ti, ref tri.V0, ref tri.V1, ref tri.V2);
IntrRay3Triangle3 ray_tri_hit = new IntrRay3Triangle3(ray, tri);
if (ray_tri_hit.Find())
{
if (ray_tri_hit.RayParameter < fNearestT)
{
fNearestT = ray_tri_hit.RayParameter;
tID = ti;
}
}
}
}
else // internal node, either 1 or 2 child boxes
{
double e = MathUtil.ZeroTolerancef;
int iChild1 = index_list[idx];
if (iChild1 < 0) // 1 child, descend if nearer than cur min-dist
{
iChild1 = (-iChild1) - 1;
double fChild1T = box_ray_intersect_t(iChild1, ray);
if (fChild1T <= fNearestT + e)
{
find_hit_triangle(iChild1, ref ray, ref fNearestT, ref tID);
}
}
else // 2 children, descend closest first
{
iChild1 = iChild1 - 1;
int iChild2 = index_list[idx + 1] - 1;
double fChild1T = box_ray_intersect_t(iChild1, ray);
double fChild2T = box_ray_intersect_t(iChild2, ray);
if (fChild1T < fChild2T)
{
if (fChild1T <= fNearestT + e)
{
find_hit_triangle(iChild1, ref ray, ref fNearestT, ref tID);
if (fChild2T <= fNearestT + e)
{
find_hit_triangle(iChild2, ref ray, ref fNearestT, ref tID);
}
}
}
else
{
if (fChild2T <= fNearestT + e)
{
find_hit_triangle(iChild2, ref ray, ref fNearestT, ref tID);
if (fChild1T <= fNearestT + e)
{
find_hit_triangle(iChild1, ref ray, ref fNearestT, ref tID);
}
}
}
}
}
}
public IntrRay3AxisAlignedBox3(Ray3d r, AxisAlignedBox3d b)
{
ray = r; box = b;
}
/// <summary>
/// test if ray intersects box.
/// expandExtents allows you to scale box for hit-testing purposes.
/// </summary>
public static bool Intersects(ref Ray3d ray, ref AxisAlignedBox3d box, double expandExtents = 0)
{
Vector3d WdU = Vector3d.Zero;
Vector3d AWdU = Vector3d.Zero;
Vector3d DdU = Vector3d.Zero;
Vector3d ADdU = Vector3d.Zero;
Vector3d AWxDdU = Vector3d.Zero;
double RHS;
Vector3d diff = ray.Origin - box.Center;
Vector3d extent = box.Extents + expandExtents;
WdU[0] = ray.Direction.x; // ray.Direction.Dot(Vector3d.AxisX);
AWdU[0] = Math.Abs(WdU[0]);
DdU[0] = diff.x; // diff.Dot(Vector3d.AxisX);
ADdU[0] = Math.Abs(DdU[0]);
if (ADdU[0] > extent.x && DdU[0] * WdU[0] >= (double)0)
{
return(false);
}
WdU[1] = ray.Direction.y; // ray.Direction.Dot(Vector3d.AxisY);
AWdU[1] = Math.Abs(WdU[1]);
DdU[1] = diff.y; // diff.Dot(Vector3d.AxisY);
ADdU[1] = Math.Abs(DdU[1]);
if (ADdU[1] > extent.y && DdU[1] * WdU[1] >= (double)0)
{
return(false);
}
WdU[2] = ray.Direction.z; // ray.Direction.Dot(Vector3d.AxisZ);
AWdU[2] = Math.Abs(WdU[2]);
DdU[2] = diff.z; // diff.Dot(Vector3d.AxisZ);
ADdU[2] = Math.Abs(DdU[2]);
if (ADdU[2] > extent.z && DdU[2] * WdU[2] >= (double)0)
{
return(false);
}
Vector3d WxD = ray.Direction.Cross(diff);
AWxDdU[0] = Math.Abs(WxD.x); // Math.Abs(WxD.Dot(Vector3d.AxisX));
RHS = extent.y * AWdU[2] + extent.z * AWdU[1];
if (AWxDdU[0] > RHS)
{
return(false);
}
AWxDdU[1] = Math.Abs(WxD.y); // Math.Abs(WxD.Dot(Vector3d.AxisY));
RHS = extent.x * AWdU[2] + extent.z * AWdU[0];
if (AWxDdU[1] > RHS)
{
return(false);
}
AWxDdU[2] = Math.Abs(WxD.z); // Math.Abs(WxD.Dot(Vector3d.AxisZ));
RHS = extent.x * AWdU[1] + extent.y * AWdU[0];
if (AWxDdU[2] > RHS)
{
return(false);
}
return(true);
}
/// <summary>
/// compute w/o allocating temporaries/etc
/// </summary>
public static double SquaredDistance(ref Ray3d ray, ref Segment3d segment,
out double rayT, out double segT)
{
Vector3d diff = ray.Origin - segment.Center;
double a01 = -ray.Direction.Dot(segment.Direction);
double b0 = diff.Dot(ray.Direction);
double b1 = -diff.Dot(segment.Direction);
double c = diff.LengthSquared;
double det = Math.Abs(1 - a01 * a01);
double s0, s1, sqrDist, extDet;
if (det >= MathUtil.ZeroTolerance)
{
// The Ray and Segment are not parallel.
s0 = a01 * b1 - b0;
s1 = a01 * b0 - b1;
extDet = segment.Extent * det;
if (s0 >= 0)
{
if (s1 >= -extDet)
{
if (s1 <= extDet) // region 0
{
// Minimum at interior points of Ray and Segment.
double invDet = (1) / det;
s0 *= invDet;
s1 *= invDet;
sqrDist = s0 * (s0 + a01 * s1 + (2) * b0) +
s1 * (a01 * s0 + s1 + (2) * b1) + c;
}
else // region 1
{
s1 = segment.Extent;
s0 = -(a01 * s1 + b0);
if (s0 > 0)
{
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
}
else
{
s0 = 0;
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
}
else // region 5
{
s1 = -segment.Extent;
s0 = -(a01 * s1 + b0);
if (s0 > 0)
{
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
}
else
{
s0 = 0;
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
}
else
{
if (s1 <= -extDet) // region 4
{
s0 = -(-a01 * segment.Extent + b0);
if (s0 > 0)
{
s1 = -segment.Extent;
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
}
else
{
s0 = 0;
s1 = -b1;
if (s1 < -segment.Extent)
{
s1 = -segment.Extent;
}
else if (s1 > segment.Extent)
{
s1 = segment.Extent;
}
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
else if (s1 <= extDet) // region 3
{
s0 = 0;
s1 = -b1;
if (s1 < -segment.Extent)
{
s1 = -segment.Extent;
}
else if (s1 > segment.Extent)
{
s1 = segment.Extent;
}
sqrDist = s1 * (s1 + (2) * b1) + c;
}
else // region 2
{
s0 = -(a01 * segment.Extent + b0);
if (s0 > 0)
{
s1 = segment.Extent;
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
}
else
{
s0 = 0;
s1 = -b1;
if (s1 < -segment.Extent)
{
s1 = -segment.Extent;
}
else if (s1 > segment.Extent)
{
s1 = segment.Extent;
}
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
}
}
else
{
// Ray and Segment are parallel.
if (a01 > 0)
{
// Opposite direction vectors.
s1 = -segment.Extent;
}
else
{
// Same direction vectors.
s1 = segment.Extent;
}
s0 = -(a01 * s1 + b0);
if (s0 > 0)
{
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
}
else
{
s0 = 0;
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
rayT = s0;
segT = s1;
// Account for numerical round-off errors.
if (sqrDist < 0)
{
sqrDist = 0;
}
return(sqrDist);
}
public static bool FindClosestRayIntersection(ISampledCurve3d c, double segRadius, Ray3d ray, out double rayT)
{
rayT = double.MaxValue;
int nNearSegment = -1;
//double fNearSegT = 0.0;
int N = c.VertexCount;
int iStop = (c.Closed) ? N : N - 1;
for (int i = 0; i < iStop; ++i)
{
DistRay3Segment3 dist = new DistRay3Segment3(ray,
new Segment3d(c.GetVertex(i), c.GetVertex((i + 1) % N)));
// raycast to line bounding-sphere first (is this going ot be faster??)
double fSphereHitT;
bool bHitBoundSphere = RayIntersection.SphereSigned(ray.Origin, ray.Direction,
dist.Segment.Center, dist.Segment.Extent + segRadius, out fSphereHitT);
if (bHitBoundSphere == false)
{
continue;
}
// find ray/seg min-distance and use ray T
double dSqr = dist.GetSquared();
if (dSqr < segRadius * segRadius)
{
if (dist.RayParameter < rayT)
{
rayT = dist.RayParameter;
//fNearSegT = dist.SegmentParameter;
nNearSegment = i;
}
}
}
return(nNearSegment >= 0);
}
public DistRay3Segment3(Ray3d rayIn, Segment3d segmentIn)
{
this.ray = rayIn; this.segment = segmentIn;
}
public IntrRay3Triangle3(Ray3d r, Triangle3d t)
{
ray = r; triangle = t;
}
static public double MinDistance(Ray3d r1, Ray3d r2)
{
return(new DistRay3Ray3(r1, r2).Get());
}