/*
 * util/VMath.cc:
 *
 * 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.
 * 
 */

#include "VMath.h"

#include <iostream>
#include <iomanip>

#include <math.h>

/**
 * http://www.swin.edu.au/astronomy/pbourke/geometry/lineline2d/
 *
 *
 */

bool VMath::lineSegmentsIntersect2d( const float * a1, 
                                     const float * a2, 
                                     const float * b1, 
                                     const float * b2, 
                                     float * u1, 
                                     float * u2 ) {
        
        const float px[4] = { a1[0], a2[0], b1[0], b2[0] };
        const float py[4] = { a1[1], a2[1], b1[1], b2[1] };
        
        /*
        if ( false )
                cout << " - segment testing line : "
                     << setw( 4 ) << px[0] << ", " << setw( 4 ) << py[0] << " --> "
                     << setw( 4 ) << px[1] << ", " << setw( 4 ) << py[1] << " VS:  "
                        
                     << setw( 4 ) << px[2] << ", " << setw( 4 ) << py[2] << " --> "
                     << setw( 4 ) << px[3] << ", " << setw( 4 ) << py[3] << endl;
        */

        const float denom =
                ( py[3] - py[2] ) * ( px[1] - px[0] ) - ( px[3] - px[2] ) * ( py[1] - py[0] );
        
        const float numer[2] = {
                ( px[3] - px[2] ) * ( py[0] - py[2] ) - ( py[3] - py[2] ) * ( px[0] - px[2] ),
                ( px[1] - px[0] ) * ( py[0] - py[2] ) - ( py[1] - py[0] ) * ( px[0] - px[2] )
        };
        
        //        cout << " - denom=" << denom << ", numer=" << numer[0] << ", " << numer[1] << endl;

        //
        // lines are parallel
        //

        if ( denom == 0 ) {

                //
                // lines are coincident
                //

                if ( numer[0] == 0 || numer[1] == 0 ) {
                        //                        cout << " - coincident" << endl;
                        return false;
                }
                        
                //                cout << " - parallel" << endl;
                return false;
        }

        //
        // moving away from line, intersection point on line one will be negative
        //

        if ( ( numer[1] == 0 || numer[0] == 0 ) && denom < 0 ) {
                //                cout << " - NEG DENOM: denom=" << denom << ", numer[0]=" << numer[0] << ", numer[1]=" << numer[1] << endl;
                return false;
        }
                        
        *u1 = numer[0] / denom;
        *u2 = numer[1] / denom;

        if ( *u1 >= 0 && *u2 >= 0 &&
             *u1 <= 1 && *u2 <= 1 ) {
                

                //                cout << " - intersect at " << *u1 << ", " << *u2 << endl;
                return true;
        }

        //        cout << " - no u1=" << *u1 << ", u2=" << *u2 << endl;

        return false;
}

bool VMath::linesIntersect2d( const float * a1, 
                              const float * a2, 
                              const float * b1, 
                              const float * b2, 
                              float * u1, 
                              float * u2 ) {
        
        const float px[4] = { a1[0], a2[0], b1[0], b2[0] };
        const float py[4] = { a1[1], a2[1], b1[1], b2[1] };
        
        /*
        cout << " - testing line : "
             << setw( 4 ) << px[0] << ", " << setw( 4 ) << py[0] << " --> "
             << setw( 4 ) << px[1] << ", " << setw( 4 ) << py[1] << " VS:  "
                
             << setw( 4 ) << px[2] << ", " << setw( 4 ) << py[2] << " --> "
             << setw( 4 ) << px[3] << ", " << setw( 4 ) << py[3] << endl;
        */

        const float denom =
                ( py[3] - py[2] ) * ( px[1] - px[0] ) - ( px[3] - px[2] ) * ( py[1] - py[0] );
        
        const float numer[2] = {
                ( px[3] - px[2] ) * ( py[0] - py[2] ) - ( py[3] - py[2] ) * ( px[0] - px[2] ),
                ( px[1] - px[0] ) * ( py[0] - py[2] ) - ( py[1] - py[0] ) * ( px[0] - px[2] )
        };
        
        //        cout << " - denom=" << denom << ", numer=" << numer[0] << ", " << numer[1] << endl;

        //
        // lines are parallel
        //

        if ( denom == 0 ) {
                
                //
                // lines are coincident
                //
                
                if ( numer[0] == 0 || numer[1] == 0 ) {
                        //                        cout << " - coincident" << endl;
                        return false;
                }
                        
                //                cout << " - parallel" << endl;
                return false;
        }

        *u1 = numer[0] / denom;
        *u2 = numer[1] / denom;

        return true;
}



float VMath::angleDiff360( float a, float b ) {
        float diff = a - b;

        if ( diff < -180 )
                diff += 360;
        else if ( diff > 180 )
                diff -= 360;

        return diff;
}
