bezier.go

Documentation: oss.terrastruct.com/d2/lib/geo

     1  // bezier.go is a manual translation of the code from (including some amendments in the comments section)
     2  // https://www.particleincell.com/2013/cubic-line-intersection/
     3  
     4  package geo
     5  
     6  import (
     7  	"math"
     8  
     9  	"gonum.org/v1/plot/font"
    10  	"gonum.org/v1/plot/tools/bezier"
    11  	"gonum.org/v1/plot/vg"
    12  )
    13  
    14  // How precise should comparisons be, avoid being too precise due to floating point issues
    15  const PRECISION = 0.0001
    16  
    17  type BezierCurve struct {
    18  	Curve  *bezier.Curve
    19  	points []*Point
    20  }
    21  
    22  func NewBezierCurve(points []*Point) *BezierCurve {
    23  	internalPoints := make([]vg.Point, len(points))
    24  	for i := 0; i < len(points); i++ {
    25  		internalPoints[i] = vg.Point{
    26  			X: font.Length(points[i].X),
    27  			Y: font.Length(points[i].Y),
    28  		}
    29  	}
    30  	internalCurve := bezier.New(internalPoints...)
    31  	curve := &BezierCurve{
    32  		Curve: &internalCurve,
    33  	}
    34  	curve.points = points
    35  	return curve
    36  }
    37  
    38  func (bc BezierCurve) Intersections(segment Segment) []*Point {
    39  	return ComputeIntersections(
    40  		[]float64{
    41  			bc.points[0].X,
    42  			bc.points[1].X,
    43  			bc.points[2].X,
    44  			bc.points[3].X,
    45  		},
    46  		[]float64{
    47  			bc.points[0].Y,
    48  			bc.points[1].Y,
    49  			bc.points[2].Y,
    50  			bc.points[3].Y,
    51  		},
    52  		[]float64{
    53  			segment.Start.X,
    54  			segment.End.X,
    55  		},
    56  		[]float64{
    57  			segment.Start.Y,
    58  			segment.End.Y,
    59  		},
    60  	)
    61  }
    62  
    63  func (bc BezierCurve) At(point float64) *Point {
    64  	curvePoint := bc.Curve.Point(point)
    65  	return NewPoint(float64(curvePoint.X), float64(curvePoint.Y))
    66  }
    67  
    68  // nolint
    69  func ComputeIntersections(px, py, lx, ly []float64) []*Point {
    70  	out := make([]*Point, 0)
    71  
    72  	var A = ly[1] - ly[0]
    73  	var B = lx[0] - lx[1]
    74  	var C = lx[0]*(ly[0]-ly[1]) + ly[0]*(lx[1]-lx[0])
    75  
    76  	var bx = bezierCoeffs(px[0], px[1], px[2], px[3])
    77  	var by = bezierCoeffs(py[0], py[1], py[2], py[3])
    78  
    79  	P := make([]float64, 4)
    80  	P[0] = A*bx[0] + B*by[0]
    81  	P[1] = A*bx[1] + B*by[1]
    82  	P[2] = A*bx[2] + B*by[2]
    83  	P[3] = A*bx[3] + B*by[3] + C
    84  
    85  	var r = cubicRoots(P)
    86  
    87  	for i := 0; i < 3; i++ {
    88  		t := r[i]
    89  
    90  		point := &Point{
    91  			X: bx[0]*t*t*t + bx[1]*t*t + bx[2]*t + bx[3],
    92  			Y: by[0]*t*t*t + by[1]*t*t + by[2]*t + by[3],
    93  		}
    94  
    95  		var s float64
    96  		if (lx[1] - lx[0]) != 0 {
    97  			s = (point.X - lx[0]) / (lx[1] - lx[0])
    98  		} else {
    99  			s = (point.Y - ly[0]) / (ly[1] - ly[0])
   100  		}
   101  
   102  		tLT0 := PrecisionCompare(t, 0, PRECISION) < 0
   103  		tGT1 := PrecisionCompare(t, 1, PRECISION) > 0
   104  		sLT0 := PrecisionCompare(s, 0, PRECISION) < 0
   105  		sGT1 := PrecisionCompare(s, 1, PRECISION) > 0
   106  		if !(tLT0 || tGT1 || sLT0 || sGT1) {
   107  			out = append(out, point)
   108  		}
   109  	}
   110  
   111  	return out
   112  }
   113  
   114  // nolint
   115  func cubicRoots(P []float64) []float64 {
   116  	if PrecisionCompare(P[0], 0, PRECISION) == 0 {
   117  		if PrecisionCompare(P[1], 0, PRECISION) == 0 {
   118  			t := make([]float64, 3)
   119  			t[0] = -1 * (P[3] / P[2])
   120  			t[1] = -1
   121  			t[2] = -1
   122  
   123  			for i := 0; i < 1; i++ {
   124  				tiLT0 := PrecisionCompare(t[i], 0, PRECISION) < 0
   125  				tiGT1 := PrecisionCompare(t[i], 1, PRECISION) > 0
   126  				if tiLT0 || tiGT1 {
   127  					t[i] = -1
   128  				}
   129  			}
   130  
   131  			t = sortSpecial(t)
   132  			return t
   133  		}
   134  
   135  		var DQ = math.Pow(P[2], 2) - 4*P[1]*P[3]
   136  		if PrecisionCompare(DQ, 0, PRECISION) >= 0 {
   137  			DQ = math.Sqrt(DQ)
   138  			t := make([]float64, 3)
   139  			t[0] = -1 * ((DQ + P[2]) / (2 * P[1]))
   140  			t[1] = ((DQ - P[2]) / (2 * P[1]))
   141  			t[2] = -1
   142  
   143  			//lint:ignore SA4008 TODO this returns before looping?
   144  			for i := 0; i < 2; i++ {
   145  				tiLT0 := PrecisionCompare(t[i], 0, PRECISION) < 0
   146  				tiGT1 := PrecisionCompare(t[i], 1, PRECISION) > 0
   147  				if tiLT0 || tiGT1 {
   148  					t[i] = -1
   149  				}
   150  
   151  				t = sortSpecial(t)
   152  
   153  				//lint:ignore SA4004 TODO this always returns on the first iteration
   154  				return t
   155  			}
   156  		}
   157  	}
   158  
   159  	var a = P[0]
   160  	var b = P[1]
   161  	var c = P[2]
   162  	var d = P[3]
   163  
   164  	var A = b / a
   165  	var B = c / a
   166  	var C = d / a
   167  
   168  	var Q, R, D, Im float64
   169  
   170  	Q = (3*B - math.Pow(A, 2)) / 9
   171  	R = (9*A*B - 27*C - 2*math.Pow(A, 3)) / 54
   172  	D = math.Pow(Q, 3) + math.Pow(R, 2)
   173  
   174  	t := make([]float64, 3)
   175  
   176  	if PrecisionCompare(D, 0, PRECISION) >= 0 {
   177  		var S = sgn(R+math.Sqrt(D)) * math.Pow(math.Abs(R+math.Sqrt(D)), (1/3.0))
   178  		var T = sgn(R-math.Sqrt(D)) * math.Pow(math.Abs(R-math.Sqrt(D)), (1/3.0))
   179  
   180  		t[0] = -A/3 + (S + T)
   181  		t[1] = -A/3 - (S+T)/2
   182  		t[2] = -A/3 - (S+T)/2
   183  		Im = math.Abs(math.Sqrt(3) * (S - T) / 2)
   184  
   185  		if PrecisionCompare(Im, 0, PRECISION) != 0 {
   186  			t[1] = -1
   187  			t[2] = -1
   188  		}
   189  
   190  	} else {
   191  		var th = math.Acos(R / math.Sqrt(-math.Pow(Q, 3)))
   192  
   193  		t[0] = 2*math.Sqrt(-Q)*math.Cos(th/3) - A/3
   194  		t[1] = 2*math.Sqrt(-Q)*math.Cos((th+2*math.Pi)/3) - A/3
   195  		t[2] = 2*math.Sqrt(-Q)*math.Cos((th+4*math.Pi)/3) - A/3
   196  		Im = 0.0
   197  	}
   198  
   199  	for i := 0; i < 3; i++ {
   200  		tiLT0 := PrecisionCompare(t[i], 0, PRECISION) < 0
   201  		tiGT1 := PrecisionCompare(t[i], 1, PRECISION) > 0
   202  		if tiLT0 || tiGT1 {
   203  			t[i] = -1
   204  		}
   205  	}
   206  
   207  	t = sortSpecial(t)
   208  
   209  	return t
   210  }
   211  
   212  // nolint
   213  func sortSpecial(a []float64) []float64 {
   214  	var flip bool
   215  	var temp float64
   216  
   217  	for {
   218  		flip = false
   219  		for i := 0; i < len(a)-1; i++ {
   220  			ai1GTEQ0 := PrecisionCompare(a[i+1], 0, PRECISION) >= 0
   221  			aiGTai1 := PrecisionCompare(a[i], a[i+1], PRECISION) > 0
   222  			aiLT0 := PrecisionCompare(a[i], 0, PRECISION) < 0
   223  			if (ai1GTEQ0 && aiGTai1) || (aiLT0 && ai1GTEQ0) {
   224  				flip = true
   225  				temp = a[i]
   226  				a[i] = a[i+1]
   227  				a[i+1] = temp
   228  
   229  			}
   230  		}
   231  		if !flip {
   232  			break
   233  		}
   234  	}
   235  	return a
   236  }
   237  
   238  // nolint
   239  func sgn(x float64) float64 {
   240  	if x < 0.0 {
   241  		return -1
   242  	}
   243  	return 1
   244  }
   245  
   246  // nolint
   247  func bezierCoeffs(P0, P1, P2, P3 float64) []float64 {
   248  	Z := make([]float64, 4)
   249  	Z[0] = -P0 + 3*P1 + -3*P2 + P3
   250  	Z[1] = 3*P0 - 6*P1 + 3*P2
   251  	Z[2] = -3*P0 + 3*P1
   252  	Z[3] = P0
   253  	return Z
   254  }
   255
View as plain text