1/* 2 * quat - calculate using quaternions of the form: a + bi + cj + dk 3 * 4 * Copyright (C) 1999,2021 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 * Under source code control: 1990/02/15 01:50:35 21 * File existed as early as: before 1990 22 * 23 * Share and enjoy! :-) http://www.isthe.com/chongo/tech/comp/calc/ 24 */ 25 26/* 27 * Routines to handle quaternions of the form: 28 * a + bi + cj + dk 29 * 30 * Note: In this module, quaternians are manipulated in the form: 31 * s + v 32 * Where s is a scalar and v is a vector of size 3. 33 */ 34 35 36obj quat {s, v}; /* definition of the quaternion object */ 37 38 39define quat(a,b,c,d) 40{ 41 local obj quat x; 42 43 x.s = isnull(a) ? 0 : a; 44 mat x.v[3]; 45 x.v[0] = isnull(b) ? 0 : b; 46 x.v[1] = isnull(c) ? 0 : c; 47 x.v[2] = isnull(d) ? 0 : d; 48 return x; 49} 50 51 52define quat_print(a) 53{ 54 print "quat(" : a.s : ", " : a.v[0] : ", " : 55 a.v[1] : ", " : a.v[2] : ")" :; 56} 57 58 59define quat_norm(a) 60{ 61 return a.s^2 + dp(a.v, a.v); 62} 63 64 65define quat_abs(a, e) 66{ 67 return sqrt(a.s^2 + dp(a.v, a.v), e); 68} 69 70 71define quat_conj(a) 72{ 73 local obj quat x; 74 75 x.s = a.s; 76 x.v = -a.v; 77 return x; 78} 79 80 81define quat_add(a, b) 82{ 83 local obj quat x; 84 85 if (!istype(b, x)) { 86 x.s = a.s + b; 87 x.v = a.v; 88 return x; 89 } 90 if (!istype(a, x)) { 91 x.s = a + b.s; 92 x.v = b.v; 93 return x; 94 } 95 x.s = a.s + b.s; 96 x.v = a.v + b.v; 97 if (x.v) 98 return x; 99 return x.s; 100} 101 102 103define quat_sub(a, b) 104{ 105 local obj quat x; 106 107 if (!istype(b, x)) { 108 x.s = a.s - b; 109 x.v = a.v; 110 return x; 111 } 112 if (!istype(a, x)) { 113 x.s = a - b.s; 114 x.v = -b.v; 115 return x; 116 } 117 x.s = a.s - b.s; 118 x.v = a.v - b.v; 119 if (x.v) 120 return x; 121 return x.s; 122} 123 124 125define quat_inc(a) 126{ 127 local x; 128 129 x = a; 130 x.s++; 131 return x; 132} 133 134 135define quat_dec(a) 136{ 137 local x; 138 139 x = a; 140 x.s--; 141 return x; 142} 143 144 145define quat_neg(a) 146{ 147 local obj quat x; 148 149 x.s = -a.s; 150 x.v = -a.v; 151 return x; 152} 153 154 155define quat_mul(a, b) 156{ 157 local obj quat x; 158 159 if (!istype(b, x)) { 160 x.s = a.s * b; 161 x.v = a.v * b; 162 } else if (!istype(a, x)) { 163 x.s = b.s * a; 164 x.v = b.v * a; 165 } else { 166 x.s = a.s * b.s - dp(a.v, b.v); 167 x.v = a.s * b.v + b.s * a.v + cp(a.v, b.v); 168 } 169 if (x.v) 170 return x; 171 return x.s; 172} 173 174 175define quat_div(a, b) 176{ 177 local obj quat x; 178 179 if (!istype(b, x)) { 180 x.s = a.s / b; 181 x.v = a.v / b; 182 return x; 183 } 184 return a * quat_inv(b); 185} 186 187 188define quat_inv(a) 189{ 190 local x, q2; 191 192 obj quat x; 193 q2 = a.s^2 + dp(a.v, a.v); 194 x.s = a.s / q2; 195 x.v = a.v / (-q2); 196 return x; 197} 198 199 200define quat_scale(a, b) 201{ 202 local obj quat x; 203 204 x.s = scale(a.s, b); 205 x.v = scale(a.v, b); 206 return x; 207} 208 209 210define quat_shift(a, b) 211{ 212 local obj quat x; 213 214 x.s = a.s << b; 215 x.v = a.v << b; 216 if (x.v) 217 return x; 218 return x.s; 219} 220 221if (config("resource_debug") & 3) { 222 print "obj quat {s, v} defined"; 223} 224