home *** CD-ROM | disk | FTP | other *** search
/ BCI NET 2 / BCI NET 2.iso / archives / programming / source / graphicgems4.lha / GemsIV / arcball / BallAux.c < prev    next >
Encoding:
C/C++ Source or Header  |  1995-02-06  |  3.4 KB  |  137 lines
  1. /***** BallAux.c *****/
  2. #include <math.h>
  3. #include "BallAux.h"
  4.  
  5. Quat qOne = {0, 0, 0, 1};
  6.  
  7. /* Return quaternion product qL * qR.  Note: order is important!
  8.  * To combine rotations, use the product Mul(qSecond, qFirst),
  9.  * which gives the effect of rotating by qFirst then qSecond. */
  10. Quat Qt_Mul(Quat qL, Quat qR)
  11. {
  12.     Quat qq;
  13.     qq.w = qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
  14.     qq.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y;
  15.     qq.y = qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z;
  16.     qq.z = qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x;
  17.     return (qq);
  18. }
  19.  
  20.  
  21. /* Construct rotation matrix from (possibly non-unit) quaternion.
  22.  * Assumes matrix is used to multiply column vector on the left:
  23.  * vnew = mat vold.  Works correctly for right-handed coordinate system
  24.  * and right-handed rotations. */
  25. HMatrix *Qt_ToMatrix(Quat q, HMatrix out)
  26. {
  27.     double Nq = q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w;
  28.     double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
  29.     double xs = q.x*s,          ys = q.y*s,      zs = q.z*s;
  30.     double wx = q.w*xs,          wy = q.w*ys,      wz = q.w*zs;
  31.     double xx = q.x*xs,          xy = q.x*ys,      xz = q.x*zs;
  32.     double yy = q.y*ys,          yz = q.y*zs,      zz = q.z*zs;
  33.     out[X][X] = 1.0 - (yy + zz); out[Y][X] = xy + wz; out[Z][X] = xz - wy;
  34.     out[X][Y] = xy - wz; out[Y][Y] = 1.0 - (xx + zz); out[Z][Y] = yz + wx;
  35.     out[X][Z] = xz + wy; out[Y][Z] = yz - wx; out[Z][Z] = 1.0 - (xx + yy);
  36.     out[X][W] = out[Y][W] = out[Z][W] = out[W][X] = out[W][Y] = out[W][Z] = 0.0;
  37.     out[W][W] = 1.0;
  38.     return ((HMatrix *)&out);
  39. }
  40.  
  41. /* Return conjugate of quaternion. */
  42. Quat Qt_Conj(Quat q)
  43. {
  44.     Quat qq;
  45.     qq.x = -q.x; qq.y = -q.y; qq.z = -q.z; qq.w = q.w;
  46.     return (qq);
  47. }
  48.  
  49. /* Return vector formed from components */
  50. HVect V3_(float x, float y, float z)
  51. {
  52.     HVect v;
  53.     v.x = x; v.y = y; v.z = z; v.w = 0;
  54.     return (v);
  55. }
  56.  
  57. /* Return norm of v, defined as sum of squares of components */
  58. float V3_Norm(HVect v)
  59. {
  60.     return ( v.x*v.x + v.y*v.y + v.z*v.z );
  61. }
  62.  
  63. /* Return unit magnitude vector in direction of v */
  64. HVect V3_Unit(HVect v)
  65. {
  66.     static HVect u = {0, 0, 0, 0};
  67.     float vlen = sqrt(V3_Norm(v));
  68.     if (vlen != 0.0) {
  69.     u.x = v.x/vlen; u.y = v.y/vlen; u.z = v.z/vlen;
  70.     }
  71.     return (u);
  72. }
  73.  
  74. /* Return version of v scaled by s */
  75. HVect V3_Scale(HVect v, float s)
  76. {
  77.     HVect u;
  78.     u.x = s*v.x; u.y = s*v.y; u.z = s*v.z; u.w = v.w;
  79.     return (u);
  80. }
  81.  
  82. /* Return negative of v */
  83. HVect V3_Negate(HVect v)
  84. {
  85.     static HVect u = {0, 0, 0, 0};
  86.     u.x = -v.x; u.y = -v.y; u.z = -v.z;
  87.     return (u);
  88. }
  89.  
  90. /* Return sum of v1 and v2 */
  91. HVect V3_Add(HVect v1, HVect v2)
  92. {
  93.     static HVect v = {0, 0, 0, 0};
  94.     v.x = v1.x+v2.x; v.y = v1.y+v2.y; v.z = v1.z+v2.z;
  95.     return (v);
  96. }
  97.  
  98. /* Return difference of v1 minus v2 */
  99. HVect V3_Sub(HVect v1, HVect v2)
  100. {
  101.     static HVect v = {0, 0, 0, 0};
  102.     v.x = v1.x-v2.x; v.y = v1.y-v2.y; v.z = v1.z-v2.z;
  103.     return (v);
  104. }
  105.  
  106. /* Halve arc between unit vectors v0 and v1. */
  107. HVect V3_Bisect(HVect v0, HVect v1)
  108. {
  109.     HVect v = {0, 0, 0, 0};
  110.     float Nv;
  111.     v = V3_Add(v0, v1);
  112.     Nv = V3_Norm(v);
  113.     if (Nv < 1.0e-5) {
  114.     v = V3_(0, 0, 1);
  115.     } else {
  116.     v = V3_Scale(v, 1/sqrt(Nv));
  117.     }
  118.     return (v);
  119. }
  120.  
  121. /* Return dot product of v1 and v2 */
  122. float V3_Dot(HVect v1, HVect v2)
  123. {
  124.     return (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z);
  125. }
  126.  
  127.  
  128. /* Return cross product, v1 x v2 */
  129. HVect V3_Cross(HVect v1, HVect v2)
  130. {
  131.     static HVect v = {0, 0, 0, 0};
  132.     v.x = v1.y*v2.z-v1.z*v2.y;
  133.     v.y = v1.z*v2.x-v1.x*v2.z;
  134.     v.z = v1.x*v2.y-v1.y*v2.x;
  135.     return (v);
  136. }
  137.