/**
*  ofxPoint3f.h
*  by stefanix
*/


#ifndef _OFX_POINT3f
#define _OFX_POINT3f

#include "ofxVec3f.h"



class ofxPoint3f : public ofPoint {


  public:

    ofxPoint3f( float _x=0.0f, float _y=0.0f, float _z=0.0f ) {
        x = _x;
        y = _y;
        z = _z;
    }




    // Getters and Setters.
    //
    //
    void set( float _x, float _y, float _z ) {
        x = _x;
        y = _y;
        z = _z;
    }

    void set( const ofxPoint3f& pnt ) {
        x = pnt.x;
        y = pnt.y;
        z = pnt.z;
    }

    float& operator[]( const int &i ) {
        switch(i) {
            case 0:  return x;
            case 1:  return y;
            case 2:  return z;
            default: return x;
        }
    }



    // Check similarity/equality.
    //
    //
    bool operator==( const ofxPoint3f pnt ) {
        return (x == pnt.x) && (y == pnt.y) && (z == pnt.z);
    }

    bool operator!=( const ofxPoint3f pnt ) {
        return (x != pnt.x) || (y != pnt.y) || (z != pnt.z);
    }

    bool match( const ofxPoint3f pnt, float tollerance=0.0001) {
        return (fabs(x - pnt.x) < tollerance)
            && (fabs(y - pnt.y) < tollerance)
            && (fabs(z - pnt.z) < tollerance);
    }



    // Translate point by vector.
    //
    //
    ofxPoint3f operator+( const ofVec3f& vec ) const {
        return ofxPoint3f( x+vec.x, y+vec.y, z+vec.z );
    }

    ofxPoint3f& operator+=( const ofVec3f& vec ) {
        x += vec.x;
        y += vec.y;
        z += vec.z;
        return *this;
    }

	ofxPoint3f operator+( const float f ) const {
        return ofxPoint3f( x+f, y+f, z+f);
    }

    ofxPoint3f& operator+=( const float f ) {
        x += f;
        y += f;
		z += f;
        return *this;
    }

    ofxPoint3f operator-( const ofVec3f& vec ) const {
        return ofxPoint3f( x-vec.x, y-vec.y, z-vec.z );
    }

    ofxPoint3f& operator-=( const ofVec3f& vec ) {
        x -= vec.x;
        y -= vec.y;
        z -= vec.z;
        return *this;
    }

	ofxPoint3f operator-( const float f ) const {
        return ofxPoint3f( x-f, y-f, z-f);
    }

    ofxPoint3f& operator-=( const float f ) {
        x -= f;
        y -= f;
		z -= f;
        return *this;
    }

    ofxPoint3f operator-() const {
        return ofxPoint3f( -x, -y, -z );
    }



    // Scale point (by scalar or vector).
    //
    //
    ofxPoint3f operator*( const float f ) const {
        return ofxPoint3f( x*f, y*f, z*f );
    }

    ofxPoint3f& operator*=( const float f ) {
        x*=f;
        y*=f;
        z*=f;
        return *this;
    }

    ofxPoint3f operator*( const ofVec3f& vec ) const {
        return ofxPoint3f( x*vec.x, y*vec.y, z*vec.z );
    }

    ofxPoint3f& operator*=( const ofVec3f& vec ) {
        x*=vec.x;
        y*=vec.y;
        z*=vec.z;
        return *this;
    }

    ofxPoint3f operator/( const float f ) const {
        return ofxPoint3f( x/f, y/f, z/f );
    }

    ofxPoint3f& operator/=( const float f ) {
        x/=f;
        y/=f;
        z/=f;
        return *this;
    }

    ofxPoint3f operator/( const ofVec3f& vec ) const {
        return ofxPoint3f( x/vec.x, y/vec.y, z/vec.z );
    }

    ofxPoint3f& operator/=( const ofVec3f& vec ) {
        x/=vec.x;
        y/=vec.y;
        z/=vec.z;
        return *this;
    }



    // Rotate point by angle (deg) around line defined by pivot and axis.
    //
    //
    ofxPoint3f rotated( float angle,
                       const ofxPoint3f& pivot,
                       const ofVec3f& axis ) const
    {
        ofVec3f ax = axis.normalized();
        float tx = x - pivot.x;
        float ty = y - pivot.y;
        float tz = z - pivot.z;

        float a = angle*DEG_TO_RAD;
        float sina = sin( a );
        float cosa = cos( a );
        float cosb = 1.0 - cosa;

        float xrot = tx*(ax.x*ax.x*cosb + cosa)
                   + ty*(ax.x*ax.y*cosb - ax.z*sina)
                   + tz*(ax.x*ax.z*cosb + ax.y*sina);
        float yrot = tx*(ax.y*ax.x*cosb + ax.z*sina)
                   + ty*(ax.y*ax.y*cosb + cosa)
                   + tz*(ax.y*ax.z*cosb - ax.x*sina);
        float zrot = tx*(ax.z*ax.x*cosb - ax.y*sina)
                   + ty*(ax.z*ax.y*cosb + ax.x*sina)
                   + tz*(ax.z*ax.z*cosb + cosa);


        return ofxPoint3f( xrot+pivot.x, yrot+pivot.y, zrot+pivot.z );
    }

    ofxPoint3f& rotate( float angle,
                       const ofxPoint3f& pivot,
                       const ofVec3f& axis )
    {
        ofVec3f ax = axis.normalized();
        x -= pivot.x;
        y -= pivot.y;
        z -= pivot.z;

        float a = angle*DEG_TO_RAD;
        float sina = sin( a );
        float cosa = cos( a );
        float cosb = 1.0 - cosa;

        float xrot = x*(ax.x*ax.x*cosb + cosa)
                   + y*(ax.x*ax.y*cosb - ax.z*sina)
                   + z*(ax.x*ax.z*cosb + ax.y*sina);
        float yrot = x*(ax.y*ax.x*cosb + ax.z*sina)
                   + y*(ax.y*ax.y*cosb + cosa)
                   + z*(ax.y*ax.z*cosb - ax.x*sina);
        float zrot = x*(ax.z*ax.x*cosb - ax.y*sina)
                   + y*(ax.z*ax.y*cosb + ax.x*sina)
                   + z*(ax.z*ax.z*cosb + cosa);

        x = xrot + pivot.x;
        y = yrot + pivot.y;
        z = zrot + pivot.z;

        return *this;
    }



    // Map point to coordinate system defined by origin, vx, vy, and vz.
    //
    //
    ofxPoint3f mapped( const ofxPoint3f& origin,
                      const ofVec3f& vx,
                      const ofVec3f& vy,
                      const ofVec3f& vz ) const
    {
        return ofxPoint3f( origin.x + x*vx.x + y*vy.x + z*vz.x,
                          origin.y + x*vx.y + y*vy.y + z*vz.y,
                          origin.z + x*vx.z + y*vy.z + z*vz.z );
    }

    ofxPoint3f& map( const ofxPoint3f& origin,
                    const ofVec3f& vx,
                    const ofVec3f& vy,
                    const ofVec3f& vz )
    {
        float xmap = origin.x + x*vx.x + y*vy.x + z*vz.x;
        float ymap =  origin.y + x*vx.y + y*vy.y + z*vz.y;
        z = origin.z + x*vx.z + y*vy.z + z*vz.z;
        x = xmap;
        y = ymap;
        return *this;
    }


    /**
    * Return the difference vector between two points.
    */
    ofVec3f operator-( const ofxPoint3f& pnt ) const {
        return ofVec3f( x-pnt.x, y-pnt.y, z-pnt.z );
    }


    // Distance between two points.
    //
    //
    float distance( const ofxPoint3f& pnt) const {
        float vx = x-pnt.x;
        float vy = y-pnt.y;
        float vz = z-pnt.z;
        return (float)sqrt(vx*vx + vy*vy + vz*vz);
    }

    float distanceSquared( const ofxPoint3f& pnt ) const {
        float vx = x-pnt.x;
        float vy = y-pnt.y;
        float vz = z-pnt.z;
        return vx*vx + vy*vy + vz*vz;
    }



    // Linear interpolation.
    //
    //
    /**
    * p==0.0 results in this point, p==0.5 results in the
    * midpoint, and p==1.0 results in pnt being returned.
    */
    ofxPoint3f interpolate( const ofxPoint3f& pnt, float p ) const {
        return ofxPoint3f( x*(1-p) + pnt.x*p,
                          y*(1-p) + pnt.y*p,
                          z*(1-p) + pnt.z*p );
    }

    ofxPoint3f middle( const ofxPoint3f& pnt ) const {
        return ofxPoint3f( (x+pnt.x)/2.0, (y+pnt.y)/2.0, (z+pnt.z)/2.0 );
    }



    // Average (centroid) among points.
    // Addition is sometimes useful for calculating averages too.
    //
    //
    ofxPoint3f average( const ofxPoint3f* points, int num ) const {
        ofxPoint3f centroid;
        for( int i=0; i<num; i++) {
            centroid.x += points[i].x;
            centroid.y += points[i].y;
            centroid.z += points[i].z;
        }
        centroid.x /= num;
        centroid.y /= num;
        centroid.z /= num;
        return centroid;
    }

    ofxPoint3f operator+( const ofxPoint3f& pnt ) const {
        return ofxPoint3f( x+pnt.x, y+pnt.y, z+pnt.z );
    }

    ofxPoint3f& operator+=( const ofxPoint3f& pnt ) {
        x+=pnt.x;
        y+=pnt.y;
        z+=pnt.z;
        return *this;
    }




    // Perpendicular normalized vector from three points.
    //
    //
    ofVec3f perpendicular( const ofxPoint3f& pnt1,
                           const ofxPoint3f& pnt2 ) const
    {
        float v1x = x - pnt1.x;
        float v1y = y - pnt1.y;
        float v1z = z - pnt1.z;
        float v2x = x - pnt2.x;
        float v2y = y - pnt2.y;
        float v2z = z - pnt2.z;

        float crossX = v1y*v2z - v1z*v2y;
        float crossY = v1z*v2x - v1x*v2z;
        float crossZ = v1x*v2y - v1y*v2x;

        float length = (float)sqrt(crossX*crossX +
                                   crossY*crossY +
                                   crossZ*crossZ);

        if( length > 0 )
            return ofVec3f( crossX/length, crossY/length, crossZ/length );
        else
            return ofVec3f();
    }


};



#endif
