SbLineard.h

Go to the documentation of this file.

/*
 *
 *  Copyright (C) 2000 Silicon Graphics, Inc.  All Rights Reserved. 
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  Further, this software is distributed without any warranty that it is
 *  free of the rightful claim of any third person regarding infringement
 *  or the like.  Any license provided herein, whether implied or
 *  otherwise, applies only to this software file.  Patent licenses, if
 *  any, provided herein do not apply to combinations of this program with
 *  other software, or any other product whatsoever.
 * 
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 *  Contact information: Silicon Graphics, Inc., 1600 Amphitheatre Pkwy,
 *  Mountain View, CA  94043, or:
 * 
 *  http://www.sgi.com 
 * 
 *  For further information regarding this notice, see: 
 * 
 *  http://oss.sgi.com/projects/GenInfo/NoticeExplan/
 *
 */
 
 
/*
 * Copyright (C) 1990,91   Silicon Graphics, Inc.
 *
 _______________________________________________________________________
 ______________  S I L I C O N   G R A P H I C S   I N C .  ____________
 |
 |   $Revision: 1.3 $
 |
 |   Description:
 |      This file contains definitions of various linear algebra classes,
 |      such as vectors, coordinates, etc..
 |
 |   Classes:
 |      SbVec3d
 |      SbVec2d
 |      SbVec4d
 |      SbRotationd
 |      SbMatrixd
 |
 |      SbLined
 |      SbPlaned
 |      SbSphered
 |
 |   Author(s)          : originally by Paul S. Strauss, Nick Thompson, 
 |                        David Mott, Alain Dumesny
 |                        adapted for double types by Wolf Spindler
 |
 ______________  S I L I C O N   G R A P H I C S   I N C .  ____________
 _______________________________________________________________________
 */
 
#ifndef _SB_LINEARD_
#define _SB_LINEARD_
 
#include <Inventor/system/SbSystem.h>
#include <math.h>
#include <Inventor/SbBasic.h>
 
 
typedef double SbMatd[4][4];
 
class SbVec3d;
class SbVec2d;
class SbVec4d;
class SbRotationd;
class SbMatrixd;
 
class SbLined;
class SbPlaned;
class SbCylinderd;
class SbSphered;
 
class SbBox3d;
 
 
 
 
 
 
class INVENTOR_API SbVec3d {
  public:
 
    SbVec3d() { vec[0] = 0.; vec[1] = 0.; vec[2] = 0.; }
 
    SbVec3d(const double v[3])
         { vec[0] = v[0]; vec[1] = v[1]; vec[2] = v[2]; }
 
    SbVec3d(double x, double y, double z)
         { vec[0] = x; vec[1] = y; vec[2] = z; }
 
    SbVec3d(SbPlaned &p0, SbPlaned &p1, SbPlaned &p2);
 
    SbVec3d     cross(const SbVec3d &v) const;
 
    double      dot(const SbVec3d &v) const;
 
    const double *getValue() const                       { return vec; }
 
    void        getValue(double &x, double &y, double &z) const;
 
    double      length() const;
 
    double      normalize();
 
    void        negate();
 
    SbVec3d &   setValue(const double v[3])
         { vec[0] = v[0]; vec[1] = v[1]; vec[2] = v[2]; return *this; }
 
    SbVec3d &   setValue(double x, double y, double z)
         { vec[0] = x; vec[1] = y; vec[2] = z; return *this; }
 
    SbVec3d &   setValue(const SbVec3d &barycentic,
                const SbVec3d &v0, const SbVec3d &v1, const SbVec3d &v2);
 
    double &       operator [](int i)            { return (vec[i]); }
    const double & operator [](int i) const      { return (vec[i]); }
 
    SbVec3d &   operator *=(double d);
 
    SbVec3d &   operator /=(double d)
        { return *this *= (1.0 / d); }
 
    SbVec3d &   operator +=(SbVec3d v);
    SbVec3d &   operator -=(SbVec3d v);
 
    SbVec3d     operator -() const;
 
    friend INVENTOR_API SbVec3d operator *(const SbVec3d &v, double d);
    friend INVENTOR_API SbVec3d operator *(double d, const SbVec3d &v);
    friend INVENTOR_API SbVec3d operator /(const SbVec3d &v, double d);
 
    friend INVENTOR_API SbVec3d operator +(const SbVec3d &v1, const SbVec3d &v2);
 
    friend INVENTOR_API SbVec3d operator -(const SbVec3d &v1, const SbVec3d &v2);
 
    friend INVENTOR_API bool       operator ==(const SbVec3d &v1, const SbVec3d &v2);
    friend INVENTOR_API bool       operator !=(const SbVec3d &v1, const SbVec3d &v2);
 
    bool                equals(const SbVec3d v, double tolerance) const;
 
    SbVec3d             getClosestAxis() const;
 
  protected:
    double       vec[3];         
};
 
INVENTOR_API SbVec3d operator *(const SbVec3d &v, double d);
inline INVENTOR_API SbVec3d operator *(double d, const SbVec3d &v)
{ return v * d; }
inline INVENTOR_API SbVec3d operator /(const SbVec3d &v, double d)
{ return v * (1.0 / d); }
 
INVENTOR_API SbVec3d operator +(const SbVec3d &v1, const SbVec3d &v2);
INVENTOR_API SbVec3d operator -(const SbVec3d &v1, const SbVec3d &v2);
 
INVENTOR_API bool       operator ==(const SbVec3d &v1, const SbVec3d &v2);
inline INVENTOR_API bool       operator !=(const SbVec3d &v1, const SbVec3d &v2)
{ return !(v1 == v2); }
 
 
 
 
 
class INVENTOR_API SbVec2d {
  public:
 
    SbVec2d() { vec[0] = 0.; vec[1] = 0.; }
 
    SbVec2d(const double v[2])                          { setValue(v); }
 
    SbVec2d(double x, double y)                         { setValue(x, y); }
 
    double       dot(const SbVec2d &v) const;
 
    const double *getValue() const                      { return vec; }
 
    void        getValue(double &x, double &y) const;
 
    double      length() const;
 
    void        negate();
 
    double      normalize();
 
    SbVec2d &   setValue(const double v[2]);
 
    SbVec2d &   setValue(double x, double y);
 
    double &       operator [](int i)            { return (vec[i]); }
    const double & operator [](int i) const      { return (vec[i]); }
 
    SbVec2d &   operator *=(double d);
 
    SbVec2d &   operator /=(double d)
        { return *this *= (1.0 / d); }
 
    SbVec2d &   operator +=(const SbVec2d &u);
    SbVec2d &   operator -=(const SbVec2d &u);
 
    SbVec2d     operator -() const;
 
    friend INVENTOR_API SbVec2d operator *(const SbVec2d &v, double d);
    friend INVENTOR_API SbVec2d operator *(double d, const SbVec2d &v);
    friend INVENTOR_API SbVec2d operator /(const SbVec2d &v, double d);
 
    friend INVENTOR_API SbVec2d operator +(const SbVec2d &v1, const SbVec2d &v2);
 
    friend INVENTOR_API SbVec2d operator -(const SbVec2d &v1, const SbVec2d &v2);
 
    friend INVENTOR_API bool               operator ==(const SbVec2d &v1, const SbVec2d &v2);
    friend INVENTOR_API bool               operator !=(const SbVec2d &v1, const SbVec2d &v2);
 
    bool                equals(const SbVec2d v, double tolerance) const;
 
  protected:
    double       vec[2];         
};
 
INVENTOR_API SbVec2d operator *(const SbVec2d &v, double d);
inline INVENTOR_API SbVec2d operator *(double d, const SbVec2d &v)
{ return v * d; }
inline INVENTOR_API SbVec2d operator /(const SbVec2d &v, double d)
{ return v * (1.0 / d); }
 
INVENTOR_API SbVec2d operator +(const SbVec2d &v1, const SbVec2d &v2);
INVENTOR_API SbVec2d operator -(const SbVec2d &v1, const SbVec2d &v2);
 
INVENTOR_API bool               operator ==(const SbVec2d &v1, const SbVec2d &v2);
inline INVENTOR_API bool               operator !=(const SbVec2d &v1, const SbVec2d &v2)
{ return !(v1 == v2); }
 
 
 
 
 
class INVENTOR_API SbVec4d {
  public:
 
    SbVec4d() { vec[0] = 0.; vec[1] = 0.; vec[2] = 0.; vec[3] = 0.; }
 
    SbVec4d(const double v[4])                         { setValue(v); }
 
    SbVec4d(double x, double y, double z, double w)    { setValue(x, y, z, w); }
 
    double      dot(const SbVec4d &v) const;
 
    void        getReal(SbVec3d &v) const;
 
    SbVec3d     getReal() const { SbVec3d tmp; getReal(tmp); return tmp; }
 
    const double *getValue() const                     { return vec; }
 
    void        getValue(double &x, double &y, double &z, double &w) const;
 
    double      length() const;
 
    void        negate();
 
    double      normalize();
 
    SbVec4d &   setValue(const double v[4]);
 
    SbVec4d &   setValue(double x, double y, double z, double w);
 
    double &       operator [](int i)            { return (vec[i]); }
    const double & operator [](int i) const      { return (vec[i]); }
 
    SbVec4d &   operator *=(double d);
 
    SbVec4d &   operator /=(double d)
        { return *this *= (1.0 / d); }
 
    SbVec4d &   operator +=(const SbVec4d &u);
    SbVec4d &   operator -=(const SbVec4d &u);
 
    SbVec4d     operator -() const;
 
    friend INVENTOR_API SbVec4d operator *(const SbVec4d &v, double d);
    friend INVENTOR_API SbVec4d operator *(double d, const SbVec4d &v);
    friend INVENTOR_API SbVec4d operator /(const SbVec4d &v, double d);
 
    friend INVENTOR_API SbVec4d operator +(const SbVec4d &v1, const SbVec4d &v2);
 
    friend INVENTOR_API SbVec4d operator -(const SbVec4d &v1, const SbVec4d &v2);
 
    friend INVENTOR_API bool               operator ==(const SbVec4d &v1, const SbVec4d &v2);
    friend INVENTOR_API bool               operator !=(const SbVec4d &v1, const SbVec4d &v2);
 
    bool                equals(const SbVec4d v, double tolerance) const;
 
  protected:
    double       vec[4];         
};
 
INVENTOR_API SbVec4d operator *(const SbVec4d &v, double d);
inline INVENTOR_API SbVec4d operator *(double d, const SbVec4d &v)
{ return v * d; }
inline INVENTOR_API SbVec4d operator /(const SbVec4d &v, double d)
{ return v * (1.0 / d); }
 
INVENTOR_API SbVec4d operator +(const SbVec4d &v1, const SbVec4d &v2);
INVENTOR_API SbVec4d operator -(const SbVec4d &v1, const SbVec4d &v2);
 
INVENTOR_API bool               operator ==(const SbVec4d &v1, const SbVec4d &v2);
inline INVENTOR_API bool               operator !=(const SbVec4d &v1, const SbVec4d &v2)
{ return !(v1 == v2); }
 
 
 
 
 
class INVENTOR_API SbRotationd {
  public:
 
    SbRotationd()
    {
      quat[0] = 0.; quat[1] = 0.; quat[2] = 0.; quat[3] = 1.;
    }
 
    SbRotationd(const double v[4])
        { setValue(v); }
 
    SbRotationd(double q0, double q1, double q2, double q3)
        { setValue(q0, q1, q2, q3); }
 
    SbRotationd(const SbMatrixd &m)
        { setValue(m); }
 
    SbRotationd(const SbVec3d &axis, double radians)
        { setValue(axis, radians); }
 
    SbRotationd(const SbVec3d &rotateFrom, const SbVec3d &rotateTo)
        { setValue(rotateFrom, rotateTo); }
 
    const double *      getValue() const
        { return (quat); }
 
    void                getValue(double &q0, double &q1,
                                 double &q2, double &q3) const;
 
    void                getValue(SbVec3d &axis, double &radians) const;
 
    void                getValue(SbMatrixd &matrix) const;
 
    SbMatrixd getMatrix() const;
    
    SbVec3d getAxis() const { SbVec3d axis; double radians; getValue(axis, radians); return axis; }
    double   getAngle() const { SbVec3d axis; double radians; getValue(axis, radians); return radians; }
 
    SbRotationd &       invert();
 
    SbRotationd         inverse() const
        { SbRotationd q = *this; return q.invert(); }
 
    SbRotationd &       setValue(const double q[4]);
 
    SbRotationd &       setValue(double q0, double q1, double q2, double q3);
 
    SbRotationd &       setValue(const SbMatrixd &m);
 
    SbRotationd &       setValue(const SbVec3d &axis, double radians);
 
    SbRotationd &       setValue(const SbVec3d &rotateFrom,
                                 const SbVec3d &rotateTo);
 
    SbRotationd &        operator *=(const SbRotationd &q);
 
    friend INVENTOR_API bool       operator ==(const SbRotationd &q1, const SbRotationd &q2);
    friend INVENTOR_API bool       operator !=(const SbRotationd &q1, const SbRotationd &q2);
 
    bool                equals(const SbRotationd &r, double tolerance) const;
 
    friend INVENTOR_API SbRotationd     operator *(const SbRotationd &q1, const SbRotationd &q2);
 
    void        multVec(const SbVec3d &src, SbVec3d &dst) const;
 
    SbVec3d     transformPoint(const SbVec3d &src) const { SbVec3d dst; multVec(src, dst); return dst; }
 
    void scaleAngle( double scaleFactor );
 
    static SbRotationd  slerp(const SbRotationd &rot0,
                              const SbRotationd &rot1, double t);
 
    static SbRotationd  identity()
        { return SbRotationd(0.0, 0.0, 0.0, 1.0); }
 
  private:
    double      quat[4];        
 
    double      norm() const;
 
    void        normalize();
};
 
INVENTOR_API bool       operator ==(const SbRotationd &q1, const SbRotationd &q2);
inline INVENTOR_API bool       operator !=(const SbRotationd &q1, const SbRotationd &q2)
{ return !(q1 == q2); }
 
INVENTOR_API SbRotationd     operator *(const SbRotationd &q1, const SbRotationd &q2);
 
 
 
 
 
 
class INVENTOR_API SbMatrixd {
  public:
 
    SbMatrixd() {
      makeIdentity();
    }
 
    SbMatrixd(double a11, double a12, double a13, double a14,
              double a21, double a22, double a23, double a24, 
              double a31, double a32, double a33, double a34, 
              double a41, double a42, double a43, double a44);
 
    SbMatrixd(const SbMatd &m);
 
    void        setValue(const SbMatd &m);
 
    void        setValue(const double* data);
 
    public:
 
    void        makeIdentity();
 
    static SbMatrixd     identity();
 
    void        setRotate(const SbRotationd &q);
 
    void        setScale(double s);
 
    void        setScale(const SbVec3d &s);
 
    void        setTranslate(const SbVec3d &t);
 
    void        setTransform(
                        const SbVec3d &translation,
                        const SbRotationd &rotation,
                        const SbVec3d &scaleFactor,
                        const SbRotationd &scaleOrientation,
                        const SbVec3d &center);
    void        setTransform(const SbVec3d &t, const SbRotationd &r,
                             const SbVec3d &s)
                { setTransform(t, r, s,
                               SbRotationd(0,0,0,1), SbVec3d(0,0,0)); }
    void        setTransform(const SbVec3d &t, const SbRotationd &r,
                             const SbVec3d &s, const SbRotationd &so)
                { setTransform(t, r, s, so, SbVec3d(0,0,0)); }
 
    void        getTransform(SbVec3d &translation,
                             SbRotationd &rotation,
                             SbVec3d &scaleFactor,
                             SbRotationd &scaleOrientation,
                             const SbVec3d &center) const;
    void        getTransform(SbVec3d &t, SbRotationd &r,
                             SbVec3d &s, SbRotationd &so) const
                { getTransform(t, r, s, so, SbVec3d(0,0,0)); }
 
 
 
    void        getValue(SbMatd &m) const;
    const SbMatd &       getValue() const { return matrix; }
 
    double       det3(int r1, int r2, int r3, int c1, int c2, int c3) const;
 
    double       det3() const { return det3(0, 1, 2, 0, 1, 2); }
 
    double       det4() const;
 
    bool        factor(SbMatrixd &r, SbVec3d &s, SbMatrixd &u,
                       SbVec3d &t, SbMatrixd &proj) const;
 
    SbMatrixd    inverse() const;
 
    bool        LUDecomposition(int index[4], double &d);
 
    void        LUBackSubstitution(int index[4], double b[4]) const;
 
    SbMatrixd    transpose() const;
 
    SbMatrixd &  multRight(const SbMatrixd &m);   // this = this * m
    SbMatrixd &  multLeft(const SbMatrixd &m);    // this = m * this
 
    SbVec3d     transformPoint(const SbVec3d &src) const { SbVec3d dst; multVecMatrix(src, dst); return dst; }
    SbVec4d     transformPoint(const SbVec4d &src) const { SbVec4d dst; multVecMatrix(src, dst); return dst; }
 
    SbVec3d     transformDirection(const SbVec3d &src) const { SbVec3d dst; multDirMatrix(src, dst); return dst; }
 
    SbLined     transformLine(const SbLined &src) const;
    
    void        multMatrixVec(const SbVec3d &src, SbVec3d &dst) const;
 
    void        multVecMatrix(const SbVec3d &src, SbVec3d &dst) const;
    void        multVecMatrix(const SbVec4d &src, SbVec4d &dst) const;
    void        multVecMatrix(const SbVec3d &src, SbVec4d &dst) const;
 
    void        multDirMatrix(const SbVec3d &src, SbVec3d &dst) const;
 
    void        multLineMatrix(const SbLined &src, SbLined &dst) const;
 
    void        print(FILE *fp) const;
 
    operator double *()          { return &matrix[0][0]; }
 
    operator SbMatd &()          { return matrix; }
 
    double *       operator [](int i)        { return &matrix[i][0]; }
    const double * operator [](int i) const  { return &matrix[i][0]; }
 
    SbMatrixd &  operator =(const SbMatd &m);
 
    SbMatrixd &  operator =(const SbMatrixd &m);
 
    SbMatrixd &  operator =(const SbRotationd &q) { setRotate(q); return *this; }
 
    SbMatrixd &  operator *=(const SbMatrixd &m)  { return multRight(m); }
 
    friend INVENTOR_API SbMatrixd        operator *(const SbMatrixd &m1, const SbMatrixd &m2);
 
    friend INVENTOR_API bool               operator ==(const SbMatrixd &m1, const SbMatrixd &m2);
    friend INVENTOR_API bool               operator !=(const SbMatrixd &m1, const SbMatrixd &m2);
 
    bool                equals(const SbMatrixd &m, double tolerance) const;
 
  private:
    SbMatd      matrix;         
 
    void        jacobi3(double evalues[3], SbVec3d evectors[3], int &rots) const;
 
    bool affine_inverse(const SbMatrixd &in, SbMatrixd &out) const;
};
 
 
INVENTOR_API SbMatrixd  operator *(const SbMatrixd &m1, const SbMatrixd &m2);
 
INVENTOR_API bool          operator ==(const SbMatrixd &m1, const SbMatrixd &m2);
inline INVENTOR_API bool operator !=(const SbMatrixd &m1, const SbMatrixd &m2)
{ return !(m1 == m2); }
 
 
 
 
class INVENTOR_API SbLined {
  public:
    SbLined()    {}
 
    SbLined(const SbVec3d &p0, const SbVec3d &p1);
 
    void                setValue(const SbVec3d &p0, const SbVec3d &p1);
 
    bool                getClosestPoints(const SbLined  &line2,
                                         SbVec3d &ptOnThis,
                                         SbVec3d &ptOnLine2 ) const;
 
    SbVec3d             getClosestPoint(const SbVec3d &point) const;
 
    const SbVec3d &     getPosition() const     { return pos; }
    const SbVec3d &     getDirection() const    { return dir; }
 
 
  SoINTERNAL public:
    bool                intersect( const SbBox3d &box,
                                   SbVec3d &enter, SbVec3d &exit ) const;
    bool                intersect( double angle, const SbBox3d &box ) const;
    bool                intersect( double angle, const SbVec3d &point ) const;
    bool                intersect( double angle, const SbVec3d &v0,
                                   const SbVec3d &v1, SbVec3d &pt ) const;
    bool                intersect( const SbVec3d &v0,
                                  const SbVec3d &v1,
                                  const SbVec3d &v2,
                                  SbVec3d &pt, SbVec3d &barycentric,
                                  bool &front ) const;
 
  private:
    SbVec3d     pos;
    SbVec3d     dir;
};
 
 
 
 
 
class INVENTOR_API SbPlaned {
  public:
    SbPlaned() { distance = 0.; }
 
    SbPlaned(const SbVec3d &p0, const SbVec3d &p1, const SbVec3d &p2);
 
    SbPlaned(const SbVec3d &n, double d);
 
    SbPlaned(const SbVec3d &n, const SbVec3d &p);
 
    void                offset(double d);
 
    bool                intersect(const SbLined &l,
                                  SbVec3d &intersection) const;
 
    void                transform(const SbMatrixd &matrix);
 
    bool                isInHalfSpace(const SbVec3d &point) const;
 
    double getDistance(const SbVec3d &point) const;
 
    const SbVec3d &     getNormal() const               { return normalVec; }
    double              getDistanceFromOrigin() const   { return distance; }
 
    friend INVENTOR_API bool               operator ==(const SbPlaned &p1, const SbPlaned &p2);
    friend INVENTOR_API bool               operator !=(const SbPlaned &p1, const SbPlaned &p2);
 
  private:
 
    SbVec3d     normalVec;
 
    double       distance;
};
 
INVENTOR_API bool               operator ==(const SbPlaned &p1, const SbPlaned &p2);
inline INVENTOR_API bool        operator !=(const SbPlaned &p1, const SbPlaned &p2)
{ return !(p1 == p2); }
 
 
 
class INVENTOR_API SbCylinderd {
  public:
 
    SbCylinderd();
 
    SbCylinderd(const SbLined &a, double r);
 
    void        setValue(const SbLined &a, double r);
 
    void        setAxis(const SbLined &a);
    void        setRadius(double r);
 
    const SbLined &     getAxis() const                 { return axis; }
    double              getRadius() const               { return radius; }
 
    bool        intersect(const SbLined &l, SbVec3d &intersection) const;
    bool        intersect(const SbLined &l,
                          SbVec3d &enter, SbVec3d &exit) const;
 
  private:
    SbLined     axis;
    double      radius;
 
    static bool         unitCylinderIntersect(const SbLined &l,
                                              SbVec3d &in, SbVec3d &out);
};
 
 
 
 
 
 
class INVENTOR_API SbSphered {
  public:
 
    SbSphered() { radius = 0.; }
 
    SbSphered(const SbVec3d &c, double r);
 
    void        setValue(const SbVec3d &c, double r);
 
    void        setCenter(const SbVec3d &c);
    void        setRadius(double r);
 
    const SbVec3d &     getCenter() const               { return center; }
    double              getRadius() const               { return radius; }
 
    void        circumscribe(const SbBox3d &box);
 
    bool        intersect(const SbLined &l, SbVec3d &intersection) const;
    bool        intersect(const SbLined &l, SbVec3d &enter, SbVec3d &exit) const;
 
  private:
    SbVec3d     center;
    double      radius;
};
 
#endif /* _SB_LINEARD_ */