Subversion Repositories gelsvn

Rev

Rev 171 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

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