intersection.py

# -*- coding: utf-8 -*-
"""
$Id$

Copyright 2008-2010 Lode Leroy

This file is part of PyCAM.

PyCAM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

PyCAM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with PyCAM.  If not, see <http://www.gnu.org/licenses/>.
"""


#import pycam.Geometry
from pycam.Utils.polynomials import poly4_roots
from pycam.Geometry.utils import INFINITE, sqrt, epsilon
from pycam.Geometry.Plane import Plane
from pycam.Geometry.Line import Line
from pycam.Geometry.PointUtils import *

def isNear(a, b):
    return abs(a - b) < epsilon

def isZero(a):
    return isNear(a, 0)

def intersect_lines(xl, zl, nxl, nzl, xm, zm, nxm, nzm):
    X = None
    Z = None
    try:
        if isZero(nzl) and isZero(nzm):
            pass
        elif isZero(nzl) or isZero(nxl):
            X = xl
            Z = zm + (xm - xl) * nxm / nzm
            return (X, Z)
        elif isZero(nzm) or isZero(nxm):
            X = xm
            Z = zl - (xm - xl) * nxl / nzl
            return (X, Z)
        else:
            X = (zl - zm +(xm * nxm / nzm - xl * nxl / nzl)) \
                    / (nxm / nzm - nxl / nzl)
            if X and xl < X and X < xm:
                Z = zl + (X -xl) * nxl / nzl
                return (X, Z)
    except ZeroDivisionError:
        pass
    return (None, None)

def intersect_cylinder_point(center, axis, radius, radiussq, direction, point):
    # take a plane along direction and axis
    n = pnormalized(pcross(direction, axis))
    # distance of the point to this plane
    d = pdot(n, point) - pdot(n, center)
    if abs(d) > radius - epsilon:
        return (None, None, INFINITE)
    # ccl is on cylinder
    d2 = sqrt(radiussq-d*d)
    ccl = padd( padd(center, pmul(n, d)), pmul(direction, d2))
    # take plane through ccl and axis
    plane = Plane(ccl, direction)
    # intersect point with plane
    (ccp, l) = plane.intersect_point(direction, point)
    return (ccp, point, -l)

def intersect_cylinder_line(center, axis, radius, radiussq, direction, edge):
    d = edge.dir
    # take a plane throught the line and along the cylinder axis (1)
    n = pcross(d, axis)
    if pnorm(n) == 0:
        # no contact point, but should check here if cylinder *always*
        # intersects line...
        return (None, None, INFINITE)
    n = pnormalized(n)
    # the contact line between the cylinder and this plane (1)
    # is where the surface normal is perpendicular to the plane
    # so line := ccl + \lambda * axis
    if pdot(n, direction) < 0:
        ccl = psub(center, pmul(n, radius))
    else:
        ccl = padd(center, pmul(n, radius))
    # now extrude the contact line along the direction, this is a plane (2)
    n2 = pcross(direction, axis)
    if pnorm(n2) == 0:
        # no contact point, but should check here if cylinder *always*
        # intersects line...
        return (None, None, INFINITE)
    n2 = pnormalized(n2)
    plane1 = Plane(ccl, n2)
    # intersect this plane with the line, this gives us the contact point
    (cp, l) = plane1.intersect_point(d, edge.p1)
    if not cp:
        return (None, None, INFINITE)
    # now take a plane through the contact line and perpendicular to the
    # direction (3)
    plane2 = Plane(ccl, direction)
    # the intersection of this plane (3) with the line through the contact point
    # gives us the cutter contact point
    (ccp, l) = plane2.intersect_point(direction, cp)
    cp = padd(ccp, pmul(direction, -l))
    return (ccp, cp, -l)

def intersect_circle_plane(center, radius, direction, triangle):
    # let n be the normal to the plane
    n = triangle.normal
    if pdot(n,direction) == 0:
        return (None, None, INFINITE)
    # project onto z=0
    n2 = (n[0], n[1], 0)
    if pnorm(n2) == 0:
        (cp, d) = triangle.plane.intersect_point(direction, center)
        ccp = psub(cp, pmul(direction, d))
        return (ccp, cp, d)
    n2 = pnormalized(n2)
    # the cutter contact point is on the circle, where the surface normal is n
    ccp = padd(center, pmul(n2, -radius))
    # intersect the plane with a line through the contact point
    (cp, d) = triangle.plane.intersect_point(direction, ccp)
    return (ccp, cp, d)

def intersect_circle_point(center, axis, radius, radiussq, direction, point):
    # take a plane through the base
    plane = Plane(center, axis)
    # intersect with line gives ccp
    (ccp, l) = plane.intersect_point(direction, point)
    # check if inside circle
    if ccp and (pnormsq(psub(center, ccp)) < radiussq - epsilon):
        return (ccp, point, -l)
    return (None, None, INFINITE)

def intersect_circle_line(center, axis, radius, radiussq, direction, edge):
    # make a plane by sliding the line along the direction (1)
    d = edge.dir
    if pdot(d, axis) == 0:
        if pdot(direction, axis) == 0:
            return (None, None, INFINITE)
        plane = Plane(center, axis)
        (p1, l) = plane.intersect_point(direction, edge.p1)
        (p2, l) = plane.intersect_point(direction, edge.p2)
        pc = Line(p1, p2).closest_point(center)
        d_sq = pnormsq(psub(pc, center))
        if d_sq >= radiussq:
            return (None, None, INFINITE)
        a = sqrt(radiussq - d_sq)
        d1 = pdot(psub(p1, pc), d)
        d2 = pdot(psub(p2, pc), d)
        ccp = None
        cp = None
        if abs(d1) < a - epsilon:
            ccp = p1
            cp = psub(p1, pmul(direction, l))
        elif abs(d2) < a - epsilon:
            ccp = p2
            cp = psub(p2, pmul(direction, l))
        elif ((d1 < -a + epsilon) and (d2 > a - epsilon)) \
                or ((d2 < -a + epsilon) and (d1 > a - epsilon)):
            ccp = pc
            cp = psub(pc, pmul(direction, l))
        return (ccp, cp, -l)
    n = pcross(d, direction)
    if pnorm(n)== 0:
        # no contact point, but should check here if circle *always* intersects
        # line...
        return (None, None, INFINITE)
    n = pnormalized(n)
    # take a plane through the base
    plane = Plane(center, axis)
    # intersect base with line
    (lp, l) = plane.intersect_point(d, edge.p1)
    if not lp:
        return (None, None, INFINITE)
    # intersection of 2 planes: lp + \lambda v
    v = pcross(axis, n)
    if pnorm(v) == 0:
        return (None, None, INFINITE)
    v = pnormalized(v)
    # take plane through intersection line and parallel to axis
    n2 = pcross(v, axis)
    if pnorm(n2) == 0:
        return (None, None, INFINITE)
    n2 = pnormalized(n2)
    # distance from center to this plane
    dist = pdot(n2, center) - pdot(n2, lp)
    distsq = dist * dist
    if distsq > radiussq - epsilon:
        return (None, None, INFINITE)
    # must be on circle
    dist2 = sqrt(radiussq - distsq)
    if pdot(d, axis) < 0:
        dist2 = -dist2
    ccp = psub(center, psub(pmul(n2, dist), pmul(v, dist2)))
    plane = Plane(edge.p1, pcross(pcross(d, direction), d))
    (cp, l) = plane.intersect_point(direction, ccp)
    return (ccp, cp, l)

def intersect_sphere_plane(center, radius, direction, triangle):
    # let n be the normal to the plane
    n = triangle.normal
    if pdot(n, direction) == 0:
        return (None, None, INFINITE)
    # the cutter contact point is on the sphere, where the surface normal is n
    if pdot(n, direction) < 0:
        ccp = psub(center, pmul(n, radius))
    else:
        ccp = padd(center, pmul(n, radius))
    # intersect the plane with a line through the contact point
    (cp, d) = triangle.plane.intersect_point(direction, ccp)
    return (ccp, cp, d)

def intersect_sphere_point(center, radius, radiussq, direction, point):
    # line equation
    # (1) x = p_0 + \lambda * d
    # sphere equation
    # (2) (x-x_0)^2 = R^2
    # (1) in (2) gives a quadratic in \lambda
    p0_x0 = psub(center, point)
    a = pnormsq(direction)
    b = 2 * pdot(p0_x0, direction)
    c = pnormsq(p0_x0) - radiussq
    d = b * b - 4 * a * c
    if d < 0:
        return (None, None, INFINITE)
    if a < 0:
        l = (-b + sqrt(d)) / (2 * a)
    else:
        l = (-b - sqrt(d)) / (2 * a)
    # cutter contact point
    ccp = padd(point, pmul(direction, -l))
    return (ccp, point, l)

def intersect_sphere_line(center, radius, radiussq, direction, edge):
    # make a plane by sliding the line along the direction (1)
    d = edge.dir
    n = pcross(n, direction)
    if pnorm(n) == 0:
        # no contact point, but should check here if sphere *always* intersects
        # line...
        return (None, None, INFINITE)
    n = pnormalized(n)

    # calculate the distance from the sphere center to the plane
    dist = - pdot(center, n) + pdot(edge.p1, n)
    if abs(dist) > radius - epsilon:
        return (None, None, INFINITE)
    # this gives us the intersection circle on the sphere

    # now take a plane through the edge and perpendicular to the direction (2)
    # find the center on the circle closest to this plane

    # which means the other component is perpendicular to this plane (2)
    n2 = pnormalized(pcross(n, d))

    # the contact point is on a big circle through the sphere...
    dist2 = sqrt(radiussq - dist * dist)

    # ... and it's on the plane (1)
    ccp = padd(center, padd(pmul(n, dist), pmul(n2, dist2)))

    # now intersect a line through this point with the plane (2)
    plane = Plane(edge.p1, n2)
    (cp, l) = plane.intersect_point(direction, ccp)
    return (ccp, cp, l)

def intersect_torus_plane(center, axis, majorradius, minorradius, direction,
        triangle):
    # take normal to the plane
    n = triangle.normal
    if pdot(n, direction) == 0:
        return (None, None, INFINITE)
    if pdot(n, axis) == 1:
        return (None, None, INFINITE)
    # find place on torus where surface normal is n
    b = pmul(n, -1)
    z = axis
    a = psub(b, pmul(z,pdot(z, b)))
    a_sq = pnormsq(a)
    if a_sq <= 0:
        return (None, None, INFINITE)
    a = pdiv(a, sqrt(a_sq))
    ccp = padd(padd(center, pmul(a, majorradius)), pmul(b, minorradius))
    # find intersection with plane
    (cp, l) = triangle.plane.intersect_point(direction, ccp)
    return (ccp, cp, l)

def intersect_torus_point(center, axis, majorradius, minorradius, majorradiussq,
        minorradiussq, direction, point):
    dist = 0
    if (direction[0] == 0) and (direction[1] == 0):
        # drop
        minlsq = (majorradius - minorradius) ** 2
        maxlsq = (majorradius + minorradius) ** 2
        l_sq = (point[0]-center[0]) ** 2 + (point[1] - center[1]) ** 2
        if (l_sq < minlsq + epsilon) or (l_sq > maxlsq - epsilon):
            return (None, None, INFINITE)
        l = sqrt(l_sq)
        z_sq = minorradiussq - (majorradius - l) ** 2
        if z_sq < 0:
            return (None, None, INFINITE)
        z = sqrt(z_sq)
        ccp = (point[0], point[1], center[2] - z)
        dist = ccp[2] - point[2]
    elif direction[2] == 0:
        # push
        z = point[2] - center[2]
        if abs(z) > minorradius - epsilon:
            return (None, None, INFINITE)
        l = majorradius + sqrt(minorradiussq - z * z)
        n = pcross(axis, direction)
        d = pdot(n, point) - pdot(n, center)
        if abs(d) > l - epsilon:
            return (None, None, INFINITE)
        a = sqrt(l * l - d * d)
        ccp = padd(padd(center, pmul(n, d)), pmul(direction, a))
        ccp = (ccp[0], ccp[1], point[2])
        dist = pdot(psub(point, ccp), direction)
    else:
        # general case
        x = psub(point, center)
        v = pmul(direction, -1)
        x_x = pdot(x, x)
        x_v = pdot(x, v)
        x1 = (x[0], x[1], 0)
        v1 = (v[0], v[1], 0)
        x1_x1 = pdot(x1, x1)
        x1_v1 = pdot(x1, v1)
        v1_v1 = pdot(v1, v1)
        R2 = majorradiussq
        r2 = minorradiussq
        a = 1.0
        b = 4 * x_v
        c = 2 * (x_x + 2 * x_v ** 2 + (R2 - r2) - 2 * R2 * v1_v1)
        d = 4 * (x_x * x_v + x_v * (R2 - r2) - 2 * R2 * x1_v1)
        e = (x_x) ** 2 + 2 * x_x * (R2 - r2) + (R2 - r2) ** 2 - 4 * R2 * x1_x1
        r = poly4_roots(a, b, c, d, e)
        if not r:
            return (None, None, INFINITE)
        else:
            l = min(r)
        ccp = padd(point, pmul(direction, -l))
        dist = l
    return (ccp, point, dist)