/*
 * util/VMath.h:
 *
 * Copyright (C) 2000 John Watson, jwatson@tempusmud.com
 * 
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 * 
 */

#ifndef __VMath_h__
#define __VMath_h__

/**
 * relative cost of operations:
 
 trig         500
 float        100
 index          6
*/

#ifndef M_PI
#define M_PI           3.14159265358979323846  /* pi */
#endif

namespace VMath {
        
        inline bool inRangeInclusive( float val, float mini, float maxi ) {
                return ( val >= mini && val <= maxi );
        }
        
        /**
         * clamp an arbitrary degree angle to the range [0-360]
         */
        inline float clampAngle360( float a ) {
                int ia = (int) a;
                a -= ia;
                
                ia %= 360;
                
                a += ia;
                
                if ( a < 0 )
                        a += 360;

                return a;
        }
        
        inline float degrees2radians( float degrees ) {
                return degrees * M_PI / 180;
        }

        inline float radians2degrees( float radians ) { 
                return radians * 180 / M_PI;
        }

        /**
         * find the closest-path angle diff between angle a and b
         * the angles a and b must be in the range [0-360],
         */
         
        float angleDiff360( float a, float b );
        
        inline float square( float num ) {
                return num * num;
        }
        
        bool linesIntersect2d( const float * a1, 
                               const float * a2, 
                               const float * b1, 
                               const float * b2, 
                               float * u1, 
                               float * u2 );
        
        bool lineSegmentsIntersect2d( const float * a1, 
                                      const float * a2, 
                                      const float * b1, 
                                      const float * b2, 
                                      float * u1, 
                                      float * u2 );

        /**
         * gets the length of the 3rd leg of a right triangle,
         * given one leg's length and the hypotenuse length
         */
        inline float triangleSide( float side, float hypot ) {
                return sqrt( square( hypot ) - square( side ) );
        }
        
        /**
         * 2 mult
         * 1 add
         */
        inline float dotProduct2d( const float * a, const float * b ) {
                return ( a[0] * b[0] + a[1] * b[1] );
        }
        
        /**
         * projection of point pt onto vector vect
         */
        float projectOnto2d( const float * pt, const float * vect );

        float distFromLine2d( const float * pt, const float * vect, float * adj = 0, float * dist = 0 );

     
        /**
         * 3 mult
         * 2 add
         * 6 index
         */
        inline float dotProduct3d( const float * a, const float * b ) {
                return ( a[0] * b[0] + a[1] * b[1] + a[2] * b[2] );
        }

        /**
         * 2 mult
         * 1 add
         * 1 sqrt
         * 4 index
         */
        inline float length2d( const float * a ) {
                return sqrt( a[0] * a[0] + a[1] * a[1] );
        }

        inline float distance2d( const float * a, const float * b ) {
                return sqrt( square( a[0] - b[0] ) + 
                             square( a[1] - b[1] ) );
        }

        inline float distance3d( const float * a, const float * b ) {
                return sqrt( square( a[0] - b[0] ) +
                             square( a[1] - b[1] ) +
                             square( a[2] - b[2] ) );
        }

        inline float hypotenuse( float a, float b ) {
                return sqrt( square( a ) + square( b ) );
        }
        
        /**
         * 3 mult
         * 2 add
         * 1 sqrt
         * 6 index
         */
        inline float length3d( const float * a ) {
                return sqrt( a[0] * a[0] + a[1] * a[1] + a[2] * a[2] );
        }

        inline void unitVector2d( float * a ) {
                float length = length2d( a );
                
                a[0] /= length;
                a[1] /= length;
        }


        inline void unitVector2d( const float * a, float * result ) {
                float length = length2d( a );

                result[0] = a[0] / length;
                result[1] = a[1] / length;                
        }


        inline void unitVector3d( const float * a, float * result ) {
                float length = length3d( a );

                result[0] = a[0] / length;
                result[1] = a[1] / length;
                result[2] = a[2] / length;
        }

        inline void unitVector3d( float * a ) {
                float length = length3d( a );

                a[0] /= length;
                a[1] /= length;
                a[2] /= length;
        }

        /**
         * 6 mult
         * 3 add
         * 3 stor
         * 15 index
         */
        inline void crossProduct( const float * a, 
                                  const float * b, 
                                  float * result ) {
                result[0] =   a[1] * b[2] - a[2] * b[1];
                result[1] = - a[0] * b[2] + a[2] * b[0];
                result[2] =   a[0] * b[1] - a[1] * b[0];
        }

        /**
         * the magnitude along the z axis
         */
        inline float crossProductMag( const float * a, const float * b  ) {
                return ( a[0] * b[1] ) - ( a[1] * b[0] );
        }
        
        /**
         * crossProduct
         * length3d
         * 3 index
         * 3 div
         * 4 stor
         */
        inline void unitNormal( const float * a, 
                                const float * b, 
                                float * result ) {
                
                crossProduct( a, b, result );
                
                unitVector3d( result );
        }

        /**
         * for clockwise-facing vector normals
         *
         * 2 stor
         * 4 index
         */
        inline void normal2d( const float * a, 
                              float * result ) {
                result[0] =   a[1];
                result[1] = - a[0];
        }

        /**
         * crossProductZ
         * length2d
         * 2 index
         * 3 stor
         * 2 div
         */
        
        inline void unitNormal2d( const float * a,
                                  float * result ) {
                
                normal2d( a, result );
                
                unitVector2d( result );
        }

        /**
         * for counter-clockwise facing fector normals
         *
         * 2 stor
         * 4 index
         */
        inline void normal2dNeg( const float * a, 
                                 float * result ) {
                result[0] = -  a[1];
                result[1] =    a[0];
        }
        
        /**
         * crossProductZNeg
         * length2d
         * 2 index
         * 3 stor
         * 2 div
         */

        inline void unitNormal2dNeg( const float * a,
                                     float * result ) {
                
                normal2dNeg( a, result );
                
                unitVector2d( result );
        }

        /**
         * http://www.swin.edu.au/astronomy/pbourke/geometry/sphereline/
         * ignore 1-point edge intersections...
         */

        inline bool lineIntersectsCircle( const float * p1, const float *p2, const float *center, float r ) {
                float a = square( p2[0] - p1[0] ) + square( p2[1] - p1[1] );
                float b = 2 * ( ( p2[0] - p1[0] ) * ( p1[0] - center[0] ) +
                                ( p2[1] - p1[1] ) * ( p1[1] - center[1] ) );
                float c = 
                        square( center[0] ) + square( center[1] ) +
                        square( p1[0] ) + square( p1[1] ) -
                        2 * ( center[0] * p1[0] + center[1] * p1[1] ) -
                        square( r );

                float res = square( b ) - 4 * a * c;
                
                if ( res > 0 )
                        return true;

                return false;
        }

        inline bool lineSegmentIntersectsCircle( const float * p1, const float *p2, const float *center, float r ) {
                float numer = 
                        ( center[0] - p1[0] ) * ( p2[0] - p1[0] ) + 
                        ( center[1] - p1[1] ) * ( p2[1] - p1[1] );
                float denom =
                        square( p2[0] - p1[0] ) + square( p2[1] - p1[1] );

                float u = numer / denom;
                
                if ( u < 0 || u > 1 )
                        return false;

                return VMath::lineIntersectsCircle( p1, p2, center, r );
        }

        int whichSide( const float * line1, const float * line2, const float * pt );
};

inline float VMath::projectOnto2d( const float * pt, const float * vect ) {
        
        float magnitude = VMath::length2d( vect );
        
        return VMath::dotProduct2d( pt, vect ) / magnitude;
}

/**
 * default adj = 0, dist = 0
 */
inline float VMath::distFromLine2d( const float * pt, const float * vect, float * adj, float * dist ) {
        
        float my_adj = VMath::projectOnto2d( pt, vect );
        
        if ( adj )
                *adj = my_adj;
        
        float distance = VMath::length2d( pt );

        if ( dist )
                *dist = distance;

        return VMath::triangleSide( my_adj, distance );
}

//
// left is front
// front == 0
// back  == 1
// colinear points are reported to be on the front
//

inline int VMath::whichSide( const float * line1, const float * line2, const float * pt ) {

        float line_vect[2] = {
                line2[0] - line1[0],
                line2[1] - line1[1]
        };

        float pt_vect[2] = {
                pt[0] - line1[0],
                pt[1] - line1[1]
        };

        float mag = VMath::crossProductMag( line_vect, pt_vect );

        if ( mag > 0 )
                return 1;

        return 0;

}
        

#endif
