Math

/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield

   Exracted from osg by cedric pinson
*/

#pragma once

#include <iostream>
#include <cmath>
#include <cfloat>
#include <algorithm>

typedef unsigned int uint;
typedef unsigned char uchar;

#define PI 3.14159265358979
#define PI_INV (1.0/PI)
#define PI2 1.5707963f
#define TAU 6.2831853f

#define MAX_SAMPLES_CACHE 524288

inline bool isNaN(float v) { return std::isnan(v); }
inline bool isNaN(double v) { return std::isnan(v); }

template<typename T>
inline T clampTo(T v,T minimum,T maximum) { return v<minimum?minimum:v>maximum?maximum:v; }

template<typename T>
inline T frac(T v) { return v-floor(v); }

class Vec2f
{
public:

    /** Data type of vector components.*/
    typedef float value_type;

    /** Number of vector components. */
    enum { num_components = 2 };

    /** Vec member variable. */
    value_type _v[2];


    /** Constructor that sets all components of the vector to zero */
    Vec2f() {_v[0]=0.0; _v[1]=0.0;}
    Vec2f(value_type x,value_type y) { _v[0]=x; _v[1]=y; }


    inline bool operator == (const Vec2f& v) const { return _v[0]==v._v[0] && _v[1]==v._v[1]; }

    inline bool operator != (const Vec2f& v) const { return _v[0]!=v._v[0] || _v[1]!=v._v[1]; }

    inline bool operator <  (const Vec2f& v) const
    {
        if (_v[0]<v._v[0]) return true;
        else if (_v[0]>v._v[0]) return false;
        else return (_v[1]<v._v[1]);
    }

    inline value_type * ptr() { return _v; }
    inline const value_type * ptr() const { return _v; }

    inline void set( value_type x, value_type y ) { _v[0]=x; _v[1]=y; }

    inline value_type & operator [] (int i) { return _v[i]; }
    inline value_type operator [] (int i) const { return _v[i]; }

    inline value_type & x() { return _v[0]; }
    inline value_type & y() { return _v[1]; }

    inline value_type x() const { return _v[0]; }
    inline value_type y() const { return _v[1]; }

    /** Dot product. */
    inline value_type operator * (const Vec2f& rhs) const
    {
        return _v[0]*rhs._v[0]+_v[1]*rhs._v[1];
    }

    /** Multiply by scalar. */
    inline const Vec2f operator * (value_type rhs) const
    {
        return Vec2f(_v[0]*rhs, _v[1]*rhs);
    }

    /** Unary multiply by scalar. */
    inline Vec2f& operator *= (value_type rhs)
    {
        _v[0]*=rhs;
        _v[1]*=rhs;
        return *this;
    }

    /** Divide by scalar. */
    inline const Vec2f operator / (value_type rhs) const
    {
        return Vec2f(_v[0]/rhs, _v[1]/rhs);
    }

    /** Unary divide by scalar. */
    inline Vec2f& operator /= (value_type rhs)
    {
        _v[0]/=rhs;
        _v[1]/=rhs;
        return *this;
    }

    /** Binary vector add. */
    inline const Vec2f operator + (const Vec2f& rhs) const
    {
        return Vec2f(_v[0]+rhs._v[0], _v[1]+rhs._v[1]);
    }

    /** Unary vector add. Slightly more efficient because no temporary
     * intermediate object.
     */
    inline Vec2f& operator += (const Vec2f& rhs)
    {
        _v[0] += rhs._v[0];
        _v[1] += rhs._v[1];
        return *this;
    }

    /** Binary vector subtract. */
    inline const Vec2f operator - (const Vec2f& rhs) const
    {
        return Vec2f(_v[0]-rhs._v[0], _v[1]-rhs._v[1]);
    }

    /** Unary vector subtract. */
    inline Vec2f& operator -= (const Vec2f& rhs)
    {
        _v[0]-=rhs._v[0];
        _v[1]-=rhs._v[1];
        return *this;
    }

    /** Negation operator. Returns the negative of the Vec2f. */
    inline const Vec2f operator - () const
    {
        return Vec2f (-_v[0], -_v[1]);
    }

    /** Length of the vector = sqrt( vec . vec ) */
    inline value_type length() const
    {
        return sqrtf( _v[0]*_v[0] + _v[1]*_v[1] );
    }

    /** Length squared of the vector = vec . vec */
    inline value_type length2( void ) const
    {
        return _v[0]*_v[0] + _v[1]*_v[1];
    }

    /** Normalize the vector so that it has length unity.
     * Returns the previous length of the vector.
     */
    inline value_type normalize()
    {
        value_type norm = Vec2f::length();
        if (norm>0.0)
        {
            value_type inv = 1.0f/norm;
            _v[0] *= inv;
            _v[1] *= inv;
        }
        return( norm );
    }

};    // end of class Vec2f


class Vec2d
{
public:

    /** Data type of vector components.*/
    typedef double value_type;

    /** Number of vector components. */
    enum { num_components = 2 };

    value_type _v[2];

    /** Constructor that sets all components of the vector to zero */
    Vec2d() {_v[0]=0.0; _v[1]=0.0;}

    Vec2d(value_type x,value_type y) { _v[0]=x; _v[1]=y; }

    inline Vec2d(const Vec2f& vec) { _v[0]=vec._v[0]; _v[1]=vec._v[1]; }

    inline operator Vec2f() const { return Vec2f(static_cast<float>(_v[0]),static_cast<float>(_v[1]));}


    inline bool operator == (const Vec2d& v) const { return _v[0]==v._v[0] && _v[1]==v._v[1]; }

    inline bool operator != (const Vec2d& v) const { return _v[0]!=v._v[0] || _v[1]!=v._v[1]; }

    inline bool operator <  (const Vec2d& v) const
    {
        if (_v[0]<v._v[0]) return true;
        else if (_v[0]>v._v[0]) return false;
        else return (_v[1]<v._v[1]);
    }

    inline value_type* ptr() { return _v; }
    inline const value_type* ptr() const { return _v; }

    inline void set( value_type x, value_type y ) { _v[0]=x; _v[1]=y; }

    inline value_type& operator [] (int i) { return _v[i]; }
    inline value_type operator [] (int i) const { return _v[i]; }

    inline value_type& x() { return _v[0]; }
    inline value_type& y() { return _v[1]; }

    inline value_type x() const { return _v[0]; }
    inline value_type y() const { return _v[1]; }

    /** Dot product. */
    inline value_type operator * (const Vec2d& rhs) const
    {
        return _v[0]*rhs._v[0]+_v[1]*rhs._v[1];
    }

    /** Multiply by scalar. */
    inline const Vec2d operator * (value_type rhs) const
    {
        return Vec2d(_v[0]*rhs, _v[1]*rhs);
    }

    /** Unary multiply by scalar. */
    inline Vec2d& operator *= (value_type rhs)
    {
        _v[0]*=rhs;
        _v[1]*=rhs;
        return *this;
    }

    /** Divide by scalar. */
    inline const Vec2d operator / (value_type rhs) const
    {
        return Vec2d(_v[0]/rhs, _v[1]/rhs);
    }

    /** Unary divide by scalar. */
    inline Vec2d& operator /= (value_type rhs)
    {
        _v[0]/=rhs;
        _v[1]/=rhs;
        return *this;
    }

    /** Binary vector add. */
    inline const Vec2d operator + (const Vec2d& rhs) const
    {
        return Vec2d(_v[0]+rhs._v[0], _v[1]+rhs._v[1]);
    }

    /** Unary vector add. Slightly more efficient because no temporary
     * intermediate object.
     */
    inline Vec2d& operator += (const Vec2d& rhs)
    {
        _v[0] += rhs._v[0];
        _v[1] += rhs._v[1];
        return *this;
    }

    /** Binary vector subtract. */
    inline const Vec2d operator - (const Vec2d& rhs) const
    {
        return Vec2d(_v[0]-rhs._v[0], _v[1]-rhs._v[1]);
    }

    /** Unary vector subtract. */
    inline Vec2d& operator -= (const Vec2d& rhs)
    {
        _v[0]-=rhs._v[0];
        _v[1]-=rhs._v[1];
        return *this;
    }

    /** Negation operator. Returns the negative of the Vec2d. */
    inline const Vec2d operator - () const
    {
        return Vec2d (-_v[0], -_v[1]);
    }

    /** Length of the vector = sqrt( vec . vec ) */
    inline value_type length() const
    {
        return sqrt( _v[0]*_v[0] + _v[1]*_v[1] );
    }

    /** Length squared of the vector = vec . vec */
    inline value_type length2( void ) const
    {
        return _v[0]*_v[0] + _v[1]*_v[1];
    }

    /** Normalize the vector so that it has length unity.
     * Returns the previous length of the vector.
     */
    inline value_type normalize()
    {
        value_type norm = Vec2d::length();
        if (norm>0.0)
        {
            value_type inv = 1.0/norm;
            _v[0] *= inv;
            _v[1] *= inv;
        }
        return( norm );
    }

};    // end of class Vec2d


class Vec3f
{
public:

    /** Data type of vector components.*/
    typedef float value_type;

    /** Number of vector components. */
    enum { num_components = 3 };

    value_type _v[3];

    /** Constructor that sets all components of the vector to zero */
    Vec3f() { _v[0]=0.0f; _v[1]=0.0f; _v[2]=0.0f;}
    Vec3f(value_type x,value_type y,value_type z) { _v[0]=x; _v[1]=y; _v[2]=z; }

    inline bool operator == (const Vec3f& v) const { return _v[0]==v._v[0] && _v[1]==v._v[1] && _v[2]==v._v[2]; }

    inline bool operator != (const Vec3f& v) const { return _v[0]!=v._v[0] || _v[1]!=v._v[1] || _v[2]!=v._v[2]; }

    inline bool operator <  (const Vec3f& v) const
    {
        if (_v[0]<v._v[0]) return true;
        else if (_v[0]>v._v[0]) return false;
        else if (_v[1]<v._v[1]) return true;
        else if (_v[1]>v._v[1]) return false;
        else return (_v[2]<v._v[2]);
    }

    inline value_type* ptr() { return _v; }
    inline const value_type* ptr() const { return _v; }

    inline void set( value_type x, value_type y, value_type z)
    {
        _v[0]=x; _v[1]=y; _v[2]=z;
    }

    inline void set( const Vec3f& rhs)
    {
        _v[0]=rhs._v[0]; _v[1]=rhs._v[1]; _v[2]=rhs._v[2];
    }

    inline value_type& operator [] (int i) { return _v[i]; }
    inline value_type operator [] (int i) const { return _v[i]; }

    inline value_type& x() { return _v[0]; }
    inline value_type& y() { return _v[1]; }
    inline value_type& z() { return _v[2]; }

    inline value_type x() const { return _v[0]; }
    inline value_type y() const { return _v[1]; }
    inline value_type z() const { return _v[2]; }

    inline Vec3f max( value_type m ) {
        return Vec3f( std::max( _v[0], m ), std::max( _v[1], m ), std::max( _v[2], m ) );
    }

    /** Dot product. */
    inline value_type operator * (const Vec3f& rhs) const
    {
        return _v[0]*rhs._v[0]+_v[1]*rhs._v[1]+_v[2]*rhs._v[2];
    }

    /** Cross product. */
    inline const Vec3f operator ^ (const Vec3f& rhs) const
    {
        return Vec3f(_v[1]*rhs._v[2]-_v[2]*rhs._v[1],
                     _v[2]*rhs._v[0]-_v[0]*rhs._v[2] ,
                     _v[0]*rhs._v[1]-_v[1]*rhs._v[0]);
    }

    /** Multiply by scalar. */
    inline const Vec3f operator * (value_type rhs) const
    {
        return Vec3f(_v[0]*rhs, _v[1]*rhs, _v[2]*rhs);
    }

    /** Unary multiply by scalar. */
    inline Vec3f& operator *= (value_type rhs)
    {
        _v[0]*=rhs;
        _v[1]*=rhs;
        _v[2]*=rhs;
        return *this;
    }

    /** Divide by scalar. */
    inline const Vec3f operator / (value_type rhs) const
    {
        return Vec3f(_v[0]/rhs, _v[1]/rhs, _v[2]/rhs);
    }

    /** Unary divide by scalar. */
    inline Vec3f& operator /= (value_type rhs)
    {
        _v[0]/=rhs;
        _v[1]/=rhs;
        _v[2]/=rhs;
        return *this;
    }

    /** Binary vector add. */
    inline const Vec3f operator + (const Vec3f& rhs) const
    {
        return Vec3f(_v[0]+rhs._v[0], _v[1]+rhs._v[1], _v[2]+rhs._v[2]);
    }

    /** Unary vector add. Slightly more efficient because no temporary
     * intermediate object.
     */
    inline Vec3f& operator += (const Vec3f& rhs)
    {
        _v[0] += rhs._v[0];
        _v[1] += rhs._v[1];
        _v[2] += rhs._v[2];
        return *this;
    }

    /** Binary vector subtract. */
    inline const Vec3f operator - (const Vec3f& rhs) const
    {
        return Vec3f(_v[0]-rhs._v[0], _v[1]-rhs._v[1], _v[2]-rhs._v[2]);
    }

    /** Unary vector subtract. */
    inline Vec3f& operator -= (const Vec3f& rhs)
    {
        _v[0]-=rhs._v[0];
        _v[1]-=rhs._v[1];
        _v[2]-=rhs._v[2];
        return *this;
    }

    /** Negation operator. Returns the negative of the Vec3f. */
    inline const Vec3f operator - () const
    {
        return Vec3f (-_v[0], -_v[1], -_v[2]);
    }

    /** Length of the vector = sqrt( vec . vec ) */
    inline value_type length() const
    {
        return sqrtf( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] );
    }

    /** Length squared of the vector = vec . vec */
    inline value_type length2() const
    {
        return _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2];
    }

    /** Normalize the vector so that it has length unity.
     * Returns the previous length of the vector.
     */
    inline value_type normalize()
    {
        value_type norm = Vec3f::length();
        if (norm>0.0)
        {
            value_type inv = 1.0f/norm;
            _v[0] *= inv;
            _v[1] *= inv;
            _v[2] *= inv;
        }
        return( norm );
    }

};    // end of class Vec3f

class Vec3d
{
public:

    /** Data type of vector components.*/
    typedef double value_type;

    /** Number of vector components. */
    enum { num_components = 3 };

    value_type _v[3];

    /** Constructor that sets all components of the vector to zero */
    Vec3d() { _v[0]=0.0; _v[1]=0.0; _v[2]=0.0;}

    /** Constructor that sets all components of the vector to zero */
    Vec3d(const Vec3f& v ) { _v[0]=v[0]; _v[1]=v[1]; _v[2]=v[2];}

    inline operator Vec3f() const { return Vec3f(static_cast<float>(_v[0]),static_cast<float>(_v[1]),static_cast<float>(_v[2]));}

    Vec3d(value_type x,value_type y,value_type z) { _v[0]=x; _v[1]=y; _v[2]=z; }

    inline bool operator == (const Vec3d& v) const { return _v[0]==v._v[0] && _v[1]==v._v[1] && _v[2]==v._v[2]; }

    inline bool operator != (const Vec3d& v) const { return _v[0]!=v._v[0] || _v[1]!=v._v[1] || _v[2]!=v._v[2]; }

    inline bool operator <  (const Vec3d& v) const
    {
        if (_v[0]<v._v[0]) return true;
        else if (_v[0]>v._v[0]) return false;
        else if (_v[1]<v._v[1]) return true;
        else if (_v[1]>v._v[1]) return false;
        else return (_v[2]<v._v[2]);
    }

    inline value_type* ptr() { return _v; }
    inline const value_type* ptr() const { return _v; }

    inline void set( value_type x, value_type y, value_type z)
    {
        _v[0]=x; _v[1]=y; _v[2]=z;
    }

    inline void set( const Vec3d& rhs)
    {
        _v[0]=rhs._v[0]; _v[1]=rhs._v[1]; _v[2]=rhs._v[2];
    }

    inline value_type& operator [] (int i) { return _v[i]; }
    inline value_type operator [] (int i) const { return _v[i]; }

    inline value_type& x() { return _v[0]; }
    inline value_type& y() { return _v[1]; }
    inline value_type& z() { return _v[2]; }

    inline value_type x() const { return _v[0]; }
    inline value_type y() const { return _v[1]; }
    inline value_type z() const { return _v[2]; }

    inline Vec3f max( value_type m ) {
        return Vec3f( std::max( _v[0], m ), std::max( _v[1], m ), std::max( _v[2], m ) );
    }


    /** Dot product. */
    inline value_type operator * (const Vec3d& rhs) const
    {
        return _v[0]*rhs._v[0]+_v[1]*rhs._v[1]+_v[2]*rhs._v[2];
    }

    /** Cross product. */
    inline const Vec3d operator ^ (const Vec3d& rhs) const
    {
        return Vec3d(_v[1]*rhs._v[2]-_v[2]*rhs._v[1],
                     _v[2]*rhs._v[0]-_v[0]*rhs._v[2] ,
                     _v[0]*rhs._v[1]-_v[1]*rhs._v[0]);
    }

    /** Multiply by scalar. */
    inline const Vec3d operator * (value_type rhs) const
    {
        return Vec3d(_v[0]*rhs, _v[1]*rhs, _v[2]*rhs);
    }

    /** Unary multiply by scalar. */
    inline Vec3d& operator *= (value_type rhs)
    {
        _v[0]*=rhs;
        _v[1]*=rhs;
        _v[2]*=rhs;
        return *this;
    }

    /** Divide by scalar. */
    inline const Vec3d operator / (value_type rhs) const
    {
        return Vec3d(_v[0]/rhs, _v[1]/rhs, _v[2]/rhs);
    }

    /** Unary divide by scalar. */
    inline Vec3d& operator /= (value_type rhs)
    {
        _v[0]/=rhs;
        _v[1]/=rhs;
        _v[2]/=rhs;
        return *this;
    }

    /** Binary vector add. */
    inline const Vec3d operator + (const Vec3d& rhs) const
    {
        return Vec3d(_v[0]+rhs._v[0], _v[1]+rhs._v[1], _v[2]+rhs._v[2]);
    }

    /** Unary vector add. Slightly more efficient because no temporary
     * intermediate object.
     */
    inline Vec3d& operator += (const Vec3d& rhs)
    {
        _v[0] += rhs._v[0];
        _v[1] += rhs._v[1];
        _v[2] += rhs._v[2];
        return *this;
    }

    /** Binary vector subtract. */
    inline const Vec3d operator - (const Vec3d& rhs) const
    {
        return Vec3d(_v[0]-rhs._v[0], _v[1]-rhs._v[1], _v[2]-rhs._v[2]);
    }

    /** Unary vector subtract. */
    inline Vec3d& operator -= (const Vec3d& rhs)
    {
        _v[0]-=rhs._v[0];
        _v[1]-=rhs._v[1];
        _v[2]-=rhs._v[2];
        return *this;
    }

    /** Negation operator. Returns the negative of the Vec3d. */
    inline const Vec3d operator - () const
    {
        return Vec3d (-_v[0], -_v[1], -_v[2]);
    }

    /** Length of the vector = sqrt( vec . vec ) */
    inline value_type length() const
    {
        return sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] );
    }

    /** Length squared of the vector = vec . vec */
    inline value_type length2() const
    {
        return _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2];
    }

    /** Normalize the vector so that it has length unity.
     * Returns the previous length of the vector.
     * If the vector is zero length, it is left unchanged and zero is returned.
     */
    inline value_type normalize()
    {
        value_type norm = Vec3d::length();
        if (norm>0.0)
        {
            value_type inv = 1.0/norm;
            _v[0] *= inv;
            _v[1] *= inv;
            _v[2] *= inv;
        }
        return( norm );
    }

};    // end of class Vec3d


class Vec4f
{
public:

    /** Data type of vector components.*/
    typedef float value_type;

    /** Number of vector components. */
    enum { num_components = 4 };

    /** Vec member variable. */
    value_type _v[4];

    // Methods are defined here so that they are implicitly inlined

    /** Constructor that sets all components of the vector to zero */
    Vec4f() { _v[0]=0.0f; _v[1]=0.0f; _v[2]=0.0f; _v[3]=0.0f;}

    Vec4f(value_type x, value_type y, value_type z, value_type w)
    {
        _v[0]=x;
        _v[1]=y;
        _v[2]=z;
        _v[3]=w;
    }

    Vec4f(const Vec3f& v3,value_type w)
    {
        _v[0]=v3[0];
        _v[1]=v3[1];
        _v[2]=v3[2];
        _v[3]=w;
    }

    inline bool operator == (const Vec4f& v) const { return _v[0]==v._v[0] && _v[1]==v._v[1] && _v[2]==v._v[2] && _v[3]==v._v[3]; }

    inline bool operator != (const Vec4f& v) const { return _v[0]!=v._v[0] || _v[1]!=v._v[1] || _v[2]!=v._v[2] || _v[3]!=v._v[3]; }

    inline bool operator <  (const Vec4f& v) const
    {
        if (_v[0]<v._v[0]) return true;
        else if (_v[0]>v._v[0]) return false;
        else if (_v[1]<v._v[1]) return true;
        else if (_v[1]>v._v[1]) return false;
        else if (_v[2]<v._v[2]) return true;
        else if (_v[2]>v._v[2]) return false;
        else return (_v[3]<v._v[3]);
    }

    inline value_type* ptr() { return _v; }
    inline const value_type* ptr() const { return _v; }

    inline void set( value_type x, value_type y, value_type z, value_type w)
    {
        _v[0]=x; _v[1]=y; _v[2]=z; _v[3]=w;
    }

    inline value_type& operator [] (unsigned int i) { return _v[i]; }
    inline value_type  operator [] (unsigned int i) const { return _v[i]; }

    inline value_type& x() { return _v[0]; }
    inline value_type& y() { return _v[1]; }
    inline value_type& z() { return _v[2]; }
    inline value_type& w() { return _v[3]; }

    inline value_type x() const { return _v[0]; }
    inline value_type y() const { return _v[1]; }
    inline value_type z() const { return _v[2]; }
    inline value_type w() const { return _v[3]; }

    inline value_type& r() { return _v[0]; }
    inline value_type& g() { return _v[1]; }
    inline value_type& b() { return _v[2]; }
    inline value_type& a() { return _v[3]; }

    inline value_type r() const { return _v[0]; }
    inline value_type g() const { return _v[1]; }
    inline value_type b() const { return _v[2]; }
    inline value_type a() const { return _v[3]; }

    inline Vec4f max( value_type m ) {
        return Vec4f( std::max( _v[0], m ), std::max( _v[1], m ), std::max( _v[2], m ), std::max( _v[3], m ) );
    }

    /** Returns true if all components have values that are not NaN. */
    inline bool valid() const { return !isNaN(); }
    /** Returns true if at least one component has value NaN. */
    inline bool isNaN() const { return ::isNaN(_v[0]) || ::isNaN(_v[1]) || ::isNaN(_v[2]) || ::isNaN(_v[3]); }

    /** Dot product. */
    inline value_type operator * (const Vec4f& rhs) const
    {
        return _v[0]*rhs._v[0]+
            _v[1]*rhs._v[1]+
            _v[2]*rhs._v[2]+
            _v[3]*rhs._v[3] ;
    }

    /** Multiply by scalar. */
    inline Vec4f operator * (value_type rhs) const
    {
        return Vec4f(_v[0]*rhs, _v[1]*rhs, _v[2]*rhs, _v[3]*rhs);
    }

    /** Unary multiply by scalar. */
    inline Vec4f& operator *= (value_type rhs)
    {
        _v[0]*=rhs;
        _v[1]*=rhs;
        _v[2]*=rhs;
        _v[3]*=rhs;
        return *this;
    }

    /** Divide by scalar. */
    inline Vec4f operator / (value_type rhs) const
    {
        return Vec4f(_v[0]/rhs, _v[1]/rhs, _v[2]/rhs, _v[3]/rhs);
    }

    /** Unary divide by scalar. */
    inline Vec4f& operator /= (value_type rhs)
    {
        _v[0]/=rhs;
        _v[1]/=rhs;
        _v[2]/=rhs;
        _v[3]/=rhs;
        return *this;
    }

    /** Binary vector add. */
    inline Vec4f operator + (const Vec4f& rhs) const
    {
        return Vec4f(_v[0]+rhs._v[0], _v[1]+rhs._v[1],
                     _v[2]+rhs._v[2], _v[3]+rhs._v[3]);
    }

    /** Unary vector add. Slightly more efficient because no temporary
     * intermediate object.
     */
    inline Vec4f& operator += (const Vec4f& rhs)
    {
        _v[0] += rhs._v[0];
        _v[1] += rhs._v[1];
        _v[2] += rhs._v[2];
        _v[3] += rhs._v[3];
        return *this;
    }

    /** Binary vector subtract. */
    inline Vec4f operator - (const Vec4f& rhs) const
    {
        return Vec4f(_v[0]-rhs._v[0], _v[1]-rhs._v[1],
                     _v[2]-rhs._v[2], _v[3]-rhs._v[3] );
    }

    /** Unary vector subtract. */
    inline Vec4f& operator -= (const Vec4f& rhs)
    {
        _v[0]-=rhs._v[0];
        _v[1]-=rhs._v[1];
        _v[2]-=rhs._v[2];
        _v[3]-=rhs._v[3];
        return *this;
    }

    /** Negation operator. Returns the negative of the Vec4f. */
    inline const Vec4f operator - () const
    {
        return Vec4f (-_v[0], -_v[1], -_v[2], -_v[3]);
    }

    /** Length of the vector = sqrt( vec . vec ) */
    inline value_type length() const
    {
        return sqrtf( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3]);
    }

    /** Length squared of the vector = vec . vec */
    inline value_type length2() const
    {
        return _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3];
    }

    /** Normalize the vector so that it has length unity.
     * Returns the previous length of the vector.
     */
    inline value_type normalize()
    {
        value_type norm = Vec4f::length();
        if (norm>0.0f)
        {
            value_type inv = 1.0f/norm;
            _v[0] *= inv;
            _v[1] *= inv;
            _v[2] *= inv;
            _v[3] *= inv;
        }
        return( norm );
    }

};    // end of class Vec4f

inline Vec3f componentMultiply(const Vec3f& lhs, const Vec3f& rhs)
{
    return Vec3f(lhs[0]*rhs[0], lhs[1]*rhs[1], lhs[2]*rhs[2]);
}

inline Vec3d componentMultiply(const Vec3d& lhs, const Vec3d& rhs)
{
    return Vec3d(lhs[0]*rhs[0], lhs[1]*rhs[1], lhs[2]*rhs[2]);
}

inline void slerp( Vec3f& res, const Vec3f& a, const Vec3f& b, float t)
{
    float angle = acosf(a * b);

    if (0.0f == angle)
    {
        res[0] = a[0];
        res[1] = a[1];
        res[2] = a[2];
    }
    else if (PI == angle)
    {
        // Can't recovert!
        res[0] = 0;
        res[1] = 0;
        res[2] = 0;
    }
    else
    {
        res[0] = (sinf((1.0-t)*angle)/sinf(angle))*a[0] + (sinf(t*angle)/sinf(angle))*b[0];
        res[1] = (sinf((1.0-t)*angle)/sinf(angle))*a[1] + (sinf(t*angle)/sinf(angle))*b[1];
        res[2] = (sinf((1.0-t)*angle)/sinf(angle))*a[2] + (sinf(t*angle)/sinf(angle))*b[2];
    }
}

template <typename T>
inline T lerp( const T& a, const T& b, float t)
{
    return a + (( b-a ) * t );
}



inline double radicalInverse_VdC(uint bits) {
    bits = (bits << 16) | (bits >> 16);
    bits = ((bits & 0x55555555) << 1) | ((bits & 0xAAAAAAAA) >> 1);
    bits = ((bits & 0x33333333) << 2) | ((bits & 0xCCCCCCCC) >> 2);
    bits = ((bits & 0x0F0F0F0F) << 4) | ((bits & 0xF0F0F0F0) >> 4);
    bits = ((bits & 0x00FF00FF) << 8) | ((bits & 0xFF00FF00) >> 8);
    return double(bits) * 2.3283064365386963e-10; // / 0x100000000
}


static Vec2f cacheHammersleySamples[MAX_SAMPLES_CACHE];
static unsigned int cacheHammersleyNbSamples = 0;

inline const Vec2f& hammersley(unsigned int i, unsigned int N) {
#if 0
    if (cacheHammersleyNbSamples != N) {
        cacheHammersleyNbSamples = N;
        for ( unsigned int  j = 0; j < N; j++ ) {
            cacheHammersleySamples[j] = Vec2f(float(j)/float(N), radicalInverse_VdC(j));
        }
        cacheHammersleyNbSamples = N;
    }
#else
    cacheHammersleySamples[i] = Vec2f(float(i)/float(N), radicalInverse_VdC(i));
#endif
    return cacheHammersleySamples[i];
}


template <typename T>
inline T cross( const T& a, const T& b ) {
    return a^b;
}

template <typename T>
inline T normalize( const T& a ) {
    return a * ( 1.0 / a.length() );
}

template <typename T>
inline typename T::value_type dot( const T& a, const T& b ) {
    return a * b;
}

template<typename T>
inline T saturate(T v) { return v<0.0?0.0:v>1.0?1.0:v; }

inline Vec3f importanceSampleGGX( unsigned int i, unsigned int numSamples, float roughnessLinear )
{
    Vec2f Xi = hammersley( i, numSamples );

    float roughness = roughnessLinear * roughnessLinear;
    float Phi = 2.0 * PI * Xi[0];
    float CosTheta = sqrt( (1.0 - Xi[1]) / ( 1.0 + ( roughness * roughness - 1.0) * Xi[1] ) );
    float SinTheta = sqrt( 1.0 - CosTheta * CosTheta );
    Vec3f H;
    H[0] = SinTheta * cos( Phi );
    H[1] = SinTheta * sin( Phi );
    H[2] = CosTheta;
    H.normalize();
    return H;
}

inline float D_GGX( float NdotH, float alpha)
{
    // use GGX / Trowbridge-Reitz, same as Disney and Unreal 4
    // cf http://blog.selfshadow.com/publications/s2013-shading-course/karis/s2013_pbs_epic_notes_v2.pdf p3
    float tmp = alpha / (NdotH*NdotH*(alpha*alpha-1.0)+1.0);
    return tmp * tmp * PI_INV;
}



static Vec4f cachePrecomputedLightSample[MAX_SAMPLES_CACHE];
static float cachePrecomputedLightRoughness = -1;
static double cachePrecomputedLightTotalWeight = 0.0;

inline bool computeLightSampleInLocalSpace(uint i, uint numSamples, uint size, float roughnessLinear, Vec4f& result)
{
    // do the computation in local space and store the computed light vector L
    // in local space. It will be transformed in the main loop from tangent space

    // https://placeholderart.wordpress.com/2015/07/28/implementation-notes-runtime-environment-map-filtering-for-image-based-lighting/
    // in local space view == normal == 0,0,1
    static const Vec3f V = Vec3f(0,0,1);

    Vec2f Xi = hammersley( i, numSamples );
    // std::cout << i << " " << Xi[0] << " " << Xi[1] << std::endl;
    float roughness = roughnessLinear * roughnessLinear;
    float Phi = 2.0 * PI * Xi[0];
    float CosTheta = sqrt( (1.0 - Xi[1]) / ( 1.0 + ( roughness * roughness - 1.0) * Xi[1] ) );
    float SinTheta = sqrt( 1.0 - CosTheta * CosTheta );
    Vec3f H;
    H[0] = SinTheta * cos( Phi );
    H[1] = SinTheta * sin( Phi );
    H[2] = CosTheta;
    H.normalize();

    Vec3f L =  H * ( dot( V, H ) * 2.0 ) - V;
    L.normalize();

    float NoL = L[2];
    if ( NoL <= 0.0f ) return false;

    // H[2] is NoH in local space
    // adds 1.e-5 to avoid D_GGX / 0.0
    float NoH = H[2] + 1.e-5;

    // data to evaluate pdf
    float VoH = NoH;

    // Probability Distribution Function
    float Pdf = D_GGX(NoH, roughnessLinear) * NoH / (4.0f * VoH);

    // Solid angle represented by this sample
    float omegaS = 1.0 / (numSamples * Pdf);

    // Solid angle covered by 1 pixel with 6 faces that are EnvMapSize X EnvMapSize
    const float omegaP = 4.0 * PI / ( 6.0 * size * size);

    // Original paper suggest biasing the mip to improve the results
    float mipBias = 1.0f; // I tested that the result is better with bias 1
    double maxLod = log2(size);
    float mipLevel = std::min( std::max(0.5 * log2(omegaS / omegaP) + mipBias, 0.0), maxLod);

    result[0] = L[0];
    result[1] = L[1];
    result[2] = L[2];
    result[3] = mipLevel;

    return true;
}

inline Vec4f* getPrecomputedLightCache() {
    return cachePrecomputedLightSample;
}

inline void precomputedLightInLocalSpace( uint numSamples, float roughnessLinear, uint size = 0 )
{
    if ( cachePrecomputedLightRoughness != roughnessLinear ) {

        Vec4f result;
        uint tryNumSamples = numSamples;
        bool found = false;
        uint nbTry = 0;
        while ( !found ) {
            nbTry++;
            uint count = 0;
            uint index = 0;
            // find the sequence to have desired sample hit NoL condition
            cachePrecomputedLightTotalWeight = 0.0;
            for ( uint a = 0; a < tryNumSamples; a++ ) {
                if ( computeLightSampleInLocalSpace(a, tryNumSamples, size, roughnessLinear, result ) ) {
                    count++;
                    cachePrecomputedLightSample[index++] = result;
                    cachePrecomputedLightTotalWeight += result[2]; // accumulate totalWeight
                }
            }
            if ( count == numSamples ) {
                found = true;
            } else {
                tryNumSamples += numSamples - count;
            }
        }

#if 0
        // for debug
        std::cout << "# roughness " << roughnessLinear << " sum samples " << numSamples << std::endl;
        std::cout << "samples = [ ";
        for ( uint a = 0; a < numSamples-1; a++ ) {
            for ( uint b = 0; b < 4; b++ )
                std::cout << cachePrecomputedLightSample[a][b] << " , ";
        }
        for ( uint b = 0; b < 3; b++ )
            std::cout << cachePrecomputedLightSample[numSamples-1][b] << " , ";
        std::cout << cachePrecomputedLightSample[numSamples-1][3] << "]" << std::endl;
#endif
        std::cout << "roughnessLin " << roughnessLinear << " : found the sequence " << tryNumSamples << " to generate " << numSamples << " samples valid in " << nbTry << " try" << std::endl;

        cachePrecomputedLightRoughness = roughnessLinear;

    }
}
// heuristics to compute faster samples
// roughness 0.2 ratio hits 99.8535%
// roughness 0.4 ratio hits 97.5098%
// roughness 0.6 ratio hits 88.5742%
// roughness 0.8 ratio hits 70.9473%
// roughness 1   ratio hits 50%

inline const Vec4f& getPrecomputedLightInLocalSpace( unsigned int i )
{
    // we use a trigger to reset the cache if needed
    return cachePrecomputedLightSample[i];
}

inline const double& getPrecomputedLightTotalWeight()
{
    // we use a trigger to reset the cache if needed
    return cachePrecomputedLightTotalWeight;
}


static Vec4f cachePrecomputedUniformSampleOnCone[MAX_SAMPLES_CACHE];
static uint cachePrecomputedUniformSampleOnConeNbSamples = 0;
static double cachePrecomputedUniformSampleOnConeWeightSum = 0.0;

inline void precomputeUniformSampleOnCone( uint numSamples, const float radius, const float sigmaSqr )
{
    if ( cachePrecomputedUniformSampleOnConeNbSamples != numSamples ) {
        cachePrecomputedUniformSampleOnConeNbSamples = numSamples;
        double wSum = 0.0;
        for ( uint i = 0; i < numSamples; i++ ) {

            Vec2f Xi = hammersley( i, numSamples );

            //  http://jsfiddle.net/d9VRu/
            float u = Xi[0];
            float v = Xi[1];
            float angle = u * PI * 2.0;

            // uniform
            float r = sqrtf( v ) * radius;

            // not uniform
            //float r = v * radius;

            float x = r * cosf(angle);
            float y = r * sinf(angle);

            // compute gaussian weight
            // https://en.wikipedia.org/wiki/Gaussian_blur
            // http://stackoverflow.com/questions/17841098/gaussian-blur-standard-deviation-radius-and-kernel-size
            // http://http.developer.nvidia.com/GPUGems3/gpugems3_ch40.html
            //float standardDeviation = 0.84089642;
            //float sigmaSqr = sigma*sigma;
            // weight = exp(-(x*x + y*y)/twoStandardDeviationSqr)/( PI * twoStandardDeviationSqr );
            double weight = exp(-0.5*(x*x + y*y)/sigmaSqr);

            Vec3f H;
            H[0] = x;
            H[1] = y;
            H[2] = 1.0;
            H.normalize();

            cachePrecomputedUniformSampleOnCone[i] = Vec4f( H[0], H[1], H[2], (float)weight );
            wSum += weight;
        }
        cachePrecomputedUniformSampleOnConeWeightSum = wSum;
    }
}

inline const Vec4f& getUniformSampleOnCone(uint i) {
    return cachePrecomputedUniformSampleOnCone[i];
}

inline const double& getUniformSampleOnConeWeightSum() {
    return cachePrecomputedUniformSampleOnConeWeightSum;
}

// vec3 hemisphereSample_uniform(float u, float v) {
//      float phi = v * 2.0 * PI;
//      float cosTheta = 1.0 - u;
//      float sinTheta = sqrt(1.0 - cosTheta * cosTheta);
//      return vec3(cos(phi) * sinTheta, sin(phi) * sinTheta, cosTheta);
//  }

//  vec3 hemisphereSample_cos(float u, float v) {
//      float phi = v * 2.0 * PI;
//      float cosTheta = sqrt(1.0 - u);
//      float sinTheta = sqrt(1.0 - cosTheta * cosTheta);
//      return vec3(cos(phi) * sinTheta, sin(phi) * sinTheta, cosTheta);
//  }

// rgb to luminance
template <typename T> T luminance(const T r, const T g, const T b) {
    return r * 0.2125f + g * 0.7154f + b * 0.0721f;
}

// solid Angle
template <typename T> T AreaElement( const T x, const T y )
{
    return atan2(x * y, sqrt(x * x + y * y + 1.0f));
}

// solid Angle: aka resolution independent results
inline double texelPixelSolidAngleCommon(const float aU, const float aV, const uint width, const uint height)
{
    // transform from [0..res - 1] to [- (1 - 1 / res) .. (1 - 1 / res)]
    // ( 0.5 is for texel center addressing)
    const double U = (2.0 * (aU  + 0.5) / width ) - 1.0;
    const double V = (2.0 * (aV  + 0.5) / height ) - 1.0;

    // Shift from a demi texel, mean 1.0 / size  with U and V in [-1..1]
    const double InvResolutionW = 1.0 / width;
    const double InvResolutionH = 1.0 / height;

    // U and V are the -1..1 texture coordinate on the current face.
    // Get projected area for this Texel
    const double x0 = U - InvResolutionW;
    const double y0 = V - InvResolutionH;
    const double x1 = U + InvResolutionW;
    const double y1 = V + InvResolutionH;
    const double SolidAngle = AreaElement(x0, y0) - AreaElement(x0, y1) - AreaElement(x1, y0)  + AreaElement(x1, y1);

    return SolidAngle;
}

// a pixel to a solidAngle
inline double texelPixelSolidAnglePanorama(const float aU, const float aV, const uint width, const uint height)
{
    return texelPixelSolidAngleCommon( aU, aV, width, height );
}

inline double texelPixelSolidAngleCubeMap(const float aU, const float aV, uint size)
{
    return texelPixelSolidAngleCommon( aU, aV , size, size );
}

// solid Angle: aka resolution independent results
inline double texelPixelSolidAnglePanorama(const double aU, const double aV, const uint width, const uint height)
{
    // transform from [0..res - 1] to [- (1 - 1 / res) .. (1 - 1 / res)]
    // ( 0.5 is for texel center addressing)
    const double U = (2.0 * (aU  + 0.5) / width ) - 1.0;
    const double V = (2.0 * (aV  + 0.5) / height ) - 1.0;

    // Shift from a demi texel, mean 1.0 / size  with U and V in [-1..1]
    const double InvResolutionW = 1.0 / width;
    const double InvResolutionH = 1.0 / height;

    // U and V are the -1..1 texture coordinate on the current face.
    // Get projected area for this Texel
    const double x0 = U - InvResolutionW;
    const double y0 = V - InvResolutionH;
    const double x1 = U + InvResolutionW;
    const double y1 = V + InvResolutionH;
    const double SolidAngle = AreaElement(x0, y0) - AreaElement(x0, y1) - AreaElement(x1, y0)  + AreaElement(x1, y1);

    return SolidAngle;
}



static Vec3f CubemapFace[6][3] = {
    { Vec3f(0,0,-1), Vec3f(0,-1,0), Vec3f(1,0,0) },// x positif
    { Vec3f(0,0,1), Vec3f(0,-1,0), Vec3f(-1,0,0) }, // x negatif

    { Vec3f(1,0,0), Vec3f(0,0,1), Vec3f(0,1,0) },  // y positif
    { Vec3f(1,0,0), Vec3f(0,0,-1),Vec3f(0,-1,0) }, // y negatif

    { Vec3f(1,0,0), Vec3f(0,-1,0), Vec3f(0,0,1) },  // z positif
    { Vec3f(-1,0,0), Vec3f(0,-1,0),Vec3f(0,0,-1) } // z negatif
};

// major axis
// direction     target                              sc     tc    ma
// ----------    ---------------------------------   ---    ---   ---
//  +rx          GL_TEXTURE_CUBE_MAP_POSITIVE_X_EXT   -rz    -ry   rx
//  -rx          GL_TEXTURE_CUBE_MAP_NEGATIVE_X_EXT   +rz    -ry   rx
//  +ry          GL_TEXTURE_CUBE_MAP_POSITIVE_Y_EXT   +rx    +rz   ry
//  -ry          GL_TEXTURE_CUBE_MAP_NEGATIVE_Y_EXT   +rx    -rz   ry
//  +rz          GL_TEXTURE_CUBE_MAP_POSITIVE_Z_EXT   +rx    -ry   rz
//  -rz          GL_TEXTURE_CUBE_MAP_NEGATIVE_Z_EXT   -rx    -ry   rz
// s   =   ( sc/|ma| + 1 ) / 2
// t   =   ( tc/|ma| + 1 ) / 2

inline void vectToTexelCoordGeneric(const Vec3f& direction, uint width, uint height, float& u, float& v, int& faceIndex)
{
    int bestAxis = 0;
    if ( fabs(direction[1]) > fabs(direction[0]) ) {
        bestAxis = 1;
        if ( fabs(direction[2]) > fabs(direction[1]) )
            bestAxis = 2;
    } else if ( fabs(direction[2]) > fabs(direction[0]) )
        bestAxis = 2;

    // select the index of cubemap face
    faceIndex = bestAxis*2 + ( direction[bestAxis] > 0 ? 0 : 1 );
    float bestAxisValue = direction[bestAxis];
    float denom = fabs( bestAxisValue );

    //float maInv = 1.0/denom;
    Vec3f dir = direction * 1.0/denom;

    float sc = CubemapFace[faceIndex][0] * dir;
    float tc = CubemapFace[faceIndex][1] * dir;
    float ppx = (sc + 1.0) * 0.5 * (width - 1);
    float ppy = (tc + 1.0) * 0.5 * (height - 1);

    u = ppx;
    v = ppy;
}

inline void vectToTexelCoordCubeMap(const Vec3f& direction, uint size, float& u, float& v, int& faceIndex ) {
    vectToTexelCoordGeneric( direction, size, size, u, v, faceIndex );
}

inline void vectToTexelCoordPanorama(const Vec3f& direction, uint width, uint height, float& u, float& v ) {
    int faceIndex = 0;
    vectToTexelCoordGeneric( direction, width, height, u, v, faceIndex );
}