/* * Copyright (c) 1993 David I. Bell * Permission is granted to use, distribute, or modify this source, * provided that this copyright notice remains intact. * * Routines to handle quaternions of the form: * a + bi + cj + dk * * Note: In this module, quaternians are manipulated in the form: * s + v * Where s is a scalar and v is a vector of size 3. */ obj quat {s, v}; /* definition of the quaternion object */ define quat(a,b,c,d) { local x; obj quat x; x.s = isnull(a) ? 0 : a; mat x.v[3]; x.v[0] = isnull(b) ? 0 : b; x.v[1] = isnull(c) ? 0 : c; x.v[2] = isnull(d) ? 0 : d; return x; } define quat_print(a) { print "quat(" : a.s : ", " : a.v[0] : ", " : a.v[1] : ", " : a.v[2] : ")" :; } define quat_norm(a) { return a.s^2 + dp(a.v, a.v); } define quat_abs(a, e) { return sqrt(a.s^2 + dp(a.v, a.v), e); } define quat_conj(a) { local x; obj quat x; x.s = a.s; x.v = -a.v; return x; } define quat_add(a, b) { local x; obj quat x; if (!istype(b, x)) { x.s = a.s + b; x.v = a.v; return x; } if (!istype(a, x)) { x.s = a + b.s; x.v = b.v; return x; } x.s = a.s + b.s; x.v = a.v + b.v; if (x.v) return x; return x.s; } define quat_sub(a, b) { local x; obj quat x; if (!istype(b, x)) { x.s = a.s - b; x.v = a.v; return x; } if (!istype(a, x)) { x.s = a - b.s; x.v = -b.v; return x; } x.s = a.s - b.s; x.v = a.v - b.v; if (x.v) return x; return x.s; } define quat_inc(a) { local x; x = a; x.s++; return x; } define quat_dec(a) { local x; x = a; x.s--; return x; } define quat_neg(a) { local x; obj quat x; x.s = -a.s; x.v = -a.v; return x; } define quat_mul(a, b) { local x; obj quat x; if (!istype(b, x)) { x.s = a.s * b; x.v = a.v * b; } else if (!istype(a, x)) { x.s = b.s * a; x.v = b.v * a; } else { x.s = a.s * b.s - dp(a.v, b.v); x.v = a.s * b.v + b.s * a.v + cp(a.v, b.v); } if (x.v) return x; return x.s; } define quat_div(a, b) { local x; obj quat x; if (!istype(b, x)) { x.s = a.s / b; x.v = a.v / b; return x; } return a * quat_inv(b); } define quat_inv(a) { local x, q2; obj quat x; q2 = a.s^2 + dp(a.v, a.v); x.s = a.s / q2; x.v = a.v / (-q2); return x; } define quat_scale(a, b) { local x; obj quat x; x.s = scale(a.s, b); x.v = scale(a.v, b); return x; } define quat_shift(a, b) { local x; obj quat x; x.s = a.s << b; x.v = a.v << b; if (x.v) return x; return x.s; } global lib_debug; if (!isnum(lib_debug) || lib_debug>0) print "obj quat {s, v} defined" if (!isnum(lib_debug) || lib_debug>0) print "quat(a, b, c, d) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_print(a) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_norm(a) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_abs(a, e) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_conj(a) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_add(a, e) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_sub(a, e) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_inc(a) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_dec(a) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_neg(a) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_mul(a, b) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_div(a, b) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_inv(a) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_scale(a, b) defined" if (!isnum(lib_debug) || lib_debug>0) print "quat_shift(a, b) defined"