Utils3D.h

Go to the documentation of this file.

#ifndef UTILS3D_DEFINED
#define UTILS3D_DEFINED


#include "cv.h"
#include "cxcore.h"
#include "highgui.h"
#include <iostream>


using namespace std;

namespace Utils3D
{
        typedef struct _STRAIGHTLINE
        { //in mm
                CvPoint3D32f loc;
                CvPoint3D32f dir;
        }STRAIGHTLINE;

        typedef struct _PLANE
        { //in mm
                CvPoint3D32f loc;
                CvPoint3D32f dir1;
                CvPoint3D32f dir2;

        }PLANE;

        typedef struct _PLANE2
        { //in mm
                CvPoint3D32f loc;
                CvPoint3D32f norm;
        }PLANE2;

        inline int sgn ( float in )
        {
            if ( in > 0)
                return 1;
            if ( in < 0 )
                return -1;
            return 0;
        }

        CvMat* createIdentityMatrix();
        CvMat* createRotationMatrixX( double angle );
        CvMat* createRotationMatrixY( double angle );
        CvMat* createRotationMatrixZ( double angle );
        CvMat* createTranslationMatrix( CvPoint3D32f* trans );

        CvMat* createRotationCoordinateSystemMatrixX( double angle );
        CvMat* createRotationCoordinateSystemMatrixY( double angle );
        CvMat* createRotationCoordinateSystemMatrixZ( double angle );

        void generatePlaneVectorsFromNormal(CvPoint3D32f*normal,CvPoint3D32f* v1,CvPoint3D32f*v2);

        CvMat* createTransformationMatrix(CvPoint3D32f* trans, double rotX, double rotY, double rotZ);
//      CvMat* createTransformationMatrix(double transX, double transY, double transZ, double rotX, double rotY, double rotZ);
        CvMat* createTransformationMatrix(CvPoint3D32f* trans, CvPoint3D32f* rot);

        void fillStraightLine(CvPoint3D32f * loc, CvPoint3D32f * dir, STRAIGHTLINE * line);
        void fillStraightLine(float locX, float locY, float locZ, float dirX, float dirY, float dirZ, STRAIGHTLINE * line);

        void fillPlane(CvPoint3D32f loc, CvPoint3D32f dir1, CvPoint3D32f dir2, PLANE * plane);
        void fillPlane(float locX, float locY, float locZ, float dir1X, float dir1Y, float dir1Z, float dir2X, float dir2Y, float dir2Z, PLANE * plane);

        void normalize(CvPoint3D32f* vec);

        void transformDirection(CvPoint3D32f* dir, CvMat* transformationMat);

        void transformPoint(CvPoint3D32f* point, CvMat* transformationMat);

        void transformStraightLine(STRAIGHTLINE* line, CvMat* transformationMat);

        void transformPlane(PLANE* plane, CvMat* transformationMat);

        double intersectLinePlane(STRAIGHTLINE line, PLANE plane);
        
        STRAIGHTLINE intersectPlanes(PLANE2 P1, PLANE2 P2);

        double getPointPlaneDistance(PLANE2 P, CvPoint3D32f p);

        CvPoint3D32f projectPointOnPlane(PLANE2 P,CvPoint3D32f p);

        CvPoint3D32f projectPointOnLine(STRAIGHTLINE L,CvPoint3D32f* p);

        double intersectLinePlane(STRAIGHTLINE line, PLANE2 plane);

        double distPoints(CvPoint3D32f* p1, CvPoint3D32f * p2);

//      double distPointLine( CvPoint3D32f* p, STRAIGHTLINE line);

        double dotProd(CvPoint3D32f* p1, CvPoint3D32f* p2 );

        double getAngle(CvPoint3D32f* p1, CvPoint3D32f* p2 );

        CvPoint3D32f * cAddPoints(CvPoint3D32f* p1, CvPoint3D32f * p2);
        
        void addPoints(CvPoint3D32f* p1, CvPoint3D32f * p2,CvPoint3D32f*res);

        CvPoint3D32f * cAddWPoints(CvPoint3D32f * p1, float w1, CvPoint3D32f * p2, float w2 );

        void addWPoints(CvPoint3D32f * p1, float w1, CvPoint3D32f * p2, float w2, CvPoint3D32f*np );

        CvPoint3D32f * cSubPoints(CvPoint3D32f* p1, CvPoint3D32f * p2);

        void subPoints(CvPoint3D32f* p1, CvPoint3D32f * p2,CvPoint3D32f* res);

        CvPoint3D32f * cCrossProduct(CvPoint3D32f* p1, CvPoint3D32f* p2);

        void crossProduct(CvPoint3D32f* p1, CvPoint3D32f* p2,CvPoint3D32f* np);

        CvPoint3D32f * cGetPoint(float x, float y, float z );

        CvPoint3D32f * cCopyPoint(CvPoint3D32f* p );

        void swapPoints(CvPoint3D32f* p1, CvPoint3D32f* p2);

        double getNorm(CvPoint3D32f* p );

        void setCoords (CvPoint3D32f * in, CvPoint3D32f * out);

        void setCoords (CvPoint3D32f * out, float x, float y, float z);

        void scalePoint(CvPoint3D32f* p, double scale);

        void orthonormalizePoints(CvPoint3D32f* p1, CvPoint3D32f* p2,CvPoint3D32f*p3);

        void orthonormalizePoints(CvPoint3D32f* p1, CvPoint3D32f* p2,CvPoint3D32f*p3,double scale);

        CvMat* cGetBasisFrom3Points(CvPoint3D32f* p1, CvPoint3D32f * p2, CvPoint3D32f * p3 );

        CvMat* cGetBasisFrom3Points3x3(CvPoint3D32f* p1, CvPoint3D32f * p2, CvPoint3D32f * p3 ) ;
        CvMat * createMatrixFrom3Points(CvPoint3D32f* p1, CvPoint3D32f* p2,CvPoint3D32f* p3);

        CvMat * cGetMat32fFrom64d(CvMat* mat64f);
        CvMat * cGetMat64dFrom32f(CvMat* mat64f);

        //Rectification Function for Ranger Calibration
                CvPoint3D32f * cRectifyPoint(CvPoint3D32f * oriPoint, float sensorLenghtScale, float sensorWidthScale, float sensorHeightScale, CvMat * map_matrix);
                CvPoint3D32f * cInverseRectifyPoint(CvPoint3D32f * oriPoint, float sensorLenghtScale, float sensorWidthScale, float sensorHeightScale, CvMat * map_matrix);
                CvMat* cCalcRectificationMatrix(CvPoint2D32f* oriP1, CvPoint2D32f* oriP2, CvPoint2D32f* oriP3, CvPoint2D32f* oriP4, CvPoint2D32f* dstP1, CvPoint2D32f* dstP2,CvPoint2D32f* dstP3,CvPoint2D32f* dstP4);


                float getShortestDistanceBetweenTwoLines( STRAIGHTLINE line1, STRAIGHTLINE line2,float *t1, float *t2);

                float getLineParameter(STRAIGHTLINE line, CvPoint3D32f p);

                CvPoint3D32f getLinePoint(STRAIGHTLINE line, float param);
} //of namespace Helper3D



#endif