quat.cal

早期freebsd实现

/*
 * 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 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 obj quat	x;

	x.s = a.s;
	x.v = -a.v;
	return x;
}


define quat_add(a, b)
{
	local 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 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 obj quat	x;

	x.s = -a.s;
	x.v = -a.v;
	return x;
}


define quat_mul(a, b)
{
	local 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 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 obj quat	x;

	x.s = scale(a.s, b);
	x.v = scale(a.v, b);
	return x;
}


define quat_shift(a, b)
{
	local 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 (lib_debug >= 0) {
    print "obj quat {s, v} defined";
    print "quat(a, b, c, d) defined";
    print "quat_print(a) defined";
    print "quat_norm(a) defined";
    print "quat_abs(a, e) defined";
    print "quat_conj(a) defined";
    print "quat_add(a, e) defined";
    print "quat_sub(a, e) defined";
    print "quat_inc(a) defined";
    print "quat_dec(a) defined";
    print "quat_neg(a) defined";
    print "quat_mul(a, b) defined";
    print "quat_div(a, b) defined";
    print "quat_inv(a) defined";
    print "quat_scale(a, b) defined";
    print "quat_shift(a, b) defined";
}