/* math library for quaternions designed to be translated into other languages and be presented on the web with XML author: Douglas B Sweetser sweetser@TheWorld.com copyright 1999 Douglas B Sweetser see GPL license info at the end of this file */ #include #include "Qlib.h" #include #define PI 3.14159265359 /* for initialization, replacement, copying */ /* constructQ, replaceQ, qcopy, rqcopy */ /* */ Q* constructQ(double T, double X, double Y, double Z) { Q *Q1; Q1 = (Q *)malloc(sizeof(Q)); Q1->T = T; Q1->X = X; Q1->Y = Y; Q1->Z = Z; return Q1; } /* */ /* */ double dtau2(Q *Q1) { return Q1->T * Q1->T - Q1->X * Q1->X - Q1->Y * Q1->Y - Q1->Z * Q1->Z; } /* */ /* */ double dabs(Q *Q1) { return sqrt(Q1->T * Q1->T + Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z); } /* */ /* */ double dabs_vector(Q *Q1) { return sqrt(Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z); } /* */ /* */ double dnorm(Q *Q1) { return Q1->T * Q1->T + Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z; } /* */ /* */ double dnorm_vector(Q *Q1) { return Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z; } /* */ /* */ double ddet(Q *Q1) { double D1 = 0; D1 = (Q1->T * Q1->T + Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z); D1 *= D1; return D1; } /* */ /* */ Q* qscalar(Q *Q1) { return constructQ(Q1->T,0,0,0); } /* */ /* */ Q* qvector(Q *Q1) { return constructQ(0, Q1->X, Q1->Y, Q1->Z); } /* */ /* */ Q* qconj(Q *Q1) { return constructQ(Q1->T, Q1->X * -1, Q1->Y * -1, Q1->Z * -1); } /* */ /* */ Q* qinv(Q *Q1) { double norm_inv = 0; double norm_neg = 0; norm_inv = 1/(Q1->T * Q1->T + Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z); norm_neg = norm_inv * -1; return constructQ(Q1->T * norm_inv, Q1->X * norm_neg, Q1->Y * norm_neg, Q1->Z * norm_neg); } /* */ /* */ Q* qadj(Q *Q1) { double norm = 0; double norm_neg = 0; norm = Q1->T * Q1->T + Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z; norm_neg = norm * -1; return constructQ(Q1->T * norm, Q1->X * norm_neg, Q1->Y * norm_neg, Q1->Z * norm_neg); } /* */ /* */ Q* qunit(Q *Q1) { double C = 0; C = dabs_inv(Q1); return constructQ(Q1->T * C, Q1->X * C, Q1->Y * C, Q1->Z * C); } /* */ /* */ Q* qsin(Q *Q1) { double C = 0; double abs_v = 0; abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { return constructQ(sin(Q1->T),0,0,0); } C = cos(Q1->T) * sinh(abs_v) / abs_v; return constructQ(sin(Q1->T) * cosh(abs_v), C * Q1->X, C * Q1->Y, C * Q1->Z); } /* */ /* */ Q* qcos(Q *Q1) { double C = 0; double abs_v = 0; abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { return constructQ(cos(Q1->T),0,0,0);; } C = - sin(Q1->T) * sinh(abs_v) / abs_v; return constructQ(cos(Q1->T) * cosh(abs_v), C * Q1->X, C * Q1->Y, C * Q1->Z); } /* */ /* */ Q* qtan(Q *Q1) { double abs_v = 0; Q *Q2; Q2 = constructQ(0,0,0,0); abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { Q2->T = tan(Q1->T); return Q2; } Q2 = qxinv(qsin(Q1), qcos(Q1)); return Q2; } /* */ /* */ Q* qasin(Q *Q1) { double C = 0; double abs_v = 0; Q *Q2; Q *Qv; Q2 = constructQ(0,0,0,0); Qv = constructQ(0,1,0,0); abs_v = dabs_vector(Q1); if (abs_v != 0) { Qv = qvector(qxs(Q1, 1/abs_v)); } Q2 = qx(Qv, qasinh(qx(Q1, qconj(Qv)))); return Q2; } /* */ /* */ Q* qacos(Q *Q1) { double C = 0; double abs_v = 0; Q *Q2; Q *Qv; Q2 = constructQ(0,0,0,0); Qv = constructQ(0,1,0,0); abs_v = dabs_vector(Q1); if (abs_v != 0) { Qv = qvector(qxs(Q1, -1/abs_v)); } Q2 = qx(Qv, qacosh(Q1)); return Q2; } /* */ /* */ Q* qatan(Q *Q1) { double C = 0; double abs_v = 0; Q *Q2; Q *Qv; Q2 = constructQ(0,0,0,0); Qv = constructQ(0,1,0,0); abs_v = dabs_vector(Q1); if (abs_v != 0) { Qv = qvector(qxs(Q1, 1/abs_v)); } Q2 = qx(qconj(Qv), qatanh(qx(Q1, Qv))); return Q2; } /* */ /* */ Q* qsinh(Q *Q1) { double C = 0; double abs_v = 0; abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { return constructQ(sinh(Q1->T),0,0,0); } C = cosh(Q1->T) * sin(abs_v) / abs_v; return constructQ(sinh(Q1->T) * cos(abs_v), C * Q1->X, C * Q1->Y, C * Q1->Z); } /* */ /* */ Q* qcosh(Q *Q1) { double C = 0; double abs_v = 0; abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { return constructQ(cosh(Q1->T),0,0,0); } C = sinh(Q1->T) * sin(abs_v) / abs_v; return constructQ(cosh(Q1->T) * cos(abs_v), C * Q1->X, C * Q1->Y, C * Q1->Z); } /* */ /* */ Q* qtanh(Q *Q1) { double abs_v = 0; Q *Q2; Q2 = constructQ(0,0,0,0); abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { Q2->T = tanh(Q1->T); return Q2; } Q2 = qxinv(qsinh(Q1), qcosh(Q1)); return Q2; } /* */ /* */ Q* qasinh(Q *Q1) { Q *Qone; Q *Q2; Qone = constructQ(1,0,0,0); Q2 = constructQ(0,0,0,0); Q2 = qln(qsum(qtotheN(qsum(qx(Q1,Q1), Qone), .5), Q1)); return Q2; } /* */ /* */ Q* qacosh(Q *Q1) { Q *Qone; Q *Q2; Q *Qsumroot; Q *Qdifroot; Qone = constructQ(1,0,0,0); Q2 = constructQ(0,0,0,0); Qsumroot = constructQ(0,0,0,0); Qdifroot = constructQ(0,0,0,0); Q2 = qtotheN(qdif(qx(Q1, Q1), Qone), .5); Qsumroot = qsum(Q1, Q2); Qdifroot = qdif(Q1, Q2); if (dnorm(Qsumroot) > dnorm(Qdifroot)) { Q2 = qln(Qsumroot); } else { Q2 = qln(Qdifroot); } return Q2; } /* */ /* */ Q* qatanh(Q *Q1) { double vnorm = 0; Q *Qone; Q *Q2; Qone = constructQ(1,0,0,0); Q2 = constructQ(0,0,0,0); Q2 = qxs(qln(qxinv(qsum(Qone, Q1), qdif(Qone, Q1))), 0.5); /* this feels like a hack... */ vnorm = dnorm_vector(Q1); if (vnorm == 0 && Q1->T > 1) { Q2->X *= -1; } return Q2; } /* */ /* */ Q* qexp(Q *Q1) { double C = 0; double abs_v = 0; abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { return constructQ(exp(Q1->T),0,0,0); } C = exp(Q1->T) * sin(abs_v) / abs_v; return constructQ(exp(Q1->T) * cos(abs_v), C * Q1->X, C * Q1->Y, C * Q1->Z); } /* */ /* */ Q* qtotheN(Q *Q1, double D) { Q *Q2; Q2 = constructQ(0,0,0,0); Q2 = qexp(qxs(qln(Q1), D)); return Q2; } /* */ /* */ Q* qtotheQ(Q *Q1, Q *Q2) { Q *Q3; Q3 = constructQ(0,0,0,0); rqcopy(qexp(qx(qln(Q1), Q2)), Q3); return Q3; } /* */ /* */ Q* qln(Q *Q1) { double C = 0; double abs_v = 0; abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { if (Q1->T > 0) { return constructQ(log(Q1->T),0,0,0); } /* wish I knew why this was valid */ else { return constructQ(log(-1 * Q1->T),PI,0,0); } } C = atan2(abs_v, Q1->T) / abs_v; return constructQ(.5 * log(Q1->T * Q1->T + abs_v * abs_v), C * Q1->X, C * Q1->Y, C * Q1->Z); } /* */ /* */ Q* qlog(Q *Q1) { Q *Q2; Q2 = constructQ(0,0,0,0); Q2 = qxs(qln(Q1), 1 / log(10)); return Q2; } /* */ /* */ Q* qsum(Q *Q1, Q *Q2) { return constructQ(Q1->T + Q2->T, Q1->X + Q2->X, Q1->Y + Q2->Y, Q1->Z + Q2->Z); } /* */ /* */ Q* qdif(Q *Q1, Q *Q2) { return constructQ(Q1->T - Q2->T, Q1->X - Q2->X, Q1->Y - Q2->Y, Q1->Z - Q2->Z); } /* */ /* */ Q* qxs(Q *Q1, double S) { return constructQ(Q1->T * S, Q1->X * S, Q1->Y * S, Q1->Z * S); } /* */ /* */ Q* qx(Q *Q1, Q *Q2) { return constructQ( Q1->T * Q2->T - Q1->X * Q2->X - Q1->Y * Q2->Y - Q1->Z * Q2->Z, Q1->T * Q2->X + Q1->X * Q2->T + Q1->Y * Q2->Z - Q1->Z * Q2->Y, Q1->T * Q2->Y + Q1->Y * Q2->T + Q1->Z * Q2->X - Q1->X * Q2->Z, Q1->T * Q2->Z + Q1->Z * Q2->T + Q1->X * Q2->Y - Q1->Y * Q2->X); } /* */ /* */ Q* qxinv(Q *Q1, Q *Q2) { double norm_inv = 0; norm_inv = 1/(Q2->T * Q2->T + Q2->X * Q2->X + Q2->Y * Q2->Y + Q2->Z * Q2->Z); return constructQ( (Q1->T * Q2->T + Q1->X * Q2->X + Q1->Y * Q2->Y + Q1->Z * Q2->Z) * norm_inv, (-Q1->T * Q2->X + Q1->X * Q2->T - Q1->Y * Q2->Z + Q1->Z * Q2->Y) * norm_inv, (-Q1->T * Q2->Y + Q1->Y * Q2->T - Q1->Z * Q2->X + Q1->X * Q2->Z) * norm_inv, (-Q1->T * Q2->Z + Q1->Z * Q2->T - Q1->X * Q2->Y + Q1->Y * Q2->X) * norm_inv); } /* */ /* */ Q* qinvx(Q *Q1, Q *Q2) { double norm_inv = 0; norm_inv = 1/(Q1->T * Q1->T + Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z); return constructQ( (Q1->T * Q2->T + Q1->X * Q2->X + Q1->Y * Q2->Y + Q1->Z * Q2->Z) * norm_inv, (Q1->T * Q2->X - Q1->X * Q2->T - Q1->Y * Q2->Z + Q1->Z * Q2->Y) * norm_inv, (Q1->T * Q2->Y - Q1->Y * Q2->T - Q1->Z * Q2->X + Q1->X * Q2->Z) * norm_inv, (Q1->T * Q2->Z - Q1->Z * Q2->T - Q1->X * Q2->Y + Q1->Y * Q2->X) * norm_inv); } /* */ /* */ Q* qxeven(Q *Q1, Q *Q2) { return constructQ( Q1->T * Q2->T - Q1->X * Q2->X - Q1->Y * Q2->Y - Q1->Z * Q2->Z, Q1->T * Q2->X + Q1->X * Q2->T, Q1->T * Q2->Y + Q1->Y * Q2->T, Q1->T * Q2->Z + Q1->Z * Q2->T); } /* */ /* */ Q* qxodd(Q *Q1, Q *Q2) { return constructQ(0, Q1->Y * Q2->Z - Q1->Z * Q2->Y, Q1->Z * Q2->X - Q1->X * Q2->Z, Q1->X * Q2->Y - Q1->Y * Q2->X); } /* */ /* */ Q* qcx(Q *Q1, Q *Q2) { return constructQ( Q1->T * Q2->T + Q1->X * Q2->X + Q1->Y * Q2->Y + Q1->Z * Q2->Z, Q1->T * Q2->X - Q1->X * Q2->T - Q1->Y * Q2->Z + Q1->Z * Q2->Y, Q1->T * Q2->Y - Q1->Y * Q2->T - Q1->Z * Q2->X + Q1->X * Q2->Z, Q1->T * Q2->Z - Q1->Z * Q2->T - Q1->X * Q2->Y + Q1->Y * Q2->X); } /* */ /* */ Q* qcxeven(Q *Q1, Q *Q2) { return constructQ( Q1->T * Q2->T + Q1->X * Q2->X + Q1->Y * Q2->Y + Q1->Z * Q2->Z, 0,0,0); } /* */ /* */ Q* qcxodd(Q *Q1, Q *Q2) { return constructQ(0, Q1->T * Q2->X - Q1->X * Q2->T - Q1->Y * Q2->Z + Q1->Z * Q2->Y, Q1->T * Q2->Y - Q1->Y * Q2->T - Q1->Z * Q2->X + Q1->X * Q2->Z, Q1->T * Q2->Z - Q1->Z * Q2->T - Q1->X * Q2->Y + Q1->Y * Q2->X); } /* */ /* */ Q* qxc(Q *Q1, Q *Q2) { return constructQ( Q1->T * Q2->T + Q1->X * Q2->X + Q1->Y * Q2->Y + Q1->Z * Q2->Z, -1 * Q1->T * Q2->X + Q1->X * Q2->T - Q1->Y * Q2->Z + Q1->Z * Q2->Y, -1 * Q1->T * Q2->Y + Q1->Y * Q2->T - Q1->Z * Q2->X + Q1->X * Q2->Z, -1 * Q1->T * Q2->Z + Q1->Z * Q2->T - Q1->X * Q2->Y + Q1->Y * Q2->X); } /* */ /* */ Q* qxceven(Q *Q1, Q *Q2) { return constructQ( Q1->T * Q2->T + Q1->X * Q2->X + Q1->Y * Q2->Y + Q1->Z * Q2->Z, 0,0,0); } /* */ /* */ Q* qxcodd(Q *Q1, Q *Q2) { return constructQ(0, - 1 * Q1->T * Q2->X + Q1->X * Q2->T - Q1->Y * Q2->Z + Q1->Z * Q2->Y, - 1 * Q1->T * Q2->Y + Q1->Y * Q2->T - Q1->Z * Q2->X + Q1->X * Q2->Z, -1 * Q1->T * Q2->Z + Q1->Z * Q2->T - Q1->X * Q2->Y + Q1->Y * Q2->X); } /* */ /* */ void printq(Q *Q1) { printf("%g %g %g %g", Q1->T, Q1->X, Q1->Y, Q1->Z); } /* */ /* */ void rqcopy(Q *Q1, Q *Q2) { Q2->T = Q1->T; Q2->X = Q1->X; Q2->Y = Q1->Y; Q2->Z = Q1->Z; } /* */ /* */ double dabs_inv(Q *Q1) { if((Q1->T == 0) && (Q1->X == 0) && (Q1->Y == 0) && (Q1->Z == 0)) { printf("dividing by zero occured in dabs_inv"); exit(1); } return 1 / sqrt(Q1->T * Q1->T + Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z); } /* */