Subversion Repositories gelsvn

Rev

Rev 306 | Rev 382 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 306 Rev 323
Line 1... Line 1...
1 
#ifndef __CGLA_ARITHQUAT_H__
 1 
#ifndef __CGLA_ARITHQUAT_H__
2 
#define __CGLA_ARITHQUAT_H__
 2 
#define __CGLA_ARITHQUAT_H__
3 
 
 3 
 
4 
#include "CGLA.h"
 4 
#include "CGLA.h"
- 
 
 5 
#include "ArithSqMatFloat.h"
5 
#include <cmath>
 6 
#include <cmath>
6 
 
 7 
 
7 
namespace CGLA {
 8 
namespace CGLA {
8 
 
 9 
 
9 
  /** \brief A T based Quaterinion class.
10 
  /** \brief A T based Quaterinion class.
Line 102... Line 103...
102 
      T tmp = std::sqrt(2*(1 + dot(s, t)));
 103 
      T tmp = std::sqrt(2*(1 + dot(s, t)));
103 
      qv = cross(s, t)*(1.0/tmp);
 104 
      qv = cross(s, t)*(1.0/tmp);
104 
      qw = tmp/2.0;
105 
      qw = tmp/2.0;
105 
    }
 106 
    }
106 
 
 107 
 
- 
 
 108 
	/// Construct a Quaternion from a rotation matrix.
- 
 
 109 
    template <class VT, class MT, unsigned int ROWS>
- 
 
 110 
    void make_rot(ArithSqMatFloat<VT,MT,ROWS>& m)
- 
 
 111 
    {
- 
 
 112 
      assert(ROWS==3 || ROWS==4);
- 
 
 113 
      T trace = m[0][0] + m[1][1] + m[2][2]  + static_cast<T>(1.0);
- 
 
 114 
 
- 
 
 115 
      //If the trace of the matrix is greater than zero, then
- 
 
 116 
      //perform an "instant" calculation.
- 
 
 117 
      if ( trace > TINY )
- 
 
 118 
      {
- 
 
 119 
        T S = sqrt(trace) * static_cast<T>(2.0);
- 
 
 120 
        qv = V(m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1] );
- 
 
 121 
        qv /= S;
- 
 
 122 
        qw = static_cast<T>(0.25) * S;
- 
 
 123 
      }
- 
 
 124 
      else
- 
 
 125 
      {
- 
 
 126 
        //If the trace of the matrix is equal to zero (or negative...) then identify
- 
 
 127 
        //which major diagonal element has the greatest value.
- 
 
 128 
        //Depending on this, calculate the following:
- 
 
 129 
 
- 
 
 130 
        if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] )  {	// Column 0:
- 
 
 131 
          T S  = sqrt( static_cast<T>(1.0) + m[0][0] - m[1][1] - m[2][2] ) * static_cast<T>(2.0);
- 
 
 132 
          qv[0] = 0.25f * S;
- 
 
 133 
          qv[1] = (m[1][0] + m[0][1] ) / S;
- 
 
 134 
          qv[2] = (m[0][2] + m[2][0] ) / S;
- 
 
 135 
          qw = (m[2][1] - m[1][3] ) / S;
- 
 
 136 
        } else if ( m[1][1] > m[2][2] ) {			// Column 1:
- 
 
 137 
          T S  = sqrt( 1.0 + m[1][1] - m[0][0] - m[2][2] ) * 2.0;
- 
 
 138 
          qv[0] = (m[1][0] + m[0][1] ) / S;
- 
 
 139 
          qv[1] = 0.25 * S;
- 
 
 140 
          qv[2] = (m[2][1] + m[1][2] ) / S;
- 
 
 141 
          qw = (m[0][2] - m[2][0] ) / S;
- 
 
 142 
        } else {						// Column 2:
- 
 
 143 
          T S  = sqrt( 1.0 + m[2][2] - m[0][0] - m[1][1] ) * 2.0;
- 
 
 144 
          qv[0] = (m[0][2] + m[2][0] ) / S;
- 
 
 145 
          qv[1] = (m[2][1] + m[1][2] ) / S;
- 
 
 146 
          qv[2] = 0.25 * S;
- 
 
 147 
          qw = (m[1][0] - m[0][1] ) / S;
- 
 
 148 
        }
- 
 
 149 
      }
- 
 
 150 
      //The quaternion is then defined as:
- 
 
 151 
      //  Q = | X Y Z W |
- 
 
 152 
    }
- 
 
 153 
 
107 
    //----------------------------------------------------------------------
154 
    //----------------------------------------------------------------------
108 
    // Binary operators
155 
    // Binary operators
109 
    //----------------------------------------------------------------------
156 
    //----------------------------------------------------------------------
110
157
111 
    bool operator==(const ArithQuat<T,V,Q>& q) const
 158 
    bool operator==(const ArithQuat<T,V,Q>& q) const