quaternions and dual-quaternions

quat_quat2.cpp

// quat
const quat quat::IDENTITY(0.0f, 0.0f, 0.0f, 1.0f);
const quat quat::ZERO(0.0f, 0.0f, 0.0f, 0.0f);

quat::quat() {}
quat::quat(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {}

quat::quat(const vec3 &axis, float angle) {
	angle *= 0.5f;
	float s = sinf(angle);
	x = axis.x * s;
	y = axis.y * s;
	z = axis.z * s;
	w = cosf(angle);
}

quat::quat(const vec3 &angle) {
	vec3 a = angle * 0.5f;
	vec3 s(sinf(a.x), sinf(a.y), sinf(a.z));
	vec3 c(cosf(a.x), cosf(a.y), cosf(a.z));

	quat qPitch (0.0f,  s.x, 0.0f, c.x);
	quat qYaw   (0.0f, 0.0f,  s.y, c.y);
	quat qRoll  ( s.z, 0.0f, 0.0f, c.z);
	*this = qYaw * qPitch * qRoll;
}

quat::quat(float lat, float lng, float angle) {
	angle *= 0.5f;
	vec3 s(sinf(lng), sinf(lat), sinf(angle));
	vec3 c(cosf(lng), cosf(lat), cosf(angle));

	x = s.z * c.y * s.x;
	y = s.z * s.y;
	z = s.z * s.y * c.x;
	w = c.z;
}

quat::quat(const vec3 &from, const vec3 &to) {
	float c = from.dot(to);
	float d = sqrtf(from.length2() + to.length2());
 
	if (c / d == -1.0f) {
		xyz = from.cross(from.axis()).normal();
		w = 0.0f;
	} else {
		xyz = from.cross(to);
		w = c + d;
		normalize();
	}
}

float& quat::operator [] (int index) const {
	return ((float*)this)[index];
}

bool quat::operator == (const quat &q) const {
	return fcmp(x, q.x) && fcmp(y, q.y) && fcmp(z, q.z) && fcmp(w, q.w);
}

bool quat::operator != (const quat &q) const {
	return !(*this == q);
}

quat& quat::operator += (const quat &q) {
	x += q.x;
	y += q.y;
	z += q.z;
	w += q.w; 
	return *this; 
}

quat& quat::operator -= (const quat &q) {
	x -= q.x;
	y -= q.y;
	z -= q.z;
	w -= q.w; 
	return *this; 
}

quat& quat::operator *= (const float s) {
	x *= s; 
	y *= s; 
	z *= s; 
	w *= s; 
	return *this; 
}

quat quat::operator - () const {
	return quat(-x, -y, -z, -w);
}

quat quat::operator + (const quat &q) const { 
	return quat(x + q.x, y + q.y, z + q.z, w + q.w); 
}

quat quat::operator - (const quat &q) const { 
	return quat(x - q.x, y - q.y, z - q.z, w - q.w); 
}

quat quat::operator * (const float s) const { 
	return quat(x * s, y * s, z * s, w * s); 
}

quat quat::operator * (const quat &q) const {
	return quat(w * q.x + x * q.w + y * q.z - z * q.y,
				w * q.y + y * q.w + z * q.x - x * q.z,
				w * q.z + z * q.w + x * q.y - y * q.x,
				w * q.w - x * q.x - y * q.y - z * q.z);
}

vec3 quat::operator * (const vec3 &v) const {
//	return v + xyz.cross(xyz.cross(v) + v * w) * 2.0f;
	return (*this * quat(v.x, v.y, v.z, 0) * inverse()).xyz;
}

bool quat::equal(const quat &q, float eps) const {
	return fabsf(x - q.x) <= eps && fabsf(y - q.y) <= eps && fabsf(z - q.z) <= eps && fabsf(w - q.w) <= eps;
}

void quat::normalize() {
	*this = normal();
}

quat quat::normal() const {
	return *this * (1.0f / length());
}

quat quat::conjugate() const {
	return quat(-x, -y, -z, w);
}

quat quat::inverse() const {
	return conjugate() * (1.0f / length2());
}

quat quat::lerp(const quat &q, float t) const {
	if (t <= 0.0f) return *this;
	if (t >= 1.0f) return q;

	return dot(q) < 0 ? (*this - (q + *this) * t) : 
						(*this + (q - *this) * t);
}

quat quat::slerp(const quat &q, float t) const {
	if (t <= 0.0f) return *this;
	if (t >= 1.0f) return q;

	quat temp;
	float omega, cosom, sinom, scale0, scale1;

	cosom = dot(q);
	if (cosom < 0.0f) {
		temp = -q;
		cosom = -cosom;
	} else
		temp = q;

	if (1.0f - cosom > EPS) {
		omega = acos(cosom);
		sinom = 1.0f / sin(omega);
		scale0 = sin((1.0f - t) * omega) * sinom;
		scale1 = sin(t * omega) * sinom;
	} else {
		scale0 = 1.0f - t;
		scale1 = t;
	}

	return *this * scale0 + temp * scale1;
}

float quat::dot(const quat &q) const {
	return x * q.x + y * q.y + z * q.z + w * q.w;
}

float quat::length2() const { 
	return dot(*this); 
}

float quat::length() const { 
	return sqrtf(length2());
}

void quat::recalc() {
	w = sqrtf(_max(0.0f, 1.0f - xyz.length2()));
}

// quat2
const quat2 quat2::IDENTITY(quat::IDENTITY, quat::ZERO);

quat2::quat2() {}
quat2::quat2(const quat &real, const quat &dual) : real(real), dual(dual) {}
quat2::quat2(const quat &rot, const vec3 &pos) {
	setRot(rot);
	setPos(pos);
}

bool quat2::operator == (const quat2 &dq) const {
	return real == dq.real && dual == dq.dual;
}

bool quat2::operator != (const quat2 &dq) const {
	return !(*this == dq);
}

quat2 quat2::operator * (const quat2 &dq) const {
	return quat2(real * dq.real, real * dq.dual + dual * dq.real);
}

vec3 quat2::operator * (const vec3 &v) const {
	return real * v + ((dual.xyz * real.w - real.xyz * dual.w + real.xyz.cross(dual.xyz)) * 2.0f);
}

quat2 quat2::inverse() const {
	return quat2(real.inverse(), dual.conjugate());
}

quat2 quat2::lerp(const quat2 &dq, float t) const {
	if (t <= 0.0f) return *this;
	if (t >= 1.0f) return dq;
	return quat2(real.slerp(dq.real, t), dual.lerp(dq.dual, t));
//	return real.dot(q.real) < 0 ? quat2(real - (q.real + real) * t, dual - (q.dual + dual) * t) :
//								  quat2(real + (q.real - real) * t, dual + (q.dual - dual) * t);
}

quat quat2::getRot() const {
	return real;
}

void quat2::setRot(const quat &rot) {
	real = rot;
}

vec3 quat2::getPos() const {
	return (dual * 2.0f * real.conjugate()).xyz;
}

void quat2::setPos(const vec3 &pos) {
	dual = quat(pos.x, pos.y, pos.z, 0.0f) * real * 0.5f;
}