Subversion Repositories gelsvn

Rev

Rev 2 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2 Rev 12
Line 1... Line 1...
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 
	void Quaternion::make_rot(float angle, const Vec3f& v)
 6 
  void Quaternion::make_rot(float angle, const Vec3f& v)
6 
	{
 7 
  {
7 
		angle = angle/2;
 8 
    angle = angle/2;
8 
		qv = CGLA::normalize(v);
 9 
    qv = CGLA::normalize(v);
9 
		qv *= sin(angle);
 10 
    qv *= sin(angle);
10 
		qw  = cos(angle);
 11 
    qw  = cos(angle);
11 
	};
 12 
  };
12 
 
 13 
 
13 
	void Quaternion::make_rot(const Vec3f& _v0, const Vec3f& _v1)
 14 
  void Quaternion::make_rot(const Vec3f& _v0, const Vec3f& _v1)
14 
	{
 15 
  {
15 
		Vec3f v0 = CGLA::normalize(_v0);
 16 
    Vec3f v0 = CGLA::normalize(_v0);
16 
		Vec3f v1 = CGLA::normalize(_v1);
 17 
    Vec3f v1 = CGLA::normalize(_v1);
17 
		qv = cross(v0, v1);
 18 
    qv = cross(v0, v1);
18 
		float l = qv.length();
 19 
    float l = qv.length();
19 
		if(l<TINY)
 20 
    if(l<TINY)
20 
			qv = Vec3f(1,0,0);
 21 
      qv = Vec3f(1,0,0);
21 
		else
 22 
    else
22 
			qv.normalize();
 23 
      qv.normalize();
23 
		float a = acos(dot(v0,v1))/2;
 24 
    float a = acos(dot(v0,v1))/2;
24 
		qw  = cos(a);
 25 
    qw  = cos(a);
25 
		qv *= sin(a);
26 
    qv *= sin(a);
26 
	};
 27 
  };
27 
 
 28 
 
28 
	void Quaternion::get_rot(float& angle, Vec3f& v)
29 
  void Quaternion::get_rot(float& angle, Vec3f& v)
29 
	{
 30 
  {
30 
		angle=2*acos(qw);
 31 
    angle=2*acos(qw);
31 
 
 32 
 
32 
		if (angle<TINY)
33 
    if (angle<TINY)
33 
			v = Vec3f(1,0,0);
 34 
      v = Vec3f(1,0,0);
34 
		else
35 
    else
35 
			v = qv / sin(angle);
 36 
      v = qv / sin(angle);
36 
 
 37 
 
37 
		if (angle>M_PI)
 38 
    if (angle>M_PI)
38 
			v = -v;
 39 
      v = -v;
39 
 
 40 
 
40 
		v.normalize();
 41 
    v.normalize();
41 
	}
 42 
  }
42 
 
 43 
 
43 
	Mat3x3f Quaternion::get_mat3x3f() const
 44 
  Mat3x3f Quaternion::get_mat3x3f() const
44 
	{
 45 
  {
45 
		float s=2/norm();
 46 
    float s=2/norm();
46 
		float m[9] = {1 - s*(qv[1]*qv[1]+qv[2]*qv[2]),
47 
    float m[9] = {1 - s*(qv[1]*qv[1]+qv[2]*qv[2]),
47 
									s*(qv[0]*qv[1]-qw*qv[2]),
48 
		  s*(qv[0]*qv[1]-qw*qv[2]),
48 
									s*(qv[0]*qv[2]+qw*qv[1]),
49 
		  s*(qv[0]*qv[2]+qw*qv[1]),
49 
									s*(qv[0]*qv[1]+qw*qv[2]),
50 
		  s*(qv[0]*qv[1]+qw*qv[2]),
50 
									1 - s*(qv[0]*qv[0]+qv[2]*qv[2]),
51 
		  1 - s*(qv[0]*qv[0]+qv[2]*qv[2]),
51 
         s*(qv[1]*qv[2]-qw*qv[0]),
52 
		  s*(qv[1]*qv[2]-qw*qv[0]),
52 
									s*(qv[0]*qv[2]-qw*qv[1]),
53 
		  s*(qv[0]*qv[2]-qw*qv[1]),
53 
									s*(qv[1]*qv[2]+qw*qv[0]),
54 
		  s*(qv[1]*qv[2]+qw*qv[0]),
54 
									1 - s*(qv[0]*qv[0]+qv[1]*qv[1])};
 55 
		  1 - s*(qv[0]*qv[0]+qv[1]*qv[1])};
55 
		Mat3x3f mat;
 56 
    Mat3x3f mat;
56 
		mat.set(m);
 57 
    raw_assign(mat, m);
57 
		return mat;
 58 
    return mat;
58 
	}
 59 
  }
59 
 
 60 
 
60 
 
 61 
 
61 
	//This function just need to call the right initialiser
 62 
  //This function just need to call the right initialiser
62 
 
 63 
 
63 
	Mat4x4f Quaternion::get_mat4x4f() const
 64 
  Mat4x4f Quaternion::get_mat4x4f() const
64 
	{
 65 
  {
65 
		float s=2/norm();
 66 
    float s=2/norm();
66 
		float m[16] = {1 - s*(qv[1]*qv[1]+qv[2]*qv[2]),
67 
    float m[16] = {1 - s*(qv[1]*qv[1]+qv[2]*qv[2]),
67 
									 s*(qv[0]*qv[1]-qw*qv[2]),
68 
		   s*(qv[0]*qv[1]-qw*qv[2]),
68 
									 s*(qv[0]*qv[2]+qw*qv[1]),
69 
		   s*(qv[0]*qv[2]+qw*qv[1]),
69 
									 float(0),
 70 
		   float(0),
70 
									 s*(qv[0]*qv[1]+qw*qv[2]),
71 
		   s*(qv[0]*qv[1]+qw*qv[2]),
71 
									 1 - s*(qv[0]*qv[0]+qv[2]*qv[2]),
72 
		   1 - s*(qv[0]*qv[0]+qv[2]*qv[2]),
72 
									 s*(qv[1]*qv[2]-qw*qv[0]),
73 
		   s*(qv[1]*qv[2]-qw*qv[0]),
73 
									 float(0),
 74 
		   float(0),
74 
									 s*(qv[0]*qv[2]-qw*qv[1]),
75 
		   s*(qv[0]*qv[2]-qw*qv[1]),
75 
									 s*(qv[1]*qv[2]+qw*qv[0]),
76 
		   s*(qv[1]*qv[2]+qw*qv[0]),
76 
									 1 - s*(qv[0]*qv[0]+qv[1]*qv[1]),
77 
		   1 - s*(qv[0]*qv[0]+qv[1]*qv[1]),
77 
									 float(0),
 78 
		   float(0),
78 
									 float(0),
79 
		   float(0),
79 
									 float(0),
80 
		   float(0),
80 
									 float(0),
81 
		   float(0),
81 
									 float(1)};
 82 
		   float(1)};
82 
		Mat4x4f mat;
 83 
    Mat4x4f mat;
83 
		mat.set(m);
 84 
    raw_assign(mat, m);
84 
		return mat;
 85 
    return mat;
85 
	}
 86 
  }
86 
 
 87 
 
87 
 
 88 
 
88 
	Vec3f Quaternion::apply(const Vec3f& vec) const
 89 
  Vec3f Quaternion::apply(const Vec3f& vec) const
89 
	{
 90 
  {
90 
		return Vec3f((*this)*Quaternion(vec)*inverse());
 91 
    return Vec3f((*this)*Quaternion(vec)*inverse());
91 
	}
 92 
  }
92 
 
 93 
 
93 
 
 94 
 
94 
}
 95 
}