/**
*  ofPoint3f.h
*  by stefanix
*/


#ifndef _OF_POINT3f
#define _OF_POINT3f

#include "ofVec3f.h"



class ofPoint3f : public ofPoint {


  public:
	
    ofPoint3f( 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 ofPoint3f& 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 ofPoint3f pnt ) { 
        return (x == pnt.x) && (y == pnt.y) && (z == pnt.z);
    }

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

    bool match( const ofPoint3f 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.
    //
    //     
    ofPoint3f operator+( const ofVec3f& vec ) const { 
        return ofPoint3f( x+vec.x, y+vec.y, z+vec.z ); 
    } 
    
    ofPoint3f& operator+=( const ofVec3f& vec ) { 
        x += vec.x;
        y += vec.y;
        z += vec.z;
        return *this; 
    } 
	
	ofPoint3f operator+( const float f ) const { 
        return ofPoint3f( x+f, y+f, z+f); 
    } 
    
    ofPoint3f& operator+=( const float f ) { 
        x += f;
        y += f;
		z += f;
        return *this; 
    } 		

    ofPoint3f operator-( const ofVec3f& vec ) const { 
        return ofPoint3f( x-vec.x, y-vec.y, z-vec.z ); 
    }
          
    ofPoint3f& operator-=( const ofVec3f& vec ) { 
        x -= vec.x;
        y -= vec.y;
        z -= vec.z;
        return *this; 
    }
	
	ofPoint3f operator-( const float f ) const { 
        return ofPoint3f( x-f, y-f, z-f); 
    } 
    
    ofPoint3f& operator-=( const float f ) { 
        x -= f;
        y -= f;
		z -= f;
        return *this; 
    } 		  
		
    ofPoint3f operator-() const { 
        return ofPoint3f( -x, -y, -z ); 
    }    


    
    // Scale point (by scalar or vector).
    //
    //
    ofPoint3f operator*( const float f ) const { 
        return ofPoint3f( x*f, y*f, z*f );
    } 
    
    ofPoint3f& operator*=( const float f ) { 
        x*=f;
        y*=f;
        z*=f;
        return *this; 
    }    
        
    ofPoint3f operator*( const ofVec3f& vec ) const { 
        return ofPoint3f( x*vec.x, y*vec.y, z*vec.z ); 
    } 
    
    ofPoint3f& operator*=( const ofVec3f& vec ) { 
        x*=vec.x;
        y*=vec.y;
        z*=vec.z;
        return *this; 
    }
    
    ofPoint3f operator/( const float f ) const { 
        return ofPoint3f( x/f, y/f, z/f );
    } 
    
    ofPoint3f& operator/=( const float f ) { 
        x/=f;
        y/=f;
        z/=f;
        return *this; 
    }    
        
    ofPoint3f operator/( const ofVec3f& vec ) const { 
        return ofPoint3f( x/vec.x, y/vec.y, z/vec.z ); 
    } 
    
    ofPoint3f& 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.
    //
    //      
    ofPoint3f rotated( float angle, 
                       const ofPoint3f& 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 ofPoint3f( xrot+pivot.x, yrot+pivot.y, zrot+pivot.z );
    }
    
    ofPoint3f& rotate( float angle, 
                       const ofPoint3f& 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.
    //
    //      
    ofPoint3f mapped( const ofPoint3f& origin, 
                      const ofVec3f& vx, 
                      const ofVec3f& vy, 
                      const ofVec3f& vz ) const
    {
        return ofPoint3f( 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 );      
    }

    ofPoint3f& map( const ofPoint3f& 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 ofPoint3f& pnt ) const { 
        return ofVec3f( x-pnt.x, y-pnt.y, z-pnt.z ); 
    }
    
	
    // Distance between two points.
    //
    //
    float distance( const ofPoint3f& 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 ofPoint3f& 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.
    */      
    ofPoint3f interpolate( const ofPoint3f& pnt, float p ) const {
        return ofPoint3f( x*(1-p) + pnt.x*p, 
                          y*(1-p) + pnt.y*p, 
                          z*(1-p) + pnt.z*p );
    }

    ofPoint3f middle( const ofPoint3f& pnt ) const {
        return ofPoint3f( (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.
    //
    //
    ofPoint3f average( const ofPoint3f* points, int num ) const {
        ofPoint3f 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;
    }
    
    ofPoint3f operator+( const ofPoint3f& pnt ) const { 
        return ofPoint3f( x+pnt.x, y+pnt.y, z+pnt.z ); 
    } 
    
    ofPoint3f& operator+=( const ofPoint3f& pnt ) { 
        x+=pnt.x;
        y+=pnt.y;
        z+=pnt.z;
        return *this; 
    }
    
    
    
    
    // Perpendicular normalized vector from three points.
    //
    //  
    ofVec3f perpendicular( const ofPoint3f& pnt1, 
                           const ofPoint3f& 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
