root/trunk/RBRapier/RBRapier/Tools/Geometry/Curves.py

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#~ Imports 
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

import math
import Numeric

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#~ Constants / Variables / Etc. 
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

_2pi = 2 * Numeric.pi

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#~ Definitions 
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

class CurveBase(object):
    _steps = 16
    NumericType = Numeric.Float

    def __init__(self, steps=None):
        if steps is not None:
            self.SetSteps(steps)

    def GetSteps(self):
        return self._steps
    def SetSteps(self, steps):
        if steps != self._steps:
            self._steps = steps
            self._Invalidate()

    def asCurve(self):
        raise NotImplementedError

    def __iter__(self):
        return iter(self.asCurve())

    def _Invalidate(self):
        pass

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#~ Bezier Based Curves
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

class BezierBase(CurveBase):
    def __init__(self, *points, **kw):
        if points:
            self.SetControlPoints(*points)
        CurveBase.__init__(self, **kw)

    def _CreateUVector(self):
        return self._BuildUVector(Numeric.arrayrange(0., 1., 1./(self.GetSteps()-1), self.NumericType), True)

    def _BuildUVector(self, u, addend=False):
        result = [u, Numeric.ones(len(u), self.NumericType)]
        count = self.GetPolyDimension()+1
        for i in xrange(2, count):
            result.insert(0, u*result[0])
        if addend:
            result = Numeric.concatenate((result, Numeric.ones((count, 1), self.NumericType)), 1)
        return Numeric.transpose(result)

    def GetUVector(self):
        result = getattr(self, '_uvector', ())
        if len(result) != self.GetSteps():
            result = self._uvector = self._CreateUVector()
        return result

    def SetControlPoints(self, *points):
        assert len(points) == self.GetPolyDimension()+1
        self.points = Numeric.asarray(points)
    def GetControlPoints(self):
        return self.points

    def CurveFromUVector(self, uvector):
        Ubezier = Numeric.dot(uvector, self.GetBezierMatrix())
        return Numeric.dot(Ubezier, self.GetControlPoints())

    def CurveAtPoints(self, *points):
        uvector = self._BuildUVector(Numeric.asarray(points, self.NumericType))
        return self.CurveFromUVector(uvector)

    def CurveRange(self, start=0., stop=1., stepsize=None):
        stepsize = stepsize or 1./(self.GetSteps()-1)
        uvector = self._BuildUVector(Numeric.arrayrange(start, stop, stepsize, self.NumericType))
        return self.CurveFromUVector(uvector)

    def asCurve(self):
        return self.CurveFromUVector(self.GetUVector())

    def GetPolyDimension(self):
        return len(self.GetBezierMatrix())-1
    def GetBezierMatrix(self):
        raise NotImplementedError

    def _Invalidate(self):
        try: delattr(self, '_uvector')
        except AttributeError: pass

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

class LinearBezier(BezierBase):
    _matrix = Numeric.asarray([
        [-1.,  1.],
        [ 1.,  0.]], 
        BezierBase.NumericType)

    def GetBezierMatrix(self):
        return self._matrix

    def GetPolyDimension(self):
        return 1

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

class QuadraticBezier(BezierBase):
    _matrix = Numeric.asarray([
        [ 1., -2.,  1.],
        [-2.,  2.,  0.],
        [ 1.,  0.,  0.]], 
        BezierBase.NumericType)

    def GetBezierMatrix(self):
        return self._matrix

    def GetPolyDimension(self):
        return 2

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

class CubicBezier(BezierBase):
    _matrix = Numeric.asarray([
        [-1.,  3., -3.,  1.],
        [ 3., -6.,  3.,  0.],
        [-3.,  3.,  0.,  0.],
        [ 1.,  0.,  0.,  0.]], 
        BezierBase.NumericType)

    def GetBezierMatrix(self):
        return self._matrix

    def GetPolyDimension(self):
        return 3

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
#~ Ellipse Based Curves
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

class Ellipse(CurveBase):
    #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    #~ Constants / Variables / Etc. 
    #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

    center = (0., 0.)
    rx = 1.
    ry = 1.
    sweep = (0., _2pi)
    xrotation = 0

    #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    #~ Public Methods 
    #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

    def __init__(self, center=(0., 0.), rx=1., ry=1., startangle=0, endangle=360, xrotation=0, inDegrees=True, **kw):
        CurveBase.__init__(self, **kw)
        self.SetCenter(center)
        self.SetRadii(rx, ry)
        if inDegrees:
            self.SetSweepAngles(startangle, endangle)
            self.SetXRotationAngle(xrotation)
        else: 
            self.SetSweep(startangle, endangle)
            self.SetXRotation(xrotation)


    def fromArc(klass, fromxy, toxy, rx=1., ry=1., xrotation=0, largeArcFlag=False, sweepFlag=False, inDegrees=True, **kw):
        """
        Derived from SVG Specification:
            http://www.w3.org/TR/SVG11/implnote.html#ArcImplementationNotes
        """
        if inDegrees:
            xrotation = math.radians(xrotation)
        fromxy = Numeric.asarray(fromxy, klass.NumericType)
        toxy = Numeric.asarray(toxy, klass.NumericType)
        if xrotation:
            x, y = Numeric.dot(klass._RotationMatrix(-xrotation), 0.5*(fromxy-toxy))
        else:
            x, y = 0.5*(fromxy-toxy)
        x2,y2 = x**2, y**2

        rx2, ry2 = rx**2, ry**2
        radiscale = x2/rx2 + y2/ry2
        if radiscale > 1:
            # ellipse is not big enough... scale it
            radiscale = Numeric.sqrt(radiscale)
            rx, ry = radiscale*rx, radiscale*ry
            rx2, ry2 = rx**2, ry**2

        tmp = rx2*y2 + ry2*x2
        radicand = (rx2*ry2-tmp)/tmp
        if radicand > 0:
            sign = (largeArcFlag != sweepFlag) and 1 or -1
            scale = sign * Numeric.sqrt(radicand)
            cxP, cyP = scale*y*rx/ry, scale*x*-ry/rx
        else:
            cxP, cyP = 0., 0.

        if xrotation:
            rot = klass._RotTranMatrix(xrotation, (0.5*(fromxy+toxy)))
            center = Numeric.dot(rot, Numeric.asarray([cxP, cyP, 1.], klass.NumericType))
        else:
            center = Numeric.asarray([cxP, cyP]) + (0.5*(fromxy+toxy))
        center = center[:2]
        
        startpt = ((x-cxP)/rx, (y-cyP)/ry)
        endpt = ((-x-cxP)/rx, (-y-cyP)/ry)
        startradians = klass._FindAngle((1., 0.), startpt)
        sweepradians = klass._FindAngle(startpt, endpt) % _2pi
        if sweepFlag:
            if sweepradians < 0:
                sweepradians += _2pi
        else:
            if sweepradians > 0:
                sweepradians -= _2pi

        endradians = startradians + sweepradians
        return klass(center, rx, ry, startradians, endradians, xrotation, inDegrees=False, **kw)
    fromArc = classmethod(fromArc)

    def SetCenter(self, center):
        self.center = center
    def GetCenter(self):
        return self.center

    def SetRadius(self, radius):
        self.SetRadii(radius, radius)
    def SetRadii(self, rx, ry):
        self.rx = rx
        self.ry = ry
    def GetRadii(self):
        return self.rx, self.ry

    def SetSweepAngles(self, startangle, endangle):
        self.SetSweep(math.radians(startangle), math.radians(endangle))
    def SetSweep(self, startradians, endradians):
        self.sweep = [startradians, endradians]
    def GetSweep(self):
        return self.sweep

    def SetXRotationAngle(self, xrotationAngle):
        self.SetXRotation(math.radians(xrotationAngle))
    def SetXRotation(self, xrotationRadians):
        self.xrotation = xrotationRadians
    def GetXRotation(self):
        return self.xrotation

    def asCurve(self):
        sweep = self._GetSweepVector()
        rx, ry = self.GetRadii()
        xrotation = self.GetXRotation()
        if xrotation:
            ellipse = Numeric.asarray([rx*Numeric.cos(sweep), ry*Numeric.sin(sweep), Numeric.ones(len(sweep), self.NumericType)], self.NumericType)
            rot = self._RotTranMatrix(xrotation, self.GetCenter())
            ellipse = Numeric.dot(rot, ellipse)
            return Numeric.transpose(ellipse[:-1,:])
        else:
            cx, cy = self.GetCenter()
            ellipse = Numeric.asarray([rx*Numeric.cos(sweep)+cx, ry*Numeric.sin(sweep)+cy], self.NumericType)
            return Numeric.transpose(ellipse)

    #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

    def _GetSweepVector(self):
        sweep = getattr(self, '_sweepvector', ())
        if len(sweep) != self.GetSteps():
            sweep = self._sweepvector = self._CreateSweepVector()
        return sweep

    def _CreateSweepVector(self):
        steps = self.GetSteps()-1 # we append the last answer so it is always end-correct... even if the last step is a little off
        startrad, endrad = self.GetSweep()
        sweep = Numeric.arrayrange(startrad, endrad, (endrad-startrad)/steps, self.NumericType)
        sweep = Numeric.concatenate((sweep, [endrad]), 0)
        return sweep

    def _RotationMatrix(klass, radians):
        """Count clockwise rotation"""
        cosr = Numeric.cos(radians)
        sinr = Numeric.sin(radians)
        return Numeric.asarray([[cosr, -sinr],[sinr, cosr]], klass.NumericType)
    _RotationMatrix = classmethod(_RotationMatrix)

    def _RotTranMatrix(klass, radians, (tx, ty)=(0.,0.)):
        """Count clockwise rotation"""
        cosr = Numeric.cos(radians)
        sinr = Numeric.sin(radians)
        return Numeric.asarray([[cosr, -sinr, tx],[sinr, cosr, ty], [0., 0., 1.]], klass.NumericType)
    _RotTranMatrix = classmethod(_RotTranMatrix)

    def _FindAngle(klass, u, v):
        cosangle = Numeric.dot(u,v)/Numeric.sqrt(Numeric.dot(u,u)*Numeric.dot(v,v))
        sign = Numeric.sign(u[0]*v[1]-u[1]*v[0]) or 1. # 0 is neither poistive or negative... but just make it positive for argument's sake...
        return sign*Numeric.arccos(cosangle)
    _FindAngle = classmethod(_FindAngle)

    def _Invalidate(self):
        try: delattr(self, '_sweepvector')
        except AttributeError: pass