Subversion Repositories gelsvn

Rev

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

Rev 48 Rev 49
Line 3... Line 3...
3 
 
 3 
 
4 
#include "Vec3f.h"
 4 
#include "Vec3f.h"
5 
#include "Vec4f.h"
 5 
#include "Vec4f.h"
6 
#include "Mat3x3f.h"
 6 
#include "Mat3x3f.h"
7 
#include "Mat4x4f.h"
 7 
#include "Mat4x4f.h"
- 
 
 8 
#include <cmath>
8 
 
 9 
 
9 
namespace CGLA {
 10 
namespace CGLA {
10 
 
 11 
 
11 
  /** A Quaterinion class. Quaternions are algebraic entities
12 
  /** A Quaterinion class. Quaternions are algebraic entities
12 
      useful for rotation. */
 13 
      useful for rotation. */
Line 70... Line 71...
70 
 
 71 
 
71 
    /// Get real part of a quaternion
 72 
    /// Get real part of a quaternion
72 
    float get_real_part() const { return qw; }
 73 
    float get_real_part() const { return qw; }
73 
 
 74 
 
74 
    /// Get a 3x3 rotation matrix from a quaternion
 75 
    /// Get a 3x3 rotation matrix from a quaternion
75 
    Mat3x3f get_mat3x3f() const;
 76 
    Mat3x3f get_mat3x3f() const
- 
 
 77 
    {
- 
 
 78 
      float s = 2/norm();
- 
 
 79 
      // note that the all q_*q_ are used twice (optimize)
- 
 
 80 
      return Mat3x3f(Vec3f(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),
- 
 
 81 
			         s*(qv[0]*qv[1] - qw*qv[2]),
- 
 
 82 
			         s*(qv[0]*qv[2] + qw*qv[1])),
- 
 
 83 
		     Vec3f(      s*(qv[0]*qv[1] + qw*qv[2]),
- 
 
 84 
  			   1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),
- 
 
 85 
			         s*(qv[1]*qv[2] - qw*qv[0])),
- 
 
 86 
		     Vec3f(      s*(qv[0]*qv[2] - qw*qv[1]),
- 
 
 87 
			         s*(qv[1]*qv[2] + qw*qv[0]),
- 
 
 88 
			   1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1])));
- 
 
 89 
    }
76 
 
 90 
 
77 
    /// Get a 4x4 rotation matrix from a quaternion
 91 
    /// Get a 4x4 rotation matrix from a quaternion
78 
    Mat4x4f get_mat4x4f() const;
 92 
    Mat4x4f get_mat4x4f() const
- 
 
 93 
    {
- 
 
 94 
      float s = 2/norm();
- 
 
 95 
      // note that the all q_*q_ are used twice (optimize?)
- 
 
 96 
      return Mat4x4f(Vec4f(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),
- 
 
 97 
			         s*(qv[0]*qv[1] - qw*qv[2]),
- 
 
 98 
			         s*(qv[0]*qv[2] + qw*qv[1]),
- 
 
 99 
		           0.0),
- 
 
 100 
		     Vec4f(      s*(qv[0]*qv[1] + qw*qv[2]),
- 
 
 101 
			   1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),
- 
 
 102 
			         s*(qv[1]*qv[2] - qw*qv[0]),
- 
 
 103 
			   0.0),
- 
 
 104 
		     Vec4f(      s*(qv[0]*qv[2] - qw*qv[1]),
- 
 
 105 
			         s*(qv[1]*qv[2] + qw*qv[0]),
- 
 
 106 
			   1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1]),
- 
 
 107 
			   0.0),
- 
 
 108 
		     Vec4f(0.0, 0.0, 0.0, 1.0));
- 
 
 109 
    }
79 
 
 110 
 
80 
    /// Obtain angle of rotation and axis
 111 
    /// Obtain angle of rotation and axis
81 
    void get_rot(float& angle, Vec3f& vec);
 112 
    void get_rot(float& angle, Vec3f& v)
- 
 
 113 
    {
- 
 
 114 
      angle = 2*std::acos(qw);
- 
 
 115 
 
- 
 
 116 
      if(angle < TINY)
- 
 
 117 
	v = Vec3f(1.0, 0.0, 0.0);
- 
 
 118 
      else
- 
 
 119 
	v = qv*(1/std::sin(angle));
- 
 
 120 
 
- 
 
 121 
      if(angle > M_PI)
- 
 
 122 
	v = -v;
- 
 
 123
- 
 
 124 
      v.normalize();
- 
 
 125 
    }
82 
 
 126 
 
83 
    /// Construct a Quaternion from an angle and axis of rotation.
 127 
    /// Construct a Quaternion from an angle and axis of rotation.
84 
    void make_rot(float angle, const Vec3f&);
 128 
    void make_rot(float angle, const Vec3f& v)
- 
 
 129 
    {
- 
 
 130 
      angle /= 2.0;
- 
 
 131 
      qv = CGLA::normalize(v)*std::sin(angle);
- 
 
 132 
      qw = std::cos(angle);
- 
 
 133 
    }
85 
 
 134 
 
86 
    /** Construct a Quaternion rotating from the direction given
 135 
    /** Construct a Quaternion rotating from the direction given
87 
	by the first argument to the direction given by the second.*/
 136 
	by the first argument to the direction given by the second.*/
88 
    void make_rot(const Vec3f&,const Vec3f&);
 137 
    void make_rot(const Vec3f& s,const Vec3f& t)
- 
 
 138 
    {
- 
 
 139 
      float tmp = std::sqrt(2*(1 + dot(s, t)));
- 
 
 140 
      qv = cross(s, t)*(1.0/tmp);
- 
 
 141 
      qw = tmp/2.0;
- 
 
 142 
    }
89 
 
 143 
 
90 
    //----------------------------------------------------------------------
 144 
    //----------------------------------------------------------------------
91 
    // Binary operators
 145 
    // Binary operators
92 
    //----------------------------------------------------------------------
 146 
    //----------------------------------------------------------------------
93
147
Line 165... Line 219...
165 
      The last argument is the parameter used to interpolate
 219 
      The last argument is the parameter used to interpolate
166 
      between the two first. SLERP - invented by Shoemake -
 220 
      between the two first. SLERP - invented by Shoemake -
167 
      is a good way to interpolate because the interpolation
 221 
      is a good way to interpolate because the interpolation
168 
      is performed on the unit sphere.
222 
      is performed on the unit sphere.
169 
  */
 223 
  */
170 
  inline Quatf slerp(Quatf q0, Quatf q1, float t);
 224 
  inline Quatf slerp(Quatf q0, Quatf q1, float t)
- 
 
 225 
  {
- 
 
 226 
    float angle = std::acos(q0.qv[0]*q1.qv[0] + q0.qv[1]*q1.qv[1]
- 
 
 227 
			    + q0.qv[2]*q1.qv[2] + q0.qw*q1.qw);
- 
 
 228 
    return (q0*std::sin((1 - t)*angle) + q1*std::sin(t*angle))*(1/std::sin(angle));
- 
 
 229 
  }
171 
 
 230 
 
172 
  /// Create an identity quaternion
 231 
  /// Create an identity quaternion
173 
  inline Quatf identity_Quatf()
 232 
  inline Quatf identity_Quatf()
174 
  {
 233 
  {
175 
    return Quatf(Vec3f(0.0));
 234 
    return Quatf(Vec3f(0.0));