/***************************************************************************
* Vector.C                                                                 *
*                                                                          *
* General Vector and Matrix classes, with all the associated methods.      *
*                                                                          *
*   HISTORY                                                                *
*      Name    Date        Description                                     *
*                                                                          *
*      arvo    08/16/2000    Revamped for CIT tools.                       *
*      arvo    10/31/1994    Combined RowVec & ColVec into Vector.         *
*      arvo    06/30/1993    Added singular value decomposition class.     *
*      arvo    06/25/1993    Major revisions.                              *
*      arvo    09/08/1991    Initial implementation.                       *
*                                                                          *
*--------------------------------------------------------------------------*
* Copyright (C) 2000, James Arvo                                           *
*                                                                          *
* 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.  See http://www.fsf.org/copyleft/gpl.html      *
*                                                                          *
* This program is distributed in the hope that it will be useful, but      *
* WITHOUT EXPRESS OR IMPLIED WARRANTY of merchantability or fitness for    *
* any particular purpose.  See the GNU General Public License for more     *
* details.                                                                 *
*                                                                          *
***************************************************************************/
#include <stream.h>
#include <assert.h>
#include <util/Math.h>
#include <util/Vector.h>

const Vector Vector::Null(0);

/*-------------------------------------------------------------------------*
 *                                                                         *
 *  C O N S T R U C T O R S                                                *
 *                                                                         *
 *-------------------------------------------------------------------------*/
Vector::Vector( const float *x, int n )
    {
    Create( n );
    for( register int i = 0; i < size; i++ ) elem[i] = x[i];
    }

Vector::Vector( const Vector &A )
    {
    Create( A.Size() );
    for( register int i = 0; i < A.Size(); i++ ) elem[i] = A(i);
    }

Vector::Vector( int n )
    {
    Create( n );
    for( register int i = 0; i < n; i++ ) elem[i] = 0.0;
    }

Vector::Vector( float x, float y )
    {
    Create( 2 );
    elem[0] = x;
    elem[1] = y;
    }

Vector::Vector( float x, float y, float z )
    {
    Create( 3 );
    elem[0] = x;
    elem[1] = y;
    elem[2] = z;
    }

void Vector::SetSize( int new_size )
    {
    if( size != new_size )
	{
        delete[] elem;
        Create( new_size );
        for( register int i = 0; i < new_size; i++ ) elem[i] = 0.0;
	}
    }

Vector &Vector::Swap( int i, int j )
    {
    float temp = elem[i];
    elem[i]    = elem[j];
    elem[j]    = temp;
    return *this;
    }

Vector Vector::GetBlock( int i, int j ) const
    {
    assert( 0 <= i && i <= j && j < size );
    int n = j - i + 1;
    Vector V( n );
    register float *v = V.Array();
    register float *e = elem + i;
    for( register int k = 0; k < n; k++ ) *v++ = *e++;
    return V;
    }

void Vector::SetBlock( int i, int j, const Vector &V )
    {
    assert( 0 <= i && i <= j && j < size );
    int n = j - i + 1;
    assert( n == V.Size() );
    register float *v = V.Array();
    register float *e = elem + i;
    for( register int k = 0; k < n; k++ ) *e++ = *v++;
    }

/*-------------------------------------------------------------------------*
 *                                                                         *
 *  O P E R A T O R S                                                      *
 *                                                                         *
 *-------------------------------------------------------------------------*/
double operator*( const Vector &A, const Vector &B )
    {
    assert( A.Size() == B.Size() );
    double sum = A(0) * B(0);
    for( register int i = 1; i < A.Size(); i++ ) sum += A(i) * B(i);
    return sum;
    }

void Vector::operator=( float c )
    {
    for( register int i = 0; i < size; i++ ) elem[i] = c;
    }

Vector operator*( const Vector &A, float s ) 
    {
    Vector C( A.Size() );
    for( register int i = 0; i < A.Size(); i++ ) C(i) = A(i) * s;
    return C;
    }

Vector operator*( float s, const Vector &A ) 
    {
    Vector C( A.Size() );
    for( register int i = 0; i < A.Size(); i++ ) C(i) = A(i) * s;
    return C;
    }

Vector operator/( const Vector &A, float s ) 
    {
    assert( s != 0.0 );
    Vector C( A.Size() );
    for( register int i = 0; i < A.Size(); i++ ) C(i) = A(i) / s;
    return C;
    }

Vector& operator+=( Vector &A, const Vector &B ) 
    {
    assert( A.Size() == B.Size() );
    for( register int i = 0; i < A.Size(); i++ ) A(i) += B(i);
    return A;
    }

Vector& operator*=( Vector &A, float scale ) 
    {
    for( register int i = 0; i < A.Size(); i++ ) A(i) *= scale;
    return A;
    }

Vector& operator/=( Vector &A, float scale ) 
    {
    for( register int i = 0; i < A.Size(); i++ ) A(i) /= scale;
    return A;
    }

Vector& Vector::operator=( const Vector &A )
    {
    SetSize( A.Size() );
    for( register int i = 0; i < size; i++ ) elem[i] = A(i);
    return *this;
    }

Vector operator+( const Vector &A, const Vector &B ) 
    {
    assert( A.Size() == B.Size() );
    Vector C( A.Size() );
    for( register int i = 0; i < A.Size(); i++ ) C(i) = A(i) + B(i);
    return C;
    }
        
Vector operator-( const Vector &A, const Vector &B ) 
    {
    assert( A.Size() == B.Size() );
    Vector C( A.Size() );
    for( register int i = 0; i < A.Size(); i++ ) C(i) = A(i) - B(i);
    return C;
    }
        
Vector operator-( const Vector &A )  // Unary minus.
    {
    Vector B( A.Size() );
    for( register int i = 0; i < A.Size(); i++ ) B(i) = -A(i);
    return B;
    }
        
Vector operator^( const Vector &A, const Vector &B )
    {
    Vector C(3);
    assert( A.Size() == B.Size() );
    if( A.Size() == 2 ) // Assume z components of A and B are zero.
        {
        C(0) = 0.0;
        C(1) = 0.0;
        C(2) = A(0) * B(1) - A(1) * B(0);
        }
    else 
        {
        assert( A.Size() == 3 );
        C(0) = A(1) * B(2) - A(2) * B(1);
        C(1) = A(2) * B(0) - A(0) * B(2);
        C(2) = A(0) * B(1) - A(1) * B(0);
        }
    return C;
    }

/*-------------------------------------------------------------------------*
 *                                                                         *
 *  M I S C E L L A N E O U S   F U N C T I O N S                          *
 *                                                                         *
 *-------------------------------------------------------------------------*/
Vector Min( const Vector &A, const Vector &B )
    {
    assert( A.Size() == B.Size() );
    Vector C( A.Size() );
    for( register int i = 0; i < A.Size(); i++ ) C(i) = Min( A(i), B(i) );
    return C;
    }

Vector Max( const Vector &A, const Vector &B )
    {
    assert( A.Size() == B.Size() );
    Vector C( A.Size() );
    for( register int i = 0; i < A.Size(); i++ ) C(i) = Max( A(i), B(i) );
    return C;
    }

Vector Unit( const Vector &A )
    {
    double norm = TwoNorm( A );
    assert( norm > 0.0 );
    return A * ( 1.0 / norm );
    }

double Normalize( Vector &A )
    {
    double norm = TwoNorm( A );
    assert( norm > 0.0 );
    for( register int i = 0; i < A.Size(); i++ ) A(i) /= norm;
    return norm;
    }

int Null( const Vector &A ) 
    {
    return A.Size() == 0;
    }

double TwoNormSqr( const Vector &A )
    {
    double sum = A(0) * A(0);
    for( register int i = 1; i < A.Size(); i++ ) sum += A(i) * A(i);
    return sum;
    }

double TwoNorm( const Vector &A )
    {
    return sqrt( TwoNormSqr( A ) );
    }

double dist( const Vector &A, const Vector &B )
    {
    return TwoNorm( A - B );
    }

double OneNorm( const Vector &A )
    {
    double norm = Abs( A(0) );
    for( register int i = 1; i < A.Size(); i++ ) norm += Abs( A(i) );
    return norm;
    }

double SupNorm( const Vector &A )
    {
    double norm = Abs( A(0) );
    for( register int i = 1; i < A.Size(); i++ )
        {
        double a = Abs( A(i) );
        if( a > norm ) norm = a;
        }
    return norm;
    }

Vec2 ToVec2( const Vector &V )
    {
    assert( V.Size() == 2 );
    return Vec2( V(0), V(1) );
    }

Vec3 ToVec3( const Vector &V )
    {
    assert( V.Size() == 3 );
    return Vec3( V(0), V(1), V(2) );
    }

Vector ToVector( const Vec2 &V )
    {
    return Vector( V.X(), V.Y() );
    }

Vector ToVector( const Vec3 &V )
    {
    return Vector( V.X(), V.Y(), V.Z() );
    }

//
// Returns a vector that is orthogonal to A (but of arbitrary length). 
//
Vector OrthogonalTo( const Vector &A )
    {
    Vector B( A.Size() );
    double c = 0.5 * SupNorm( A );

    if( A.Size() < 2 ) 
        {
        // Just return the zero-vector.
        }
    else if( c == 0.0 ) 
        {
        B(0) = 1.0;
        }
    else for( register int i = 0; i < A.Size(); i++ )
        {
        if( Abs( A(i)) > c )
            {
            int k = ( i > 0 ) ? i - 1 : i + 1;
            B(k) = -A(i);
            B(i) =  A(k);
            break;
            }
        }
    return B;
    }

ostream &operator<<( ostream &out, const Vector &A )
    {
    if( A.Size() == 0 )
	{
        out << "NULL";
	}
    else for( register int i = 0; i < A.Size(); i++ )
        {
        out << form( "%3d:  %10.5g\n", i, A(i) );
        }
    out << endl;
    return out;
    }