/**
* Clip out a section of the arm, using internal parameter values
*
* @param c0 : start of section to clip out
* @param c1 : end of section to clip out; if < c0, c0 & c1 are swapped
*/
private void clip0(double c0, double c1) {
final boolean db = false;
if (c0 > c1) {
double t = c0;
c0 = c1;
c1 = t;
}
// array to store new clip elements in
DArray nc = new DArray();
if (db) {
System.out.println("clip " + Tools.f(c0) + "..." + Tools.f(c1));
}
for (int i = 0; i < visSeg.size(); i += 2) {
double t0 = visSeg.getDouble(i), t1 = visSeg.getDouble(i + 1);
if (db) {
System.out.println(" seg " + i + " " + Tools.f(t0) + "..." + Tools.f(t1));
}
// if this seg is outside clip region, leave intact (add it)
if (t0 >= c1 || t1 <= c0) {
nc.addDouble(t0);
nc.addDouble(t1);
continue;
}
if (t0 >= c0 && t1 <= c1) {
if (db) {
System.out.println(" clipping entire segment");
}
// remove entire segment, by skipping it
continue;
}
// add unclipped portion
if (t0 < c0) {
nc.addDouble(t0);
nc.addDouble(c0);
}
if (t1 > c1) {
nc.addDouble(c1);
nc.addDouble(t1);
}
}
visSeg = nc;
if (db) {
System.out.println(" after clipping:" + visSeg);
}
}
/**
* Determine closest point on hyperbolic arm to a point pt
*
* @param pt : point
* @return t value for closest point; not necessarily within clipped range
*/
public double closestPointTo(FPoint2 pt) {
final boolean db = false;
pt = toCurveSpace(pt, null);
double U = B + 1;
double V = -pt.y;
double W = B * pt.x;
Polyn p =
new Polyn( //
B * U * U, //
2 * B * U * V, //
B * V * V + A * U * U - W * W, //
2 * A * U * V, //
A * V * V //
);
if (db) Streams.out.println("closestPointTo, pt=" + pt + "\n" + p);
double ret = 0;
try {
DArray r = new DArray();
if (Math.abs(p.c(0)) < 1e-5) r.addDouble(0);
else p.solve(r);
if (r.isEmpty()) {
throw new FPError("can't find closest point, poly=\n" + p);
}
double bestDist = 0;
for (int i = 0; i < r.size(); i++) {
double t = r.getDouble(i);
FPoint2 apt = calcPoint(t);
double dist = apt.distance(pt);
if (i == 0 || dist < bestDist) {
bestDist = dist;
ret = t;
}
}
} catch (FPError e) {
Tools.warn("caught FPError");
// Streams.out.println("caught:\n" + e);
ret = (this.minParameter() + this.maxParameter()) * .5;
}
return ret;
}
/**
* Calc possible hull from a set of polygons
*
* @param polySet set of polygons
* @param first first polygon to include
* @param last last polygon to include
* @return polygon containing possible hull
*/
private static EdPolygon calcPHullRange(EdPolygon[] polySet, int first, int last) {
final boolean db = true;
int len = last + 1 - first;
if (len == 1) return polySet[first];
if (len > 2) {
if (db && T.update()) {
DArray a = new DArray();
for (int i = first; i <= last; i++) a.add(polySet[i]);
T.msg("calc possible hull of multiple polygons" + T.show(a, MyColor.cRED, STRK_THICK, -1));
}
int n = len / 2;
return calcPHull(
calcPHullRange(polySet, first, first + n - 1), calcPHullRange(polySet, first + n, last));
}
return calcPHull(polySet[first], polySet[first + 1]);
}
/**
* Convert array of hyperbolas to hyperbolas with single connected range
*
* @param h1
* @return
*/
public static Hyperbola[] singlify(Hyperbola[] h1) {
DArray e = new DArray();
for (int i = 0; i < h1.length; i++) {
Hyperbola hi = h1[i];
if (hi.visSeg.size() == 1) {
e.add(hi);
} else {
for (int j = 0; j < hi.visSeg.size(); j += 2) {
double t0 = hi.visSeg.getDouble(j + 0);
double t1 = hi.visSeg.getDouble(j + 1);
Hyperbola h = new Hyperbola(hi);
h.visSeg.clear();
h.visSeg.addDouble(t0);
h.visSeg.addDouble(t1);
e.add(h);
}
}
}
return (Hyperbola[]) e.toArray(Hyperbola.class);
}
/**
* Find intersections of hyperbolic arm with a line.
*
* @param s0 : first point on line
* @param s1 : second point on line
* @param ipts : intersection points returned here
*/
public DArray findLineIntersect(FPoint2 s0, FPoint2 s1, DArray ipts) {
if (ipts == null) ipts = new DArray();
ipts.clear();
// transform both line points to curve space
FPoint2 c0 = toCurveSpace(s0, null), c1 = toCurveSpace(s1, null);
double a = c0.x, b = c0.y, c = c1.x, d = c1.y;
double e = d - b;
double f = c - a;
double D = f * f - B * e * e;
double E = 2 * a * f - 2 * b * B * e;
double F = a * a - A - B * b * b;
Polyn q = new Polyn(D, E, F);
DArray roots = new DArray();
q.solve(roots);
for (int i = 0; i < roots.size(); i++) {
double k = roots.getDouble(i);
FPoint2 pt =
// ipts.add(
new FPoint2(s0.x + (s1.x - s0.x) * k, s0.y + (s1.y - s0.y) * k);
// );
// Make sure this point is actually on the arm.
FPoint2 cpt = toCurveSpace(pt, null);
if (cpt.x < 0) {
continue;
}
ipts.add(pt);
}
return ipts;
}
/**
* Find intersections of hyperbolic arm with an axes-aligned line segment. Arm must intersect
* segment, not its underlying (infinite) line. The intersection points are sorted into increasing
* parameter values w.r.t. the arm.
*
* @param s0 : start of line segment
* @param s1 : end of line segment
* @param ipts : intersection points returned here
* @param reverseOrder : if true, points are sorted by decreasing parameter values
* @param dbFlag : true to print debug information
*/
public void findOrthogonalIntersect(
FPoint2 s0, FPoint2 s1, DArray ipts, boolean reverseOrder, boolean dbFlag) {
final boolean db = true && dbFlag;
if (db) {
System.out.println(
"findOrthogonalIntersect " + s0 + " -> " + s1 + " rev:" + Tools.f(reverseOrder));
}
// Determine whether this is a vertical or horizontal line segment.
boolean vert = (Math.abs(s1.y - s0.y) > Math.abs(s1.x - s0.x));
if (db) {
System.out.println(" vert=" + Tools.f(vert));
}
// Find the quadratic to solve.
Polyn p;
PlaneCurve cv = getCurve();
if (vert) {
p = cv.solveForX(s0.x);
} else {
p = cv.solveForY(s0.y);
}
DArray lst = new DArray();
p.solve(lst);
if (db) {
System.out.println(" curve=" + cv.toString(true));
System.out.println(" polyn=" + p.toString(true));
System.out.println(" roots=" + Tools.d(lst));
}
// Sort points, discarding those not on line segment,
// and those not on the correct arm.
ipts.clear();
for (int i = 0; i < lst.size(); i++) {
double ta = lst.getDouble(i);
FPoint2 pt;
if (vert) {
pt = new FPoint2(s0.x, ta);
} else {
pt = new FPoint2(ta, s0.y);
}
if (db) {
System.out.println(" position on arm for ta=" + ta + " is " + pt);
}
double t = MyMath.positionOnSegment(pt, s0, s1);
if (db) {
System.out.println(" pt=" + pt + " t=" + t);
}
if (t < 0 || t > 1) {
if (db) {
System.out.println(" not on segment, skipping");
}
continue;
}
FPoint2 cpt = toCurveSpace(pt, null);
if (db) {
System.out.println(" curveSpace=" + cpt);
}
if (cpt.x < 0) {
if (db) {
System.out.println(" skipping...");
}
continue;
}
double t1 = calcParameter(pt);
int j = 0;
while (true) {
if (j == ipts.size()) {
break;
}
double t2 = calcParameter(ipts.getFPoint2(j));
if (!reverseOrder) {
if (t1 < t2) {
break;
}
} else if (t1 > t2) {
break;
}
j++;
}
ipts.add(j, pt);
}
if (db) {
System.out.println(" ipts=" + Tools.d(ipts));
}
}
public void render(Color c, int stroke, int markType) {
final boolean db = false;
// Get array of visible segments. If no such array exists,
// use default.
DArray vseg = visSeg;
boolean dashed = false;
// if (step == 0) {
double step = renderStep();
// }
// vp V = TestBed.view();
if (db) Streams.out.println(" step=" + step);
// plot each visible segment
for (int seg = 0; seg < vseg.size(); seg += 2) {
double t0 = vseg.getDouble(seg + 0), t1 = vseg.getDouble(seg + 1);
t0 = MyMath.clamp(t0, -500.0, 500.0);
t1 = MyMath.clamp(t1, -500.0, 500.0);
// render() expects external parameters.
double s0 = toExt(t0), s1 = toExt(t1);
if (s0 > s1) {
double tmp = s0;
s0 = s1;
s1 = tmp;
}
FPoint2 p0 = calcPoint(s0), p1 = calcPoint(s1);
if (db) Streams.out.println(" p0=" + p0 + ", p1=" + p1);
if (isLine() && !dashed) {
V.drawLine(p0, p1);
} else {
/*
if (Math.abs(s0) >= 500
||Math.abs(s1) >= 500)
System.out.println("Rendering "+t0+" to "+t1+" step "+step);
*/
if (dashed) V.pushStroke(Globals.STRK_RUBBERBAND);
{
// int count = 0;
boolean first = true;
for (double t = t0; ; t += step) { // , count++) {
boolean last = (t >= t1);
if (last) {
t = t1;
}
calcPointInternal(t, p1);
if (!p1.isValid()) {
if (last) {
break;
}
continue;
}
if (db) {
System.out.println(" calcPt " + Tools.f(toExt(t)) + " = " + p1.x + "," + p1.y);
}
if (!first) {
V.drawLine(p0, p1);
if (false) {
Tools.warn("highlighting int");
V.mark(p0);
}
}
if (last) {
break;
}
p0.setLocation(p1);
first = false;
}
}
if (dashed) V.popStroke();
}
}
}
/**
* Find intersections between two hyperbolas
*
* @param a Hyperbola
* @param b Hyperbola
* @param iPts where to store intersection points; null to construct
*/
public static DArray findIntersections(Hyperbola a, Hyperbola b, DArray iPts) {
final boolean db = false;
if (iPts == null) iPts = new DArray();
iPts.clear();
final DArray jPts = new DArray();
// a.initClipIfNec();
// b.initClipIfNec();
if (db) {
System.out.println(
"finding intersections between:\n" + a.toString(true) + "\n" + b.toString(true));
}
PlaneCurve.findIntersect(a.getCurve(), b.getCurve(), jPts);
// filter out false intersections
if (db) {
System.out.println("prefilter # intersections= " + jPts.size());
}
FPoint2 ptc = new FPoint2();
for (int i = 0; i < jPts.size(); i++) {
FPoint2 pt = jPts.getFPoint2(i);
// make sure calculated intersect point is actually on both arms
{
FPoint2 data = a.calcParameterAndDistance(pt);
if (data.y > .001) continue;
data = b.calcParameterAndDistance(pt);
if (data.y > .001) continue;
}
if (!a.isLine()) {
// put in curve space to verify it's to the right of the y axis
a.toCurveSpace(pt, ptc);
if (db) {
System.out.println("Filter " + i + " in curveA= " + ptc);
}
if (ptc.x <= 0) {
if (db) {
System.out.println(" < 0");
}
continue;
}
}
if (!b.isLine()) {
b.toCurveSpace(pt, ptc);
if (db) {
System.out.println(" in curveB= " + ptc);
}
if (ptc.x <= 0) {
if (db) {
System.out.println(" < 0");
}
continue;
}
}
if (db) {
System.out.println(" adding " + pt);
}
iPts.add(pt);
}
return iPts;
}
/**
* Constructor
*
* @param f1 FPoint2
* @param f2 FPoint2
* @param pt FPoint2, or null for bisector
*/
private void construct(FPoint2 f1, FPoint2 f2, FPoint2 pt) {
// userData[LEFT] = new DArray();
// userData[RIGHT] =new DArray();
final boolean db = false;
if (db) {
System.out.println("Hyperbola constructor\n f1=" + f1 + "\n f2=" + f2 + "\n pt=" + pt);
}
boolean bisector = (pt == null);
initializeVisibleSegments();
// if point on arm is closer to f2 than f1, swap f1 & f2.
if (!bisector && FPoint2.distanceSquared(f1, pt) > FPoint2.distanceSquared(f2, pt)) {
flipped = true;
}
this.foci[RIGHT] = new FPoint2(f1);
this.foci[LEFT] = new FPoint2(f2);
if (!bisector) {
this.pt = new FPoint2(pt);
}
double fociDist = FPoint2.distance(f1, f2);
if (fociDist == 0) {
throw new FPError("Hyperbola foci are same point");
}
c = fociDist * .5;
// calculate the translation of the hyperbola away from
// standard position.
FPoint2 rFocus = getFocus(0), lFocus = getFocus(1);
origin = new FPoint2(.5 * (rFocus.x + lFocus.x), .5 * (rFocus.y + lFocus.y));
// calculate the angle of rotation of the hyperbola away
// from the standard position.
double theta = Math.atan2(rFocus.y - lFocus.y, rFocus.x - lFocus.x);
Matrix fromCenterInW = Matrix.getTranslate(origin, true);
Matrix rotToE = Matrix.getRotate(-theta);
toE2 = rotToE;
Matrix.mult(toE2, fromCenterInW, toE2);
// calculate inverse
toW2 = toE2.invert(null);
// Matrix toCenterInW = Matrix.translationMatrix(origin, false);
// Matrix rotToW = Matrix.getRotate2D(theta);
//
// toW2 = toCenterInW;
// Matrix.mult(toW2, rotToW, toW2);
// Tools.warn("just invert matrix here");
//
if (bisector) {
valid = true;
} else {
// get the arm point in hyperbola space.
FPoint2 workPt = toE2.apply(pt, null);
double xs = workPt.x * workPt.x;
double cs = c * c;
Polyn q = new Polyn(1, -(cs + xs + workPt.y * workPt.y), cs * xs);
if (db) {
System.out.println("a2 quadratic:\n" + q);
}
final DArray qsoln = new DArray();
q.solve(qsoln);
if (db) {
Streams.out.println(qsoln);
}
double val = q.c(1) * -.5;
int ql = qsoln.size();
if (ql >= 1) {
val = qsoln.getDouble(0);
}
// choose the root that is less than c*c.
if (ql == 2) {
if (val > qsoln.getDouble(1)) {
val = qsoln.getDouble(1);
if (db) {
System.out.println(" two roots, choosing smaller.");
}
}
}
if (db) {
System.out.println(" root chosen=" + val);
}
a = Polyn.sqrt(val);
A = a * a;
B = A / (c * c - A);
}
valid = true;
if (db) {
System.out.println(" ==> " + this);
}
}
private static void OLDexpandHull(
PtEntry convHullEntry, PtEntry aHull, PtEntry bHull, boolean ccw) {
boolean db__OLD = C.vb(DB_HULLEXPAND);
if (db__OLD && T.update()) T.msg("expandHull" + T.show(convHullEntry) + " ccw=" + ccw);
PtEntry[] opp = new PtEntry[2];
opp[0] = aHull;
opp[1] = bHull;
PtEntry old____hEnt = convHullEntry;
boolean advanced = false;
do {
if (old____hEnt != convHullEntry) advanced = true;
inf.update();
if (old____hEnt.source() == null) {
if (db__OLD && T.update())
T.msg("expandHull, source unknown, guaranteed not convex" + T.show(old____hEnt));
old____hEnt = old____hEnt.next(ccw);
continue;
}
int w = (old____hEnt.source() == opp[0].source()) ? 1 : 0;
PtEntry oppEnt = opp[w];
boolean isTangent =
!COper3.right(old____hEnt, oppEnt, oppEnt.next(true), ccw)
&& !COper3.right(old____hEnt, oppEnt, oppEnt.prev(true), ccw);
if (!isTangent) {
if (db__OLD && T.update())
T.msg(
"expandHull, advance tangent line"
+ T.show(oppEnt.toPolygon(), MyColor.cDARKGRAY, -1, MARK_X)
+ T.show(old____hEnt)
+ tl(old____hEnt, oppEnt));
opp[w] = oppEnt.next(ccw);
continue;
}
if (COper3.left(old____hEnt, oppEnt, old____hEnt.next(ccw), ccw)
&& COper3.left(old____hEnt, oppEnt, old____hEnt.prev(ccw), ccw)) {
DArray dispPts = new DArray();
// delete points until cross tangent line
PtEntry next = old____hEnt.next(ccw);
while (true) {
PtEntry prev = next;
dispPts.add(prev);
next = prev.delete(ccw);
inf.update();
if (COper3.right(old____hEnt, oppEnt, next, ccw)) {
FPoint2 cross = MyMath.linesIntersection(old____hEnt, oppEnt, prev, next, null);
old____hEnt = old____hEnt.insert(new PtEntry(cross), ccw);
if (db__OLD && T.update())
T.msg(
"expandHull, clipped to shadow region"
+ tl(old____hEnt, oppEnt)
+ T.show(old____hEnt)
+ T.show(dispPts));
break;
}
}
} else {
if (db__OLD && T.update())
T.msg(
"expandHull, not dipping into shadow region"
+ T.show(old____hEnt.next(ccw))
+ tl(old____hEnt, oppEnt));
}
old____hEnt = old____hEnt.next(ccw);
} while (!advanced || old____hEnt != convHullEntry);
}
/**
* Apply hull expansion procedure
*
* @param convHullEntry an entry of the hull (should be on the convex hull, so it is not deleted
* or replaced and is still valid for subsequent calls)
* @param aHull entry on convex hull of polygon A
* @param bHull entry on convex hull of polygon B
* @param ccw true to move in ccw direction, else cw
*/
private static void expandHull(PtEntry convHullEntry, PtEntry aHull, PtEntry bHull, boolean ccw) {
if (C.vb(OLDMETHOD)) {
OLDexpandHull(convHullEntry, aHull, bHull, ccw);
return;
}
boolean db = C.vb(DB_HULLEXPAND);
if (db && T.update()) T.msg("expandHull" + T.show(convHullEntry) + " ccw=" + ccw);
// tangent points for A, B
PtEntry[] tangentPoints = new PtEntry[2];
tangentPoints[0] = aHull;
tangentPoints[1] = bHull;
PtEntry hEnt = convHullEntry;
do {
inf.update();
// calculate tangent ray R
PtEntry tangentPt = null;
while (true) {
int tanIndex = (hEnt.source() == tangentPoints[0].source()) ? 1 : 0;
tangentPt = tangentPoints[tanIndex];
if (!COper3.right(hEnt, tangentPt, tangentPt.next(true), ccw)
&& !COper3.right(hEnt, tangentPt, tangentPt.prev(true), ccw)) break;
tangentPt = tangentPt.next(ccw);
if (db && T.update())
T.msg(
"expandHull, advance tangent line"
+ T.show(tangentPt.toPolygon(), MyColor.cDARKGRAY, -1, MARK_DISC)
+ T.show(hEnt)
+ tl(hEnt, tangentPt));
tangentPoints[tanIndex] = tangentPt;
}
if (COper3.left(hEnt, tangentPt, hEnt.next(ccw), ccw)) {
DArray dispPts = new DArray();
// delete points until cross tangent line
PtEntry next = hEnt.next(ccw);
while (true) {
PtEntry prev = next;
dispPts.add(prev);
next = prev.delete(ccw);
if (COper3.right(hEnt, tangentPt, next, ccw)) {
FPoint2 cross = MyMath.linesIntersection(hEnt, tangentPt, prev, next, null);
hEnt = hEnt.insert(new PtEntry(cross), ccw);
if (db && T.update())
T.msg(
"expandHull, clipped to shadow region"
+ tl(hEnt, tangentPt)
+ T.show(hEnt)
+ T.show(dispPts));
break;
}
}
} else {
if (db && T.update())
T.msg(
"expandHull, not dipping into shadow region"
+ T.show(hEnt.next(ccw))
+ tl(hEnt, tangentPt));
}
while (true) {
hEnt = hEnt.next(ccw);
if (COper3.left(hEnt.prev(ccw), hEnt, hEnt.next(ccw), ccw)) break;
if (db && T.update()) T.msg("skipping reflex vertex" + T.show(hEnt));
}
} while (hEnt != convHullEntry);
}