// used for the segment intersection with radius code, // if there is not a collision, find the nearest endpoint via point-line distance // then don't forget that we move back the parameter t by enough to add a buffer from the collision private double NonIntersectingSegmentDistance(myLineSegment o, double t, double radius) { double mindist2 = double.MaxValue; mindist2 = Math.Min(mindist2, o.DistToPoint2(p1)); mindist2 = Math.Min(mindist2, o.DistToPoint2(p2)); mindist2 = Math.Min(mindist2, DistToPoint2(o.p1)); mindist2 = Math.Min(mindist2, DistToPoint2(o.p2)); if (mindist2 < radius * radius) { // there is a collision without intersection, move up as far as we can without collision // note: we use the t term that we passed in, and subtract enough so that the perpendicular distance equals radius // also note: if t == 0 that means that the collision is already happening without a move... we're stuck? //if (t < 1e-6) // return 0.0; // Scoot t back so the radius is equal to our radius double sintheta = Math.Abs(o.v.cross(v)) / (v.mag() * o.v.mag()); t -= (radius / sintheta) / v.mag(); // also remove the collision buffer, then return it t -= (CollisionBuffer / v.mag()); return(t < 0.0 ? 0.0 : t); } // if there is still no collision, just return the full 1.0 parameter value return(1.0); }
public double SegmentIntersectionWithRadius(myLineSegment o, double radius) { // get determinate and check for parallel lines double D = v.cross(o.v); if (Math.Abs(D) < 1e-6) { // Are we already within the line radius? if (o.DistToPoint2(p1) < radius * radius) { return(0.0); } // Check if we are now moving onto the line segement. else if (NonIntersectingSegmentDistance(o, 0.0, radius) < 0.1) { // return 1.0; return(Math.Min(Geometry.MovingCirlcePointCast(this, radius, o.p1), Geometry.MovingCirlcePointCast(this, radius, o.p2))); } // No collision else { return(1.0); } } // relative vector from the other segments starting point to my own myVector relVect = new myVector(p1, o.p1); double t = relVect.cross(o.v) / D; double u = relVect.cross(v) / D; // find if the intersection happens along the other line segment if (u < 0.0 || u > 1.0) { return(NonIntersectingSegmentDistance(o, t, radius)); } // find if the intersection happens along this line segment if (t < 0.0 || t > 1.0) { return(NonIntersectingSegmentDistance(o, t, radius)); } // An intersection is happening! Scoot the t parameter back a bit for the radius and buffer then return it // Scoot t back so the radius is equal to our radius double sintheta = Math.Abs(o.v.cross(v)) / (v.mag() * o.v.mag()); t -= (radius / sintheta) / v.mag(); // also remove the collision buffer, then return it t -= (CollisionBuffer / v.mag()); return(t < 0.0 ? 0.0 : t); }