/*******************************************************************************
* E.S.O. - VLT project
*
* "@(#) $Id: AP_pixel.C,v 2.43 2004/06/17 23:04:27 vltsccm Exp $"
*
* who       when      what
* --------  --------  ----------------------------------------------
* hummel  13/03/98  created 
* aaguayo 04/12/02  APR2003
*/

/************************************************************************
*   NAME  
*   AP_pixel - Automatic Positioning pixel functions
* 
*   SYNOPSIS
*       AP_pixel();
*   
*   PARENT CLASS
* 
*   DESCRIPTION          
*       This C++ class contains simple geometric functions. 
*       The coordinate system is CCD pixels 
*
*   PUBLIC METHODS       
*       float py_on_line(float px, float py,float g1x,float g1y,
*                                           float g2x,float g2y)
*                        returns the y-component of the distance 
*                        between p point (px,py) and a line. The 
*                        line is defined by two points (g1x,g1y) 
*                        and (g2x,g2y). 
*       float px_on_line(float px, float py,float g1x,float g1y,
*                                           float g2x,float g2y)
*                        returns the x-component of the distance 
*                        between point p (px,py) and a line. The 
*                        line is defined by two points (g1x,g1y) 
*                        and (g2x,g2y). 
*       float pr_on_line(float px, float py,float g1x,float g1y,
*                                           float g2x,float g2y)
*                        returns the distance between point p   
*                        (px,py) and a line. The line is defined by 
*                        two points (g1x,g1y) and (g2x,g2y).
*       int p_in_area(int max, float px, float py,float *x, float *y)
*                        boolean function returns 1 (true) if 
*                        point p (px,py) is inside a rectangle.  
*                        *x and *y are pointers to an array of 
*                        length max, containing the corner points 
*                        of the rectangle. max must be 4 in the 
*                        present version. 
*       void rot(float *px, float *py,float cx, float cy, float rot_ang)
*                        rotates a point p (px,py) around the 
*                        center at c (cx,y) by rot_ang degrees.
*       void test()      
*                        tests all class member functions 
*
*   PUBLIC DATA MEMBERS
*       const float ra2deg=57.29578;
*       const float deg2ra=0.0174532;
*
*   PROTECTED METHODS
*
*   PROTECTED DATA MEMBERS
*
*   PRIVATE METHODS
*
*   PRIVATE DATA MEMBERS
*
*   FILES                    
*       AP_pixel.C,  AP_pixel.h
*
*   ENVIRONMENT
*
*   COMMANDS
*
*   RETURN VALUES
*
*   CAUTIONS 
*
*   EXAMPLES
*
*   SEE ALSO
*
*   BUGS   
* 
*------------------------------------------------------------------------
*/

#define _POSIX_SOURCE 1

#include <stdlib.h>
#include <iostream>
#include <math.h>
#include "AP_pixel.h" 
using namespace std;

static char *rcsId="@(#) $Id: AP_pixel.C,v 2.43 2004/06/17 23:04:27 vltsccm Exp $";
static void *use_rcsId = ((void)&use_rcsId,(void *) &rcsId);


float AP_pixel::ra2deg=57.295779513082320876798154814105170332405472466564;
float AP_pixel::deg2ra=0.017453292519943295769236907684886127134428718885417;

//----------------------------------------------
float AP_pixel::py_on_line(float px, float py,
                        float g1x,float g1y,
                        float g2x,float g2y)
//----------------------------------------------
           {
      // return the y-distance of a point px,py 
      // from a line, given by G1, G2
      float m,b; 
      if (g1x>g2x) { m=g1x; g1x=g2x; g2x=m;
                     m=g1y; g1y=g2y; g2y=m; }
      if (g1x==g2x) { // caution : |
                    return 0.0; 
                    }
      else { 
             // ordinary: slope up or slope down 
             // line: y=m*x+b
           m=(g2y-g1y)/(g2x-g1x);
           b=(g1y*g2x-g1x*g2y)/(g2x-g1x);
           return (py-m*px-b);
           }
      }


//----------------------------------------------
float AP_pixel::px_on_line(float px, float py,
                        float g1x,float g1y,
                        float g2x,float g2y)
//----------------------------------------------
           {
      // return the x-distance of a point px,py 
      // from a line, given by G1, G2
      float m,b; 
      if (g1x>g2x) { m=g1x; g1x=g2x; g2x=m;
                     m=g1y; g1y=g2y; g2y=m; }
      if (g1y==g2y) { // caution : -
                    return 0.0; 
                    }
      else 
           // ordinary: slope up or slope down
           // line: y=m*x+b
           if (g1x==g2x) { 
                         return (px-g1x);
                         }
           else {
           m=(g2y-g1y)/(g2x-g1x);
           b=(g1y*g2x-g1x*g2y)/(g2x-g1x);
           return (px-(py-b)/m);
           }
      }

//----------------------------------------------
float AP_pixel::pr_on_line(float px, float py,
                        float g1x,float g1y,
                        float g2x,float g2y)
//----------------------------------------------
           {
      // return the r-distance of a point px,py 
      // from a line, given by G1, G2
      float m,b,m2,dx,dy; 
      if (g1x>g2x) { m=g1x; g1x=g2x; g2x=m;
                     m=g1y; g1y=g2y; g2y=m; }
      if (g1y==g2y) { // caution : -
                    return (py-g1y); 
                    }
     else 
     if (g1x==g2x) { // caution : | 
                   return (px-g1x);                     
                   }
     else {
           m=(g2y-g1y)/(g2x-g1x);
           m2=m+1/m;
           b=(g1y*g2x-g1x*g2y)/(g2x-g1x);

           dx=(py-b-m*px)/m2;
           dy=((b-py)/m+px)/m2;
           dx=dx*dx+dy*dy;
           return sqrt(dx);
           }
      }


//----------------------------------------------------
int AP_pixel::p_in_area(int max, float px, float py, float *x, float *y)
//----------------------------------------------------
          {
          int i,index=1,index2=2;
          float check,ymax;
          // checks if the point p is inside 
          // any figure, spanned by four points
          // 4 points must be in drawing order
          // like clockwise rotation ->  | 
          //                       ^     \/
          //                       |  <-
          // if more than 4 points then look also
          // for lowest y-point 
          // x[0]=px; y[0]=py, 
          // x[1] .. y[1] are the four points
          // 1. look for point with max in y 
          ymax=*(y+max); index=max;
          for (i=1; i<max; i++)
              if ( *(y+i) > ymax) { ymax = *(y+i); index=i; }   

          // check on first line 
          if (index<max) index2=index+1;
          if (index==max) index2=1; 
          check=py_on_line(px,py,*(x+index),*(y+index),*(x+index2),*(y+index2));
          if (check>0) return 0;  // outside 
          if (check==0) { // either | or on the raizers edge
          check=px_on_line(px,py,*(x+index),*(y+index),*(x+index2),*(y+index2));
          if (check>0) return 0;  // outside 
          }

          // check on second line 
          index++; 
          if (index==max+1) index=1;
          if (index<max) index2=index+1;
          if (index==max) index2=1; 
          check=py_on_line(px,py,*(x+index),*(y+index),*(x+index2),*(y+index2));
          if (check<0) return 0;  // outside 
          if (check==0) { // either | or on the raizers edge
          check=px_on_line(px,py,*(x+index),*(y+index),*(x+index2),*(y+index2));
          if (check>0) return 0;  // outside 
          }

          // check on third line
          index++; 
          if (index==max+1) index=1;
          if (index<max) index2=index+1;
          if (index==max) index2=1; 
          check=py_on_line(px,py,*(x+index),*(y+index),*(x+index2),*(y+index2));
          if (check<0) return 0;  // outside 
          if (check==0) { // either | or on the raizers edge
          check=px_on_line(px,py,*(x+index),*(y+index),*(x+index2),*(y+index2));
          if (check<0) return 0;  // outside 
          }

          // check on the 4th line 
          index++; 
          if (index==max+1) index=1;
          if (index<max) index2=index+1;
          if (index==max) index2=1; 
          check=py_on_line(px,py,*(x+index),*(y+index),*(x+index2),*(y+index2));
          if (check>0) return 0;  // outside 
          if (check==0) { // either | or on the raizers edge
          check=px_on_line(px,py,*(x+index),*(y+index),*(x+index2),*(y+index2));
          if (check<0) return 0;  // outside 
          }

          return 1; 
          }

//-----------------------------------------------------------------------
void AP_pixel::rot(float *px, float *py, float cx, float cy, float rot_ang)
//-----------------------------------------------------------------------
        // rotate point p around c with rot_ang in degrees
        {
        float ac=cos(rot_ang*deg2ra),as=sin(rot_ang*deg2ra);
        float a1=ac*(*px-cx) - as*(*py-cy);
        float a2=as*(*px-cx) + ac*(*py-cy);
        *px=(a1+cx);
        *py=(a2+cy);
        }


//---------------------
void AP_pixel::test()
//---------------------
     {
     cout << " class AP_pixel, method test " << endl; 

     float  qx, qy, ax,ay,bx,by;

     // 1/2 diagonal, normal case
     qx=0.5; qy=3.0; ax=-1.0; ay=-0.5; bx=1.0; by=0.5;
     cout << py_on_line(qx,qy,ax,ay,bx,by) << " py expect 2.75\n"; 

     qx=0.5; qy=-2.0; ax=-1.0; ay=-0.5; bx=1.0; by=0.5;
     cout << py_on_line(qx,qy,ax,ay,bx,by) << " py expect -2.25\n";

     qx=0.5; qy=3.0; ax=-1.0; ay=-0.5; bx=1.0; by=0.5;
     cout << px_on_line(qx,qy,ax,ay,bx,by) << " px expect < 0\n"; 
     
     qx=0.5; qy=-3.0; ax=-1.0; ay=-0.5; bx=1.0; by=0.5;
     cout << px_on_line(qx,qy,ax,ay,bx,by) << " px expect  > 0\n";

     qx=1.0; qy=0.0; ax=0.0; ay=0.0; bx=1.0; by=1.0;
     cout << pr_on_line(qx,qy,ax,ay,bx,by) << " pr expect  " << sqrt(2.0)/2 << "\n";
     qx=0.0; qy=1.0; ax=0.0; ay=0.0; bx=1.0; by=1.0;
     cout << pr_on_line(qx,qy,ax,ay,bx,by) << " pr expect  " << sqrt(2.0)/2 << "\n";
     
     // special case 
     cout << " lines like - and | " << endl;
     qx=0.5; qy=3.0; ax=1.0; ay=-10; bx=1.0; by=10;
     cout << px_on_line(qx,qy,ax,ay,bx,by) << " px expect -0.5\n"; 
     
     qx=0.5; qy=3.0; ax=1.0; ay=-10; bx=1.0; by=10;
     cout << py_on_line(qx,qy,ax,ay,bx,by) << " py expect  0\n";

     qx=0.5; qy=3.0; ax=10.0; ay=1; bx=-10.0; by=1;
     cout << py_on_line(qx,qy,ax,ay,bx,by) << " py expect 2.0\n"; 
     
     qx=0.5; qy=3.0; ax=10.0; ay=1; bx=-10.0; by=1;
     cout << px_on_line(qx,qy,ax,ay,bx,by) << " px expect  0\n";

     qx=0.0; qy=0.0; ax=10.0; ay=1; bx=-10.0; by=1;
     cout << pr_on_line(qx,qy,ax,ay,bx,by) << " pr expect  -1\n";

     qx=0.5; qy=3.0; ax=10.0; ay=-11; bx=10.0; by=1;
     cout << pr_on_line(qx,qy,ax,ay,bx,by) << " pr expect  -9.5\n";

     //                                1  2
     // test the area the unit square  x--x
     //                                |  |
     //                              4 x--x 3
     float x[5],y[5];
     x[1]=0.0; y[1]=1;
     x[2]=1.0; y[2]=1.0;
     x[3]=1.0; y[3]=0.0;
     x[4]=0;   y[4]=0.0;
     x[0]=0.5; y[0]=0.5; cout <<  p_in_area(4,x[0],y[0],x,y) << " expect 1\n"; 
     x[0]=2.5; y[0]=0.5; cout <<  p_in_area(4,x[0],y[0],x,y) << " expect 0\n"; 
     x[0]=0.5; y[0]=2.5; cout <<  p_in_area(4,x[0],y[0],x,y) << " expect 0\n"; 
     x[0]=-3;  y[0]=0.5; cout <<  p_in_area(4,x[0],y[0],x,y) << " expect 0\n"; 
     x[0]=0.5; y[0]=-3;  cout <<  p_in_area(4,x[0],y[0],x,y) << " expect 0\n"; 

     // test the area the unit raute   x 1
     //                               / \ 
     //                            4 x   x 2
     //                               \ /
     //                                x 3
     x[1]=0.0; y[1]=1;
     x[2]=1.0; y[2]=0.0;
     x[3]=0.0; y[3]=-1.0;
     x[4]=-1.0;y[4]=0.0;
     x[0]=0.5; y[0]=0.5; cout <<  p_in_area(4,x[0],y[0],x,y) << " expect 1\n"; 
     x[0]=2.5; y[0]=0.5; cout <<  p_in_area(4,x[0],y[0],x,y) << " expect 0\n"; 
     x[0]=0.5; y[0]=2.5; cout <<  p_in_area(4,x[0],y[0],x,y) << " expect 0\n"; 
     x[0]=-3;  y[0]=0.5; cout <<  p_in_area(4,x[0],y[0],x,y) << " expect 0\n"; 
     x[0]=0.5; y[0]=-3;  cout <<  p_in_area(4,x[0],y[0],x,y) << " expect 0\n"; 

     // test rotation
     qx=3;qy=1;rot(&qx,&qy,3.0,0.0,45.0);
     cout << "rot: " << qx << " " << qy << " expect 3-0.707..  0.707.."  << endl;
     cout << " class AP_pixel, method test oOo" << endl; 
     }


/*___oOo___*/