R/intersectn.R

In geometry: Mesh Generation and Surface Tessellation

##' Compute convex hull of intersection of two sets of points
##' @param ps1 First set of points
##' @param ps2 Second set of points
##' @param tol Tolerance used to determine if a feasible point lies
##'   within the convex hulls of both points and to round off the
##'   points generated by the halfspace intersection, which sometimes
##'   produces points very close together.
##' @param return.chs If \code{TRUE} (default) return the convex hulls
##'   of the first and second sets of points, as well as the convex
##'   hull of the intersection.
##' @param options Options passed to \code{\link{halfspacen}}. By
##'   default this is \code{Tv}.
##' @param fp Coordinates of feasible point, i.e. a point known to lie
##'   in the hulls of \code{ps1} and \code{ps2}. The feasible point is
##'   required for \code{\link{halfspacen}} to find the intersection.
##'   \code{intersectn} tries to find the feasible point automatically
##'   using the linear program in \code{\link{feasible.point}}, but
##'   currently the linear program fails on some examples where there
##'   is an obvious solution. This option overrides the automatic
##'   search for a feasible point
##' @param autoscale \emph{Experimental in v0.4.2} Automatically scale
##'   the points to lie in a sensible numeric range. May help to
##'   correct some numerical issues.
##' @return List containing named elements: \code{ch1}, the convex
##'   hull of the first set of points, with volumes, areas and normals
##'   (see \code{\link{convhulln}}; \code{ch2}, the convex hull of the
##'   first set of points, with volumes, areas and normals; \code{ps},
##'   the intersection points of convex hulls \code{ch1} and
##'   \code{ch2}; and \code{ch}, the convex hull of the intersection
##'   points, with volumes, areas and normals.
##' @export
##' @examples
##' # Two overlapping boxes
##' ps1 <- rbox(0, C=0.5)
##' ps2 <- rbox(0, C=0.5) + 0.5
##' out <- intersectn(ps1, ps2)
##' message("Volume of 1st convex hull: ", out$ch1$vol)
##' message("Volume of 2nd convex hull: ", out$ch2$vol)
##' message("Volume of intersection convex hull: ", out$ch$vol)
##' @author David Sterratt
##' @note \code{intersectn} was introduced in geometry 0.4.0, and is
##'   still under development. It is worth checking results for
##'   unexpected behaviour.
##' @seealso \code{\link{convhulln}}, \code{\link{halfspacen}},
##'   \code{\link{inhulln}}, \code{\link{feasible.point}}
##' @importFrom utils packageDescription
intersectn <- function(ps1, ps2, tol=0, return.chs=TRUE, options="Tv",
                       fp=NULL, autoscale=FALSE) {
  distinct <-
    any(apply(ps1, 2, min) > apply(ps2, 2, max)) ||
    any(apply(ps1, 2, max) < apply(ps2, 2, min))
  
  if (distinct & !return.chs) {
    return(list(ch=list(vol=0)))
  }
  
  ch1 <- convhulln(ps1, "n FA")
  ch2 <- convhulln(ps2, "n FA")

  if (distinct) {
    return(list(ch1=ch1, ch2=ch2, ch=list(vol=0)))
  }

  ch1s <- ch1
  ch2s <- ch2
  if (autoscale) {
    pmean <- colMeans(rbind(ps1, ps2))
    ch1s <- convhulln(t(t(ps1) - pmean), "n FA")
    ch2s <- convhulln(t(t(ps2) - pmean), "n FA")
    if (!is.null(fp)) {
      fp <- fp - pmean
    }
  }
  
  ## Find feasible point in which points could overlap
  if (is.null(fp)) {
    fp <-tryCatch(feasible.point(ch1s, ch2s, tol=tol),
                  error=function(e){
                    stop("feasible.point() failed with error ",
                         e$message, "\n",
                         "If you can find a feasible point (i.e. point that lies in both hulls)\n",
                         "for this input, supply this with the \"fp\" option.\n",
                         "Otherwise, report bug, including inputs to intersectn() at\n",
                         utils::packageDescription("geometry", fields="BugReports"), "\n",
                         "or to ",
                         utils::packageDescription("geometry", fields="Maintainer"))
                  })
    if (all(is.na(fp))) {
      if (return.chs) {
        return(list(ch1=ch1, ch2=ch2, ch=list(vol=0)))
      }
      return(list(ch=list(vol=0)))
    }
  } else {
    ## fp supplied
    if (!is.numeric(fp)) {
      stop("fp should be numeric")
    }
    if (length(fp) != ncol(ps1)) {
      stop("fp should have same dimension as ps1 and ps2")
    }
  }
  
  ## Find intersections of halfspaces about feasible point. Catch error
  ## (code QH6023) when fixed point is not in intersection, due to
  ## precision issue.
  ps <- tryCatch(halfspacen(rbind(ch1s$normals, ch2s$normals), fp, options=options),
                 error=function(e) {
                   if (grepl("QH6023", e$message)) {
                     return(NA)
                   }
                   errmess <- e$message
                   if (!grepl("QJ", options)) {
                     errmess <- paste(errmess, "\nTry calling intersectn with options=\"Tv QJ\"")
                   }
                   stop(errmess)
                 })
  if (all(is.na(ps)) || is.null(ps)) {
    if (return.chs) {
      return(list(ch1=ch1, ch2=ch2, ch=list(vol=0)))
    }
    return(list(ch=list(vol=0)))
  }

  if (autoscale) {
    ps <- t(t(ps) + pmean)
  }
  
  ## Occasionally the halfspace creates points very close together. We
  ## can impose a tolerance to merge them
  if (tol != 0) {
    ps <- round(ps, ceiling(-log10(abs(tol/2))))
    ps <- ps[!duplicated(ps),]
  }
  ch <- convhulln(ps, "n FA")

  ## Check for gross volume errors
  if ((ch$vol > ch1$vol * (1 + 1E-4))) {
    warning("Volume of final intersection hull is bigger than first of the original hulls\n",
         "ch1 vol = ", ch1$vol, "\n",
         "ch vol = ", ch$vol, "\n",
         "Returning ch1")
    ch <- ch1
  }
  if ((ch$vol > ch2$vol * (1 + 1E-4))) {
    warning("Volume of final intersection hull is bigger than first of the original hulls\n",
         "ch2 vol = ", ch2$vol, "\n",
         "ch vol = ", ch$vol, "\n",
         "Returning ch2")
    ch <- ch2
  }
  
  
  if (return.chs) {
    out <- list(ch1=ch1, ch2=ch2, ps=ps, ch=ch)
    class(out) <- "intersectn"
    return(out)
  }
  return(list(ch=ch))
}

##' Find point in intersection of convex hulls
##'
##' Find point that lies somewhere in interesction of two convex
##' hulls. If such a point does not exist, return \code{NA}. The
##' feasible point is found using a linear program similar to the one
##' suggested at \url{../doc/qhull/html/qhalf.html#notes}
##' 
##' @param ch1 First convex hull with normals
##' @param ch2 Second convex hull with normals
##' @param tol The point must be at least this far within the facets
##'   of both convex hulls
##' @export
feasible.point <- function(ch1, ch2, tol=0) {
  N <- ncol(ch1$p)                      # Number of dimensions
  M <- nrow(ch1$normals) + nrow(ch2$normals) # Total number of normals
  
  ## Find the point that bounds all points from below. Becuase
  ## lpSolve::lp() implicitly gives solutions in the positive
  ## quadrant, we need to subtract p0 from the search point, and then
  ## add it to the optimised solution. This will ensure that solutions
  ## not in the positive quadrant are found.
  p0 <- apply(rbind(ch1$p, ch2$p), 2, min)
  objective.in <- c(rep(0, N), 1)
  const.mat <- rbind(cbind(ch1$normals[,-(N + 1)], 1),
                     cbind(ch2$normals[,-(N + 1)], 1),
                     c(rep(0, N), -1))

  ## p0 is incorporated into the matrix here
  const.rhs <- -c(const.mat[1:M, 1:N] %*% cbind(p0) +
                  c(ch1$normals[,N + 1], ch2$normals[,N + 1]),
                  tol)
  const.dir <- c(rep("<", length(const.rhs)))

  ## Scaling: The scale option of lpSolve::lp() determines options
  ## used for scaling, and is crucial to avoid errors in some edge
  ## cases. For list of options, see:
  ## http://lpsolve.sourceforge.net/5.1/set_scaling.htm
  ## http://lpsolve.sourceforge.net/5.1/scaling.htm
  ## See also https://github.com/davidcsterratt/geometry/issues/35
  ##
  ## After testing on 10,000+ examples, it appears that to get
  ## maxiumum coverage, in some cases multiple combinations of options
  ## need to be tried. This code cycles through options, starting with
  ## the combinations most likely to work.

  ## DYNUPDATE == 0 may also work, but DYNUPDATE == 1 may work better
  ## for some 4D examples
  DYNUPDATE <- 1
  ## Both options may help
  for (POWER2 in 0:1) {
    ## Both options may help
    for (QUADRATIC in 0:1) {
      ## 1 (SCALE_EXTREME) and 3 (SCALE_EXTREME) didn't work well in
      ## tests
      ##
      ## 4 (SCALE_GEOMETRIC) on one example (included in tests) caused
      ## some sort of infinite loop or race condition leading to the
      ## process up 100%
      ##
      ## 2 (SCALE_RANGE) and 7 (SCALE_CURTISREID) seem to work OK for
      ## most cases. SCALE_CURTISREID (with the 5.6.13 version of
      ## lpSolve, at least) has caused some sort of infinite loop or
      ## race condition leading to the process up 100% on 2 out of
      ## millions of intersections computed on some processors, hence
      ## it is put as the second option.
      for (MAIN in c(2, 7)) {
        ## Both options may help
        for (LOGARITHMIC in 0:1) {
          ## Equilibriate seemed to help for some cases, but caused a
          ## crash in the example of inst/extdata
          EQUILIBRIATE <- 0 
          scale <-
            MAIN +
            QUADRATIC    *   8 +
            LOGARITHMIC  *  16 +
            POWER2       *  32 +
            EQUILIBRIATE *  64 +
            DYNUPDATE    * 256
          opt <- lpSolve::lp(direction = "max",
                             objective.in,
                             const.mat,
                             const.dir,
                             const.rhs,
                             scale=scale)
          ## See http://lpsolve.sourceforge.net/5.5/solve.htm for status codes
          ## Infeasible solution
          if (opt$status == 2) return(NA)
          ## Optimal
          if (opt$status == 0) return(opt$solution[1:N] + p0)
        }
      }
    }
  }
  ## Debugging output
  if (!is.null(getOption("geometry.debug"))) {
    opt <- linprog::writeMps("feasible-point.mps",
                             cvec=objective.in,
                             bvec=const.rhs,
                             Amat=const.mat, "Feasible point")
    writeLines(gsub("_ ", "_", readLines("feasible-point-tmp.mps")), "feasible-point.mps")
    message("Debugging output saved to feasible-point.mps\n",
            "Test on command line by running\n",
            "  lp_solve -max -fmps feasible-point.mps")
  }
  stop("lpSolve::lp() returned error code ", opt$status, "\n",
       "See http://lpsolve.sourceforge.net/5.5/solve.htm for explanation of errors.\n",
       "Rescaling your problem may help")
}

##' @method plot intersectn
##' @export 
plot.intersectn <- function(x, y, ...) {
  args <- list(...)
  add <- FALSE
  if ("add" %in% names(args)) {
    add <- args$add
    args$add <- NULL
  }
  xlim <- ylim <- NULL
  if ("xlim" %in% names(args)) {
    xlim <- args$xlim
    args$xlim <- NULL
  }
  if ("ylim" %in% names(args)) {
    ylim <- args$ylim
    args$xlim <- NULL
  }
  if (ncol(x$p) == 2) {
    if (!add) {
      p <- rbind(x$ch1$p, x$ch2$p)
      if (is.null(xlim)) xlim <- range(p[,1])
      if (is.null(ylim)) ylim <- range(p[,2])
      plot.new()
      do.call(plot.window, c(list(xlim=xlim, ylim=ylim), args))
    }
    plot(x$ch1, add=TRUE, lty=2)
    plot(x$ch2, add=TRUE, lty=2)
    plot(x$ch, add=TRUE, lwd=2)
  }
}