Vec3d.h

//
// This file is released under the terms of the NASA Open Source Agreement (NOSA)
// version 1.3 as detailed in the LICENSE file which accompanies this software.
//
 
//******************************************************************************
//
//   3D Point Double Class
//
//   J.R. Gloudemans - 7/7/93
//******************************************************************************
 
#ifndef VSPVEC3D_H
#define VSPVEC3D_H
 
 
 
namespace Eigen
{
    template < typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols >
    class Matrix;
}
 
typedef Eigen::Matrix< double, 1, 3, 0x1, 1, 3 > threed_point_type;
 
 
#include <iostream>
#include <vector>
 
class vec2d;

class vec3d;
class Matrix4d;
 
//==== Forward declare to get around MS Compiler bug ====//
 
vec3d operator+( const vec3d& a, const vec3d& b );

 
vec3d operator-( const vec3d& a, const vec3d& b );

 
vec3d operator*( const vec3d& a, double b );

 
vec3d operator*( const vec3d& a, const vec3d& b );

 
vec3d operator/( const vec3d& a, double b );
 

class vec3d
{
private:
 
 
public:
    double v[3];
 
    vec3d();           // vec3d x or new vec3d
 
    ~vec3d()  {}        // delete vec3d
 
    vec3d( double xx, double yy, double zz );
 
    vec3d( const vec3d& a ); // vec3d x = y
 
    vec3d( const threed_point_type &a );
 
    vec3d( const double a[3] );
    vec3d( const float a[3] );
    vec3d( const std::vector<double> &a );
 
    vec3d& operator=( const vec3d& a ); // x = y
    vec3d& operator=( const vec2d& a );
    vec3d& operator=( double a );      // x = 35.
    vec3d& operator=( const threed_point_type &a );
 
    double& operator [] ( int i )
    {
        return v[i];
    }
    const double& operator [] ( int i ) const
    {
        return v[i];
    }
 
    vec2d as_vec2d_xy();
 
    // Set Point Values
 
    vec3d& set_xyz( double xx, double yy, double zz );
 
    vec3d& set_vec( const std::vector<double> &a );
    vec3d& set_arr( const double a[] );
    vec3d& set_arr( const float a[] );
 
    vec3d& set_x( double xx );

 
    vec3d& set_y( double yy );

 
    vec3d& set_z( double zz );
 
 
    vec3d& set_refx( const vec3d &a );
    vec3d& set_refy( const vec3d &a );
    vec3d& set_refz( const vec3d &a );
 
    // Get Point Values
    void get_pnt( double pnt[3] ) const
    {
        pnt[0] = v[0];
        pnt[1] = v[1];
        pnt[2] = v[2];
    }
    void get_pnt( float  pnt[3] ) const
    {
        pnt[0] = ( float )v[0];
        pnt[1] = ( float )v[1];
        pnt[2] = ( float )v[2];
    }
    void get_pnt( threed_point_type &pnt ) const;

 
    double x() const
 
    {
        return( v[0] );
    }

 
    double y() const
 
    {
        return( v[1] );
    }

 
    double z() const
 
    {
        return( v[2] );
    }
 
    double* data()
    {
        return( v );
    }
 
    void Transform( const Matrix4d &m );
    void FlipNormal()                          {};
 
    // Rotate About Axis --> Change Internal Values
 
    void rotate_x( double theta );

 
    void rotate_y( double theta );

 
    void rotate_z( double theta );
 
    // Scale Up Single Coord  --> Change Internal Values
 
    void scale_x( double scale )
 
    {
        v[0] *= scale;
    };

 
    void scale_y( double scale )
 
    {
        v[1] *= scale;
    };

 
    void scale_z( double scale )
 
    {
        v[2] *= scale;
    };

 
    void offset_x( double offset )
 
    {
        v[0] += offset;
    };

 
    void offset_y( double offset )
 
    {
        v[1] += offset;
    };

 
    void offset_z( double offset )
 
    {
        v[2] += offset;
    };
    void offset_i( double offset, int idir )
    {
        v[idir] += offset;
    };

 
    vec3d reflect_xy() const
 
    {
        return( vec3d( v[0],  v[1], -v[2] ) );
    }

 
    vec3d reflect_xz() const
 
    {
        return( vec3d( v[0], -v[1],  v[2] ) );
    }

 
    vec3d reflect_yz() const
 
    {
        return( vec3d( -v[0],  v[1],  v[2] ) );
    }
 
    vec3d swap_xy() const
    {
        return( vec3d( v[1],  v[0],  v[2] ) );
    }
    vec3d swap_xz() const
    {
        return( vec3d( v[2],  v[1],  v[0] ) );
    }
    vec3d swap_yz() const
    {
        return( vec3d( v[0],  v[2],  v[1] ) );
    }
 
    // x = a + b, x = a - b, a*scale, a/scale
    friend vec3d operator+( const vec3d& a, const vec3d& b );
    friend vec3d operator-( const vec3d& a, const vec3d& b );
    friend vec3d operator*( const vec3d& a, double b );
    friend vec3d operator*( double b, const vec3d& a );
    friend vec3d operator*( const vec3d& a, const vec3d& b );
    friend vec3d operator/( const vec3d& a, double b );
    vec3d& operator+=( const vec3d& b );
    vec3d& operator-=( const vec3d& b );
    vec3d& operator*=( double b );
    vec3d& operator+=( double b[] );
    vec3d& operator-=( double b[] );
    vec3d& operator+=( float b[] );
    vec3d& operator-=( float b[] );
 
    friend vec3d operator-( const vec3d & in );
 
    friend bool operator==( const vec3d &a, const vec3d& b );
    friend bool operator!=( const vec3d &a, const vec3d& b );
 
    // cout << a
//    friend ostream& operator<< (ostream& out, const vec3d& a);
 
    friend double dist( const vec3d& a, const vec3d& b );
    friend double dist_squared( const vec3d& a, const vec3d& b );

 
    double mag() const;                // x = a.mag()
 
    double magsq() const;

 
    void normalize();           // a.normalize()
 
 
    int major_comp() const;
    int minor_comp() const;
 
    bool isnan() const;
    bool isinf() const;
    bool isfinite() const;
 
    void print( const char* label = "" ) const;
 
    friend double dot( const vec3d& a, const vec3d& b ); // x = dot(a,b)
    friend vec3d cross( const vec3d& a, const vec3d& b ); // a = cross(b,c)
    friend double angle( const vec3d& a, const vec3d& b );
    friend double signed_angle( const vec3d& a, const vec3d& b, const vec3d& ref );
    friend double cos_angle( const vec3d& a, const vec3d& b );
    friend double radius_of_circle( const vec3d& p1, const vec3d& p2, const vec3d& p3 );
    friend void center_of_circle( const vec3d& p1, const vec3d& p2, const vec3d& p3, vec3d& center );
    friend bool triangle_plane_intersect_test( const vec3d& org, const vec3d& norm, const vec3d& p1, const vec3d& p2, const vec3d& p3 );
    friend double triangle_plane_minimum_dist( const vec3d& org, const vec3d& norm, const vec3d& p1, const vec3d& p2, const vec3d& p3, vec3d &pa, vec3d &pb );
    friend double triangle_plane_maximum_dist( const vec3d& org, const vec3d& norm, const vec3d& p1, const vec3d& p2, const vec3d& p3, vec3d &pa, vec3d &pb );
    friend bool plane_plane_intersection( const vec3d &p0, const vec3d &n0, const vec3d &p1, const vec3d &n1, vec3d &p, vec3d &v );
    friend double angle_pnt_2_plane( const vec3d& ptplane, vec3d norm, const vec3d& ptaxis, vec3d axis, const vec3d &pt, int ccw, vec3d &prot );
    friend double signed_dist_pnt_2_plane( const vec3d& org, const vec3d& norm, const vec3d& pnt );
    friend double dist_pnt_2_plane( const vec3d& org, const vec3d& norm, const vec3d& pnt );
    friend double dist_pnt_2_line( const vec3d& line_pt1, const vec3d& line_pt2, const vec3d& pnt );
    friend double dist_pnt_2_ray( const vec3d& line_pt1, const vec3d& dir_unit_vec, const vec3d& pnt );
//   friend double dist_pnt_2_line_seg( vec3d& line_pt1,  vec3d& line_pt2,  vec3d& pnt);
    friend vec3d proj_u_on_v( const vec3d& u, const vec3d& v );
    friend vec3d proj_pnt_on_line_seg( const vec3d& line_pt1, const vec3d& line_pt2, const vec3d& pnt );
    friend vec3d proj_pnt_on_ray( const vec3d& line_pt1, const vec3d& line_dir, const vec3d& pnt );
    friend vec3d proj_pnt_on_line( const vec3d& line_pt1, const vec3d& line_pt2, const vec3d& pnt );
    friend vec3d proj_pnt_to_plane( const vec3d& org, const vec3d& plane_ln1, const vec3d& plane_ln2, const vec3d& pnt );
    friend vec3d proj_vec_to_plane( const vec3d& vec, const vec3d& norm );
    friend int tri_seg_intersect( const vec3d& A, const vec3d& B, const vec3d& C, const vec3d& D, const vec3d& E,
                                  double& u, double& w, double& t );
    friend int tri_ray_intersect( const vec3d& A, const vec3d& B, const vec3d& C, const vec3d& D, const vec3d& E,
                                  double& u, double& w, double& t );
    friend int plane_ray_intersect( const vec3d& A, const vec3d& B, const vec3d& C, const vec3d& D, const vec3d& E, double& t );
    friend int plane_ray_intersect( const vec3d& orig, const vec3d& norm, const vec3d& D, const vec3d& E, double& t );
    friend int ray_ray_intersect( const vec3d& A, const vec3d& B, const vec3d& C, const vec3d& D, vec3d& int_pnt1, vec3d& int_pnt2 );
    friend double tetra_volume( const vec3d& A, const vec3d& B, const vec3d& C );
    friend double area( const vec3d& A, const vec3d& B, const vec3d& C );
    friend double poly_area( const std::vector< vec3d > & pnt_vec );
    friend double dist3D_Segment_to_Segment( const vec3d& S1P0, const vec3d& S1P1, const vec3d& S2P0, const vec3d& S2P1 );
    friend double dist3D_Segment_to_Segment( const vec3d& S1P0, const vec3d& S1P1, const vec3d& S2P0, const vec3d& S2P1,
            double* Lt, vec3d* Ln, double* St, vec3d* Sn );
    friend double nearSegSeg( const vec3d& L0, const vec3d& L1, const vec3d& S0, const vec3d& S1, double* Lt, vec3d* Ln, double* St,  vec3d* Sn );
    friend double pointLineDistSquared( const vec3d & p, const vec3d& lp0, const vec3d& lp1, double &t, vec3d &pon );
    friend double pointSegDistSquared( const vec3d& p, const vec3d& sp0, const vec3d& sp1, double &t, vec3d &pon );
    friend vec3d  point_on_line( const vec3d & lp0, const vec3d & lp1, const double & t );
 
    friend vec2d MapToPlane( const vec3d & p, const vec3d & planeOrig, const vec3d & planeVec1, const vec3d & planeVec2 );
    friend vec3d MapFromPlane( const vec2d & uw, const vec3d & planeOrig, const vec3d & planeVec1, const vec3d & planeVec2 );
 
    friend int plane_half_space( const vec3d & planeOrig, const vec3d & planeNorm, const vec3d & pnt );
 
    friend bool line_line_intersect( const vec3d & p1, const vec3d & p2, const vec3d & p3, const vec3d & p4, double* s, double* t );
 
    friend vec3d RotateArbAxis( const vec3d & p, double theta, const vec3d & r );
 
    friend bool PtInTri( const vec3d & v0, const vec3d & v1, const vec3d & v2, const vec3d & p );
    friend vec3d BarycentricWeights( const vec3d & v0, const vec3d & v1, const vec3d & v2, const vec3d & p );
    friend void BilinearWeights( const vec3d & p0, const vec3d & p1, const vec3d & p, std::vector< double > & weights );
    friend double tri_tri_min_dist( const vec3d & v0, const vec3d & v1, const vec3d & v2, const vec3d & v3, const vec3d & v4, const vec3d & v5, vec3d &p1, vec3d &p2 );
    friend double pnt_tri_min_dist( const vec3d & v0, const vec3d & v1, const vec3d & v2, const vec3d & pnt, vec3d &pnearest );
 
    friend vec3d slerp( const vec3d& a, const vec3d& b, const double &t );
    friend void printpt( const vec3d & v );
    friend vec3d ToSpherical( const vec3d & v );
    friend vec3d ToSpherical2( const vec3d & v, const vec3d & vdet );
    friend vec3d ToCartesian( const vec3d & v );
 
    friend void FitPlane( const std::vector < vec3d > & pts, vec3d & cen, vec3d & norm );
};

 
double dist( const vec3d& a, const vec3d& b );

 
double dist_squared( const vec3d& a, const vec3d& b );

 
double dot( const vec3d& a, const vec3d& b );

 
vec3d cross( const vec3d& a, const vec3d& b );

 
double angle( const vec3d& a, const vec3d& b );

 
double signed_angle( const vec3d& a, const vec3d& b, const vec3d& ref );
 // TODO: verify description
 
double cos_angle( const vec3d& a, const vec3d& b );

 
vec3d RotateArbAxis( const vec3d & p, double theta, const vec3d & r );
 
 
namespace std
{
string to_string( const vec3d &v);
}
 
vec3d slerp( const vec3d& a, const vec3d& b, const double &t );
 
void FitPlane( const std::vector < vec3d > & pts, vec3d & cen, vec3d & norm );
 
// Vector version that does not branch on magnitude of values.
vec3d compsum( const std::vector < vec3d > &x );
 
// Perform compensated summation of a vector of values.  Should be robust to rounding errors when some elements of x
// are substantially smaller than the running sum.  And vis-versa.
template < typename T >
T compsum( const std::vector < T > &x )
{
    T e(0.0); // constructor must set to zero
    T sum(0.0);
 
    for ( int i = 0; i < x.size(); i++ )
    {
        T t = sum + x[i];
        if ( std::abs( sum ) >= std::abs( x[i] ) )
        {
            e += ( sum - t ) + x[i];
        }
        else
        {
            e += ( x[i] - t ) + sum;
        }
        sum = t;
    }
    sum += e;
 
    return sum;
}
 
#endif