两直角坐标系内点坐标的转换

后续文章见：《两直角坐标系内点坐标的转换（使用新算法，提高了精度）》http://blog.vckbase.com/bruceteen/archive/2012/03/12/55951.html

一个实物，通过照相机拍摄出来，可能位置、角度、大小发生了变化。
本程序是通过两个已知点的坐标来计算出 坐标平移，旋转角度，伸缩比例，然后则给出实物上某个坐标位置，就可以算出在图像中对应 的坐标位置，反之亦然。

#include

#include

 

struct Point

{

     long double x, y;

 

     Point( long double x_, long double y_ ) : x(x_), y(y_) {}

     bool operator==( const Point& pt ) const { return x==pt.x && y==pt.y; }

     bool operator!=( const Point& pt ) const { return x!=pt.x || y!=pt.y; }

};

 

class CoordCvt

{

public:

     CoordCvt( long double costheta, long double sintheta, long double offsetx, long double offsety, long double scale )

     {

         costheta_ = costheta;

         sintheta_ = sintheta;

         offsetx_  = offsetx;

         offsety_  = offsety;

         scale_    = scale;

     }

     CoordCvt( const Point& A, const Point& B, const Point& Am, const Point& Bm )

     {

         assert( A!=B && Am!=Bm );

 

         long double aa = ((B.x-A.x)-(Bm.x-Am.x))*((B.x-A.x)-(Bm.x-Am.x)) + ((B.y-A.y)-(Bm.y-Am.y))*((B.y-A.y)-(Bm.y-Am.y));

         long double bb = (B.x-A.x)*(B.x-A.x) + (B.y-A.y)*(B.y-A.y);

         long double cc = (Bm.x-Am.x)*(Bm.x-Am.x) + (Bm.y-Am.y)*(Bm.y-Am.y);

         scale_ = sqrt(cc/bb);

 

         costheta_ = (bb+cc-aa)/(2*sqrt(bb*cc));

         sintheta_ = sqrt( 1 - costheta_*costheta_ );

         //sintheta_ = sqrt( (2*aa*bb+2*bb*cc+2*cc*aa-aa*aa-bb*bb-cc*cc)/(bb*cc) )/2;

         if( (Bm.y-Am.y)*(B.x-A.x) < (B.y-A.y)*(Bm.x-Am.x) ) // 3th 4th quadrant

              sintheta_ = -sintheta_;

        

         offsetx_ = Am.x - (A.x*costheta_ - A.y*sintheta_)*scale_;

         offsety_ = Am.y - (A.x*sintheta_ + A.y*costheta_)*scale_;

     }

 

     Point Covert( const Point& pt ) const

     {

         return Point( (pt.x*costheta_ - pt.y*sintheta_)*scale_ + offsetx_

              , (pt.x*sintheta_ + pt.y*costheta_)*scale_ + offsety_ );

     }

 

private:

     long double costheta_;

     long double sintheta_;

     long double offsetx_;

     long double offsety_;

     long double scale_;

};

 

/////////////////////////// test ///////////////////////////

 

void ApproximatelyEqual( const Point& a, const Point& b )

{

     assert( fabs(a.x-b.x)<1e-6 );

     assert( fabs(a.y-b.y)<1e-6 );

}

int main()

{

     { // 45

         CoordCvt a( Point(1,1), Point(2,1), Point(3,1), Point(4,2) );

         Point m = a.Covert( Point(1,2) );

         ApproximatelyEqual( m, Point(2,2) );

     }

     { // 90

         CoordCvt a( Point(1,1), Point(2,1), Point(6,1), Point(6,3) );

         Point m = a.Covert( Point(1,2) );

         ApproximatelyEqual( m, Point(4,1) );

     }

     { // 135

          CoordCvt a( Point(1,1), Point(2,1), Point(8,2), Point(7,3) );

         Point m = a.Covert( Point(1,2) );

         ApproximatelyEqual( m, Point(7,1) );

     }

     { // 180

         CoordCvt a( Point(1,1), Point(2,1), Point(11,3), Point(9,3) );

         Point m = a.Covert( Point(1,2) );

         ApproximatelyEqual( m, Point(11,1) );

     }

     { // -45

         CoordCvt a( Point(1,1), Point(2,1), Point(2,-2), Point(3,-3) );

         Point m = a.Covert( Point(1,2) );

         ApproximatelyEqual( m, Point(3,-1) );

     }

 

     { // -90

         CoordCvt a( Point(1,1), Point(2,1), Point(4,-1), Point(4,-3) );

         Point m = a.Covert( Point(1,2) );

         ApproximatelyEqual( m, Point(6,-1) );

     }

     { // -135

         CoordCvt a( Point(1,1), Point(2,1), Point(8,-1), Point(7,-2) );

         Point m = a.Covert( Point(1,2) );

         ApproximatelyEqual( m, Point(9,-2) );

     }

     { // -.180

         CoordCvt a( Point(1,1), Point(2,1), Point(11,-1), Point(9,-1) );

         Point m = a.Covert( Point(1,2) );

         ApproximatelyEqual( m, Point(11,-3) );

     }

 

     return 0;

}