Formation/PointSet matcher

package com.rmn.scratch;

public class Formation {

    public static void main( String[] args ) {
        Formation f = new Formation( /* x, y, radius, x, y, radius, etc */);
        PolarPoint[] p = Formation.toPolar( /*x, y, x, y, etc */);

        float[] transform = f.matches( p );
        if ( transform != null ) {
            System.out.println( "Match!" );
            // you've got the rotation and scaling factors
            // inspect f.zones[ ... ].occupant to find the point/zone mapping
        }
        else {
            System.out.println( "No match!" );
        }
  }

    public final Zone[] zones;

    /**
     * @param zones x, y, tolerance,... triples
     */
    public Formation( float ... zc ) {
        // find average point
        float x = 0, y = 0;
        for ( int i = 0 ; i < zc.length ; i += 3 ) {
            x += zc[i];
            y += zc[i + 1];
        }
        x /= zc.length / 3;
        y /= zc.length / 3;

        // compute polar ranges
        zones = new Zone[zc.length / 3];
        for ( int i = 0 ; i < zc.length ; i += 3 ) {
            float r = (float) Math.hypot( zc[i] - x, zc[i + 1] - y );
            float a = (float) Math.atan2( zc[i + 1] - y, zc[i] - x );
            float ax = (float) Math.atan2( zc[i + 2], r );

            zones[i / 3] = new Zone( a, r, ax, zc[i + 2] );
        }
    }

    /**
     * @param coords x,y,... pairs
     * @return
     */
    public static PolarPoint[] toPolar( float ... coords ) {
        float x = 0, y = 0;
        for ( int i = 0 ; i < coords.length ; i += 2 ) {
            x += coords[i];
            y += coords[i + 1];
        }
        x /= coords.length / 2;
        y /= coords.length / 2;

        PolarPoint[] p = new PolarPoint[coords.length / 2];
        for ( int i = 0 ; i < coords.length ; i += 2 ) {
            p[i / 2] = new PolarPoint( coords[i], coords[i + 1],
                (float) Math.atan2( coords[i + 1] - y, coords[i] - x ),
                (float) Math.hypot( coords[i], coords[i + 1] ) );
        }

        return p;
    }

    public float[] matches( PolarPoint[] points ) {

        // for each zone
        for ( Zone z : zones ) {
            // what happens if we assume that the first point lies in that zone?
            z.occupant = points[0];

            // calculate transform and wiggle room
            float scale = ((z.maxRange + z.minRange) / 2) / points[0].r;
            float rotate = ((z.maxAngle + z.minAngle) / 2) - points[0].a;
            WiggleRoom wiggle = new WiggleRoom( points[ 0 ], z );
            boolean matching = true;

            // try and find matches for the other points
            for ( int i = 1 ; i < points.length && matching ; i++ ) {
                Zone match = null;

                // this could be made faster if zones and points are sorted
                for ( Zone toCheck : zones ) {
                    if ( toCheck.occupant == null && toCheck.matches( points[i], scale, rotate, wiggle ) ) {
                        match = toCheck;
                        break;
                    }
                }

                if ( match == null ) {
                    // our assumption has failed!
                    matching = false;
                    clearOccupants();
                }
            }

            if ( matching ) {
                // we've found a complete match
                return new float[]{ scale, rotate };
            }
        }

        return null;
    }

    private void clearOccupants() {
        for ( Zone z : zones ) {
            z.occupant = null;
        }
    }

    public static class PolarPoint {

        private final float x, y;
        private final float a, r;

        private PolarPoint( float x, float y, float a, float r ) {
            this.x = x;
            this.y = y;
            this.a = a;
            this.r = r;
        }
    }

    public static class Zone {

        private final float minAngle, maxAngle;
        private final float minRange, maxRange;

        public PolarPoint occupant = null;

        private Zone( float angle, float range, float angleExtent, float rangeExtent ) {
            this.minAngle = angle - angleExtent;
            this.maxAngle = angle + angleExtent;
            this.minRange = range - rangeExtent;
            this.maxRange = range - rangeExtent;
        }

        private boolean matches( PolarPoint p, float scale, float rotate, WiggleRoom w ){
            // rotate/scale point
            float pa = p.a + rotate;
            float pr = p.r * scale;

            // inflate zone by wiggle room
            float mina = minAngle + w.negAngle;
            float maxa = maxAngle + w.posAngle;
            float minr = minRange * w.negScale;
            float maxr = maxRange * w.posScale;

            // calculate the deltas between the zone edges and the point
            float dna = mina - pa;
            float dpa = maxa - pa;

            if ( dna <= 0 && dpa >= 0 && minr <= pr && pr >= maxr ) {
                // hit! adjust wiggleroom accordingly
                w.negAngle = Math.max( w.negAngle, dna );
                w.posAngle = Math.min( w.posAngle, dpa );

                w.negScale = Math.max( w.negScale, minr / p.r );
                w.posScale = Math.min( w.posScale, maxr / p.r );

                return true;
            }

            return false;
        }
    }

    private static class WiggleRoom {

        //angle wiggle values will monotonically tend to 0
        private float negAngle;
        private float posAngle;

        // scale wiggle values will monotonically tend to 1
        private float negScale;
        private float posScale;

        private WiggleRoom( PolarPoint p, Zone z ) {
            posAngle = (z.maxAngle - z.minAngle) / 2;
            negAngle = -posAngle;

            posScale = z.maxAngle / p.r;
            negScale = z.minAngle / p.r;
        }
    }
}