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 #include <cstring>
  18 #include "Path.h"
  19
  20
  21 static float32 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   float32 mag = l.Normalize();
  28   float32 dist = b2Cross( w, l );
  29   if ( withinLine ) {
  30     float32 dot = b2Dot( l, w );
  31     *withinLine = ( dot >= 0.0f && dot <= mag );
  32   }
  33   return b2Abs( dist );
  34 }
  35
  36
  37 static float32 calcDistance( const Vec2& l1, const Vec2& l2 )
  38 {
  39   return b2Vec2(l1-l2).Length();
  40 }
  41
  42
  43 float32 Segment::distanceTo( const Vec2& p )
  44 {
  45   bool withinLine;
  46   float32 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 Path::Path( const char *s )
  60 {
  61   float32 x,y;
  62   while ( sscanf( s, "%f,%f", &x, &y )==2) {
  63     append( Vec2((int)x,(int)y) );
  64     while ( *s && *s!=' ' && *s!='\t' ) s++;
  65     while ( *s==' ' || *s=='\t' ) s++;
  66   }
  67 }
  68
  69 void Path::makeRelative()
  70 {
  71   for (int i=size()-1; i>=0; i--)
  72     at(i)-=at(0);
  73 }
  74
  75
  76 Path& Path::translate(const Vec2& xlate)
  77 {
  78   for (int i=0;i<size();i++)
  79     at(i) += xlate;
  80   return *this;
  81 }
  82
  83 Path& Path::rotate(const b2Mat22& rot)
  84 {
  85   float32 j1 = rot.col1.x;
  86   float32 k1 = rot.col1.y;
  87   float32 j2 = rot.col2.x;
  88   float32 k2 = rot.col2.y;
  89   Vec2 v;
  90
  91   for (int i=0;i<size();i++) {
  92     //at(i) = b2Mul( rot, at(i) );
  93     at(i) = Vec2( j1 * at(i).x + j2 * at(i).y,
  94                   k1 * at(i).x + k2 * at(i).y );
  95   }
  96   return *this;
  97 }
  98
  99 Path& Path::scale(float32 factor)
 100 {
 101   for (int i=0;i<size();i++) {
 102     at(i).x = at(i).x * factor;
 103     at(i).y = at(i).y * factor;
 104   }
 105   return *this;
 106 }
 107
 108 void Path::simplify( float32 threshold )
 109 {
 110   bool keepflags[size()];
 111   memset( &keepflags[0], 0, sizeof(keepflags) );
 112
 113   keepflags[0] = keepflags[size()-1] = true;
 114   simplifySub( 0, size()-1, threshold, &keepflags[0] );
 115
 116   int k=0;
 117   for ( int i=0; i<size(); i++ ) {
 118     if ( keepflags[i] ) {
 119       at(k++) = at(i);
 120     }
 121   }
 122   //printf("simplify %f %dpts to %dpts\n",threshold,size(),k);
 123   trim( size() - k );
 124
 125   // remove duplicate points (shouldn't be any)
 126   for ( int i=size()-1; i>0; i-- ) {
 127     if ( at(i) == at(i-1) ) {
 128       //printf("alert: duplicate pt!\n");
 129       erase( i );
 130     }
 131   }
 132 }
 133
 134 void Path::simplifySub( int first, int last, float32 threshold, bool* keepflags )
 135 {
 136   float32 furthestDist = threshold;
 137   int furthestIndex = 0;
 138   if ( last - first > 1 ) {
 139     Segment s( at(first), at(last) );
 140     for ( int i=first+1; i<last; i++ ) {
 141       float32 d = s.distanceTo( at(i) );
 142       if ( d > furthestDist ) {
 143         furthestDist = d;
 144         furthestIndex = i;
 145       }
 146     }
 147     if ( furthestIndex != 0 ) {
 148       keepflags[furthestIndex] = true;
 149       simplifySub( first, furthestIndex, threshold, keepflags );
 150       simplifySub( furthestIndex, last, threshold, keepflags );
 151     }
 152   }
 153 }
 154
 155 Rect Path::bbox() const
 156 {
 157   Rect r( at(0), at(0) );
 158   for ( int i=1; i<size(); i++ ) {
 159     r.expand( at(i) );
 160   }
 161   return r;
 162 }