You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
594 lines
11 KiB
594 lines
11 KiB
/*
|
|
**************************************************************************
|
|
description
|
|
--------------------
|
|
copyright : (C) 2000-2001 by Andreas Zehender
|
|
email : zehender@kde.org
|
|
**************************************************************************
|
|
|
|
**************************************************************************
|
|
* *
|
|
* 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. *
|
|
* *
|
|
**************************************************************************/
|
|
|
|
|
|
#include "pmvector.h"
|
|
#include "pmmath.h"
|
|
#include "pmmatrix.h"
|
|
#include "pmdebug.h"
|
|
|
|
#include <tqtextstream.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
|
|
double PMVector::s_dummy = 0;
|
|
|
|
double& PMVector::operator[] ( int index )
|
|
{
|
|
if( ( index >= 0 ) && ( index < ( signed ) m_size ) )
|
|
return m_coord[index];
|
|
kdError( PMArea ) << "Bad index in PMVector operator []\n";
|
|
return s_dummy;
|
|
}
|
|
|
|
const double& PMVector::operator[] ( int index ) const
|
|
{
|
|
if( ( index >= 0 ) && ( index < ( signed ) m_size ) )
|
|
return m_coord[index];
|
|
kdError( PMArea ) << "Bad index in PMVector operator []\n";
|
|
return s_dummy;
|
|
}
|
|
|
|
void PMVector::allocateMemory( unsigned int size )
|
|
{
|
|
m_size = size;
|
|
|
|
if( m_size == 0 )
|
|
m_coord = 0;
|
|
else
|
|
m_coord = ( double* ) malloc( sizeof( double ) * m_size );
|
|
}
|
|
|
|
PMVector::PMVector( const PMVector& v )
|
|
{
|
|
unsigned int i;
|
|
allocateMemory( v.m_size );
|
|
|
|
for( i = 0; i < m_size; i++ )
|
|
m_coord[i] = v.m_coord[i];
|
|
}
|
|
|
|
PMVector::PMVector( )
|
|
{
|
|
unsigned int i;
|
|
|
|
allocateMemory( 3 );
|
|
|
|
for( i = 0; i < 3; i++ )
|
|
m_coord[i] = 0.0;
|
|
}
|
|
|
|
PMVector::PMVector( unsigned int s )
|
|
{
|
|
unsigned int i;
|
|
|
|
allocateMemory( s );
|
|
|
|
for( i = 0; i < s; i++ )
|
|
m_coord[i] = 0.0;
|
|
}
|
|
|
|
PMVector::PMVector( const double x, const double y )
|
|
{
|
|
allocateMemory( 2 );
|
|
|
|
m_coord[0] = x;
|
|
m_coord[1] = y;
|
|
}
|
|
|
|
PMVector::PMVector( const double x, const double y, const double z )
|
|
{
|
|
allocateMemory( 3 );
|
|
|
|
m_coord[0] = x;
|
|
m_coord[1] = y;
|
|
m_coord[2] = z;
|
|
}
|
|
|
|
PMVector::PMVector( const double x, const double y, const double z,
|
|
const double t )
|
|
{
|
|
allocateMemory( 4 );
|
|
|
|
m_coord[0] = x;
|
|
m_coord[1] = y;
|
|
m_coord[2] = z;
|
|
m_coord[3] = t;
|
|
}
|
|
|
|
PMVector::~PMVector( )
|
|
{
|
|
if( m_coord )
|
|
free( m_coord );
|
|
}
|
|
|
|
|
|
void PMVector::resize( unsigned int s )
|
|
{
|
|
unsigned int i;
|
|
|
|
if( s != m_size )
|
|
{
|
|
m_coord = ( double* ) realloc( m_coord, sizeof( double ) * s );
|
|
|
|
for( i = m_size; i < s; i++ )
|
|
m_coord[i] = 0.0;
|
|
|
|
if( m_coord )
|
|
m_size = s;
|
|
else
|
|
m_size = 0; // possibly out of memory
|
|
}
|
|
}
|
|
|
|
void PMVector::transform( const PMMatrix& m )
|
|
{
|
|
(*this) = m * (*this);
|
|
}
|
|
|
|
PMVector& PMVector::operator= ( const PMVector& p )
|
|
{
|
|
unsigned int i;
|
|
|
|
resize( p.m_size );
|
|
for( i = 0; i < m_size; i++ )
|
|
m_coord[i] = p[i];
|
|
return *this;
|
|
}
|
|
|
|
PMVector& PMVector::operator= ( const double d )
|
|
{
|
|
unsigned int i;
|
|
|
|
for( i = 0; i < m_size; i++ )
|
|
m_coord[i] = d;
|
|
return *this;
|
|
}
|
|
|
|
PMVector& PMVector::operator*= ( const PMVector& p )
|
|
{
|
|
unsigned int i;
|
|
|
|
if( m_size != p.m_size )
|
|
resize( p.m_size );
|
|
|
|
for( i = 0; i < m_size; i++ )
|
|
m_coord[i] *= p[i];
|
|
|
|
return *this;
|
|
}
|
|
|
|
PMVector& PMVector::operator/= ( const PMVector& p )
|
|
{
|
|
unsigned int i;
|
|
|
|
if( m_size > p.m_size )
|
|
{
|
|
kdError( PMArea ) << "Vector p is too small in PMVector& PMVector::operator/= ( const PMVector& p )\n";
|
|
}
|
|
else
|
|
{
|
|
for( i = 0; i < m_size; i++ )
|
|
{
|
|
if( approxZero( p[i] ) )
|
|
kdError( PMArea ) << "Division by zero in PMVector::operator/= " << "\n";
|
|
else
|
|
m_coord[i] *= p[i];
|
|
}
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
PMVector& PMVector::operator*= ( double d )
|
|
{
|
|
unsigned int i;
|
|
|
|
for( i = 0; i < m_size; i++ )
|
|
m_coord[i] *= d;
|
|
|
|
return *this;
|
|
}
|
|
|
|
PMVector& PMVector::operator/= ( double d )
|
|
{
|
|
if( approxZero( d ) )
|
|
{
|
|
kdError( PMArea ) << "Division by zero in PMVector::operator/= " << "\n";
|
|
}
|
|
else
|
|
{
|
|
unsigned int i;
|
|
|
|
for( i = 0; i < m_size; i++ )
|
|
m_coord[i] /= d;
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
PMVector& PMVector::operator+= ( double d )
|
|
{
|
|
unsigned int i;
|
|
|
|
for( i = 0; i < m_size; i++ )
|
|
m_coord[i] += d;
|
|
|
|
return *this;
|
|
}
|
|
|
|
PMVector& PMVector::operator-= ( double d )
|
|
{
|
|
unsigned int i;
|
|
|
|
for( i = 0; i < m_size; i++ )
|
|
m_coord[i] -= d;
|
|
|
|
return *this;
|
|
}
|
|
|
|
PMVector& PMVector::operator+= ( const PMVector& p )
|
|
{
|
|
unsigned int i;
|
|
|
|
if( m_size < p.m_size )
|
|
resize( p.m_size );
|
|
|
|
for( i = 0; i < p.m_size; i++ )
|
|
m_coord[i] += p[i];
|
|
|
|
return *this;
|
|
}
|
|
|
|
PMVector& PMVector::operator-= ( const PMVector& p )
|
|
{
|
|
unsigned int i;
|
|
|
|
if( m_size < p.m_size )
|
|
resize( p.m_size );
|
|
|
|
for( i = 0; i < m_size; i++ )
|
|
m_coord[i] -= p[i];
|
|
|
|
return *this;
|
|
}
|
|
|
|
PMVector operator- ( const PMVector& p )
|
|
{
|
|
PMVector result( p.m_size );
|
|
unsigned int i;
|
|
|
|
for( i = 0; i < p.m_size; i++ )
|
|
result[i] = -p[i];
|
|
|
|
return result;
|
|
}
|
|
|
|
PMVector operator+ ( const PMVector& p1, const PMVector& p2 )
|
|
{
|
|
PMVector result( p1 );
|
|
result += p2;
|
|
|
|
return result;
|
|
}
|
|
|
|
PMVector operator- ( const PMVector& p1, const PMVector& p2 )
|
|
{
|
|
PMVector result( p1 );
|
|
result -= p2;
|
|
|
|
return result;
|
|
}
|
|
|
|
PMVector operator* ( const PMVector& p, const double d )
|
|
{
|
|
PMVector result( p.m_size );
|
|
unsigned int i;
|
|
|
|
for( i = 0; i < p.m_size; i++ )
|
|
result[i] = p[i] * d;
|
|
|
|
return result;
|
|
}
|
|
|
|
PMVector operator/ ( const PMVector& p, const double d )
|
|
{
|
|
PMVector result( p.m_size );
|
|
unsigned int i;
|
|
|
|
if( approxZero( d ) )
|
|
kdError( PMArea ) << "Division by zero in PMVector::operator/ ( PMVector& p, double d ) " << "\n";
|
|
else
|
|
for( i = 0; i < p.m_size; i++ )
|
|
result[i] = p[i] / d;
|
|
|
|
return result;
|
|
}
|
|
|
|
PMVector operator+ ( const PMVector& p, const double d )
|
|
{
|
|
PMVector result( p.m_size );
|
|
unsigned int i;
|
|
|
|
for( i = 0; i < p.m_size; i++ )
|
|
result[i] = p[i] + d;
|
|
|
|
return result;
|
|
}
|
|
|
|
PMVector operator- ( const PMVector& p, const double d )
|
|
{
|
|
PMVector result( p.m_size );
|
|
unsigned int i;
|
|
|
|
for( i = 0; i < p.m_size; i++ )
|
|
result[i] = p[i] - d;
|
|
|
|
return result;
|
|
}
|
|
|
|
PMVector operator* ( const double d, const PMVector& p )
|
|
{
|
|
PMVector result( p.m_size );
|
|
unsigned int i;
|
|
|
|
for( i = 0; i < p.m_size; i++ )
|
|
result[i] = p[i] * d;
|
|
|
|
return result;
|
|
}
|
|
|
|
PMVector operator* ( const PMMatrix& m, const PMVector& p )
|
|
{
|
|
PMVector result( 3 );
|
|
int c, i;
|
|
// for homogenous coordinates
|
|
double u;
|
|
|
|
if( p.m_size != 3 )
|
|
kdError( PMArea ) << "Vector has not size 3 in PMVector operator* ( const PMVector& p, const PMMatrix& m ) \n";
|
|
else
|
|
{
|
|
for( c=0; c<3; c++ )
|
|
{
|
|
result[c] = 0.0;
|
|
for( i=0; i<4; i++ )
|
|
result[c] += m[i][c] * ( i<3 ? p[i] : 1.0 );
|
|
}
|
|
|
|
u = 0.0;
|
|
for( i=0; i<4; i++ )
|
|
u += m[i][3] * ( i<3 ? p[i] : 1.0 );
|
|
if( !approxZero( u ) )
|
|
for( i=0; i<3; i++ )
|
|
result[i] /= u;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
PMVector operator* ( const PMVector& p1, const PMVector& p2 )
|
|
{
|
|
PMVector result( p1 );
|
|
result *= p2;
|
|
|
|
return result;
|
|
}
|
|
|
|
PMVector operator/ ( const PMVector& p1, const PMVector& p2 )
|
|
{
|
|
PMVector result( p1 );
|
|
result /= p2;
|
|
|
|
return result;
|
|
}
|
|
|
|
bool PMVector::operator== ( const PMVector& p ) const
|
|
{
|
|
unsigned int i;
|
|
if( m_size != p.m_size )
|
|
return false;
|
|
if( m_size == 0 )
|
|
return true;
|
|
|
|
for( i = 0; i < m_size; i++ )
|
|
if( p.m_coord[i] != m_coord[i] )
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
bool PMVector::approxEqual( const PMVector& p, double epsilon ) const
|
|
{
|
|
unsigned int i;
|
|
if( m_size != p.m_size )
|
|
return false;
|
|
if( m_size == 0 )
|
|
return true;
|
|
|
|
for( i = 0; i < m_size; i++ )
|
|
if( ! approx( p.m_coord[i], m_coord[i], epsilon ) )
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
bool PMVector::operator!= ( const PMVector& p ) const
|
|
{
|
|
return !( *this == p );
|
|
}
|
|
|
|
|
|
PMVector PMVector::cross( const PMVector& v1, const PMVector& v2 )
|
|
{
|
|
PMVector result;
|
|
if( ( v1.size( ) == 3 ) && ( v2.size( ) == 3 ) )
|
|
{
|
|
result[0] = v1[1]*v2[2] - v1[2]*v2[1];
|
|
result[1] = v1[2]*v2[0] - v1[0]*v2[2];
|
|
result[2] = v1[0]*v2[1] - v1[1]*v2[0];
|
|
}
|
|
else
|
|
kdError( PMArea ) << "Wrong sizes in PMVector::cross( )\n";
|
|
|
|
return result;
|
|
}
|
|
|
|
double PMVector::dot( const PMVector& v1, const PMVector& v2 )
|
|
{
|
|
double result = 0.0;
|
|
unsigned int i;
|
|
|
|
if( v1.size( ) == v2.size( ) )
|
|
{
|
|
for( i = 0; i < v1.size( ); i++ )
|
|
result += v1[i] * v2[i];
|
|
}
|
|
else
|
|
kdError( PMArea ) << "Wrong sizes in PMVector::dot( )\n";
|
|
|
|
return result;
|
|
}
|
|
|
|
double PMVector::angle( const PMVector& v1, const PMVector& v2 )
|
|
{
|
|
PMVector cr;
|
|
double s, c, n;
|
|
double a = 0;
|
|
int i;
|
|
|
|
if( ( v1.size( ) == 3 ) && ( v2.size( ) == 3 ) )
|
|
{
|
|
n = v1.abs( ) * v2.abs( );
|
|
|
|
if( approxZero( n ) )
|
|
a = 0;
|
|
else
|
|
{
|
|
cr = cross( v1, v2 );
|
|
s = cr.abs( ) / n;
|
|
|
|
c = 0;
|
|
for( i = 0; i < 3; i++ )
|
|
c += v1[i] * v2[i];
|
|
|
|
c = c / n;
|
|
|
|
a = pmatan( s, c );
|
|
}
|
|
}
|
|
else
|
|
kdError( PMArea ) << "Wrong sizes in PMVector::angle( )\n";
|
|
|
|
return a;
|
|
}
|
|
|
|
double PMVector::abs( ) const
|
|
{
|
|
unsigned int i;
|
|
double a = 0.0;
|
|
|
|
for( i = 0; i < m_size; i++ )
|
|
a += m_coord[i] * m_coord[i];
|
|
|
|
return sqrt( a );
|
|
}
|
|
|
|
PMVector PMVector::orthogonal( ) const
|
|
{
|
|
PMVector result;
|
|
double l, rl;
|
|
|
|
l = abs( );
|
|
if( approxZero( l ) )
|
|
{
|
|
kdError( PMArea ) << "Can't calculate an orthogonal vector from a null vector\n";
|
|
return PMVector( 1, 0, 0 );
|
|
}
|
|
|
|
result = cross( (*this) / l, PMVector( 0, 0, 1 ) );
|
|
rl = result.abs( );
|
|
if( rl < 0.001 )
|
|
{
|
|
result = cross( (*this) / l, PMVector( 1, 0, 0 ) );
|
|
rl = result.abs( );
|
|
}
|
|
return result / rl;
|
|
}
|
|
|
|
TQString PMVector::serialize( ) const
|
|
{
|
|
TQString result;
|
|
TQTextStream str( &result, IO_WriteOnly );
|
|
unsigned int i;
|
|
|
|
if( m_size > 0 )
|
|
{
|
|
str << '<';
|
|
for( i = 0; i < m_size; i++ )
|
|
{
|
|
if( i > 0 )
|
|
str << ", ";
|
|
str << m_coord[i];
|
|
}
|
|
str << '>';
|
|
}
|
|
else
|
|
kdError( PMArea ) << "Can't serialize a vector with size 0\n";
|
|
|
|
return result;
|
|
}
|
|
|
|
TQString PMVector::serializeXML( ) const
|
|
{
|
|
TQString result;
|
|
TQTextStream str( &result, IO_WriteOnly );
|
|
unsigned int i;
|
|
|
|
if( m_size > 0 )
|
|
{
|
|
for( i = 0; i < m_size; i++ )
|
|
{
|
|
if( i > 0 )
|
|
str << ' ';
|
|
str << m_coord[i];
|
|
}
|
|
}
|
|
else
|
|
kdError( PMArea ) << "Can't serialize a vector with size 0\n";
|
|
|
|
return result;
|
|
}
|
|
|
|
bool PMVector::loadXML( const TQString& str )
|
|
{
|
|
int i;
|
|
int size = str.contains( ' ' ) + 1;
|
|
TQString tmp( str );
|
|
TQTextStream s( &tmp, IO_ReadOnly );
|
|
TQString val;
|
|
bool ok;
|
|
|
|
resize( size );
|
|
for( i = 0; i < size; i++ )
|
|
{
|
|
s >> val;
|
|
m_coord[i] = val.toDouble( &ok );
|
|
if( !ok )
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|