Subversion Repositories gelsvn

Rev

Rev 48 | Rev 51 | 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 "Vec3d.h"
 4 
#include "Vec3d.h"
5 
#include "Vec4d.h"
 5 
#include "Vec4d.h"
6 
#include "Mat3x3d.h"
 6 
#include "Mat3x3d.h"
7 
#include "Mat4x4d.h"
 7 
#include "Mat4x4d.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 
    double get_real_part() const { return qw; }
 73 
    double 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 
    Mat3x3d get_mat3x3d() const;
 76 
    Mat3x3d get_mat3x3d() const
- 
 
 77 
    {
- 
 
 78 
      double s = 2/norm();
- 
 
 79 
      // note that the all q_*q_ are used twice (optimize)
- 
 
 80 
      return Mat3x3d(Vec3d(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 
		     Vec3d(      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 
		     Vec3d(      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 
    Mat4x4d get_mat4x4d() const;
 92 
    Mat4x4d get_mat4x4d() const
- 
 
 93 
    {
- 
 
 94 
      double s = 2/norm();
- 
 
 95 
      // note that the all q_*q_ are used twice (optimize?)
- 
 
 96 
      return Mat4x4d(Vec4d(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 
		     Vec4d(      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 
		     Vec4d(      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 
		     Vec4d(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(double& angle, Vec3d& vec);
 112 
    void get_rot(double& angle, Vec3d& vec)
- 
 
 113 
    {
- 
 
 114 
      angle = 2*std::acos(qw);
- 
 
 115 
 
- 
 
 116 
      if(angle < TINY)
- 
 
 117 
	v = Vec3d(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(double angle, const Vec3d&);
 128 
    void make_rot(double angle, const Vec3d&)
- 
 
 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 Vec3d&,const Vec3d&);
 137 
    void make_rot(const Vec3d&,const Vec3d&)
- 
 
 138 
    {
- 
 
 139 
      double 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 Quatd slerp(Quatd q0, Quatd q1, double t);
 224 
  inline Quatd slerp(Quatd q0, Quatd q1, double t)
- 
 
 225 
  {
- 
 
 226 
    double 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 Quatd identity_Quatd()
 232 
  inline Quatd identity_Quatd()
174 
  {
 233 
  {
175 
    return Quatd(Vec3d(0.0));
 234 
    return Quatd(Vec3d(0.0));