home *** CD-ROM | disk | FTP | other *** search
/ MacFormat 1994 November / macformat-018.iso / Utility Spectacular / Utilities / Calc / lib / quat.cal < prev    next >
Encoding:
Text File  |  1992-02-24  |  3.5 KB  |  224 lines  |  [TEXT/????]
  1. /*
  2.  * Copyright (c) 1992 David I. Bell
  3.  * Permission is granted to use, distribute, or modify this source,
  4.  * provided that this copyright notice remains intact.
  5.  *
  6.  * Routines to handle quaternions of the form:
  7.  *    a + bi + cj + dk
  8.  *
  9.  * Note: In this module, quaternians are manipulated in the form:
  10.  *    s + v
  11.  * Where s is a scalar and v is a vector of size 3.
  12.  */
  13.  
  14. obj quat {s, v};        /* definition of the quaternion object */
  15.  
  16.  
  17. define quat(a,b,c,d)
  18. {
  19.     local x;
  20.  
  21.     obj quat x;
  22.     x.s = isnull(a) ? 0 : a;
  23.     mat x.v[3];
  24.     x.v[0] = isnull(b) ? 0 : b;
  25.     x.v[1] = isnull(c) ? 0 : c;
  26.     x.v[2] = isnull(d) ? 0 : d;
  27.     return x;
  28. }
  29.  
  30.  
  31. define quat_print(a)
  32. {
  33.     print "quat(" : a.s : ", " : a.v[0] : ", " : a.v[1] : ", " : a.v[2] : ")" :;
  34. }
  35.  
  36.  
  37. define quat_norm(a)
  38. {
  39.     return a.s^2 + dp(a.v, a.v);
  40. }
  41.  
  42.  
  43. define quat_abs(a, e)
  44. {
  45.     return sqrt(a.s^2 + dp(a.v, a.v), e);
  46. }
  47.  
  48.  
  49. define quat_conj(a)
  50. {
  51.     local    x;
  52.  
  53.     obj quat x;
  54.     x.s = a.s;
  55.     x.v = -a.v;
  56.     return x;
  57. }
  58.  
  59.  
  60. define quat_add(a, b)
  61. {
  62.     local x;
  63.  
  64.     obj quat x;
  65.     if (!istype(b, x)) {
  66.         x.s = a.s + b;
  67.         x.v = a.v;
  68.         return x;
  69.     }
  70.     if (!istype(a, x)) {
  71.         x.s = a + b.s;
  72.         x.v = b.v;
  73.         return x;
  74.     }
  75.     x.s = a.s + b.s;
  76.     x.v = a.v + b.v;
  77.     if (x.v)
  78.         return x;
  79.     return x.s;
  80. }
  81.  
  82.  
  83. define quat_sub(a, b)
  84. {
  85.     local x;
  86.  
  87.     obj quat x;
  88.     if (!istype(b, x)) {
  89.         x.s = a.s - b;
  90.         x.v = a.v;
  91.         return x;
  92.     }
  93.     if (!istype(a, x)) {
  94.         x.s = a - b.s;
  95.         x.v = -b.v;
  96.         return x;
  97.     }
  98.     x.s = a.s - b.s;
  99.     x.v = a.v - b.v;
  100.     if (x.v)
  101.         return x;
  102.     return x.s;
  103. }
  104.  
  105.  
  106. define quat_inc(a)
  107. {
  108.     local    x;
  109.  
  110.     x = a;
  111.     x.s++;
  112.     return x;
  113. }
  114.  
  115.  
  116. define quat_dec(a)
  117. {
  118.     local    x;
  119.  
  120.     x = a;
  121.     x.s--;
  122.     return x;
  123. }
  124.  
  125.  
  126. define quat_neg(a)
  127. {
  128.     local    x;
  129.  
  130.     obj quat x;
  131.     x.s = -a.s;
  132.     x.v = -a.v;
  133.     return x;
  134. }
  135.  
  136.  
  137. define quat_mul(a, b)
  138. {
  139.     local x;
  140.  
  141.     obj quat x;
  142.     if (!istype(b, x)) {
  143.         x.s = a.s * b;
  144.         x.v = a.v * b;
  145.     } else if (!istype(a, x)) {
  146.         x.s = b.s * a;
  147.         x.v = b.v * a;
  148.     } else {
  149.         x.s = a.s * b.s - dp(a.v, b.v);
  150.         x.v = a.s * b.v + b.s * a.v + cp(a.v, b.v);
  151.     }
  152.     if (x.v)
  153.         return x;
  154.     return x.s;
  155. }
  156.  
  157.  
  158. define quat_div(a, b)
  159. {
  160.     local    x;
  161.  
  162.     obj quat x;
  163.     if (!istype(b, x)) {
  164.         x.s = a.s / b;
  165.         x.v = a.v / b;
  166.         return x;
  167.     }
  168.     return a * quat_inv(b);
  169. }
  170.  
  171.  
  172. define quat_inv(a)
  173. {
  174.     local    x, q2;
  175.  
  176.     obj quat x;
  177.     q2 = a.s^2 + dp(a.v, a.v);
  178.     x.s = a.s / q2;
  179.     x.v = a.v / (-q2);
  180.     return x;
  181. }
  182.  
  183.  
  184. define quat_scale(a, b)
  185. {
  186.     local    x;
  187.  
  188.     obj quat x;
  189.     x.s = scale(a.s, b);
  190.     x.v = scale(a.v, b);
  191.     return x;
  192. }
  193.  
  194.  
  195. define quat_shift(a, b)
  196. {
  197.     local    x;
  198.  
  199.     obj quat x;
  200.     x.s = a.s << b;
  201.     x.v = a.v << b;
  202.     if (x.v)
  203.         return x;
  204.     return x.s;
  205. }
  206.  
  207. global lib_debug;
  208. if (!isnum(lib_debug) || lib_debug>0) print "obj quat {s, v} defined"
  209. if (!isnum(lib_debug) || lib_debug>0) print "quat(a, b, c, d) defined"
  210. if (!isnum(lib_debug) || lib_debug>0) print "quat_print(a) defined"
  211. if (!isnum(lib_debug) || lib_debug>0) print "quat_norm(a) defined"
  212. if (!isnum(lib_debug) || lib_debug>0) print "quat_abs(a, e) defined"
  213. if (!isnum(lib_debug) || lib_debug>0) print "quat_conj(a) defined"
  214. if (!isnum(lib_debug) || lib_debug>0) print "quat_add(a, e) defined"
  215. if (!isnum(lib_debug) || lib_debug>0) print "quat_sub(a, e) defined"
  216. if (!isnum(lib_debug) || lib_debug>0) print "quat_inc(a) defined"
  217. if (!isnum(lib_debug) || lib_debug>0) print "quat_dec(a) defined"
  218. if (!isnum(lib_debug) || lib_debug>0) print "quat_neg(a) defined"
  219. if (!isnum(lib_debug) || lib_debug>0) print "quat_mul(a, b) defined"
  220. if (!isnum(lib_debug) || lib_debug>0) print "quat_div(a, b) defined"
  221. if (!isnum(lib_debug) || lib_debug>0) print "quat_inv(a) defined"
  222. if (!isnum(lib_debug) || lib_debug>0) print "quat_scale(a, b) defined"
  223. if (!isnum(lib_debug) || lib_debug>0) print "quat_shift(a, b) defined"
  224.