SbMatrix.h

/*=======================================================================
 *** THE CONTENT OF THIS WORK IS PROPRIETARY TO FEI S.A.S, (FEI S.A.S.),            ***
 ***              AND IS DISTRIBUTED UNDER A LICENSE AGREEMENT.                     ***
 ***                                                                                ***
 ***  REPRODUCTION, DISCLOSURE,  OR USE,  IN WHOLE OR IN PART,  OTHER THAN AS       ***
 ***  SPECIFIED  IN THE LICENSE ARE  NOT TO BE  UNDERTAKEN  EXCEPT WITH PRIOR       ***
 ***  WRITTEN AUTHORIZATION OF FEI S.A.S.                                           ***
 ***                                                                                ***
 ***                        RESTRICTED RIGHTS LEGEND                                ***
 ***  USE, DUPLICATION, OR DISCLOSURE BY THE GOVERNMENT OF THE CONTENT OF THIS      ***
 ***  WORK OR RELATED DOCUMENTATION IS SUBJECT TO RESTRICTIONS AS SET FORTH IN      ***
 ***  SUBPARAGRAPH (C)(1) OF THE COMMERCIAL COMPUTER SOFTWARE RESTRICTED RIGHT      ***
 ***  CLAUSE  AT FAR 52.227-19  OR SUBPARAGRAPH  (C)(1)(II)  OF  THE RIGHTS IN      ***
 ***  TECHNICAL DATA AND COMPUTER SOFTWARE CLAUSE AT DFARS 52.227-7013.             ***
 ***                                                                                ***
 ***                   COPYRIGHT (C) 1996-2024 BY FEI S.A.S,                        ***
 ***                        BORDEAUX, FRANCE                                        ***
 ***                      ALL RIGHTS RESERVED                                       ***
**=======================================================================*/
/*=======================================================================
** Author      : VSG (MMM YYYY)
**=======================================================================*/
#ifndef _SB_MATRIX_
#define _SB_MATRIX_
 
#include <Inventor/SbBase.h>
#include <Inventor/STL/iostream>
#include <Inventor/SbVec.h>
#include <string.h>
 
class SbLine;
class SbLined;
class SbRotation;
class SbRotationd;
class SbMatrixd;
 
typedef float SbMat[4][4];
typedef float SbMat3[3][3];
typedef double SbMatd[4][4];
 
//
//  Class: SbMatrix
//
//  4x4 matrix of floating-point elements.
//
 
 
 
class  SbMatrix {
public:
 
  SbMatrix()    { memset(matrix, 0, sizeof(SbMat)); }
 
  SbMatrix(float a11, float a12, float a13, float a14,
    float a21, float a22, float a23, float a24,
    float a31, float a32, float a33, float a34,
    float a41, float a42, float a43, float a44);
 
  SbMatrix(const SbMat &m);
 
  void  setValue(const SbMat &m);
 
  void    setValue(const float *pMat)
  {
    matrix[0][0] = *pMat++ ;
    matrix[0][1] = *pMat++ ;
    matrix[0][2] = *pMat++ ;
    matrix[0][3] = *pMat++ ;
 
    matrix[1][0] = *pMat++ ;
    matrix[1][1] = *pMat++ ;
    matrix[1][2] = *pMat++ ;
    matrix[1][3] = *pMat++ ;
 
    matrix[2][0] = *pMat++ ;
    matrix[2][1] = *pMat++ ;
    matrix[2][2] = *pMat++ ;
    matrix[2][3] = *pMat++ ;
 
    matrix[3][0] = *pMat++ ;
    matrix[3][1] = *pMat++ ;
    matrix[3][2] = *pMat++ ;
    matrix[3][3] = *pMat++ ;
  } ;
 
  void  setValue(const SbMatrixd &md) ;
 
public:
 
  void  makeIdentity();
 
  static SbMatrix  identity();
 
  void  setRotate(const SbRotation &q);
 
  void  setScale(float s);
 
  void  setScale(const SbVec3f &s);
 
  void  setTranslate(const SbVec3f &t);
 
  void  setTransform(const SbVec3f &translation,
    const SbRotation &rotation,
    const SbVec3f &scaleFactor,
    const SbRotation &scaleOrientation,
    const SbVec3f &center);
 
  void setTransform(const SbVec3f &t, const SbRotation &r, const SbVec3f &s);
 
  void setTransform(const SbVec3f &t, const SbRotation &r,
    const SbVec3f &s, const SbRotation &so);
 
  void  getTransform(SbVec3f &translation,
    SbRotation &rotation,
    SbVec3f &scaleFactor,
    SbRotation &scaleOrientation,
    const SbVec3f &center) const;
 
  void  getTransform(SbVec3f &t, SbRotation &r,
    SbVec3f &s, SbRotation &so) const;
 
  // The following methods return matrix values and other info:
 
  void  getValue(SbMat &m) const;
 
#ifdef _WIN32
  SbMat &getValue() { SbMat &rMat = matrix; return rMat; }
#endif
 
  const SbMat &getValue() const { return matrix; }
 
  float  det3(int r1, int r2, int r3, int c1, int c2, int c3) const;
 
  float  det3() const { return det3(0, 1, 2, 0, 1, 2); }
 
  float  det4() const;
 
  SbBool factor(SbMatrix &r, SbVec3f &s, SbMatrix &u,
    SbVec3f &t, SbMatrix &proj) const;
 
  SbMatrix  inverse() const;
 
  void translate(const SbVec3f& translation);
 
  void scale(const SbVec3f& scaleFactor);
 
  SbBool  LUDecomposition(int index[4], float &d);
 
  void  LUBackSubstitution(int index[4], float b[4]) const;
 
  SbMatrix  transpose() const;
 
 
  // The following methods provide Mx/mx and mx/vec arithmetic:
 
  SbMatrix &multRight(const SbMatrix &m);
 
  SbMatrix &multLeft(const SbMatrix &m);
 
  void  multMatrixVec(const SbVec3f &src, SbVec3f &dst) const;
 
  inline void  multVecMatrix(const SbVec3f &src, SbVec3f &dst) const
  {
    float    x,y,z,w;
 
    x = src[0]*matrix[0][0] + src[1]*matrix[1][0] +
        src[2]*matrix[2][0] + matrix[3][0];
    y = src[0]*matrix[0][1] + src[1]*matrix[1][1] +
        src[2]*matrix[2][1] + matrix[3][1];
    z = src[0]*matrix[0][2] + src[1]*matrix[1][2] +
        src[2]*matrix[2][2] + matrix[3][2];
    w = src[0]*matrix[0][3] + src[1]*matrix[1][3] +
        src[2]*matrix[2][3] + matrix[3][3];
 
    w = 1.f/w;
    dst.setValue(x*w, y*w, z*w);
  }
 
  void  multMatrixVec(const SbVec3f &src, SbVec4f &dst) const;
 
  void  multVecMatrix(const SbVec3f &src, SbVec4f &dst) const;
 
  void  multDirMatrix(const SbVec3f &src, SbVec3f &dst) const;
 
  void  multLineMatrix(const SbLine &src, SbLine &dst) const;
 
 
  // The following methods are miscellaneous Mx functions:
 
  void  print(FILE *fp) const;
 
 
  operator float *() { return &matrix[0][0]; }
 
  operator SbMat &() { return matrix; }
 
  float * operator [](int i)   { return &matrix[i][0]; }
 
  const float * operator [](int i) const  { return &matrix[i][0]; }
 
  SbMatrix & operator =(const SbMat &m);
 
  SbMatrix & operator =(const SbMatrix &m);
 
  SbMatrix & operator =(const SbRotation &q)  { setRotate(q); return *this; }
 
  SbMatrix & operator *=(const SbMatrix &m)  { return multRight(m); }
 
  SbVec4f operator *(const SbVec4f& v);
 
   friend SbVec4f operator *(const SbVec4f &v, const SbMatrix &m);
 
   friend SbMatrix operator *(const SbMatrix &m1, const SbMatrix &m2);
 
   friend int operator ==(const SbMatrix &m1, const SbMatrix &m2);
   friend int operator !=(const SbMatrix &m1, const SbMatrix &m2)
  { return !(m1 == m2); }
 
  SbBool    equals(const SbMatrix &m, float tolerance) const;
 
  bool isInvertible() const;
 
private:
  void jacobi3(float evalues[3], SbVec3f evectors[3], int &rots) const;
 
private:
  // Storage for 4x4 matrix
  SbMat matrix;
 
  SbBool affine_inverse(const SbMatrix &in, SbMatrix &out) const;
 
 
};
 
//
//  Class: SbMatrixd
//
//  4x4 matrix of double precision floating-point elements.
//
 
class  SbMatrixd {
public:
 
  SbMatrixd()    { memset(matrix, 0, sizeof(SbMatd)); }
 
  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 *pMat)
  {
    matrix[0][0] = *pMat++ ;
    matrix[0][1] = *pMat++ ;
    matrix[0][2] = *pMat++ ;
    matrix[0][3] = *pMat++ ;
 
    matrix[1][0] = *pMat++ ;
    matrix[1][1] = *pMat++ ;
    matrix[1][2] = *pMat++ ;
    matrix[1][3] = *pMat++ ;
 
    matrix[2][0] = *pMat++ ;
    matrix[2][1] = *pMat++ ;
    matrix[2][2] = *pMat++ ;
    matrix[2][3] = *pMat++ ;
 
    matrix[3][0] = *pMat++ ;
    matrix[3][1] = *pMat++ ;
    matrix[3][2] = *pMat++ ;
    matrix[3][3] = *pMat++ ;
  } ;
 
  void  setValue(const SbMatrix &m) ;
 
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);
 
  void setTransform(const SbVec3d &t, const SbRotationd &r,
    const SbVec3d &s, const SbRotationd &so);
 
  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;
 
 
  // The following methods return matrix values and other info:
 
  void  getValue(SbMatd &m) const;
 
#ifdef _WIN32
 
  SbMatd &getValue() { SbMatd &rMat = matrix; return rMat; }
#endif
 
  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;
 
  SbBool  factor(SbMatrixd &r, SbVec3d &s, SbMatrixd &u,
    SbVec3d &t, SbMatrixd &proj) const;
 
  SbMatrixd  inverse() const;
 
  void translate(const SbVec3d& translation);
 
  void scale(const SbVec3d& scaleFactor);
 
  SbBool  LUDecomposition(int index[4], double &d);
 
  void  LUBackSubstitution(int index[4], double b[4]) const;
 
  SbMatrixd  transpose() const;
 
 
  // The following methods provide Mx/mx and mx/vec arithmetic:
 
  SbMatrixd &multRight(const SbMatrixd &m);
 
  SbMatrixd &multLeft(const SbMatrixd &m);
 
  void  multMatrixVec(const SbVec3d &src, SbVec3d &dst) const;
 
  void  multVecMatrix(const SbVec3d &src, SbVec3d &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;
 
  SbVec4d operator *(const SbVec4d& v);
 
 
  // The following methods are miscellaneous Mx functions:
 
  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 SbVec4d operator *(const SbVec4d &v, const SbMatrixd &m);
 
   friend SbMatrixd operator *(const SbMatrixd &m1, const SbMatrixd &m2);
 
   friend int operator ==(const SbMatrixd &m1, const SbMatrixd &m2);
   friend int operator !=(const SbMatrixd &m1, const SbMatrixd &m2)
  { return !(m1 == m2); }
 
  SbBool    equals(const SbMatrixd &m, double tolerance) const;
 
  bool isInvertible() const;
 
private:
 
  void  jacobi3(double evalues[3], SbVec3d evectors[3], int &rots) const;
 
private:
  // Storage for 4x4 matrix
  SbMatd  matrix;
 
  SbBool affine_inverse(const SbMatrixd &in, SbMatrixd &out) const;
 
 
};
 
//
//  Class: SbMatrix3
//
//  3x3 matrix of floating-point elements.
//
 
 
 
class  SbMatrix3 {
public:
 
  SbMatrix3() { memset(matrix, 0, sizeof(SbMat3)); }
 
  SbMatrix3(float a11, float a12, float a13,
    float a21, float a22, float a23,
    float a31, float a32, float a33);
 
  SbMatrix3(const SbMat3 &m);
 
  void  setValue(const SbMat3 &m);
 
  void    setValue(const float *pMat)
  {
    matrix[0][0] = *pMat++ ;
    matrix[0][1] = *pMat++ ;
    matrix[0][2] = *pMat++ ;
 
    matrix[1][0] = *pMat++ ;
    matrix[1][1] = *pMat++ ;
    matrix[1][2] = *pMat++ ;
 
    matrix[2][0] = *pMat++ ;
    matrix[2][1] = *pMat++ ;
    matrix[2][2] = *pMat++ ;
  } ;
 
  void  makeIdentity();
 
  static SbMatrix3  identity();
 
  void  setRotate(const SbRotation &q);
 
  void  setScale(float s);
 
  void  setScale(const SbVec3f &s);
 
  void  getValue(SbMat3 &m) const;
 
  const SbMat3 &getValue() const { return matrix; }
 
  SbMatrix3 &multRight(const SbMatrix3 &m);
 
  SbMatrix3 &multLeft(const SbMatrix3 &m);
 
  void  multMatrixVec(const SbVec3f &src, SbVec3f &dst) const;
 
  void  multVecMatrix(const SbVec3f &src, SbVec3f &dst) const;
 
  float det() const;
 
  SbMatrix3 inverse() const;
 
  void  print(FILE *fp) const;
 
  operator float *() { return &matrix[0][0]; }
 
  operator SbMat3 &() { return matrix; }
 
  float * operator [](int i)   { return &matrix[i][0]; }
 
  const float * operator [](int i) const  { return &matrix[i][0]; }
 
  SbVec3f operator *(const SbVec3f &v) const;
 
  SbMatrix3 & operator =(const SbMat3 &m);
 
  SbMatrix3 & operator =(const SbMatrix3 &m);
 
  SbMatrix3 & operator =(const SbRotation &q)  { setRotate(q); return *this; }
 
  SbMatrix3 & operator *=(const SbMatrix3 &m)  { return multRight(m); }
 
   friend SbVec3f operator *(const SbVec3f &v, const SbMatrix3 &m);
 
   friend SbMatrix3 operator *(const SbMatrix3 &m1, const SbMatrix3 &m2);
 
   friend int operator ==(const SbMatrix3 &m1, const SbMatrix3 &m2);
   friend int operator !=(const SbMatrix3 &m1, const SbMatrix3 &m2)
  { return !(m1 == m2); }
 
private:
  SbMat3 matrix;    // Storage for 3x3 matrix
};
 
inline std::ostream& operator << (std::ostream& os, const SbMatrix& mat)
{
  for ( int j = 0; j < 4; j++ )
  {
    os << "[ ";
    for ( int i = 0; i < 4; i++ )
      os << mat[j][i] << " ";
    os << "]" << std::endl;
  }
  return os;
}
 
inline std::ostream& operator << (std::ostream& os, const SbMatrix3& mat)
{
  for ( int j = 0; j < 3; j++ )
  {
    os << "[ ";
    for ( int i = 0; i < 3; i++ )
      os << mat[j][i] << " ";
    os << "]" << std::endl;
  }
  return os;
}
 
#endif /* _SB_MATRIX_ */