public override SignedDistance signedDistance(Vector2 origin, out double param) { Vector2 qa = _p0 - origin; Vector2 ab = _p1 - _p0; Vector2 br = _p0 + _p2 - _p1 - _p1;// double a = Vector2.dotProduct(br, br); double b = 3 * Vector2.dotProduct(ab, br); double c = 2 * Vector2.dotProduct(ab, ab) + Vector2.dotProduct(qa, br); double d = Vector2.dotProduct(qa, ab); EqResult t = new EqResult(); int solutions = EquationSolver.SolveCubic(ref t, a, b, c, d); double minDistance = nonZeroSign(Vector2.crossProduct(ab, qa)) * qa.Length(); // distance from A param = -Vector2.dotProduct(qa, ab) / Vector2.dotProduct(ab, ab); { double distance = nonZeroSign( Vector2.crossProduct(_p2 - _p1, _p2 - origin)) * (_p2 - origin).Length(); // distance from B if (Math.Abs(distance) < Math.Abs(minDistance)) { minDistance = distance; param = Vector2.dotProduct(origin - _p1, _p2 - _p1) / Vector2.dotProduct(_p2 - _p1, _p2 - _p1); } } //possible solution -1,0,1,2 for (int i = 0; i < solutions; ++i) { double ti = t[i]; if (ti > 0 && ti < 1) { Vector2 endpoint = _p0 + 2 * ti * ab + ti * ti * br; double distance = nonZeroSign( Vector2.crossProduct(_p2 - _p0, endpoint - origin)) * (endpoint - origin).Length(); if (Math.Abs(distance) <= Math.Abs(minDistance)) { minDistance = distance; param = ti; } } } if (param >= 0 && param <= 1) { return(new SignedDistance(minDistance, 0)); } if (param < .5) { return(new SignedDistance(minDistance, Math.Abs(Vector2.dotProduct(ab.normalize(), qa.normalize())))); } else { return(new SignedDistance(minDistance, Math.Abs(Vector2.dotProduct((_p2 - _p1).normalize(), (_p2 - origin).normalize())))); } }
public override SignedDistance signedDistance(Vector2 origin, out double param) { Vector2 qa = p[0] - origin; Vector2 ab = p[1] - p[0]; Vector2 br = p[0] + p[2] - p[1] - p[1]; double a = Vector2.dotProduct(br, br); double b = 3 * Vector2.dotProduct(ab, br); double c = 2 * Vector2.dotProduct(ab, ab) + Vector2.dotProduct(qa, br); double d = Vector2.dotProduct(qa, ab); double[] t = new double[3]; int solutions = EquationSolver.SolveCubic(t, a, b, c, d); double minDistance = nonZeroSign(Vector2.crossProduct(ab, qa)) * qa.Length(); // distance from A param = -Vector2.dotProduct(qa, ab) / Vector2.dotProduct(ab, ab); { double distance = nonZeroSign( Vector2.crossProduct(p[2] - p[1], p[2] - origin)) * (p[2] - origin).Length(); // distance from B if (Math.Abs(distance) < Math.Abs(minDistance)) { minDistance = distance; param = Vector2.dotProduct(origin - p[1], p[2] - p[1]) / Vector2.dotProduct(p[2] - p[1], p[2] - p[1]); } } for (int i = 0; i < solutions; ++i) { if (t[i] > 0 && t[i] < 1) { Vector2 endpoint = p[0] + 2 * t[i] * ab + t[i] * t[i] * br; double distance = nonZeroSign( Vector2.crossProduct(p[2] - p[0], endpoint - origin)) * (endpoint - origin).Length(); if (Math.Abs(distance) <= Math.Abs(minDistance)) { minDistance = distance; param = t[i]; } } } if (param >= 0 && param <= 1) { return(new SignedDistance(minDistance, 0)); } if (param < .5) { return(new SignedDistance(minDistance, Math.Abs(Vector2.dotProduct(ab.normalize(), qa.normalize())))); } else { return(new SignedDistance(minDistance, Math.Abs(Vector2.dotProduct((p[2] - p[1]).normalize(), (p[2] - origin).normalize())))); } }