Subversion Repositories gelsvn

Rev

Rev 188 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 188 Rev 207
1 
#include "Vec3f.h"
 1 
#include "Vec3f.h"
2 
#include "Quaternion.h"
 2 
#include "Quaternion.h"
3 
 
 3 
 
4 
namespace CGLA
4 
namespace CGLA
5 
{
 5 
{
6 
 
 6 
 
7 
	void Quaternion::make_rot(const Mat4x4f& m)
 7 
	void Quaternion::make_rot(const Mat4x4f& m)
8 
	{
 8 
	{
9 
		float trace = m[0][0] + m[1][1] + m[2][2]  + 1.0f;
 9 
		float trace = m[0][0] + m[1][1] + m[2][2]  + 1.0f;
10 
 
 10 
 
11 
		//If the trace of the matrix is greater than zero, then
 11 
		//If the trace of the matrix is greater than zero, then
12 
		//perform an "instant" calculation.
 12 
		//perform an "instant" calculation.
13 
		if ( trace > TINY )
13 
		if ( trace > TINY )
14 
		{
 14 
		{
15 
			float S = sqrt(trace) * 2.0f;
 15 
			float S = sqrt(trace) * 2.0f;
16 
			qv = Vec3f(m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1] );
 16 
			qv = Vec3f(m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1] );
17 
			qv /= S;
 17 
			qv /= S;
18 
			qw = 0.25f * S;
 18 
			qw = 0.25f * S;
19 
		}
 19 
		}
20 
		else
 20 
		else
21 
		{
 21 
		{
22 
			//If the trace of the matrix is equal to zero (or negative...) then identify
 22 
			//If the trace of the matrix is equal to zero (or negative...) then identify
23 
			//which major diagonal element has the greatest value.
 23 
			//which major diagonal element has the greatest value.
24 
			//Depending on this, calculate the following:
 24 
			//Depending on this, calculate the following:
25 
 
 25 
 
26 
			if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] )  {	// Column 0:
26 
			if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] )  {	// Column 0:
27 
				float S  = sqrt( 1.0 + m[0][0] - m[1][1] - m[2][2] ) * 2.0f;
 27 
				float S  = sqrt( 1.0 + m[0][0] - m[1][1] - m[2][2] ) * 2.0f;
28 
				qv[0] = 0.25f * S;
 28 
				qv[0] = 0.25f * S;
29 
				qv[1] = (m[1][0] + m[0][1] ) / S;
 29 
				qv[1] = (m[1][0] + m[0][1] ) / S;
30 
				qv[2] = (m[0][2] + m[2][0] ) / S;
 30 
				qv[2] = (m[0][2] + m[2][0] ) / S;
31 
				qw = (m[2][1] - m[1][3] ) / S;
 31 
				qw = (m[2][1] - m[1][3] ) / S;
32 
			} else if ( m[1][1] > m[2][2] ) {			// Column 1:
32 
			} else if ( m[1][1] > m[2][2] ) {			// Column 1:
33 
				float S  = sqrt( 1.0 + m[1][1] - m[0][0] - m[2][2] ) * 2.0f;
 33 
				float S  = sqrt( 1.0 + m[1][1] - m[0][0] - m[2][2] ) * 2.0f;
34 
				qv[0] = (m[1][0] + m[0][1] ) / S;
 34 
				qv[0] = (m[1][0] + m[0][1] ) / S;
35 
				qv[1] = 0.25f * S;
 35 
				qv[1] = 0.25f * S;
36 
				qv[2] = (m[2][1] + m[1][2] ) / S;
 36 
				qv[2] = (m[2][1] + m[1][2] ) / S;
37 
				qw = (m[0][2] - m[2][0] ) / S;
 37 
				qw = (m[0][2] - m[2][0] ) / S;
38 
			} else {						// Column 2:
 38 
			} else {						// Column 2:
39 
				float S  = sqrt( 1.0 + m[2][2] - m[0][0] - m[1][1] ) * 2.0f;
 39 
				float S  = sqrt( 1.0 + m[2][2] - m[0][0] - m[1][1] ) * 2.0f;
40 
				qv[0] = (m[0][2] + m[2][0] ) / S;
 40 
				qv[0] = (m[0][2] + m[2][0] ) / S;
41 
				qv[1] = (m[2][1] + m[1][2] ) / S;
 41 
				qv[1] = (m[2][1] + m[1][2] ) / S;
42 
				qv[2] = 0.25f * S;
 42 
				qv[2] = 0.25f * S;
43 
				qw = (m[1][0] - m[0][1] ) / S;
 43 
				qw = (m[1][0] - m[0][1] ) / S;
44 
			}
 44 
			}
45 
		}
 45 
		}
46 
		//The quaternion is then defined as:
 46 
		//The quaternion is then defined as:
47 
		//  Q = | X Y Z W |
 47 
		//  Q = | X Y Z W |
48 
	}
 48 
	}
49 
 
 49 
 
50 
	void Quaternion::make_rot(float angle, const Vec3f& v)
 50 
	void Quaternion::make_rot(float angle, const Vec3f& v)
51 
  {
 51 
  {
52 
    angle = angle/2;
 52 
    angle = angle/2;
53 
    qv = CGLA::normalize(v);
 53 
    qv = CGLA::normalize(v);
54 
    qv *= sin(angle);
 54 
    qv *= sin(angle);
55 
    qw  = cos(angle);
 55 
    qw  = cos(angle);
56 
  };
 56 
  };
57 
 
 57 
 
58 
  void Quaternion::make_rot(const Vec3f& _v0, const Vec3f& _v1)
 58 
  void Quaternion::make_rot(const Vec3f& _v0, const Vec3f& _v1)
59 
  {
 59 
  {
60 
    Vec3f v0 = CGLA::normalize(_v0);
 60 
    Vec3f v0 = CGLA::normalize(_v0);
61 
    Vec3f v1 = CGLA::normalize(_v1);
 61 
    Vec3f v1 = CGLA::normalize(_v1);
62 
    qv = cross(v0, v1);
 62 
    qv = cross(v0, v1);
63 
    float l = qv.length();
 63 
    float l = qv.length();
64 
    if(l<TINY)
 64 
    if(l<TINY)
65 
      qv = Vec3f(1,0,0);
 65 
      qv = Vec3f(1,0,0);
66 
    else
 66 
    else
67 
      qv.normalize();
 67 
      qv.normalize();
68 
    float a = acos(dot(v0,v1))/2;
 68 
    float a = acos(dot(v0,v1))/2;
69 
    qw  = cos(a);
 69 
    qw  = cos(a);
70 
    qv *= sin(a);
70 
    qv *= sin(a);
71 
  };
 71 
  };
72 
 
 72 
 
73 
  void Quaternion::get_rot(float& angle, Vec3f& v)
73 
  void Quaternion::get_rot(float& angle, Vec3f& v)
74 
  {
 74 
  {
75 
    angle=2*acos(qw);
 75 
    angle=2*acos(qw);
76 
 
 76 
 
77 
    if (angle<TINY)
77 
    if (angle<TINY)
78 
      v = Vec3f(1,0,0);
 78 
      v = Vec3f(1,0,0);
79 
    else
79 
    else
80 
      v = qv / sin(angle);
 80 
      v = qv / sin(angle);
81 
 
 81 
 
82 
    if (angle>M_PI)
 82 
    if (angle>M_PI)
83 
      v = -v;
 83 
      v = -v;
84 
 
 84 
 
85 
    v.normalize();
 85 
    v.normalize();
86 
  }
 86 
  }
87 
 
 87 
 
88 
  Mat3x3f Quaternion::get_mat3x3f() const
 88 
  Mat3x3f Quaternion::get_mat3x3f() const
89 
  {
 89 
  {
90 
    float s=2/norm();
 90 
    float s=2/norm();
91 
    float m[9] = {1 - s*(qv[1]*qv[1]+qv[2]*qv[2]),
91 
    float m[9] = {1 - s*(qv[1]*qv[1]+qv[2]*qv[2]),
92 
		  s*(qv[0]*qv[1]-qw*qv[2]),
92 
		  s*(qv[0]*qv[1]-qw*qv[2]),
93 
		  s*(qv[0]*qv[2]+qw*qv[1]),
93 
		  s*(qv[0]*qv[2]+qw*qv[1]),
94 
		  s*(qv[0]*qv[1]+qw*qv[2]),
94 
		  s*(qv[0]*qv[1]+qw*qv[2]),
95 
		  1 - s*(qv[0]*qv[0]+qv[2]*qv[2]),
95 
		  1 - s*(qv[0]*qv[0]+qv[2]*qv[2]),
96 
		  s*(qv[1]*qv[2]-qw*qv[0]),
96 
		  s*(qv[1]*qv[2]-qw*qv[0]),
97 
		  s*(qv[0]*qv[2]-qw*qv[1]),
97 
		  s*(qv[0]*qv[2]-qw*qv[1]),
98 
		  s*(qv[1]*qv[2]+qw*qv[0]),
98 
		  s*(qv[1]*qv[2]+qw*qv[0]),
99 
		  1 - s*(qv[0]*qv[0]+qv[1]*qv[1])};
 99 
		  1 - s*(qv[0]*qv[0]+qv[1]*qv[1])};
100 
    Mat3x3f mat;
 100 
    Mat3x3f mat;
101 
    raw_assign(mat, m);
 101 
    raw_assign(mat, m);
102 
    return mat;
 102 
    return mat;
103 
  }
 103 
  }
104 
 
 104 
 
105 
 
 105 
 
106 
  //This function just need to call the right initialiser
 106 
  //This function just need to call the right initialiser
107 
 
 107 
 
108 
  Mat4x4f Quaternion::get_mat4x4f() const
 108 
  Mat4x4f Quaternion::get_mat4x4f() const
109 
  {
 109 
  {
110 
    float s=2/norm();
 110 
    float s=2/norm();
111 
    float m[16] = {1 - s*(qv[1]*qv[1]+qv[2]*qv[2]),
111 
    float m[16] = {1 - s*(qv[1]*qv[1]+qv[2]*qv[2]),
112 
		   s*(qv[0]*qv[1]-qw*qv[2]),
112 
		   s*(qv[0]*qv[1]-qw*qv[2]),
113 
		   s*(qv[0]*qv[2]+qw*qv[1]),
113 
		   s*(qv[0]*qv[2]+qw*qv[1]),
114 
		   float(0),
 114 
		   float(0),
115 
		   s*(qv[0]*qv[1]+qw*qv[2]),
115 
		   s*(qv[0]*qv[1]+qw*qv[2]),
116 
		   1 - s*(qv[0]*qv[0]+qv[2]*qv[2]),
116 
		   1 - s*(qv[0]*qv[0]+qv[2]*qv[2]),
117 
		   s*(qv[1]*qv[2]-qw*qv[0]),
117 
		   s*(qv[1]*qv[2]-qw*qv[0]),
118 
		   float(0),
 118 
		   float(0),
119 
		   s*(qv[0]*qv[2]-qw*qv[1]),
119 
		   s*(qv[0]*qv[2]-qw*qv[1]),
120 
		   s*(qv[1]*qv[2]+qw*qv[0]),
120 
		   s*(qv[1]*qv[2]+qw*qv[0]),
121 
		   1 - s*(qv[0]*qv[0]+qv[1]*qv[1]),
121 
		   1 - s*(qv[0]*qv[0]+qv[1]*qv[1]),
122 
		   float(0),
 122 
		   float(0),
123 
		   float(0),
123 
		   float(0),
124 
		   float(0),
124 
		   float(0),
125 
		   float(0),
125 
		   float(0),
126 
		   float(1)};
 126 
		   float(1)};
127 
    Mat4x4f mat;
 127 
    Mat4x4f mat;
128 
    raw_assign(mat, m);
 128 
    raw_assign(mat, m);
129 
    return mat;
 129 
    return mat;
130 
  }
 130 
  }
131 
 
 131 
 
132 
 
 132 
 
133 
  Vec3f Quaternion::apply(const Vec3f& vec) const
 133 
  Vec3f Quaternion::apply(const Vec3f& vec) const
134 
  {
 134 
  {
135 
    return Vec3f((*this)*Quaternion(vec)*inverse());
 135 
    return Vec3f((*this)*Quaternion(vec)*inverse());
136 
  }
 136 
  }
137 
 
 137 
 
138 
 
 138 
 
139 
}
 139 
}
140 
 
 140