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
  • 相关阅读:
    错因集锦
    组合数学12
    硬币购物
    考试套路整理
    考前模板整理
    我的友链
    P4127 [AHOI2009]同类分布
    P1836 数页码_NOI导刊2011提高(04)
    P4124 [CQOI2016]手机号码
    数位DP小结
  • 原文地址:https://www.cnblogs.com/hyamw/p/476569.html
Copyright © 2011-2022 走看看