fastmarching.h

来自「fast marching method」· C头文件 代码 · 共 527 行 · 第 1/2 页

                    if (y>0) __UpdatePointVoro(x,y-1,z);
                    if (y<height-1) __UpdatePointVoro(x,y+1,z);
                    if (z>0) __UpdatePointVoro(x,y,z-1);
                    if (z<depth-1) __UpdatePointVoro(x,y,z+1);
                }
            }
        }


    private:
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // En: Update of a point
        // Fr: Mise a jour d'un point
       void __UpdatePoint(const int x, const int y, const int z) {
			const int n = _offset(x,y,z);
            const eState st = (eState)state[n];
            if (st == eFar) {
                // En: If it was a far point, we put it as trial,  we compute its value and we add it to the queue
                // Fr: Si c'etait un point far, on le rend trial, on calcule sa valeur et on l'ajoute a la queue
                state[n] = eTrial;
                const T val = _UpdateValue(x,y,z);
                data[n] = sign*val;
                tabnode[n] = trial_queue.push(stCoordVal<T>(x,y,z,n,val));
            }
            else if (st == eTrial) {
                // En: If it was a far point, we recompute its value
                // En: if the new value is inferior, we accept it and we increase the point in the queue
                // Fr: Si c'etait deja un point trial, on recalcule sa valeur
                // Fr: Si la nouvelle valeur est inferieure, on l'accepte et on fait remonter le point dans la queue
                const T val = _UpdateValue(x,y,z);
                if (val<sign*data[n]) {
                    data[n] = sign*val;
                    trial_queue.increase(tabnode[n],stCoordVal<T>(x,y,z,n,val));
                }
            }
        }
        // voro must be non-null
        void __UpdatePointVoro(const int x, const int y, const int z) {
			const int n = _offset(x,y,z);
            int mx = -1, my = -1, mz = -1;
            const eState st = (eState)state[n];
            if (st == eFar) {
                // En: If it was a far point, we put it as trial,  we compute its value and we add it to the queue
                // Fr: Si c'etait un point far, on le rend trial, on calcule sa valeur et on l'ajoute a la queue
                const T val = _UpdateValue(x,y,z,mx,my,mz);
                state[n] = eTrial;
                data[n] = sign*val;
                if ((mx != -1) && (my != -1) && (mz != -1)) voro[n] = voro[_offset(mx, my, mz)];
                tabnode[n] = trial_queue.push(stCoordVal<T>(x,y,z,n,val));
            }
            else if (st == eTrial) {
                // En: If it was a far point, we recompute its value
                // En: if the new value is inferior, we accept it and we increase the point in the queue
                // Fr: Si c'etait deja un point trial, on recalcule sa valeur
                // Fr: Si la nouvelle valeur est inferieure, on l'accepte et on fait remonter le point dans la queue
                const T val = _UpdateValue(x,y,z,mx,my,mz);
                //if ((mx < 0) || (mx > width-1) || (my < 0) || (my > height-1) || (mz < 0) || (mz > depth-1)) return;
                if (val<sign*data[n]) {
                    data[n] = sign*val;
                    voro[n] = voro[_offset(mx,my,mz)];
                    trial_queue.increase(tabnode[n],stCoordVal<T>(x,y,z,n,val));
                }
            }
        }

	protected:
      	////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // En: Acces to a  point
        // Fr: Acces a un point
        int _offset(const int x, const int y, const int z) const { return x+width*(y+height*z); }
        T _GetValue(const int x, const int y, const int z) const { return _GetValue(_offset(x,y,z)); }
        T _GetValue(const int n) const {
            if (state[n] <= eTrial) return sign*data[n];
            return big;
		}

        ////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // Fr: Computation of the value of a point : have to be defined in the inherited class
        // En: Calcul de la valeur d'un point : a definir dans les classes derivees
        virtual T _UpdateValue(const int x, const int y, const int z) const = 0;
        virtual T _UpdateValue(const int x, const int y, const int z, int &mx, int &my, int &mz) const = 0;

        ////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // En: resolution of a second degree trinomial equation
        // Fr: Resolution d'un trinome du second degre
        bool _SolveTrinome(const T a, const T b, const T c, T &sol_max, T &sol_min) const {
            const T delta = b*b - 4.*a*c;
            if (delta < 0) return false;
            const T sqrtDelta = std::sqrt(delta);
            sol_max = (- b + sqrtDelta) / a / 2.;
            sol_min = (- b - sqrtDelta) / a / 2.;
            return true;
        }
    };

    
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // En: 2D Iconale Equation 
    // Fr: Equation iconale en 2D
    template <typename T = float, int sign = +1>
    class Eikonal2D : public FastMarching<T,sign> {
            
      public:
        Eikonal2D(T *_data, int _width, int _height) :  FastMarching<T,sign>(_data,_width,_height) {}
        virtual ~Eikonal2D() {}
         
      protected:
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////     
        // En: Update of a point
        // Fr: Mise a jour d'un point
        virtual T _UpdateValue(const int x, const int y, const int z) const {
   
            // En: we take the minimum of each couple of neighbourhood
            // Fr: On prend le minimum de chaque paire de voisins
            const T A = (x==0) ? this->_GetValue(x+1,y,z) : (x==this->width-1) ? this->_GetValue(x-1,y,z) : std::min( this->_GetValue(x+1,y,z), this->_GetValue(x-1,y,z) );
            const T B = (y==0) ? this->_GetValue(x,y+1,z) : (y==this->height-1) ? this->_GetValue(x,y-1,z) : std::min( this->_GetValue(x,y+1,z), this->_GetValue(x,y-1,z) );
            return _Solve(A,B);
        }
        virtual T _UpdateValue(const int x, const int y, const int z, int &mx, int &my, int &mz) const {
   
            // En: we take the minimum of each couple of neighbourhood
            // Fr: On prend le minimum de chaque paire de voisins
            mx = (x==0) ? x+1 : (x==this->width-1) ? x-1 : (this->_GetValue(x+1,y,z) < this->_GetValue(x-1,y,z) ? x+1 : x-1);
            my = (y==0) ? y+1 : (y==this->height-1) ? y-1 : (this->_GetValue(x,y+1,z) < this->_GetValue(x,y-1,z) ? y+1 : y-1);
            mz = z;
            return _Solve(x,y,z,mx,my);
        }

        ////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // En: Resolution of the trinomial equation
        // Fr: Resolution du trinome
        T _Solve(T A, T B) const {

            // En: we get rid of the trival cases 
            // Fr: On se debarasse des cas triviaux
            if (A == this->big) return B+1;
            if (B == this->big) return A+1;

            // En: we reorder the values in order to have  B>=A
            // Fr: On reordonne les valeurs pour avoir B>=A
            if (A>B) std::swap(A,B);
           
            //En: We assume that  u>=B : we solve the trinomial equation. If we have u>=B, it is ok
            //Fr: On supppose u>=B : trinome. Si on a bien u>=B, on a gagne
            T sol_max, sol_min;
            if (B<this->big && _SolveTrinome(2, -2.*(A+B), A*A+B*B-1., sol_max, sol_min) && sol_max+EPS>=B) return sol_max;

            // En: We assume A<=u<B
            // Fr: On suppose A<=u<B
            return A+1.;
        }
        T _Solve(const int x, const int y, const int z, int &mx, int &my) const {

            // En: we get rid of the trival cases 
            // Fr: On se debarasse des cas triviaux
            const T A = this->_GetValue(mx,y,z);
            const T B = this->_GetValue(x,my,z);
            if (A == this->big) { mx = x; return B+1; }
            if (B == this->big) { my = y; return A+1; }

            // En: we reorder the values in order to have  B>=A
            // Fr: On reordonne les valeurs pour avoir B>=A
            if (A>B) { std::swap(A,B); mx = x; }
            else { my = y; }
            //En: We assume that  u>=B : we solve the trinomial equation. If we have u>=B, it is ok
            //Fr: On supppose u>=B : trinome. Si on a bien u>=B, on a gagne
            T sol_max, sol_min;
            if (B<this->big && _SolveTrinome(2, -2.*(A+B), A*A+B*B-1., sol_max, sol_min) && sol_max+EPS>=B) return sol_max;

            // En: We assume A<=u<B
            // Fr: On suppose A<=u<B
            return A+1.;
        }
    };

    
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////
    // En: 3D Iconale Equation 
    // Fr: Equation iconale en 3D
    template <typename T = float, int sign = +1>
    class Eikonal3D : public FastMarching<T,sign> {
    
    public:
        Eikonal3D(T *_data, int _width, int _height, int _depth) :  FastMarching<T,sign>(_data,_width,_height,_depth) {}
        virtual ~Eikonal3D() {}

    protected:
        ////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // En: Update of a point
        // Fr: Mise a jour d'un point
        virtual T _UpdateValue(const int x, const int y, const int z) const {

            // En: we take the minimum of each couple of neighbourhood
            // Fr: On prend le minimum de chaque paire de voisins
            const T A = (x==0) ? this->_GetValue(x+1,y,z) : (x==this->width-1) ? this->_GetValue(x-1,y,z) : std::min( this->_GetValue(x+1,y,z), this->_GetValue(x-1,y,z) );
            const T B = (y==0) ? this->_GetValue(x,y+1,z) : (y==this->height-1) ? this->_GetValue(x,y-1,z) : std::min( this->_GetValue(x,y+1,z), this->_GetValue(x,y-1,z) );
            const T C = (z==0) ? this->_GetValue(x,y,z+1) : (z==this->depth-1) ? this->_GetValue(x,y,z-1) : std::min( this->_GetValue(x,y,z+1), this->_GetValue(x,y,z-1) );
            return _Solve(A,B,C);
        }
        virtual T _UpdateValue(const int x, const int y, const int z, int &mx, int &my, int &mz) const {

            // En: we take the minimum of each couple of neighbourhood
            // Fr: On prend le minimum de chaque paire de voisins
            mx = (x==0) ? x+1 : (x==this->width-1) ? x-1 : (this->_GetValue(x+1,y,z) < this->_GetValue(x-1,y,z) ? x+1 : x-1);
            my = (y==0) ? y+1 : (y==this->height-1) ? y-1 : (this->_GetValue(x,y+1,z) < this->_GetValue(x,y-1,z) ? y+1 : y-1);
            mz = (z==0) ? z+1 : (z==this->depth-1) ? z-1 : (this->_GetValue(x,y,z+1) < this->_GetValue(x,y,z-1) ? z+1 : z-1);
            return _Solve(x,y,z,mx,my,mz);
        }

        ////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // En: Resolution of the trinomial equation
        // Fr: Resolution du trinome
        T _Solve(T A, T B, T C) const {

            // En: we reorder the values in order to have  C>=B>=A
            // Fr: On reordonne les valeurs pour avoir C>=B>=A
            if (A>B) std::swap(A,B);
            if (B>C) std::swap(B,C);
            if (A>B) std::swap(A,B);

            // En: We assume sol>=C : first trinomial equation. If we have sol>=C, it is ok
            // Fr: On suppose sol>=C : premier trinome. Si on a bien sol>=C, on a gagne
            T sol_max, sol_min;
            if (C<this->big && _SolveTrinome(3, -2*(A+B+C), A*A+B*B+C*C-1, sol_max, sol_min) && sol_max+EPS>=C) return sol_max;

            // En: We assume B<=sol<C : second trinomial equation. If we have sol>=B, it is ok
            // Fr: On supppose B<=sol<C : deuxieme trinome. Si on a bien sol>=B, on a gagne
            if (B<this->big && _SolveTrinome(2, -2*(A+B), A*A+B*B-1, sol_max, sol_min) && sol_max+EPS>=B) return sol_max;
                
            // En: We assume A<=sol<B
            // Fr: On suppose A<=sol<B
            return A+1;
        }
        // à faire
        T _Solve(const int x, const int y, const int z, int &mx, int &my, int &mz) const {

            const T A = this->_GetValue(mx,y,z);
            const T B = this->_GetValue(x,my,z);
            const T C = this->_GetValue(x,y,mz);
            // En: we reorder the values in order to have  C>=B>=A
            // Fr: On reordonne les valeurs pour avoir C>=B>=A
            if (A>B) std::swap(A,B);
            if (B>C) std::swap(B,C);
            if (A>B) std::swap(A,B);

            // En: We assume sol>=C : first trinomial equation. If we have sol>=C, it is ok
            // Fr: On suppose sol>=C : premier trinome. Si on a bien sol>=C, on a gagne
            T sol_max, sol_min;
            if (C<this->big && _SolveTrinome(3, -2*(A+B+C), A*A+B*B+C*C-1, sol_max, sol_min) && sol_max+EPS>=C) return sol_max;

            // En: We assume B<=sol<C : second trinomial equation. If we have sol>=B, it is ok
            // Fr: On supppose B<=sol<C : deuxieme trinome. Si on a bien sol>=B, on a gagne
            if (B<this->big && _SolveTrinome(2, -2*(A+B), A*A+B*B-1, sol_max, sol_min) && sol_max+EPS>=B) return sol_max;
                
            // En: We assume A<=sol<B
            // Fr: On suppose A<=sol<B
            return A+1;
        }
    };
}

#endif