VerdictVector.cpp

Go to the documentation of this file.

/*=========================================================================

  Module:    $RCSfile: VerdictVector.cpp,v $

  Copyright (c) 2006 Sandia Corporation.
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

/*
 *
 * VerdictVector.cpp contains implementation of Vector operations
 *
 * This file is part of VERDICT
 *
 */

#define VERDICT_EXPORTS

#include "moab/verdict.h"
#include <cmath>
#include "VerdictVector.hpp"
#include <cfloat>

#if defined( __BORLANDC__ )
#pragma warn - 8004 /* "assigned a value that is never used" */
#endif

const double TWO_VERDICT_PI = 2.0 * VERDICT_PI;

VerdictVector& VerdictVector::length( const double new_length )
{
    double len = this->length();
    xVal *= new_length / len;
    yVal *= new_length / len;
    zVal *= new_length / len;
    return *this;
}

double VerdictVector::distance_between( const VerdictVector& test_vector )
{
    double xv = xVal - test_vector.x();
    double yv = yVal - test_vector.y();
    double zv = zVal - test_vector.z();

    return ( sqrt( xv * xv + yv * yv + zv * zv ) );
}

/*
void VerdictVector::print_me()
{
  PRINT_INFO("X: %f\n",xVal);
  PRINT_INFO("Y: %f\n",yVal);
  PRINT_INFO("Z: %f\n",zVal);

}
*/

double VerdictVector::interior_angle( const VerdictVector& otherVector )
{
    double cosAngle = 0., angleRad = 0., len1, len2 = 0.;

    if( ( ( len1 = this->length() ) > 0 ) && ( ( len2 = otherVector.length() ) > 0 ) )
        cosAngle = ( *this % otherVector ) / ( len1 * len2 );
    else
    {
        assert( len1 > 0 );
        assert( len2 > 0 );
    }

    if( ( cosAngle > 1.0 ) && ( cosAngle < 1.0001 ) )
    {
        cosAngle = 1.0;
        angleRad = acos( cosAngle );
    }
    else if( cosAngle < -1.0 && cosAngle > -1.0001 )
    {
        cosAngle = -1.0;
        angleRad = acos( cosAngle );
    }
    else if( cosAngle >= -1.0 && cosAngle <= 1.0 )
        angleRad = acos( cosAngle );
    else
    {
        assert( cosAngle < 1.0001 && cosAngle > -1.0001 );
    }

    return ( ( angleRad * 180. ) / VERDICT_PI );
}

// Interpolate between two vectors.
// Returns (1-param)*v1 + param*v2
VerdictVector interpolate( const double param, const VerdictVector& v1, const VerdictVector& v2 )
{
    VerdictVector temp = ( 1.0 - param ) * v1;
    temp += param * v2;
    return temp;
}

void VerdictVector::xy_to_rtheta()
{
    // careful about overwriting
    double r_     = length();
    double theta_ = atan2( y(), x() );
    if( theta_ < 0.0 ) theta_ += TWO_VERDICT_PI;

    r( r_ );
    theta( theta_ );
}

void VerdictVector::rtheta_to_xy()
{
    // careful about overwriting
    double x_ = r() * cos( theta() );
    double y_ = r() * sin( theta() );

    x( x_ );
    y( y_ );
}

void VerdictVector::rotate( double angle, double )
{
    xy_to_rtheta();
    theta() += angle;
    rtheta_to_xy();
}

void VerdictVector::blow_out( double gamma, double rmin )
{
    // if gamma == 1, then
    // map on a circle : r'^2 = sqrt( 1 - (1-r)^2 )
    // if gamma ==0, then map back to itself
    // in between, linearly interpolate
    xy_to_rtheta();
    //  r() = sqrt( (2. - r()) * r() ) * gamma  + r() * (1-gamma);
    assert( gamma > 0.0 );
    // the following limits should really be roundoff-based
    if( r() > rmin * 1.001 && r() < 1.001 )
    {
        r() = rmin + pow( r(), gamma ) * ( 1.0 - rmin );
    }
    rtheta_to_xy();
}

void VerdictVector::reflect_about_xaxis( double, double )
{
    yVal = -yVal;
}

void VerdictVector::scale_angle( double gamma, double )
{
    const double r_factor     = 0.3;
    const double theta_factor = 0.6;

    xy_to_rtheta();

    // if neary 2pi, treat as zero
    // some near zero stuff strays due to roundoff
    if( theta() > TWO_VERDICT_PI - 0.02 ) theta() = 0;
    // the above screws up on big sheets - need to overhaul at the sheet level

    if( gamma < 1 )
    {
        // squeeze together points of short radius so that
        // long chords won't cross them
        theta() += ( VERDICT_PI - theta() ) * ( 1 - gamma ) * theta_factor * ( 1 - r() );

        // push away from center of circle, again so long chords won't cross
        r( ( r_factor + r() ) / ( 1 + r_factor ) );

        // scale angle by gamma
        theta() *= gamma;
    }
    else
    {
        // scale angle by gamma, making sure points nearly 2pi are treated as zero
        double new_theta = theta() * gamma;
        if( new_theta < 2.5 * VERDICT_PI || r() < 0.2 ) theta( new_theta );
    }
    rtheta_to_xy();
}

double VerdictVector::vector_angle_quick( const VerdictVector& vec1, const VerdictVector& vec2 )
{
    //- compute the angle between two vectors in the plane defined by this vector
    // build yAxis and xAxis such that xAxis is the projection of
    // vec1 onto the normal plane of this vector

    // NOTE: vec1 and vec2 are Vectors from the vertex of the angle along
    //       the two sides of the angle.
    //       The angle returned is the right-handed angle around this vector
    //       from vec1 to vec2.

    // NOTE: vector_angle_quick gives exactly the same answer as vector_angle below
    //       providing this vector is normalized.  It does so with two fewer
    //       cross-product evaluations and two fewer vector normalizations.
    //       This can be a substantial time savings if the function is called
    //       a significant number of times (e.g Hexer) ... (jrh 11/28/94)
    // NOTE: vector_angle() is much more robust. Do not use vector_angle_quick()
    //       unless you are very sure of the safety of your input vectors.

    VerdictVector ry = ( *this ) * vec1;
    VerdictVector rx = ry * ( *this );

    double xv = vec2 % rx;
    double yv = vec2 % ry;

    double angle;
    assert( xv != 0.0 || yv != 0.0 );

    angle = atan2( yv, xv );

    if( angle < 0.0 )
    {
        angle += TWO_VERDICT_PI;
    }
    return angle;
}

VerdictVector vectorRotate( const double angle, const VerdictVector& normalAxis, const VerdictVector& referenceAxis )
{
    // A new coordinate system is created with the xy plane corresponding
    // to the plane normal to the normal axis, and the x axis corresponding to
    // the projection of the reference axis onto the normal plane.  The normal
    // plane is the tangent plane at the root point.  A unit vector is
    // constructed along the local x axis and then rotated by the given
    // ccw angle to form the new point.  The new point, then is a unit
    // distance from the global origin in the tangent plane.

    double x, y;

    // project a unit distance from root along reference axis

    VerdictVector yAxis = normalAxis * referenceAxis;
    VerdictVector xAxis = yAxis * normalAxis;
    yAxis.normalize();
    xAxis.normalize();

    x = cos( angle );
    y = sin( angle );

    xAxis *= x;
    yAxis *= y;
    return VerdictVector( xAxis + yAxis );
}

double VerdictVector::vector_angle( const VerdictVector& vector1, const VerdictVector& vector2 ) const
{
    // This routine does not assume that any of the input vectors are of unit
    // length. This routine does not normalize the input vectors.
    // Special cases:
    //     If the normal vector is zero length:
    //         If a new one can be computed from vectors 1 & 2:
    //             the normal is replaced with the vector cross product
    //         else the two vectors are colinear and zero or 2PI is returned.
    //     If the normal is colinear with either (or both) vectors
    //         a new one is computed with the cross products
    //         (and checked again).

    // Check for zero length normal vector
    VerdictVector normal = *this;
    double normal_lensq  = normal.length_squared();
    double len_tol       = 0.0000001;
    if( normal_lensq <= len_tol )
    {
        // null normal - make it the normal to the plane defined by vector1
        // and vector2. If still null, the vectors are colinear so check
        // for zero or 180 angle.
        normal       = vector1 * vector2;
        normal_lensq = normal.length_squared();
        if( normal_lensq <= len_tol )
        {
            double cosine = vector1 % vector2;
            if( cosine > 0.0 )
                return 0.0;
            else
                return VERDICT_PI;
        }
    }

    // Trap for normal vector colinear to one of the other vectors. If so,
    // use a normal defined by the two vectors.
    double dot_tol = 0.985;
    double dot     = vector1 % normal;
    if( dot * dot >= vector1.length_squared() * normal_lensq * dot_tol )
    {
        normal       = vector1 * vector2;
        normal_lensq = normal.length_squared();

        // Still problems if all three vectors were colinear
        if( normal_lensq <= len_tol )
        {
            double cosine = vector1 % vector2;
            if( cosine >= 0.0 )
                return 0.0;
            else
                return VERDICT_PI;
        }
    }
    else
    {
        // The normal and vector1 are not colinear, now check for vector2
        dot = vector2 % normal;
        if( dot * dot >= vector2.length_squared() * normal_lensq * dot_tol )
        {
            normal = vector1 * vector2;
        }
    }

    // Assume a plane such that the normal vector is the plane's normal.
    // Create yAxis perpendicular to both the normal and vector1. yAxis is
    // now in the plane. Create xAxis as the perpendicular to both yAxis and
    // the normal. xAxis is in the plane and is the projection of vector1
    // into the plane.

    normal.normalize();
    VerdictVector yAxis = normal;
    yAxis *= vector1;
    double yv = vector2 % yAxis;
    //  yAxis memory slot will now be used for xAxis
    yAxis *= normal;
    double xv = vector2 % yAxis;

    //  assert(x != 0.0 || y != 0.0);
    if( xv == 0.0 && yv == 0.0 )
    {
        return 0.0;
    }
    double angle = atan2( yv, xv );

    if( angle < 0.0 )
    {
        angle += TWO_VERDICT_PI;
    }
    return angle;
}

bool VerdictVector::within_tolerance( const VerdictVector& vectorPtr2, double tolerance ) const
{
    if( ( fabs( this->x() - vectorPtr2.x() ) < tolerance ) && ( fabs( this->y() - vectorPtr2.y() ) < tolerance ) &&
        ( fabs( this->z() - vectorPtr2.z() ) < tolerance ) )
    {
        return true;
    }

    return false;
}

void VerdictVector::orthogonal_vectors( VerdictVector& vector2, VerdictVector& vector3 )
{
    double xv[3];
    unsigned short i    = 0;
    unsigned short imin = 0;
    double rmin         = 1.0E20;
    unsigned short iperm1[3];
    unsigned short iperm2[3];
    unsigned short cont_flag = 1;
    double vec1[3], vec2[3];
    double rmag;

    // Copy the input vector and normalize it
    VerdictVector vector1 = *this;
    vector1.normalize();

    // Initialize perm flags
    iperm1[0] = 1;
    iperm1[1] = 2;
    iperm1[2] = 0;
    iperm2[0] = 2;
    iperm2[1] = 0;
    iperm2[2] = 1;

    // Get into the array format we can work with
    vector1.get_xyz( vec1 );

    while( i < 3 && cont_flag )
    {
        if( fabs( vec1[i] ) < 1e-6 )
        {
            vec2[i]         = 1.0;
            vec2[iperm1[i]] = 0.0;
            vec2[iperm2[i]] = 0.0;
            cont_flag       = 0;
        }

        if( fabs( vec1[i] ) < rmin )
        {
            imin = i;
            rmin = fabs( vec1[i] );
        }
        ++i;
    }

    if( cont_flag )
    {
        xv[imin]         = 1.0;
        xv[iperm1[imin]] = 0.0;
        xv[iperm2[imin]] = 0.0;

        // Determine cross product
        vec2[0] = vec1[1] * xv[2] - vec1[2] * xv[1];
        vec2[1] = vec1[2] * xv[0] - vec1[0] * xv[2];
        vec2[2] = vec1[0] * xv[1] - vec1[1] * xv[0];

        // Unitize
        rmag = sqrt( vec2[0] * vec2[0] + vec2[1] * vec2[1] + vec2[2] * vec2[2] );
        vec2[0] /= rmag;
        vec2[1] /= rmag;
        vec2[2] /= rmag;
    }

    // Copy 1st orthogonal vector into VerdictVector vector2
    vector2.set( vec2 );

    // Cross vectors to determine last orthogonal vector
    vector3 = vector1 * vector2;
}

//- Find next point from this point using a direction and distance
void VerdictVector::next_point( const VerdictVector& direction, double distance, VerdictVector& out_point )
{
    VerdictVector my_direction = direction;
    my_direction.normalize();

    // Determine next point in space
    out_point.x( xVal + ( distance * my_direction.x() ) );
    out_point.y( yVal + ( distance * my_direction.y() ) );
    out_point.z( zVal + ( distance * my_direction.z() ) );

    return;
}

VerdictVector::VerdictVector( const double xyz[3] ) : xVal( xyz[0] ), yVal( xyz[1] ), zVal( xyz[2] ) {}