Проблемы с коррекцией движения объекта

Хочу сделать что-то типа джойстика на андроиде. Есть квадратный тип и круглый тип. Квадратный работает идеально,чего нельзя сказать о круглом. Там вообще какая-то вакханалия творится...не пойму в чем может быть проблема..вроде всё верно сделал...(п.с.основная цель кода - ограничить движения внутреннего квадрата по внешнему...в коде описал где именно идет код ограничителя и какой из них не работает). Продемонстрирована проблема тут:

public class Joystick extends FrameLayout {
    int w,h,w1,h1;
    int type;
    boolean center = true;
    onDragStartListener listener1 = new onDragStartListener() {
        @Override
        public void onDragStart(Joystick joystick, float X, float Y) {
            Log.w("DEFAULT JOYSTICK LISTENER","Drag started at point "+X+" "+Y);
        }
    };
    onDragListener listener2 = new onDragListener() {
        @Override
        public void onDrag(Joystick joystick, float X, float Y) {
        }
    };
    onDragEndListener listener3 = new onDragEndListener() {
        @Override
        public void onDragEnd(Joystick joystick, float X, float Y) {
            Log.w("DEFAULT JOYSTICK LISTENER","Drag ended at point "+X+" "+Y);
        }
    };
    View joystickPoint;
    Joystick current;
    FrameLayout frame;
    float r;
    public Joystick(Context context,int _type) {
        super(context);
        this.type = _type;
        this.current = this;
        this.frame = this;
        frame.setBackgroundColor(Color.parseColor("#00FF00"));
        joystickPoint = new View(context);
        joystickPoint.setLayoutParams(new LayoutParams(100,100));
        w1 = 100;
        h1 = 100;
        joystickPoint.setBackgroundColor(Color.parseColor("#FF0000"));
        frame.addView(joystickPoint);
        joystickPoint.setX(frame.getX()+frame.getWidth()/2);
        joystickPoint.setY(frame.getY()+frame.getHeight()/2);
        if(type == Type.SQUARE) {
            joystickPoint.setOnTouchListener(new OnTouchListener() {
                PointF DownPT = new PointF();
                PointF StartPT = new PointF();
                @Override
                public boolean onTouch(View v, MotionEvent event) {
                    int eid = event.getAction();
                    switch (eid) {
                        case MotionEvent.ACTION_MOVE:
    //ТУТ КОРРЕКЦИЯ С КВАДРАТНЫМ ТИПОМ
                            listener2.onDrag(current, joystickPoint.getX(), joystickPoint.getY());
                            PointF mv = new PointF(event.getX() - DownPT.x, event.getY() - DownPT.y);
                            Rect rect = new Rect();
                            frame.getLocalVisibleRect(rect);
                            if ((StartPT.x + mv.x >= rect.left) && (StartPT.x + mv.x <= rect.right - joystickPoint.getWidth())) {
                                joystickPoint.setX((int) (StartPT.x + mv.x));
                            } else {
                                if (!(StartPT.x + mv.x >= rect.left)) {
                                    joystickPoint.setX(rect.left);
                                }
                                if (!(StartPT.x + mv.x <= rect.right - joystickPoint.getWidth())) {
                                    joystickPoint.setX(rect.right - joystickPoint.getWidth());
                                }
                            }
                            if ((StartPT.y + mv.y >= rect.top) && (StartPT.y + mv.y <= rect.bottom - joystickPoint.getHeight())) {
                                joystickPoint.setY((int) (StartPT.y + mv.y));
                            } else {
                                if (!(StartPT.y + mv.y >= rect.top)) {
                                    joystickPoint.setY(rect.top);
                                }
                                if (!(StartPT.y + mv.y <= rect.bottom - joystickPoint.getHeight())) {
                                    joystickPoint.setY(rect.bottom - joystickPoint.getHeight());
                                }
                            }
                            StartPT = new PointF(joystickPoint.getX(), joystickPoint.getY());
                            break;
                        case MotionEvent.ACTION_DOWN:
                            listener1.onDragStart(current, joystickPoint.getX(), joystickPoint.getY());
                            DownPT.x = event.getX();
                            DownPT.y = event.getY();
                            StartPT = new PointF(joystickPoint.getX(), joystickPoint.getY());
                            break;
                        case MotionEvent.ACTION_UP:
                            listener3.onDragEnd(current, joystickPoint.getX(), joystickPoint.getY());
                            if(center) {
                                joystickPoint.setX(frame.getWidth()/2-joystickPoint.getWidth()/2);
                                joystickPoint.setY(frame.getHeight()/2-joystickPoint.getHeight()/2);
                            }
                            break;
                        default:
                            break;
                    }
                    return true;
                }
            });
        }
        if(type == Type.CIRCLE) {
            joystickPoint.setOnTouchListener(new OnTouchListener() {
                PointF DownPT = new PointF();
                PointF StartPT = new PointF();
                @Override
                public boolean onTouch(View v, MotionEvent event) {
                    int eid = event.getAction();
                    switch (eid) {
                        case MotionEvent.ACTION_MOVE:
    //ТУТ КОРРЕКЦИЯ С КРУГЛЫМ ТИПОМ(нерабочий)
                            listener2.onDrag(current, joystickPoint.getX(), joystickPoint.getY());
                            PointF mv = new PointF(event.getX() - DownPT.x, event.getY() - DownPT.y);
                            Rect rect = new Rect();
                            frame.getLocalVisibleRect(rect);
                            if(abs((StartPT.x + mv.x - (frame.getWidth()/2))) + abs((StartPT.y + mv.y - (frame.getHeight()/2))) > r) {
                                joystickPoint.setX(StartPT.x + mv.x);
                                joystickPoint.setX(StartPT.y + mv.y);
                            } else {
                                PointF coord = new PointF();
                                float a = (float) toRadians(calculateAngle(joystickPoint.getX(),joystickPoint.getY(),StartPT.x + mv.x,StartPT.y + mv.y));
                                coord.x = (float) (frame.getWidth()/2 + r * cos(a));
                                coord.y = (float) (frame.getHeight()/2 + r * sin(a));
                                joystickPoint.setX(coord.x-joystickPoint.getWidth()/2);
                                joystickPoint.setY(coord.y-joystickPoint.getHeight()/2);
                            }
                            StartPT = new PointF(joystickPoint.getX(), joystickPoint.getY());
                            break;
                        case MotionEvent.ACTION_DOWN:
                            listener1.onDragStart(current, joystickPoint.getX(), joystickPoint.getY());
                            DownPT.x = event.getX();
                            DownPT.y = event.getY();
                            StartPT = new PointF(joystickPoint.getX(), joystickPoint.getY());
                            break;
                        case MotionEvent.ACTION_UP:
                            listener3.onDragEnd(current, joystickPoint.getX(), joystickPoint.getY());
                            if(center) {
                                joystickPoint.setX(frame.getWidth()/2-joystickPoint.getWidth()/2);
                                joystickPoint.setY(frame.getHeight()/2-joystickPoint.getHeight()/2);
                            }
                            break;
                        default:
                            break;
                    }
                    return true;
                }
            });
        }
    }
    public void setAutoRecenter(boolean v1) {
        this.center = v1;
    }
    public void setSize(int width,int height) {
        this.setLayoutParams(new LayoutParams(width,height));
        this.w = width;
        this.h = height;
    }
    public void setRadius(float radius) {
        if(radius+w1/2>w/2) {
            this.r = w/2-w1/2;
            Log.e("JOYSTICK SETUP ERROR","Invalid raius given("+radius+"),automatically resied to optimal size("+(w/2-w1/2)+")");
        } else {
            this.r = radius;
        }
    }
    public void setDragListener(onDragStartListener listener) {
        listener1 = listener;
    }
    public void setDragListener(onDragListener listener) {
        listener2 = listener;
    }
    public void setDragListener(onDragEndListener listener) {
        listener3 = listener;
    }
    private static double calculateAngle(double x1, double y1, double x2, double y2)
    {
        double angle = Math.toDegrees(Math.atan2(x2 - x1, y2 - y1));
        angle = angle + Math.ceil(-angle / 360) * 360;
        return abs(360-angle);
    }
    public interface Type {
        int SQUARE = 0;
        int CIRCLE = 1;
    }
    public interface onDragStartListener {
        public void onDragStart(Joystick joystick,float X,float Y);
    }
    public interface onDragEndListener {
        public void onDragEnd(Joystick joystick,float X,float Y);
    }
    public interface onDragListener {
        public void onDrag(Joystick joystick,float X,float Y);
    }
}