c_cpp/etc/calc/cal/quat.cal

   1 /*
   2  * quat - alculate using quaternions of the form: a + bi + cj + dk
   3  *
   4  * Copyright (C) 1999  David I. Bell
   5  *
   6  * Calc is open software; you can redistribute it and/or modify it under
   7  * the terms of the version 2.1 of the GNU Lesser General Public License
   8  * as published by the Free Software Foundation.
   9  *
  10  * Calc is distributed in the hope that it will be useful, but WITHOUT
  11  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  12  * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General
  13  * Public License for more details.
  14  *
  15  * A copy of version 2.1 of the GNU Lesser General Public License is
  16  * distributed with calc under the filename COPYING-LGPL.  You should have
  17  * received a copy with calc; if not, write to Free Software Foundation, Inc.
  18  * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  19  *
  20  * @(#) $Revision: 30.2 $
  21  * @(#) $Id: quat.cal,v 30.2 2013/08/11 08:41:38 chongo Exp $
  22  * @(#) $Source: /usr/local/src/bin/calc/cal/RCS/quat.cal,v $
  23  *
  24  * Under source code control:   1990/02/15 01:50:35
  25  * File existed as early as:    before 1990
  26  *
  27  * Share and enjoy!  :-)        http://www.isthe.com/chongo/tech/comp/calc/
  28  */
  29
  30 /*
  31  * Routines to handle quaternions of the form:
  32  *      a + bi + cj + dk
  33  *
  34  * Note: In this module, quaternians are manipulated in the form:
  35  *      s + v
  36  * Where s is a scalar and v is a vector of size 3.
  37  */
  38
  39
  40 obj quat {s, v};                /* definition of the quaternion object */
  41
  42
  43 define quat(a,b,c,d)
  44 {
  45         local obj quat  x;
  46
  47         x.s = isnull(a) ? 0 : a;
  48         mat x.v[3];
  49         x.v[0] = isnull(b) ? 0 : b;
  50         x.v[1] = isnull(c) ? 0 : c;
  51         x.v[2] = isnull(d) ? 0 : d;
  52         return x;
  53 }
  54
  55
  56 define quat_print(a)
  57 {
  58         print "quat(" : a.s : ", " : a.v[0] : ", " :
  59               a.v[1] : ", " : a.v[2] : ")" :;
  60 }
  61
  62
  63 define quat_norm(a)
  64 {
  65         return a.s^2 + dp(a.v, a.v);
  66 }
  67
  68
  69 define quat_abs(a, e)
  70 {
  71         return sqrt(a.s^2 + dp(a.v, a.v), e);
  72 }
  73
  74
  75 define quat_conj(a)
  76 {
  77         local obj quat  x;
  78
  79         x.s = a.s;
  80         x.v = -a.v;
  81         return x;
  82 }
  83
  84
  85 define quat_add(a, b)
  86 {
  87         local obj quat  x;
  88
  89         if (!istype(b, x)) {
  90                 x.s = a.s + b;
  91                 x.v = a.v;
  92                 return x;
  93         }
  94         if (!istype(a, x)) {
  95                 x.s = a + b.s;
  96                 x.v = b.v;
  97                 return x;
  98         }
  99         x.s = a.s + b.s;
 100         x.v = a.v + b.v;
 101         if (x.v)
 102                 return x;
 103         return x.s;
 104 }
 105
 106
 107 define quat_sub(a, b)
 108 {
 109         local obj quat  x;
 110
 111         if (!istype(b, x)) {
 112                 x.s = a.s - b;
 113                 x.v = a.v;
 114                 return x;
 115         }
 116         if (!istype(a, x)) {
 117                 x.s = a - b.s;
 118                 x.v = -b.v;
 119                 return x;
 120         }
 121         x.s = a.s - b.s;
 122         x.v = a.v - b.v;
 123         if (x.v)
 124                 return x;
 125         return x.s;
 126 }
 127
 128
 129 define quat_inc(a)
 130 {
 131         local   x;
 132
 133         x = a;
 134         x.s++;
 135         return x;
 136 }
 137
 138
 139 define quat_dec(a)
 140 {
 141         local   x;
 142
 143         x = a;
 144         x.s--;
 145         return x;
 146 }
 147
 148
 149 define quat_neg(a)
 150 {
 151         local obj quat  x;
 152
 153         x.s = -a.s;
 154         x.v = -a.v;
 155         return x;
 156 }
 157
 158
 159 define quat_mul(a, b)
 160 {
 161         local obj quat  x;
 162
 163         if (!istype(b, x)) {
 164                 x.s = a.s * b;
 165                 x.v = a.v * b;
 166         } else if (!istype(a, x)) {
 167                 x.s = b.s * a;
 168                 x.v = b.v * a;
 169         } else {
 170                 x.s = a.s * b.s - dp(a.v, b.v);
 171                 x.v = a.s * b.v + b.s * a.v + cp(a.v, b.v);
 172         }
 173         if (x.v)
 174                 return x;
 175         return x.s;
 176 }
 177
 178
 179 define quat_div(a, b)
 180 {
 181         local obj quat  x;
 182
 183         if (!istype(b, x)) {
 184                 x.s = a.s / b;
 185                 x.v = a.v / b;
 186                 return x;
 187         }
 188         return a * quat_inv(b);
 189 }
 190
 191
 192 define quat_inv(a)
 193 {
 194         local   x, q2;
 195
 196         obj quat x;
 197         q2 = a.s^2 + dp(a.v, a.v);
 198         x.s = a.s / q2;
 199         x.v = a.v / (-q2);
 200         return x;
 201 }
 202
 203
 204 define quat_scale(a, b)
 205 {
 206         local obj quat  x;
 207
 208         x.s = scale(a.s, b);
 209         x.v = scale(a.v, b);
 210         return x;
 211 }
 212
 213
 214 define quat_shift(a, b)
 215 {
 216         local obj quat  x;
 217
 218         x.s = a.s << b;
 219         x.v = a.v << b;
 220         if (x.v)
 221                 return x;
 222         return x.s;
 223 }
 224
 225 if (config("resource_debug") & 3) {
 226     print "obj quat {s, v} defined";
 227 }