zoukankan      html  css  js  c++  java
  • 轴角和四元数和空间点

    1.公式
    轴角转四元数。
    q=[cos(theta/2),nx*sin(theta/2),ny*sin(theta/2),nz*sin(theta/2)]
    四元数转轴角
    theta=2arccosq0,
    [nx,ny,nz].T=[q1,q2,q3].t/sin(theta/2)
    经轴角旋转后的空间点
    这时旋转矩阵是

    也就是

    2实际计算
    这里的轴角变成了angle_axis=[nx,ny,nz].t*theta
    2.1轴角变成四元数
    那么[q1,q2,q3]=[a0,a1,a2]*sin(theta/2)/theta
    这里theta=sqrt(a0*a0+a1*a1+a2*a2)
    定义sin(theta/2)/theta为k
    当角不为0的时候,如常计算k.
    当角趋近于0的时候,k趋近于0.5.
    2.2四元数变轴角
    [a0,a1,a2]=[q1,q2,q3]*theta/sin(theta/2)
    定义theta/sin(theta/2)为k
    这里sin(theta/2)为sqrt(q1*q1+q2*q2+q3*q3)
    阈值用sin(theta/2)的平方。
    如果不为0,theta为2*arctan(-sin(theta/2),-q0)或者2*arctan(sin(theta/2),q0).
    如果为0,k直接趋近于2.
    2.3求旋转后的点,用轴角算的。

    这里theta=sqrt(a0*a0+a1*a1+a2*a2),用theta的平方做了一个阈值判断。
    如果不为0.计算w=[a0,a1,a2]/theta,也就是n.
    计算w cross pt结果为w_cross_pt.(1-cos(theta))*w*pt为tmp.tmp是一个数值来着。
    result[0]=cos(theta)*pt[0]+sin(theta)*w_cross_pt[0]+w[0]*tmp
    如果为0.
    cos(theta)=1,sin(theta)~theta
    所以R=I+hat(w)*theta=I+hat(angle_axis)
    令w_cross_pt为angle_axis和pt的叉乘结果
    所以result[0]=pt[0]+w_cross_pt[0]

  • 相关阅读:
    go 代理
    mongo创建用户
    博客搬家&留言板
    noip2020 SD选手迷惑行为大赏
    noip2020游记
    P4174 [NOI2006] 最大获利
    P3327 [SDOI2015]约数个数和
    P5069 [Ynoi2015]纵使日薄西山
    P3747 相逢是问候
    HDE6315 Naive Operations
  • 原文地址:https://www.cnblogs.com/talugirl/p/7388499.html
Copyright © 2011-2022 走看看