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