zoukankan      html  css  js  c++  java
  • Rotate a vector by using quaternion

    Assuming you want to rotate vectors around the origin of your coordinate system. (If you want to rotate around some other point, subtract its coordinates from the point you are rotating, do the rotation, and then add back what you subtracted.) In 3-D, you need not only an angle, but also an axis. (In higher dimensions it gets much worse, very quickly.) Actually, you need 3 independent numbers, and these come in a variety of flavors. The flavor I recommend is unit quaternions: 4 numbers that square and add up to +1. You can write these as [(x,y,z),w], with 4 real numbers, or [v,w], with v, a 3-D vector pointing along the axis. The concept of an axis is unique to 3-D. It is a line through the origin containing all the points which do not move during the rotation. So we know if we are turning forwards or back, we use a vector pointing out along the line. Suppose you want to use unit vector u as your axis, and rotate by 2t degrees. (Yes, that's twice t.) Make a quaternion [u sin t, cos t]. You can use the quaternion -- call it q -- directly on a vector v with quaternion multiplication, q v q^-1, or just convert the quaternion to a 3x3 matrix M. If the components of q are {(x,y,z),w], then you want the matrix

    M = {{1-2(yy+zz), 2(xy-wz), 2(xz+wy)},
    { 2(xy+wz),1-2(xx+zz), 2(yz-wx)},
    { 2(xz-wy), 2(yz+wx),1-2(xx+yy)}}.

    Rotations, translations, and much more are explained in all basic computer graphics texts. Quaternions are covered briefly in [Foley], and more extensively in several Graphics Gems, and the SIGGRAPH 85 proceedings.

    /* The following routine converts an angle and a unit axis vector
    * to a matrix, returning the corresponding unit quaternion at no
    * extra cost. It is written in such a way as to allow both fixed
    * point and floating point versions to be created by appropriate
    * definitions of FPOINT, ANGLE, VECTOR, QUAT, MATRIX, MUL, HALF,
    * TWICE, COS, SIN, ONE, and ZERO.
    * The following is an example of floating point definitions.
    #define FPOINT double
    #define ANGLE FPOINT
    #define VECTOR QUAT
    typedef struct {FPOINT x,y,z,w;} QUAT;
    enum Indices {X,Y,Z,W};
    typedef FPOINT MATRIX[4][4];
    #define MUL(a,b) ((a)*(b))
    #define HALF(a) ((a)*0.5)
    #define TWICE(a) ((a)*2.0)
    #define COS cos
    #define SIN sin
    #define ONE 1.0
    #define ZERO 0.0
    */
    QUAT MatrixFromAxisAngle(VECTOR axis, ANGLE theta, MATRIX m)
    {
    QUAT q;
    ANGLE halfTheta = HALF(theta);
    FPOINT cosHalfTheta = COS(halfTheta);
    FPOINT sinHalfTheta = SIN(halfTheta);
    FPOINT xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
    q.x = MUL(axis.x,sinHalfTheta);
    q.y = MUL(axis.y,sinHalfTheta);
    q.z = MUL(axis.z,sinHalfTheta);
    q.w = cosHalfTheta;
    xs = TWICE(q.x);  ys = TWICE(q.y);  zs = TWICE(q.z);
    wx = MUL(q.w,xs); wy = MUL(q.w,ys); wz = MUL(q.w,zs);
    xx = MUL(q.x,xs); xy = MUL(q.x,ys); xz = MUL(q.x,zs);
    yy = MUL(q.y,ys); yz = MUL(q.y,zs); zz = MUL(q.z,zs);
    m[X][X] = ONE - (yy + zz); m[X][Y] = xy - wz; m[X][Z] = xz + wy;
    m[Y][X] = xy + wz; m[Y][Y] = ONE - (xx + zz); m[Y][Z] = yz - wx;
    m[Z][X] = xz - wy; m[Z][Y] = yz + wx; m[Z][Z] = ONE - (xx + yy);
    /* Fill in remainder of 4x4 homogeneous transform matrix. */
    m[W][X] = m[W][Y] = m[W][Z] = m[X][W] = m[Y][W] = m[Z][W] = ZERO;
    m[W][W] = ONE;
    return (q);
    }
    /* The routine just given, MatrixFromAxisAngle, performs rotation about
    * an axis passing through the origin, so only a unit vector was needed
    * in addition to the angle. To rotate about an axis not containing the
    * origin, a point on the axis is also needed, as in the following. For
    * mathematical purity, the type POINT is used, but may be defined as:
    #define POINT VECTOR
    */
    QUAT MatrixFromAnyAxisAngle(POINT o, VECTOR axis, ANGLE theta, MATRIX m)
    {
    QUAT q;
    q = MatrixFromAxisAngle(axis,theta,m);
    m[X][W] = o.x-(MUL(m[X][X],o.x)+MUL(m[X][Y],o.y)+MUL(m[X][Z],o.z));
    m[Y][W] = o.y-(MUL(m[Y][X],o.x)+MUL(m[Y][Y],o.y)+MUL(m[Y][Z],o.z));
    m[Z][W] = o.x-(MUL(m[Z][X],o.x)+MUL(m[Z][Y],o.y)+MUL(m[Z][Z],o.z));
    return (q);
    }
    /* An axis can also be specified by a pair of points, with the direction
    * along the line assumed from the ordering of the points. Although both
    * the previous routines assumed the axis vector was unit length without
    * checking, this routine must cope with a more delicate possibility. If
    * the two points are identical, or even nearly so, the axis is unknown.
    * For now the auxiliary routine which makes a unit vector ignores that.
    * It needs definitions like the following for floating point.
    #define SQRT sqrt
    #define RECIPROCAL(a) (1.0/(a))
    */
    VECTOR Normalize(VECTOR v)
    {
    VECTOR u;
    FPOINT norm = MUL(v.x,v.x)+MUL(v.y,v.y)+MUL(v.z,v.z);
    /* Better to test for (near-)zero norm before taking reciprocal. */
    FPOINT scl = RECIPROCAL(SQRT(norm));
    u.x = MUL(v.x,scl); u.y = MUL(v.y,scl); u.z = MUL(v.z,scl);
    return (u);
    }
    QUAT MatrixFromPointsAngle(POINT o, POINT p, ANGLE theta, MATRIX m)
    {
    QUAT q;
    VECTOR diff, axis;
    diff.x = p.x-o.x; diff.y = p.y-o.y; diff.z = p.z-o.z;
    axis = Normalize(diff);
    return (MatrixFromAnyAxisAngle(o,axis,theta,m));
    }
    原文见:http://www.exaflop.org/docs/cgafaq/cga5.html
  • 相关阅读:
    chrome扩展及应用开发 李喆pdf完整版
    Chrome插件(扩展)开发资料
    Fiddler下载地址
    如果没有 Android 世界会是什么样子?
    一张图告诉你:Android系统哪代强?
    Android开发的16条小经验总结
    Android上实现MVP模式的途径
    Android事件总线还能怎么玩?
    Android性能优化典范(二)
    安卓listView实现下拉刷新上拉加载滑动仿QQ的删除功能
  • 原文地址:https://www.cnblogs.com/hyamw/p/476569.html
Copyright © 2011-2022 走看看