mat3d.h

任意给定三维空间的点集

/*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*
 * mat3d.h -
 *     Rotations via matrics fo 3d.
 * 
 *     Code is not as efficient as it can be.
\*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*/

#ifndef  __MAT3D__H
#define  __MAT3D__H

class  Transform3d
{
private:
    double  mat[ 4 ][ 4 ];

public:
    void  init_rot_around_z( double  q )
    {
        memset( mat, 0, sizeof( mat ) );
        mat[ 0 ][ 0 ] = cos( q );
        mat[ 0 ][ 1 ] = sin( q );
        mat[ 1 ][ 0 ] = -sin( q );
        mat[ 1 ][ 1 ] = cos( q );
        mat[ 2 ][ 2 ] = 1;
        mat[ 3 ][ 3 ] = 1;
    }
    void  init_rot_around_x( double  q )
    {
        memset( mat, 0, sizeof( mat ) );

        mat[ 0 ][ 0 ] = 1;
        mat[ 1 ][ 1 ] = cos( q );
        mat[ 1 ][ 2 ] = sin( q );
        mat[ 2 ][ 1 ] = -sin( q );
        mat[ 2 ][ 2 ] = cos( q );
        mat[ 3 ][ 3 ] = 1;
    }

    void  transform( Point3d  & pnt ) const {
        double  vec[ 4 ];
        double  vec_out[ 4 ];
        double  sum;

        vec[ 0 ] = pnt[ 0 ];
        vec[ 1 ] = pnt[ 1 ];
        vec[ 2 ] = pnt[ 2 ];
        vec[ 3 ] = 1;

        for  ( int  row = 0; row < 4; row++ ) {
            sum = 0;
            for  ( int  col = 0; col < 4; col++ ) 
                sum += vec[ col ] * mat[ row ][ col ];
            vec_out[ row ] = sum;
        }

        pnt[ 0 ] = vec_out[ 0 ];
        pnt[ 1 ] = vec_out[ 1 ];
        pnt[ 2 ] = vec_out[ 2 ];
    }
};


#else   /* __MAT3D__H */
#error  Header file mat3d.h included twice
#endif  /* __MAT3D__H */

/* mat3d.h - End of File ------------------------------------------*/