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);
}
// will return null if no edges need to be split!
public List <Vector3d> SplitResample(ISampledCurve3d curve, double fMaxEdgeLen)
{
double fMaxSqr = fMaxEdgeLen * fMaxEdgeLen;
int N = curve.VertexCount;
int Nstop = (curve.Closed) ? N + 1 : N;
if (lengths == null || lengths.Length < Nstop)
{
lengths = new double[Nstop];
}
bool bFoundSplit = false;
for (int i = 0; i < Nstop; ++i)
{
lengths[i] = curve.GetVertex(i).DistanceSquared(curve.GetVertex((i + 1) % N));
if (lengths[i] > fMaxSqr)
{
bFoundSplit = true;
}
}
if (!bFoundSplit)
{
return(null);
}
List <Vector3d> vNew = new List <Vector3d>();
Vector3d prev = curve.GetVertex(0);
vNew.Add(prev);
for (int i = 0; i < Nstop - 1; ++i)
{
Vector3d next = curve.GetVertex((i + 1) % N);
if (lengths[i] > fMaxSqr)
{
double fLen = Math.Sqrt(lengths[i]);
int nSteps = (int)(fLen / fMaxEdgeLen) + 1;
for (int k = 1; k < nSteps; ++k)
{
double t = (double)k / (double)nSteps;
Vector3d mid = Vector3d.Lerp(prev, next, t);
vNew.Add(mid);
}
}
vNew.Add(next);
prev = next;
}
return(vNew);
}
public static int FindNearestIndex(ISampledCurve3d c, Vector3d v)
{
int iNearest = -1;
double dNear = Double.MaxValue;
int N = c.VertexCount;
for (int i = 0; i < N; ++i)
{
double dSqr = (c.GetVertex(i) - v).LengthSquared;
if (dSqr < dNear)
{
dNear = dSqr;
iNearest = i;
}
}
return(iNearest);
}
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 DCurve3(ISampledCurve3d icurve)
{
this.vertices = new List <Vector3d>(icurve.Vertices);
Closed = icurve.Closed;
Timestamp = 1;
}
// will return null if no edges need to be split!
public List <Vector3d> SplitCollapseResample(ISampledCurve3d curve, double fMaxEdgeLen, double fMinEdgeLen)
{
double fMaxSqr = fMaxEdgeLen * fMaxEdgeLen;
double fMinSqr = fMinEdgeLen * fMinEdgeLen;
int N = curve.VertexCount;
int Nstop = (curve.Closed) ? N + 1 : N;
if (lengths == null || lengths.Length < Nstop)
{
lengths = new double[Nstop];
}
bool bFoundSplit = false;
bool bFoundCollapse = false;
for (int i = 0; i < Nstop - 1; ++i)
{
lengths[i] = curve.GetVertex(i).DistanceSquared(curve.GetVertex((i + 1) % N));
if (lengths[i] > fMaxSqr)
{
bFoundSplit = true;
}
else if (lengths[i] < fMinSqr)
{
bFoundCollapse = true;
}
}
if (bFoundSplit == false && bFoundCollapse == false)
{
return(null);
}
List <Vector3d> vNew = new List <Vector3d>();
Vector3d prev = curve.GetVertex(0);
vNew.Add(prev);
double collapse_accum = 0;
for (int i = 0; i < Nstop - 1; ++i)
{
Vector3d next = curve.GetVertex((i + 1) % N);
// accumulate collapsed edges. if we accumulate past min-edge length,
// then need to drop a vertex
if (lengths[i] < fMinSqr)
{
collapse_accum += Math.Sqrt(lengths[i]);
if (collapse_accum > fMinEdgeLen)
{
collapse_accum = 0;
vNew.Add(next);
}
prev = next;
continue;
}
// if we have been accumulating collapses, then we need to
// drop a new vertex (todo: is this right? shouldn't we just
// continue from previous?)
if (collapse_accum > 0)
{
vNew.Add(prev);
collapse_accum = 0;
}
// split edge if it is too long
if (lengths[i] > fMaxSqr)
{
double fLen = Math.Sqrt(lengths[i]);
int nSteps = (int)(fLen / fMaxEdgeLen) + 1;
for (int k = 1; k < nSteps; ++k)
{
double t = (double)k / (double)nSteps;
Vector3d mid = Vector3d.Lerp(prev, next, t);
vNew.Add(mid);
}
}
vNew.Add(next);
prev = next;
}
return(vNew);
}
