// Formulae for geometry.

// Find the length of the hypotenuse of a right triangle given sides a and b.
hypotenuseLength[a, b] := sqrt[a^2 + b^2]

// Calculate area of arbitrary triangle given 3 sides of known length.
// Calculate using Heron's formula.
triangleArea[a, b, c] :=
{
   // Semiperimeter s
   s = 1/2 (a + b + c)

   area = sqrt[s (s-a) (s-b) (s-c)]

   return area
}


// Find the distance between a point (x0,y0) and a line specified by
// two points (x1, y1), (x2, y2)
pointToLineDistance[x0, y0, x1, y1, x2, y2] :=
{
   abs[ (x2-x1)(y1-y0) - (x1-x0)(y2-y1)] / sqrt[(x2-x1)^2+(y2-y1)^2 ]
}

// Find the intersection between two lines specified by two points on each
// line.  The first line is specified by the points
// (x1, y1) and (x2, y2)
// and the second line is specified by the points
// (x3, y3) and (x4, y4)
//
//  This returns the intersection point as [x, y] or undef if the lines
// are paralllel.
lineIntersection[x1, y1, x2, y2, x3, y3, x4, y4] :=
{
   det = (x1 - x2)(y3 - y4) - (y1 - y2)(x3 - x4)
   if det == 0
      return undef

   t1 = (x1 y2 - y1 x2)
   t2 = (x3 y4 - y3 x4)
   px = (t1 (x3 - x4) - t2 (x1 - x2)) / det
   py = (t1 (y3 - y4) - t2 (y1 - y2)) / det
   return [px, py]
}

// Find the intersection between a plane and a line.
// The plane is defined by three points 1,2,3 and the line is defined
// by two points 4,5.  Each point's coordinates are given by (x1,y1,z1),
// (x2,y2,z2)...
// See:
// https://mathworld.wolfram.com/Line-PlaneIntersection.html
//
// returns [x,y,z,t] of which [x,y,z] is the intersection point (if one
// exists) and t is a parameter indicating where on the parameterized line
// equation it intersects with the plane:
//
//   x(t) = x4 + (x5-x4) t
//   y(t) = y4 + (y5-y4) t
//   z(t) = z4 + (z5-z4) t
planeLineIntersection[x1,y1,z1, x2,y2,z2, x3,y3,z3, x4,y4,z4, x5,y5,z5] :=
{
   num = x3 y2 z1 - x4 y2 z1 - x2 y3 z1 + x4 y3 z1 + x2 y4 z1 - x3 y4 z1 - x3 y1 z2 + x4 y1 z2 + x1 y3 z2 - x4 y3 z2 - x1 y4 z2 + x3 y4 z2 + x2 y1 z3 - x4 y1 z3 - x1 y2 z3 + x4 y2 z3 + x1 y4 z3 - x2 y4 z3 - x2 y1 z4 + x3 y1 z4 + x1 y2 z4 - x3 y2 z4 - x1 y3 z4 + x2 y3 z4
   
   det = (z5 - z4) (x1 y2 - x1 y3 - x2 y1 + x2 y3 + x3 y1 - x3 y2) - (y5 - y4) (x1 z2 - x1 z3 - x2 z1 + x2 z3 + x3 z1 - x3 z2) + (x5 - x4) (y1 z2 - y1 z3 - y2 z1 + y2 z3 + y3 z1 - y3 z2)

//   println["num=$num, det=$det"]
   if det == 0
      if num == 0  // Line is coplanar with plane (embedded in plane,
                   // infinite solutions along entire line)
         return [undef, undef, undef, 0]
      else
         return [undef, undef, undef, undef] // Does not intersect, no solutions
   
   t = - num/det

   x = x4 + (x5-x4) t
   y = y4 + (y5-y4) t
   z = z4 + (z5-z4) t

   return [x,y,z,t]
}


// Returns true if a point is inside a polygon.
// Adapted from
// http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
//
// points is an array of two-dimensional arrays of points.
// x,y is the point to test.
//
// Returns:
//   true if [x,y] is inside the polygon, false otherwise.
pointInPolygon[points, x, y] :=
{
   c = false
   i = 0
   npol = length[points]
   j = npol-1
   while i < npol
   {
      xi = points@i@0
      xj = points@j@0
      yi = points@i@1
      yj = points@j@1
      if ((((yi<=y) and (y<yj)) or ((yj<=y) and (y<yi))) and (x < (xj - xi) * (y - yi) / (yj - yi) + xi))
         c = !c
      j = i
      i = i + 1
   }

   return c;
}

// Calculates the area of a two-dimensional polygon.
//
// points is an array of [x,y] coordinates
//
// Returns: [cx, cy]
//
//  See:  http://en.wikipedia.org/wiki/Polygon#Area_and_centroid
//
//  The Frink polygon classes now implement area and centroid calculations
// using this code.
polygonArea[points] :=
{
   size = length[points]
   selfClosed = first[points] == last[points]

   if selfClosed
      n = size - 1
   else
      n = size

   if size == 0
      return 0

   if size <= 2
      return 0 * points@0@0 * points@0@1

   // Maintain units of measure.
   xs = 0 * points@0@0
   ys = 0 * points@0@1

   ps = xs ys
   xs = xs * ps
   ys = ys * ps
   
   for i = 0 to n-1
   {
      [xi, yi] = points@i

      ii = i+1
      if ii == n and !selfClosed  // Make last point first point
         ii = 0

      [xi1, yi1] = points@ii
      pp = (xi yi1 - xi1 yi)
      ps = ps + pp
   }

   A = 1/2 ps
   return abs[A]
}

// Calculates the centroid of a two-dimensional polygon.
//
// points is an array of [x,y] coordinates
//
// Returns: [cx, cy]
//
//  See:  http://en.wikipedia.org/wiki/Polygon#Area_and_centroid
polygonCentroid[points] :=
{
   size = length[points]
   selfClosed = first[points] == last[points]

   if selfClosed
      n = size - 1
   else
      n = size

   if size == 0
      return undef

   if size == 1
      return points@0

   // Maintain units of measure.
   xs = 0 * points@0@0
   ys = 0 * points@0@1

   ps = xs ys
   xs = xs * ps
   ys = ys * ps
   
   for i = 0 to n-1
   {
      [xi, yi] = points@i

      ii = i+1
      if ii == n and !selfClosed  // Make last point first point
         ii = 0

      [xi1, yi1] = points@ii

      pp = (xi yi1 - xi1 yi)
      xs = xs + (xi + xi1) pp
      ys = ys + (yi + yi1) pp
      ps = ps + pp
   }

   A = 1/2 ps
   cx = xs/(6 A)
   cy = ys/(6 A)

   return[cx, cy]
}

// Rotate the point [x,y] around the point [cx,cy] by the specified angle
// and return the coordinates of the new point, [x2, y2]
rotateAroundPoint[x,y, cx, cy, angle] :=
{
   ca = cos[angle]
   sa = sin[-angle]
   dx = x-cx
   dy = y-cy
   return [cx + ca dx - sa dy, cy + sa dx + ca dy]
}

// Finds the intersection[s] between a line and a circle given two points on
// the line [x1, y1], [x2, y2], the center of the circle [cx, cy],
// and the radius of the circle.
//
//  Returns:
//   an array, each element containing an array with [x,y] coordinates for
//  each intersection point.  There can 
//  be 0, 1, or 2 points of intersection.  For example, this may return
//  something like  [[1,1], [2,1]]
//
// See:
// http://mathworld.wolfram.com/Circle-LineIntersection.html
circleLineIntersections[x1, y1, x2, y2, cx, cy, r] :=
{
   // First shift coordinates so the center of the circle is at 0,0
   x1a = x1 - cx
   y1a = y1 - cy
   x2a = x2 - cx
   y2a = y2 - cy

   dx = x2 - x1
   dy = y2 - y1
   dr2 = dx^2 + dy^2
   D = x1a y2a - x2a y1a
   delta = r^2 dr2 - D^2

   // If delta < 0 there is no intersection.
   if delta < 0
      return new array

   sgndy = dy < 0 ? -1 : 1
   sqrtDelta = sqrt[delta]

   Ddy = D dy
   Ddx = - D dx
   
   // if delta == 0 there is only one point of intersecion
   if delta == 0
      return [[Ddy / dr2 + cx, Ddx / dr2 + cy]]
   
   rightx = sgndy dx sqrtDelta
   righty = abs[dy] sqrtDelta

   return [[(Ddy + rightx) / dr2 + cx, (Ddx + righty) / dr2 + cy],
           [(Ddy - rightx) / dr2 + cx, (Ddx - righty) / dr2 + cy]]
}


/** This performs an arbitrary affine transformation on the point [x,y]
    using the affine coordinates a,b,c,d,e,f.

    where the transformation matrix is defined by:

    x'    a c e     x
    y' =  b d f  *  y
    1     0 0 1     1

    (This is the order of the coordinates in the SVG and HTML5 standards.)

    which gives the equation:

    x' = a x + c y + e
    y' = b x + d y + f

    returns:  [x', y']
*/
affineTransform[x, y, a, b, c, d, e, f] := [a x + c y + e, b x + d y + f]


/** Performs an affineTransform with the coefficients specified as a list. */
affineTransform[x, y, coeffs] :=
{
   [a,b,c,d,e,f] = coeffs
   return affineTransform[x, y, a, b, c, d, e, f]
}


/** This performs the inverse of the arbitrary affine transformation above,
    converting the point [x',y'] back into the original coordinates [x,y]
    using the same affine transform coefficients a,b,c,d,e,f as passed to
    the affineTransform function above.

    returns:  [x, y]
*/
inverseAffineTransform[xp, yp, a, b, c, d, e, f] :=
{
   determinant = a d - b c
   if determinant == 0
      return [undef, undef]

   x =   (c (f - yp) - d (e - xp)) / determinant
   y =  -(a (f - yp) - b (e - xp)) / determinant
   
   return [x, y]
}

/* This performs the inverse of the arbitrary affine transformation above,
    converting the point [x',y'] back into the original coordinates [x,y]
    using the same affine transform coefficients coeffs=[a,b,c,d,e,f] as
    passed to the affineTransform function above.

    returns:  [x, y]
*/
inverseAffineTransform[xp, yp, coeffs] :=
{
   [a,b,c,d,e,f] = coeffs
   return inverseAffineTransform[xp, yp, a, b, c, d, e, f]
}

/** This composes a set of transformation coefficients with another set of
    transformation coefficients.  The inputs should be an array of 6 elements
    containing the coefficients [a,b,c,d,e,f] as described in the documentation
    for the affineTransformation function above.

    Returns a vector [a,b,c,d,e,f] of the
    new coefficients. */
composeTransformation[coeffsA, coeffsB] :=
{
   [a1, b1, c1, d1, e1, f1] = coeffsA
   [a2, b2, c2, d2, e2, f2] = coeffsB
   return composeTransformation[a1, b1, c1, d1, e1, f1, a2, b2, c2, d2, e2, f2]
}

/** This composes a set of transformation coefficients with another set of
    transformation coefficients.  Returns a vector [a,b,c,d,e,f] of the
    new coefficients.  This is just an unrolled, optimized matrix
    multiplication, eliminating always-zero terms. */
composeTransformation[a1, b1, c1, d1, e1, f1, a2, b2, c2, d2, e2, f2] :=
{
   return [a1 a2 + b2 c1,        // a
           a2 b1 + b2 d1,        // b
           a1 c2 + c1 d2,        // c
           b1 c2 + d1 d2,        // d
           a1 e2 + c1 f2 + e1,   // e
           b1 e2 + d1 f2 + f1]   // f
}

/** Makes a set of transformation coefficients that correspond to a translation
    by dx, dy.

    This corresponds to the matrix:

    x'    a=1  c=0  e=dx     x
    y' =  b=0  d=1  f=dy  *  y
    1       0    0    1      1

    which gives the equation:

    x' = x + dx
    y' = y + dy
*/
makeTranslate[dx, dy] :=
{
   return [1,   // a
           0,   // b
           0,   // c
           1,   // d
           dx,  // e
           dy]  // f
}


/** Create a scaling transformation that scales the coordinate system
    by sx, sy around 0,0.  This corresponds to the matrix:

    x'    a=sx  c=0   e=0     x
    y' =  b=0   d=sy  f=0  *  y
    1       0     0     1     1

    which gives the equation:

    x' = x sx
    y' = y sy
*/
makeScale[sx, sy] :=
{
   return [sx,  // a
           0,   // b
           0,   // c
           sy,  // d
           0,   // e
           0]   // f
}


/** Create a rotating transformation that rotates the coordinate system
    around 0,0 by the angle theta.  This corresponds to the matrix:

    x'    a=cos[theta]  c=-sin[theta]   e=0     x
    y' =  b=sin[theta]  d= cos[theta]   f=0  *  y
    1       0              0              1     1

    which gives the equations:

    x' = x cos[theta] - y sin[theta]
    y' = x sin[theta] + y cos[theta]
*/
makeRotate[theta] :=
{
   c = cos[theta]
   s = sin[theta]
   return [c,   // a
           s,   // b
           -s,  // c
           c,   // d
           0,   // e
           0]   // f
}

/** Create a rotating transformation that rotates the coordinate system
    around cx, cy by the angle theta.  This is equivalent to:

    translate[cx,cy]
    rotate[theta]
    translate[-cx, -cy]

    This corresponds to the matrix:

x'    a=cos[theta]  c=-sin[theta]  e=cx (1 - cos[theta]) + cy sin[theta]     x
y' =  b=sin[theta]  d= cos[theta]  f=cy (1 - cos[theta]) - cx sin[theta] *   y 
1       0              0             1                                       1

    which gives the equations:

    x' = (x - cx) cos[theta] - (y - cy) sin[theta] + cx
    y' = (x - cx) sin[theta] + (y - cy) cos[theta] + cy
*/
makeRotateAround[cx, cy, theta] :=
{
   transform = makeTranslate[cx, cy]
   transform = composeTransformation[transform, makeRotate[theta]]
   transform = composeTransformation[transform, makeTranslate[-cx, -cy]]
   return transform
}

/** Create a scaling transformation that scales the coordinate system
    around cx, cy by the scales sx, sy.  This is equivalent to:

    translate[cx, cy]
    scale[sx, sy]
    translate[-cx, -cy]

    This corresponds to the matrix:

    x'    a=sx   c=0    e=cx (1-sx)        x
    y' =  b=0    d=sy   f=cy (1-sy)    *   y 
    1       0      0      1                1

    which gives the equations:

    x' = sx (x-cx) + cx
    y' = sy (y-cy) + cy

   or

   x' = sx x + cx (1 - sx)
   y' = sy y + cy (1 - sy)
*/
makeScaleAround[cx, cy, sx, sy] :=
{
   transform = makeTranslate[cx, cy]
   transform = composeTransformation[transform, makeScale[sx, sy]]
   transform = composeTransformation[transform, makeTranslate[-cx, -cy]]
   return transform
}

/** Make an arbitrary transform with the specified coordinates. */
makeArbitrary[a, b, c, d, e, f] :=
{
   return [a, b, c, d, e, f]
}

/** Creates the inverse transform of a general transform.  For this to be
    valid, the determinant (a d - b c) must be nonzero. */
makeInverse[a, b, c, d, e, f] :=
{
   det = 1 / (a d - b c)
   return [ d det,
           -b det,
           -c det,
            a det,
           (c f - d e) det,
           (b e - a f) det]
}

/** Creates the inverse transform of a general transform.  For this to be
    valid, the determinant (a d - b c) must be nonzero. */
makeInverse[array] :=
{
   [a,b,c,d,e,f] = array
   makeInverse[a,b,c,d,e,f]
}

/** Calculates the cross product of two vectors in 3D space and returns the
    vector cross product.  This does *NOT* normalize the resultant vector.

    The physical meaning of the cross product is to find a third vector which
    is perpendicular to both of the other two vectors.

   args:
    V and W are 3-d vectors represented by arrays of [x, y, z] components.

   returns:
     A non-normalized vector of [x, y, z] components of the cross-product.
*/
crossProduct[V, W] :=
{
   Nx = (V@1 W@2) - (V@2 W@1)
   Ny = (V@2 W@0) - (V@0 W@2)
   Nz = (V@0 W@1) - (V@1 W@0)
   return [Nx, Ny, Nz]
}

/** Calculates the cross product of three points in 3D space (as listed in
    counterclockwise order as seen from the "outside") and returns the vector
    cross product.  The resulting vector points to the "outside" of the object.
    This does *NOT* normalize the resultant vector.

    The physical meaning of the cross product is to find a vector which is
    perpendicular to the plane defined by the three points.

   args:
    p1, p2, p3 are 3-d points represented by arrays of [x, y, z] components.

   returns:
     A non-normalized vector of [x, y, z] components of the cross-product.
*/
crossProduct[p1, p2, p3] :=
{
   V = [p2@0 - p1@0, p2@1 - p1@1, p2@2 - p1@2]
   W = [p3@0 - p1@0, p3@1 - p1@1, p3@2 - p1@2]
   return crossProduct[V,W]
}

/** Normalize a vector so that its length is 1 but points in the same
    direction as the original vector.   This works on any dimension of vector
    which is passed in as an array of components [x, y, z, ...]
*/
normalize[V] :=
{
   sqr = {|x| x^2}
   L = sqrt[sum[mapList[sqr, V]]]

   normalize = {|x, data| x / data}
   return mapList[normalize, V, L]
}

/** Returns the normal of the 3 points of a triangle.  The vertices of the
    triangle should be listed in counterclockwise order as seen from the
    "outside" of the object.  The vector points to the outsite.
    The result is normalized to be of length 1. */
normal[p1, p2, p3] :=
{
   V = [p2@0 - p1@0, p2@1 - p1@1, p2@2 - p1@2]
   W = [p3@0 - p1@0, p3@1 - p1@1, p3@2 - p1@2]
   return normalize[crossProduct[V,W]]
}

/** Returns the dot product of two vectors.  This works on vectors of arbitrary
    number of dimensions, each represented as an array.  The result is a single
    scalar value.

    The physical meaning of the dot product is to measure how closely the two
    vectors point in the same direction.  For this, the vectors should first
    be normalized to have length 1.  The dot product will range from 1 for two
    vectors which point in the same direction to 0 for two perpendicular
    vectors to -1 for vectors which point in opposite directions.

    To obtain the angle between two normalized vectors, use:
    angle = arccos[dotProduct[v1, v2]]
*/
dotProduct[v1, v2] :=
{
   mult = {|c1,c2| c1 * c2}
   return sum[map[mult, zip[v1, v2]]]
}

/** Returns the coefficients A,B,C,D of a plane in the equation
    A x + B y + C z + D = 0
    passing through 3 3-dimensional points
    p1, p2, p3
    (listed in the counterclockwise direction as seen from the "outside" of
    the object.)

    This can be used to test a point relative to any plane:

    Ax + By + Cz + D < 0   ==> inside
    Ax + By + Cz + D > 0   ==> outside

    The normal to the plane (pointing outward) is [A,B,C].

    This does not test that the three points are non-colinear.
*/
planeCoefficients[p1, p2 ,p3] :=
{
   [A, B, C] = crossProduct[p1, p2, p3]
   D = (-A * p1@0) + (-B * p1@1) + (-C * p1@2)
   return [A, B, C, D]
}


"Geometry.frink included OK"