coordinate.h

基于多假设的地图匹配算法。程序能够根据车辆在行驶过程中收集到的GPS/DR数据正确得到当前车辆所在的道路位置。

/******************************************************************************

    File Name:    include/Coordinate.h
    Description:  planar Cartesian coordinates class COORD
                  including definition, operators and functions

******************************************************************************/

/******************************************************************************

    Author:       [e-mail]	
    Upadate:
      2008/09/16  File founded

    Copyright 2008-2009 Robot Lab., Dept.of Automation, Tsinghua University

******************************************************************************/

#ifndef __COORDINATE_H__
#define __COORDINATE_H__

#include <math.h>
#include "../include/Setting.h"

/******************************                   Data structures of COORD                  ***************************************************/
typedef struct COORD
{
    DATATYPE        east;                                               /*East distance to the reference point, in meters*/
    DATATYPE        north;                                              /*East distance to the reference point, in meters*/

    COORD(){};                                                          /*Default constructor*/
    COORD(const COORD& coord):east(coord.east),north(coord.north){};    /*Copy-constructor*/
    inline DATATYPE ProdWith(const COORD& coord) const;                 /*Calculate inner product*/
    inline DATATYPE DistanceTo(const COORD& coord) const;               /*Calculate distance between coordinates*/
    inline DATATYPE DisSquareTo(const COORD& coord) const;              /*Calculate square of the distance between coordinates*/
    inline DATATYPE AngleFrom(const COORD& coord) const;                /*Calculate absolute direction from a given coordinate*/

    /*Reloaded operators*/
    inline COORD& operator= (const COORD& coord);
    inline COORD& operator+= (const COORD& coord);
    inline COORD operator+ (const COORD& coord) const;
    inline COORD& operator-= (const COORD& coord);
    inline COORD operator- (const COORD& coord) const;
    inline COORD& operator*= (const DATATYPE& gain);
    inline COORD operator* (const DATATYPE& gain) const;
}Coordinate;


/******************************                   Function implementations of COORD         ***************************************************/
DATATYPE COORD::ProdWith(const COORD& coord) const
{
    return DATATYPE(east*coord.east + north*coord.north);
}

DATATYPE COORD::DistanceTo(const COORD& coord) const
{
    return DATATYPE( sqrt( (east-coord.east)*(east-coord.east) + (north-coord.north)*(north-coord.north)) );
}

DATATYPE COORD::DisSquareTo(const COORD& coord) const
{
    return DATATYPE( (east-coord.east)*(east-coord.east) + (north-coord.north)*(north-coord.north) );
}

DATATYPE COORD::AngleFrom(const COORD& coord) const
{
    return DATATYPE( atan2(north-coord.north,east-coord.east) );
}

COORD& COORD:: operator= (const COORD& coord)
{
    east = coord.east;
    north = coord.north;
    return *this;
}

COORD& COORD::operator+= (const COORD& coord)
{
    east += coord.east;
    north += coord.north;
    return *this;
}

COORD COORD::operator+ (const COORD& coord) const
{
    COORD temp(*this);
    temp.east += coord.east;
    temp.north += coord.north;
    return temp;
}

COORD& COORD::operator-= (const COORD& coord)
{
    east -= coord.east;
    north -= coord.north;
    return *this;
}

COORD COORD::operator- (const COORD& coord) const
{
    COORD temp(*this);
    temp.east -= coord.east;
    temp.north -= coord.north;
    return temp;
}

COORD& COORD::operator*= (const DATATYPE& gain)
{
    east *= gain;
    north *= gain;
    return (*this);
}

COORD COORD::operator* (const DATATYPE& gain) const
{
    COORD temp(*this);
    temp.east *= gain;
    temp.north *= gain;
    return temp;
}

#endif /*__COORDINATE_H__*/