Path.cpp

   1 /*
   2  * This file is part of NumptyPhysics
   3  * Copyright (C) 2008 Tim Edmonds
   4  *
   5  * This program is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU General Public License as
   7  * published by the Free Software Foundation; either version 3 of the
   8  * License, or (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful, but
  11  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13  * General Public License for more details.
  14  *
  15  */
  16
  17
  18 #include "Path.h"
  19
  20
  21 static float calcDistanceToLine( const Vec2& pt,
  22                                  const Vec2& l1, const Vec2& l2,
  23                                  bool* withinLine=NULL )
  24 {
  25   b2Vec2 l = l2 - l1;
  26   b2Vec2 w = pt - l1;
  27   float mag = l.Normalize();
  28   float dist = b2Cross( w, l );
  29   if ( withinLine ) {
  30     float dot = b2Dot( l, w );
  31     *withinLine = ( dot >= 0.0f && dot <= mag );
  32   }
  33   return b2Abs( dist );
  34 }
  35
  36
  37 static float calcDistance( const Vec2& l1, const Vec2& l2 )
  38 {
  39   return b2Vec2(l1-l2).Length();
  40 }
  41
  42
  43 float Segment::distanceTo( const Vec2& p )
  44 {
  45   bool withinLine;
  46   float d = calcDistanceToLine( p, m_p1, m_p2, &withinLine );
  47   if ( !(m_p1 == m_p2) && withinLine ) {
  48     return d;
  49   } else {
  50     return b2Min( calcDistance( p, m_p2 ), calcDistance( p, m_p1 ) );
  51   }
  52 }
  53
  54
  55 Path::Path() : Array<Vec2>() {}
  56
  57 Path::Path( int n, Vec2* p ) : Array<Vec2>(n, p) {}
  58
  59 void Path::makeRelative()
  60 {
  61   for (int i=size()-1; i>=0; i--)
  62     at(i)-=at(0);
  63 }
  64
  65
  66 Path& Path::translate(const Vec2& xlate)
  67 {
  68   for (int i=0;i<size();i++)
  69     at(i) += xlate;
  70   return *this;
  71 }
  72
  73 Path& Path::rotate(const b2Mat22& rot)
  74 {
  75   int j1 = FLOAT_TO_FIXED(rot.col1.x);
  76   int k1 = FLOAT_TO_FIXED(rot.col1.y);
  77   int j2 = FLOAT_TO_FIXED(rot.col2.x);
  78   int k2 = FLOAT_TO_FIXED(rot.col2.y);
  79   Vec2 v;
  80
  81   for (int i=0;i<size();i++) {
  82     //at(i) = b2Mul( rot, at(i) );
  83     at(i) = Vec2( FIXED_TO_INT(j1 * at(i).x + j2 * at(i).y),
  84                   FIXED_TO_INT(k1 * at(i).x + k2 * at(i).y) );
  85   }
  86   return *this;
  87 }
  88
  89 Path& Path::scale(float factor)
  90 {
  91   int f = FLOAT_TO_FIXED(factor);
  92   for (int i=0;i<size();i++) {
  93     at(i).x = FIXED_TO_INT(at(i).x * f);
  94     at(i).y = FIXED_TO_INT(at(i).y * f);
  95   }
  96   return *this;
  97 }
  98
  99 void Path::simplify( float threshold )
 100 {
 101   bool keepflags[size()];
 102   memset( &keepflags[0], 0, sizeof(keepflags) );
 103
 104   keepflags[0] = keepflags[size()-1] = true;
 105   simplifySub( 0, size()-1, threshold, &keepflags[0] );
 106
 107   int k=0;
 108   for ( int i=0; i<size(); i++ ) {
 109     if ( keepflags[i] ) {
 110       at(k++) = at(i);
 111     }
 112   }
 113   //printf("simplify %f %dpts to %dpts\n",threshold,size(),k);
 114   trim( size() - k );
 115
 116   // remove duplicate points (shouldn't be any)
 117   for ( int i=size()-1; i>0; i-- ) {
 118     if ( at(i) == at(i-1) ) {
 119       //printf("alert: duplicate pt!\n");
 120       erase( i );
 121     }
 122   }
 123 }
 124
 125 void Path::simplifySub( int first, int last, float threshold, bool* keepflags )
 126 {
 127   float furthestDist = threshold;
 128   int furthestIndex = 0;
 129   if ( last - first > 1 ) {
 130     Segment s( at(first), at(last) );
 131     for ( int i=first+1; i<last; i++ ) {
 132       float d = s.distanceTo( at(i) );
 133       if ( d > furthestDist ) {
 134         furthestDist = d;
 135         furthestIndex = i;
 136       }
 137     }
 138     if ( furthestIndex != 0 ) {
 139       keepflags[furthestIndex] = true;
 140       simplifySub( first, furthestIndex, threshold, keepflags );
 141       simplifySub( furthestIndex, last, threshold, keepflags );
 142     }
 143   }
 144 }
 145
 146 Rect Path::bbox() const
 147 {
 148   Rect r( at(0), at(0) );
 149   for ( int i=1; i<size(); i++ ) {
 150     r.expand( at(i) );
 151   }
 152   return r;
 153 }