//---------------------------------------------------------
double CSG_Vector::Get_Angle(const CSG_Vector &Vector) const
{
if( Get_N() > Vector.Get_N() )
{
return( Vector.Get_Angle(*this) );
}
int i;
double A, B, z, *Z = Get_Data();
if( (A = Get_Length()) > 0.0 && (B = Vector.Get_Length()) > 0.0 )
{
for(i=0, z=0.0; i<Get_N(); i++)
{
z += Vector[i] * Z[i];
}
for(i=Get_N(); i<Vector.Get_N(); i++)
{
z += Vector[i];
}
return( acos(z / (A * B)) );
}
return( 0.0 );
}
//---------------------------------------------------------
bool CSG_Vector::Assign(const CSG_Vector &Vector)
{
if( Create(Vector.Get_N()) )
{
memcpy(Get_Data(), Vector.Get_Data(), Get_N() * sizeof(double));
return( true );
}
return( false );
}
//---------------------------------------------------------
double CSG_Vector::Multiply_Scalar(const CSG_Vector &Vector) const
{
double z = 0.0;
if( Get_N() == Vector.Get_N() )
{
for(int i=0; i<Get_N(); i++)
{
z += Get_Data()[i] * Vector[i];
}
}
return( z );
}
//---------------------------------------------------------
bool CSG_Vector::Subtract(const CSG_Vector &Vector)
{
if( Get_N() == Vector.Get_N() && Get_N() > 0 )
{
for(int i=0; i<Get_N(); i++)
{
Get_Data()[i] -= Vector.Get_Data()[i];
}
return( true );
}
return( false );
}
//---------------------------------------------------------
bool CSG_Vector::Multiply(const CSG_Vector &Vector)
{
if( Get_N() == Vector.Get_N() && Get_N() == 3 )
{
CSG_Vector v(*this);
Get_Data()[0] = v[1] * Vector[2] - v[2] * Vector[1];
Get_Data()[1] = v[2] * Vector[0] - v[0] * Vector[2];
Get_Data()[2] = v[0] * Vector[1] - v[1] * Vector[0];
return( true );
}
return( false );
}
//---------------------------------------------------------
bool CSG_Vector::is_Equal(const CSG_Vector &Vector) const
{
if( Get_N() == Vector.Get_N() )
{
for(int i=0; i<Get_N(); i++)
{
if( Get_Data(i) != Vector.Get_Data(i) )
{
return( false );
}
}
return( true );
}
return( false );
}
//---------------------------------------------------------
bool SG_Matrix_Solve(CSG_Matrix &Matrix, CSG_Vector &Vector, bool bSilent)
{
bool bResult = false;
int n = Vector.Get_N();
if( n > 0 && n == Matrix.Get_NX() && n == Matrix.Get_NY() )
{
int *Permutation = (int *)SG_Malloc(n * sizeof(int));
if( SG_Matrix_LU_Decomposition(n, Permutation, Matrix.Get_Data(), bSilent) )
{
SG_Matrix_LU_Solve(n, Permutation, Matrix, Vector.Get_Data(), bSilent);
bResult = true;
}
SG_Free(Permutation);
}
return( bResult );
}
CSG_Vector CSG_Matrix::Multiply(const CSG_Vector &Vector) const
{
CSG_Vector v;
if( m_nx == Vector.Get_N() && v.Create(m_ny) )
{
for(int y=0; y<m_ny; y++)
{
double z = 0.0;
for(int x=0; x<m_nx; x++)
{
z += m_z[y][x] * Vector(x);
}
v[y] = z;
}
}
return( v );
}
bool CSG_Matrix::Ins_Row(int iRow, const CSG_Vector &Data)
{
return( m_ny == 0 ? Add_Row(Data) : m_nx == Data.Get_N() ? Ins_Row(iRow, Data.Get_Data()) : false );
}
bool CSG_Matrix::Ins_Col(int iCol, const CSG_Vector &Data)
{
return( m_nx == 0 ? Add_Col(Data) : m_ny == Data.Get_N() ? Ins_Col(iCol, Data.Get_Data()) : false );
}
bool CSG_Matrix::Add_Row(const CSG_Vector &Data)
{
return( m_ny == 0 ? Create(Data.Get_N(), 1, Data.Get_Data()) : m_nx == Data.Get_N() ? Add_Row(Data.Get_Data()) : false );
}
bool SG_Matrix_Tridiagonal_QL(CSG_Matrix &Q, CSG_Vector &d, CSG_Vector &e)
{
if( Q.Get_NX() != Q.Get_NY() || Q.Get_NX() != d.Get_N() || Q.Get_NX() != e.Get_N() )
{
return( false );
}
int m, l, iter, i, k, n;
double s, r, p, g, f, dd, c, b;
n = d.Get_N();
for(i=1; i<n; i++)
{
e[i - 1] = e[i];
}
e[n - 1] = 0.0;
for(l=0; l<n; l++)
{
iter = 0;
do
{
for(m=l; m<n-1; m++)
{
dd = fabs(d[m]) + fabs(d[m + 1]);
if( fabs(e[m]) + dd == dd )
{
break;
}
}
if( m != l )
{
if( iter++ == 30 )
{
return( false ); // erhand("No convergence in TLQI.");
}
g = (d[l+1] - d[l]) / (2.0 * e[l]);
r = sqrt((g * g) + 1.0);
g = d[m] - d[l] + e[l] / (g + M_SET_SIGN(r, g));
s = c = 1.0;
p = 0.0;
for(i = m-1; i >= l; i--)
{
f = s * e[i];
b = c * e[i];
if (fabs(f) >= fabs(g))
{
c = g / f;
r = sqrt((c * c) + 1.0);
e[i+1] = f * r;
c *= (s = 1.0/r);
}
else
{
s = f / g;
r = sqrt((s * s) + 1.0);
e[i+1] = g * r;
s *= (c = 1.0/r);
}
g = d[i+1] - p;
r = (d[i] - g) * s + 2.0 * c * b;
p = s * r;
d[i+1] = g + p;
g = c * r - b;
for(k=0; k<n; k++)
{
f = Q[k][i+1];
Q[k][i+1] = s * Q[k][i] + c * f;
Q[k][i] = c * Q[k][i] - s * f;
}
}
d[l] = d[l] - p;
e[l] = g;
e[m] = 0.0;
}
}
while( m != l );
}
return( true );
}
bool CSG_Matrix::Set_Row(int iRow, const CSG_Vector &Data)
{
return( m_nx == Data.Get_N() ? Set_Row(iRow, Data.Get_Data()) : false );
}
bool CSG_Matrix::Set_Col(int iCol, const CSG_Vector &Data)
{
return( m_ny == Data.Get_N() ? Set_Col(iCol, Data.Get_Data()) : false );
}
