Chinaunix首页 | 论坛 | 博客
  • 博客访问: 5064113
  • 博文数量: 1200
  • 博客积分: 12961
  • 博客等级: 上将
  • 技术积分: 14456
  • 用 户 组: 普通用户
  • 注册时间: 2009-01-09 11:25
  • 认证徽章:
个人简介

偷得浮生半桶水(半日闲), 好记性不如抄下来(烂笔头). 信息爆炸的时代, 学习是一项持续的工作.

文章分类

全部博文(1200)

文章存档

2019年(177)

2018年(81)

2017年(80)

2016年(70)

2015年(52)

2014年(41)

2013年(51)

2012年(85)

2011年(45)

2010年(231)

2009年(287)

分类: 其他平台

2019-03-21 20:27:12

参考 https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles 

点击(此处)折叠或打开

  1. Quaterniond toQuaternion( double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
  2. {
  3.     // Abbreviations for the various angular functions
  4.     double cy = cos(yaw * 0.5);
  5.     double sy = sin(yaw * 0.5);
  6.     double cp = cos(pitch * 0.5);
  7.     double sp = sin(pitch * 0.5);
  8.     double cr = cos(roll * 0.5);
  9.     double sr = sin(roll * 0.5);

  10.     Quaterniond q;
  11.     q.w() = cy * cp * cr + sy * sp * sr;
  12.     q.x() = cy * cp * sr - sy * sp * cr;
  13.     q.y() = sy * cp * sr + cy * sp * cr;
  14.     q.z() = sy * cp * cr - cy * sp * sr;
  15.     return q;
  16. }


点击(此处)折叠或打开

  1. static void toEulerAngle(const Quaterniond& q, double& roll, double& pitch, double& yaw)
  2. {
  3.     // roll (x-axis rotation)
  4.     double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z());
  5.     double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
  6.     roll = atan2(sinr_cosp, cosr_cosp);

  7.     // pitch (y-axis rotation)
  8.     double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
  9.     if (fabs(sinp) >= 1)
  10.         pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
  11.     else
  12.         pitch = asin(sinp);

  13.     // yaw (z-axis rotation)
  14.     double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
  15.     double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
  16.     yaw = atan2(siny_cosp, cosy_cosp);
  17. }

阅读(6994) | 评论(0) | 转发(0) |
给主人留下些什么吧!~~
评论热议
请登录后评论。

登录 注册