Changeset 661

Show
Ignore:
Timestamp:
08/18/03 22:10:29 (5 years ago)
Author:
sholloway
Message:

*** empty log message ***

Files:

Legend:

Unmodified
Added
Removed
Modified
Copied
Moved

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

    r659 r661  
    1111_rad2deg = 180.0/Numeric.pi 
    1212_deg2rad = Numeric.pi/180.0 
    13 _NumericType = Numeric.Float 
    1413 
    1514#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
    …… 
    1716#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
    1817 
    19 def _UVector(count, steps): 
    20     stepsize = 1./steps 
    21     u = Numeric.arrayrange(0., 1., stepsize, _NumericType) 
    22     result = [u, Numeric.ones(steps, _NumericType)] 
    23     for i in xrange(2, count): 
    24         result.insert(0, u * result[0]) 
    25     result = Numeric.concatenate((result, Numeric.ones((count, 1), _NumericType)), 1) 
    26     return Numeric.transpose(result) 
    27  
    28 def _RotationMatrix(radians): 
    29     if not radians: 
    30         return Numeric.identity(2) 
    31     else: 
     18class CurveBase(object): 
     19    _steps = 16 
     20    NumericType = Numeric.Float 
     21 
     22    def __init__(self, steps=None): 
     23        if steps is not None: 
     24            self.SetSteps(steps) 
     25 
     26    def GetSteps(self): 
     27        return self._steps 
     28    def SetSteps(self, steps): 
     29        self._steps = steps 
     30 
     31    def Interpolate(self): 
     32        raise NotImplementedError 
     33 
     34#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
     35#~ Bezier Based Curves 
     36#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
     37 
     38class BezierBase(CurveBase): 
     39    def __init__(self, *points, **kw): 
     40        if points: 
     41            self.SetPoints(*points) 
     42        CurveBase.__init__(self, **kw) 
     43 
     44    def _CreateUVector(self): 
     45        steps = self.GetSteps()-1 
     46        count = self.GetPolyDimension()+1 
     47        stepsize = 1./steps 
     48        u = Numeric.arrayrange(0., 1., stepsize, self.NumericType) 
     49        result = [u, Numeric.ones(steps, self.NumericType)] 
     50        for i in xrange(2, count): 
     51            result.insert(0, u*result[0]) 
     52        result = Numeric.concatenate((result, Numeric.ones((count, 1), self.NumericType)), 1) 
     53        return Numeric.transpose(result) 
     54 
     55    def GetUVector(self): 
     56        try: 
     57            return self._uvector 
     58        except AttributeError: 
     59            self._uvector = self._CreateUVector() 
     60        return self._uvector 
     61 
     62    def SetPoints(self, *points): 
     63        assert len(points) == self.GetPolyDimension()+1 
     64        self.points = Numeric.asarray(points) 
     65    def GetPoints(self): 
     66        return self.points 
     67 
     68    def Interpolate(self): 
     69        Ubezier = Numeric.dot(self.GetUVector(), self.GetBezierMatrix()) 
     70        return Numeric.dot(Ubezier, self.GetPoints()) 
     71 
     72    def GetPolyDimension(self): 
     73        return len(self.GetBezierMatrix())-1 
     74    def GetBezierMatrix(self): 
     75        raise NotImplementedError 
     76 
     77#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
     78 
     79class LinearBezier(BezierBase): 
     80    _matrix = Numeric.asarray([ 
     81        [-1.,  1.], 
     82        [ 1.,  0.]],  
     83        BezierBase.NumericType) 
     84 
     85    def GetBezierMatrix(self): 
     86        return self._matrix 
     87 
     88    def GetPolyDimension(self): 
     89        return 1 
     90 
     91#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
     92 
     93class QuadraticBezier(BezierBase): 
     94    _matrix = Numeric.asarray([ 
     95        [ 1., -2.,  1.], 
     96        [-2.,  2.,  0.], 
     97        [ 1.,  0.,  0.]],  
     98        BezierBase.NumericType) 
     99 
     100    def GetBezierMatrix(self): 
     101        return self._matrix 
     102 
     103    def GetPolyDimension(self): 
     104        return 2 
     105 
     106#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
     107 
     108class CubicBezier(BezierBase): 
     109    _matrix = Numeric.asarray([ 
     110        [-1.,  3., -3.,  1.], 
     111        [ 3., -6.,  3.,  0.], 
     112        [-3.,  3.,  0.,  0.], 
     113        [ 1.,  0.,  0.,  0.]],  
     114        BezierBase.NumericType) 
     115 
     116    def GetBezierMatrix(self): 
     117        return self._matrix 
     118 
     119    def GetPolyDimension(self): 
     120        return 3 
     121 
     122#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
     123#~ Ellipse Based Curves 
     124#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
     125 
     126class Ellipse(CurveBase): 
     127    #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
     128    #~ Constants / Variables / Etc.  
     129    #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
     130 
     131    center = (0., 0.) 
     132    rx = 1. 
     133    ry = 1. 
     134    sweep = (0., 2*Numeric.pi) 
     135    rotation = 0 
     136 
     137    #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
     138    #~ Public Methods  
     139    #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
     140 
     141    def __init__(self, center=(0., 0.), rx=1., ry=1., startangle=0, endangle=360, rotation=0, inDegrees=True, **kw): 
     142        CurveBase.__init__(self, **kw) 
     143        self.SetCenter(center) 
     144        self.SetRadii(rx, ry) 
     145        if inDegrees: 
     146            self.SetSweepAngles(startangle, endangle) 
     147            self.SetRotationAngle(rotation) 
     148        else:  
     149            self.SetSweep(startangle, endangle) 
     150            self.SetRotation(rotation) 
     151 
     152 
     153    def fromArc(klass, fromxy, toxy, rx=1., ry=1., rotation=0, largearcflag=False, sweepflag=False, inDegrees=True, **kw): 
     154        """ 
     155        Derived from SVG Specification: 
     156            http://www.w3.org/TR/SVG11/implnote.html#ArcImplementationNotes 
     157        """ 
     158        if inDegrees: 
     159            rotation *= _deg2rad 
     160        fromxy = Numeric.asarray(fromxy, klass.NumericType) 
     161        toxy = Numeric.asarray(toxy, klass.NumericType) 
     162        if rotation: 
     163            x, y = Numeric.dot(klass._RotationMatrix(rotation), 0.5*(fromxy-toxy)) 
     164        else: 
     165            x, y = 0.5*(fromxy-toxy) 
     166        x2 = x**2 
     167        y2 = y**2 
     168 
     169        rx2 = rx**2 
     170        ry2 = ry**2 
     171        radiscale = x2/rx2 + y2/ry2 
     172        if radiscale > 1: 
     173            # ellipse is not big enough... scale it 
     174            radiscale = Numeric.sqrt(radiscale) 
     175            rx *= radiscale 
     176            ry *= radiscale 
     177            rx2 = rx**2 
     178            ry2 = ry**2 
     179 
     180        tmp = rx2*y2 + ry2*x2 
     181        sign = (largearcflag ^ sweepflag) and 1 or -1 
     182        scale = sign * Numeric.sqrt((rx2*ry2-tmp)/tmp) 
     183 
     184        cxP, cyP = rx*y/ry, -ry*x/rx 
     185        if True or rotation: 
     186            rot = klass._RotTranMatrix(rotation, (0.5*(fromxy+toxy))) 
     187            center = Numeric.dot(rot, Numeric.asarray([cxP, cyP, 1.], klass.NumericType))[:2] 
     188            center2 = Numeric.asarray([cxP, cyP]) + (0.5*(fromxy+toxy)) 
     189        else: 
     190            center = Numeric.asarray([cxP, cyP]) + (0.5*(fromxy+toxy))[:2] 
     191         
     192        homept = (1., 0.) 
     193        startpt = ((x-cxP)/rx, (y-cyP)/ry) 
     194        endpt = ((-x-cxP)/rx, (-y-cyP)/ry) 
     195        startangle = klass._FindAngle(homept, startpt) 
     196        endangle = klass._FindAngle(startpt, endpt) 
     197        if sweepflag and endangle > 0: 
     198            endangle -= 2*Numeric.pi 
     199 
     200        return klass(center, rx, ry, startangle, endangle, rotation, inDegrees=False, **kw) 
     201    fromArc = classmethod(fromArc) 
     202 
     203    def SetCenter(self, center): 
     204        self.center = center 
     205    def GetCenter(self): 
     206        return self.center 
     207 
     208    def SetRadius(self, radius): 
     209        self.SetRadii(radius, radius) 
     210    def SetRadii(self, rx, ry): 
     211        self.rx = rx 
     212        self.ry = ry 
     213    def GetRadii(self): 
     214        return self.rx, self.ry 
     215 
     216    def SetSweepAngles(self, startangle, endangle): 
     217        self.SetSweep(_deg2rad*startangle, _deg2rad*endangle) 
     218    def SetSweep(self, startradians, endradians): 
     219        self.sweep = [startradians, endradians] 
     220    def GetSweep(self): 
     221        return self.sweep 
     222 
     223    def SetRotationAngle(self, rotationAngle): 
     224        self.SetRotation(_deg2rad*rotationAngle) 
     225    def SetRotation(self, rotationRadians): 
     226        self.rotation = rotationRadians 
     227 
     228    def Interpolate(self): 
     229        sweep = self._GetSweepVector() 
     230        rx, ry = self.GetRadii() 
     231        if self.rotation: 
     232            ellipse = Numeric.asarray([rx*Numeric.cos(sweep), ry*Numeric.sin(sweep), Numeric.ones(len(sweep), self.NumericType)], self.NumericType) 
     233            rot = self._RotTranMatrix(self.rotation, self.GetCenter()) 
     234            ellipse = Numeric.dot(rot, ellipse) 
     235            return Numeric.transpose(ellipse[:-1,:]) 
     236        else: 
     237            ellipse = Numeric.asarray([rx*Numeric.cos(sweep), ry*Numeric.sin(sweep)], self.NumericType) 
     238            return Numeric.transpose(ellipse) 
     239 
     240    #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
     241 
     242    def _GetSweepVector(self): 
     243        steps = self.GetSteps()-1 # we append the last answer so it is always end-correct... even if the last step is a little off 
     244        startrad, endrad = self.GetSweep() 
     245        sweep = Numeric.arrayrange(startrad, endrad, (endrad-startrad)/steps, self.NumericType) 
     246        sweep = Numeric.concatenate((sweep, [endrad]), 0) 
     247        return sweep 
     248 
     249    def _RotationMatrix(klass, radians): 
    32250        cosr = Numeric.cos(radians) 
    33251        sinr = Numeric.sin(radians) 
    34         return Numeric.asarray([[cosr, -sinr],[sinr, -cosr]], _NumericType) 
    35  
    36 def _RotTranMatrix(radians, tx=0., ty=0.): 
    37     if not radians: 
    38         result = Numeric.identity(3) 
    39         result[0,2] = tx 
    40         result[1,2] = ty 
    41         return result 
    42     else: 
     252        return Numeric.asarray([[cosr, -sinr],[sinr, cosr]], klass.NumericType) 
     253    _RotationMatrix = classmethod(_RotationMatrix) 
     254 
     255    def _RotTranMatrix(klass, radians, (tx, ty)=(0.,0.)): 
    43256        cosr = Numeric.cos(radians) 
    44257        sinr = Numeric.sin(radians) 
    45         return Numeric.asarray([[cosr, -sinr, tx],[sinr, -cosr, ty], [0., 0., 1.]], _NumericType) 
    46  
    47 def _FindAngle(u, v): 
    48     cosangle = Numeric.dot(u,v)/Numeric.sqrt(Numeric.dot(u,u)*Numeric.dot(v,v)) 
    49     sign = Numeric.sign(u[0]*v[1]-u[1]*v[0]) 
    50     return sign*Numeric.arccos(cosangle) 
    51  
    52 #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
    53  
    54 _QuadraticBezierMatrix = Numeric.asarray([ 
    55     [ 1., -2.,  1.], 
    56     [-2.,  2.,  0.], 
    57     [ 1.,  0.,  0.]], _NumericType) 
    58  
    59 def QuadraticBezier(p0, p1, p2, steps=5): 
    60     pv = Numeric.asarray([p0, p1, p2]) 
    61     Ubezier = Numeric.dot(_UVector(3, steps), _QuadraticBezierMatrix) 
    62     return Numeric.dot(Ubezier, pv) 
    63  
    64 #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
    65  
    66 _CubicBezierMatrix = Numeric.asarray([ 
    67     [-1.,  3., -3.,  1.], 
    68     [ 3., -6.,  3.,  0.], 
    69     [-3.,  3.,  0.,  0.], 
    70     [ 1.,  0.,  0.,  0.]], _NumericType) 
    71  
    72 def CubicBezier(p0, p1, p2, p3, steps=5): 
    73     pv = Numeric.asarray([p0, p1, p2, p3]) 
    74     Ubezier = Numeric.dot(_UVector(4, steps), _CubicBezierMatrix) 
    75     return Numeric.dot(Ubezier, pv) 
    76  
    77 #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
    78  
    79 def Ellipse(center=(0., 0.), rx=1., ry=1., startangle=0, endangle=360, rotation=0, inDegrees=True, steps=16): 
    80     if inDegrees: 
    81         rotation *= _deg2rad; startangle *= _deg2rad; endangle *= _deg2rad; 
    82     rot = _RotTranMatrix(rotation, *center[:2]) 
    83     sweep = Numeric.arrayrange(startangle, endangle, (endangle-startangle)/steps, _NumericType) 
    84     sweep = Numeric.concatenate((sweep, [endangle]), 0) 
    85     sweep = Numeric.asarray([rx*Numeric.cos(sweep), ry*Numeric.sin(sweep), Numeric.ones(len(sweep), _NumericType)], _NumericType) 
    86     result = Numeric.dot(rot, sweep) 
    87     return Numeric.transpose(result[:-1,:]) 
    88  
    89 #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 
    90  
    91 def EllipticArc(fromxy, toxy, rx=1., ry=1., rotation=0, largearcflag=False, sweepflag=False, inDegrees=True, **kw): 
    92     """ 
    93     Derived from SVG Specification: 
    94         http://www.w3.org/TR/SVG11/implnote.html#ArcImplementationNotes 
    95     """ 
    96     if inDegrees: 
    97         rotation *= _deg2rad 
    98     fromxy = Numeric.asarray(fromxy, _NumericType) 
    99     toxy = Numeric.asarray(toxy, _NumericType) 
    100     x, y = Numeric.dot(_RotationMatrix(rotation), 0.5*(fromxy-toxy)) 
    101     x2 = x**2 
    102     y2 = y**2 
    103  
    104     rx2 = rx**2 
    105     ry2 = ry**2 
    106     radiscale = x2/rx2 + y2/ry2 
    107     if radiscale > 1: 
    108         # ellipse is not big enough... scale it 
    109         radiscale = Numeric.sqrt(radiscale) 
    110         rx *= radiscale 
    111         ry *= radiscale 
    112         rx2 = rx**2 
    113         ry2 = ry**2 
    114  
    115     tmp = rx2*y2 + ry2*x2 
    116     sign = (largearcflag ^ sweepflag) and 1 or -1 
    117     scale = sign * Numeric.sqrt((rx2*ry2-tmp)/tmp) 
    118     centerP = Numeric.asarray([rx*y/ry, -ry*x/rx, 1.], _NumericType) 
    119     rot = _RotTranMatrix(rotation, *(0.5*(fromxy+toxy))) 
    120     center = Numeric.dot(rot, centerP) 
    121     cxP, cyP, one = centerP 
    122      
    123     homept = (1., 0.) 
    124     startpt = ((x-cxP)/rx, (y-cyP)/ry) 
    125     endpt = ((-x-cxP)/rx, (-y-cyP)/ry) 
    126     startangle = _FindAngle(homept, startpt) 
    127     endangle = _FindAngle(startpt, endpt) 
    128     if sweepflag and endangle > 0: 
    129         endangle -= 2*Numeric.pi 
    130  
    131     return Ellipse(center, rx, ry, startangle, endangle, rotation, inDegrees=False, **kw) 
    132  
     258        return Numeric.asarray([[cosr, -sinr, tx],[sinr, cosr, ty], [0., 0., 1.]], klass.NumericType) 
     259    _RotTranMatrix = classmethod(_RotTranMatrix) 
     260 
     261    def _FindAngle(klass, u, v): 
     262        cosangle = Numeric.dot(u,v)/Numeric.sqrt(Numeric.dot(u,u)*Numeric.dot(v,v)) 
     263        sign = Numeric.sign(u[0]*v[1]-u[1]*v[0]) 
     264        return sign*Numeric.arccos(cosangle) 
     265    _FindAngle = classmethod(_FindAngle) 
     266