Subversion Repositories gelsvn

Rev

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

Rev 306 Rev 501
Line 30... Line 30...
30 
 
 30 
 
31
31
32 
		/// Get a 3x3 rotation matrix from a quaternion
 32 
		/// Get a 3x3 rotation matrix from a quaternion
33 
    Mat3x3d get_Mat3x3d() const
 33 
    Mat3x3d get_Mat3x3d() const
34 
    {
 34 
    {
35 
      double s = 2/norm();
 35 
      double s = 2.0/norm();
36 
      // note that the all q_*q_ are used twice (optimize)
 36 
      // note that the all q_*q_ are used twice (optimize)
37 
      return Mat3x3d(Vec3d(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),
 37 
      return Mat3x3d(Vec3d(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),
38 
			         s*(qv[0]*qv[1] - qw*qv[2]),
 38 
			         s*(qv[0]*qv[1] - qw*qv[2]),
39 
			         s*(qv[0]*qv[2] + qw*qv[1])),
 39 
			         s*(qv[0]*qv[2] + qw*qv[1])),
40 
		     Vec3d(      s*(qv[0]*qv[1] + qw*qv[2]),
 40 
		     Vec3d(      s*(qv[0]*qv[1] + qw*qv[2]),