/Users/barrand/private/dev/softinex/old/inexlib-1.2/inlib/inlib/a3

Go to the documentation of this file.

// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file inlib.license for terms.

#ifndef inlib_a3
#define inlib_a3

#include "sqm"

#include <cmath> //sqrt

namespace inlib {
namespace a3 {

// vec is an a3

template <class T>
class vec : public array<T> {
public:
  vec():array<T>(1,3){}
  vec(const T a_vec[3]):array<T>(1,3) {
    array<T>::m_vector[0] = a_vec[0];
    array<T>::m_vector[1] = a_vec[1];
    array<T>::m_vector[2] = a_vec[2];
  }
  vec(const T& a0,const T& a1,const T& a2):array<T>(1,3) {
    array<T>::m_vector[0] = a0;
    array<T>::m_vector[1] = a1;
    array<T>::m_vector[2] = a2;
  }
  virtual ~vec() {}
public:
  vec(const vec& a_from):array<T>(a_from){}
  vec& operator=(const vec& a_from) {
    array<T>::operator=(a_from);
    return *this;
  }
public:
  T v0() const { return array<T>::m_vector[0];}
  T v1() const { return array<T>::m_vector[1];}
  T v2() const { return array<T>::m_vector[2];}

  void v0(const T& a_value) { array<T>::m_vector[0] = a_value;}
  void v1(const T& a_value) { array<T>::m_vector[1] = a_value;}
  void v2(const T& a_value) { array<T>::m_vector[2] = a_value;}

  void set_value(const T& a1,const T& a2,const T& a3) {
    array<T>::m_vector[0] = a1;
    array<T>::m_vector[1] = a2;
    array<T>::m_vector[2] = a3;
  }
  void set_value(const T aV[3]) {
    array<T>::m_vector[0] = aV[0];
    array<T>::m_vector[1] = aV[1];
    array<T>::m_vector[2] = aV[2];
  }
  void value(T& a1,T& a2,T& a3) const {
    a1 = array<T>::m_vector[0];
    a2 = array<T>::m_vector[1];
    a3 = array<T>::m_vector[2];
  }    

  bool set_value(unsigned int a_index,const T& a_value) { 
    if(a_index>=3) return false;
    array<T>::m_vector[a_index] = a_value;
    return true;
  }

  T length() const {
    T m0 = array<T>::m_vector[0];
    T m1 = array<T>::m_vector[1];
    T m2 = array<T>::m_vector[2];
    return (T)::sqrt(m0*m0+m1*m1+m2*m2);
  }
  T normalize() {
    T norme = length();
    if(norme==T()) return 0;
    divide(norme);
    return norme;
  }

  T dot(const vec& aV) const {
    return (v0() * aV.v0() + 
            v1() * aV.v1() + 
            v2() * aV.v2());
  }   

public: //operators
  //vec operator-(const vec& a_v) const {
  //  return vec(v0()-a_v.v0(),
  //             v1()-a_v.v1(),
  //             v1()-a_v.v2());
  //}    

};

// sqm is an a33

template <class T>
class sqm : public inlib::sqm<T> {
public:
  sqm():inlib::sqm<T>(3){}
  virtual ~sqm() {}
public:
  sqm(const sqm& a_from):inlib::sqm<T>(a_from){}
  sqm& operator=(const sqm& a_from) {
    inlib::sqm<T>::operator=(a_from);
    return *this;
  }
public:
  void v00(const T& a_value) { array<T>::m_vector[0 + 0 * 3] = a_value;}
  void v10(const T& a_value) { array<T>::m_vector[1 + 0 * 3] = a_value;}
  void v20(const T& a_value) { array<T>::m_vector[2 + 0 * 3] = a_value;}

  void v01(const T& a_value) { array<T>::m_vector[0 + 1 * 3] = a_value;}
  void v11(const T& a_value) { array<T>::m_vector[1 + 1 * 3] = a_value;}
  void v21(const T& a_value) { array<T>::m_vector[2 + 1 * 3] = a_value;}

  void v02(const T& a_value) { array<T>::m_vector[0 + 2 * 3] = a_value;}
  void v12(const T& a_value) { array<T>::m_vector[1 + 2 * 3] = a_value;}
  void v22(const T& a_value) { array<T>::m_vector[2 + 2 * 3] = a_value;}

  T v00() const { return array<T>::m_vector[0 + 0 * 3];}
  T v10() const { return array<T>::m_vector[1 + 0 * 3];}
  T v20() const { return array<T>::m_vector[2 + 0 * 3];}

  T v01() const { return array<T>::m_vector[0 + 1 * 3];}
  T v11() const { return array<T>::m_vector[1 + 1 * 3];}
  T v21() const { return array<T>::m_vector[2 + 1 * 3];}

  T v02() const { return array<T>::m_vector[0 + 2 * 3];}
  T v12() const { return array<T>::m_vector[1 + 2 * 3];}
  T v22() const { return array<T>::m_vector[2 + 2 * 3];}

  virtual T determinant() const {
    T e00 =  D22(v11(),v12(),v21(),v22());
    T e01 = -D22(v01(),v02(),v21(),v22());
    T e02 =  D22(v01(),v02(),v11(),v12());
    return (  v00() * e00
            + v10() * e01
            + v20() * e02);

  }

  bool invert(sqm<T>& a_result) const {
    a_result.v00(
      D22(v11(),v12(),
          v21(),v22()));
    a_result.v01(
      -D22(v01(),v02(),
           v21(),v22()));
    a_result.v02(
      D22(v01(),v02(),
          v11(),v12()));
                  
    T deter =
        v00() * a_result.v00()
      + v10() * a_result.v01()
      + v20() * a_result.v02();

    if(deter==array<T>::zero()) return false;
                  
    a_result.v10(
     -D22(v10(),v12(),
          v20(),v22()));
    a_result.v11(
      D22(v00(),v02(),
          v20(),v22()));
    a_result.v12(
     -D22(v00(),v02(),
          v10(),v12()));
                    
    a_result.v20(
      D22(v10(),v11(),
          v20(),v21()));
    a_result.v21(
     -D22(v00(),v01(),
          v20(),v21()));
    a_result.v22(
      D22(v00(),v01(),
          v10(),v11()));
                    
    a_result.multiply(1./deter);  
  
    return true;
  }
private:
  T D22(const T& a_11,const T& a_12,const T& a_21,const T& a_22) const {
    return (a_11 * a_22 - a_21 * a_12);
  }
};

template <class T>
class cbm : public array<T> { //cbm for cubic matrix.
public:
  cbm():array<T>(3,3){}
  virtual ~cbm() {}
public:
  cbm(const cbm& a_from):array<T>(a_from){}
  cbm& operator=(const cbm& a_from){
    inlib::array<T>::operator=(a_from);
    return *this;
  }
public:
  T value(unsigned int a1,unsigned int a2,unsigned int a3) const { 
    return array<T>::m_vector[a1 + a2 * 3 + a3 * 9];
  }

  void value(unsigned int a1,unsigned int a2,unsigned int a3,const T& a_value) { 
    array<T>::m_vector[a1 + a2 * 3 + a3 * 9] = a_value;
  }

  void v000(const T& a_value) {array<T>::m_vector[0+0*3+0*9] = a_value;}
  void v100(const T& a_value) {array<T>::m_vector[1+0*3+0*9] = a_value;}
  void v200(const T& a_value) {array<T>::m_vector[2+0*3+0*9] = a_value;}
  void v010(const T& a_value) {array<T>::m_vector[0+1*3+0*9] = a_value;}
  void v110(const T& a_value) {array<T>::m_vector[1+1*3+0*9] = a_value;}
  void v210(const T& a_value) {array<T>::m_vector[2+1*3+0*9] = a_value;}
  void v020(const T& a_value) {array<T>::m_vector[0+2*3+0*9] = a_value;}
  void v120(const T& a_value) {array<T>::m_vector[1+2*3+0*9] = a_value;}
  void v220(const T& a_value) {array<T>::m_vector[2+2*3+0*9] = a_value;}

  void v001(const T& a_value) {array<T>::m_vector[0+0*3+1*9] = a_value;}
  void v101(const T& a_value) {array<T>::m_vector[1+0*3+1*9] = a_value;}
  void v201(const T& a_value) {array<T>::m_vector[2+0*3+1*9] = a_value;}
  void v011(const T& a_value) {array<T>::m_vector[0+1*3+1*9] = a_value;}
  void v111(const T& a_value) {array<T>::m_vector[1+1*3+1*9] = a_value;}
  void v211(const T& a_value) {array<T>::m_vector[2+1*3+1*9] = a_value;}
  void v021(const T& a_value) {array<T>::m_vector[0+2*3+1*9] = a_value;}
  void v121(const T& a_value) {array<T>::m_vector[1+2*3+1*9] = a_value;}
  void v221(const T& a_value) {array<T>::m_vector[2+2*3+1*9] = a_value;}

  void v002(const T& a_value) {array<T>::m_vector[0+0*3+2*9] = a_value;}
  void v102(const T& a_value) {array<T>::m_vector[1+0*3+2*9] = a_value;}
  void v202(const T& a_value) {array<T>::m_vector[2+0*3+2*9] = a_value;}
  void v012(const T& a_value) {array<T>::m_vector[0+1*3+2*9] = a_value;}
  void v112(const T& a_value) {array<T>::m_vector[1+1*3+2*9] = a_value;}
  void v212(const T& a_value) {array<T>::m_vector[2+1*3+2*9] = a_value;}
  void v022(const T& a_value) {array<T>::m_vector[0+2*3+2*9] = a_value;}
  void v122(const T& a_value) {array<T>::m_vector[1+2*3+2*9] = a_value;}
  void v222(const T& a_value) {array<T>::m_vector[2+2*3+2*9] = a_value;}

};

template <class T>
inline sqm<T> operator-(const sqm<T>& a1,const sqm<T>& a2) {
  sqm<T> res(a1);
  res.subtract(a2);
  return res;
}
template <class T>
inline sqm<T> operator+(const sqm<T>& a1,const sqm<T>& a2) {
  sqm<T> res(a1);
  res.add(a2);
  return res;
}
template <class T>
inline sqm<T> operator*(const sqm<T>& a1,const sqm<T>& a2) {
  sqm<T> res;
  a1.mx_mul(a2,res);
  return res;
}

template <class T>
inline sqm<T> commutator(const sqm<T>& a1,const sqm<T>& a2) {
  return a1*a2-a2*a1;
}
template <class T>
inline sqm<T> anticommutator(const sqm<T>& a1,const sqm<T>& a2) {
  return a1*a2+a2*a1;
}


//template <class T>
//class point : public vec<T> {
//};

// Parametric description:
//  l(t) = pos + t * dir

template <class T>
class line {
public:
  line(){}
  line(const vec<T>& a_p0,const vec<T>& a_p1) {
    // Construct a line from two points lying on the line.  If you
    // want to construct a line from a position and a direction, use
    // line(p, p + d).
    // line is directed from p0 to p1.
    m_pos = a_p0;
    //m_dir = a_p1-a_p0;
    m_dir = a_p0;
    m_dir.multiply(-1);
    m_dir.add(a_p1);
    m_dir.normalize();
  }
  virtual ~line() {}
public:
  line(const line& a_from)
  :m_pos(a_from.m_pos)
  ,m_dir(a_from.m_dir)
  {}
  line& operator=(const line& a_from) {
    m_pos = a_from.m_pos;
    m_dir = a_from.m_dir;
    return *this;
  }
public:
  void set_value(const vec<T>& a_p0,const vec<T>& a_p1) {
    // Set that value!
    m_pos = a_p0;
    //m_dir = a_p1-a_p0;
    m_dir = a_p0;
    m_dir.multiply(-1);
    m_dir.add(a_p1);
    m_dir.normalize();
  }

  const vec<T>& position() const {return m_pos;}
  const vec<T>& direction() const {return m_dir;}
private:
  vec<T> m_pos;
  vec<T> m_dir; //normalized.
};

template <class T>
class plane {
public:
  plane(){}

  plane(const vec<T>& a_p0,const vec<T>& a_p1,const vec<T>& a_p2) {
    // Construct a plane given 3 points.
    // Orientation is computed by taking (p1 - p0) x (p2 - p0) and
    // pointing the normal in that direction.
  
    vec<T> P = a_p1;
    P.subtract(a_p0);
    vec<T> P2 = a_p2;
    P2.subtract(a_p0);
    m_normal = P.cross(P2);
    m_normal.normalize();
    m_distance = 
      m_normal.v0() * a_p0.v0() + 
      m_normal.value1() * a_p0.value1() + 
      m_normal.value2() * a_p0.value2();
  }

  plane(const vec<T>& a_normal,const T& a_distance){
    // Construct a plane given normal and distance from origin along normal.
    // Orientation is given by the normal vector n.
    m_normal = a_normal;
    m_normal.normalize();
    m_distance = a_distance;
  }

  plane(const vec<T>& a_normal,const vec<T>& a_point){
    // Construct a plane given normal and a point to pass through
    // Orientation is given by the normal vector n.
    m_normal = a_normal;
    m_normal.normalize();
    m_distance = 
      m_normal.v0() * a_point.v0() + 
      m_normal.v1() * a_point.v1() + 
      m_normal.v2() * a_point.v2();
  }

  virtual ~plane() {}
public:
  plane(const plane& a_from)
  :m_normal(a_from.m_normal)
  ,m_distance(a_from.m_distance)
  {}
  plane& operator=(const plane& a_from) {
    m_normal = a_from.m_normal;
    m_distance = a_from.m_distance;
    return *this;
  }

public:
  void offset(const T& a_distance){
    // Offset a plane by a given distance.
    m_distance += a_distance;
  }
  
  bool intersect(const line<T>& a_line,vec<T>& a_intersection) const {
    // Intersect line and plane, returning true if there is an intersection
    // false if line is parallel to plane
    const vec<T>& pos = a_line.position();
    const vec<T>& dir = a_line.direction();
    T d = m_normal.dot(dir);
    if(d==array<T>::zero()) return false;
    T t = (m_distance - m_normal.dot(pos))/d;
    a_intersection = dir;
    a_intersection.multiply(t);
    a_intersection.add(pos);
    //a_intersection = pos + t * dir;
    return true;
  }
  
  bool is_in_half_space(const vec<T>& a_point) const {
    // Returns true if the given point is within the half-space
    // defined by the plane
    //vec pos = m_normal * m_distance;
    vec<T> pos = m_normal;
    pos.multiply(-m_distance);
    pos.add(a_point);
    return (m_normal.dot(pos) >= array<T>::zero() ? true : false);
  }
  
  const vec<T>& normal() const {return m_normal;}
  T distance_from_origin() const {return m_distance;}
  
  T distance(const vec<T>& a_point) const {
    // Return the distance from point to plane. Positive distance means
    // the point is in the plane's half space.
    return a_point.dot(m_normal) - m_distance;
  }
private:
  vec<T> m_normal; //normalized.
  T m_distance;
};

// not tested yet.
template <class T>
class clip {
public:
  clip():m_cur(0){}
  virtual ~clip() {}
private:
  clip(const clip& a_from):m_cur(0){}
  clip& operator=(const clip& a_from){return *this;}
public:
  void reset() {
    m_data[0].clear();
    m_data[1].clear();
    m_cur = 0;
  }
  void add(const vec<T>& a_point) {
    m_data[m_cur].push_back(a_point);
  }

  void execute(const plane<T>& plane) {
    //Clip polygon against plane. This might change the number of
    //vertices in the polygon.

    unsigned int n = m_data[m_cur].size();
    if (n == 0) return;

    // create a loop :
    vec<T> dummy = m_data[m_cur][0];
    m_data[m_cur].push_back(dummy);

    const vec<T>& planeN = plane.normal();

    for(unsigned int i = 0; i < n; i++) {
      vec<T> v0 = m_data[m_cur][i];
      vec<T> v1 = m_data[m_cur][i+1];
  
      T d0 = plane.distance(v0);
      T d1 = plane.distance(v1);
  
      if (d0 >= 0.0f && d1 < 0.0f) { // exit plane
        vec<T> dir = v1-v0;
        // we know that v0 != v1 since we got here
        dir.normalize();
        T dot = dir.dot(planeN);
        vec<T> newvertex = v0 - dir * (d0/dot);
        out_point(newvertex);
      } else if (d0 < 0.0f && d1 >= 0.0f) { // enter plane
        vec<T> dir = v1-v0;
        // we know that v0 != v1 since we got here
        dir.normalize();
        T dot = dir.dot(planeN);
        vec<T> newvertex = v0 - dir * (d0/dot);
        out_point(newvertex);
        out_point(v1);
      } else if (d0 >= 0.0f && d1 >= 0.0f) { // in plane
        out_point(v1);
      }
    }
    m_data[m_cur].clear();
    m_cur ^= 1;
  }

  const std::vector< vec<T> >& result() const {
    return m_data[m_cur];
  }

private:
  void out_point(const vec<T>& a_p) {
    m_data[m_cur ^ 1].push_back(a_p);
  }
  
private:
  std::vector< vec<T> > m_data[2];
  unsigned int m_cur;
};

}}

#endif