/* 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 #define DEGREES_TO_RADIANS 0.01745329252 /* 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; } /* */ /* */ void replaceQ(double T, double X, double Y, double Z, Q *Q1) { Q1->T = T; Q1->X = X; Q1->Y = Y; Q1->Z = Z; } /* */ /* */ Q* qcopy(Q *Q1) { return constructQ(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; } /* */ /* Unary functions, result is a double */ /* dtau2, dabs, dabs_vector, dnorm, dnorm_vector, ddet */ /* */ 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; } /* */ /* Unary functions inverses, result is a double */ /* dtau2_inv, dabs_inv, dabs_vector_inv */ /* dnorm_inv, dnorm_vector_inv, ddet_inv */ /* */ double dtau2_inv(Q *Q1) { if((Q1->T == 0) && (Q1->X == 0) && (Q1->Y == 0) && (Q1->Z == 0)) { printf("dividing by zero occured in dtau2_inv"); exit(1); } return 1 / (Q1->T * Q1->T - Q1->X * Q1->X - Q1->Y * Q1->Y - Q1->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); } /* */ /* */ double dabs_vector_inv(Q *Q1) { if((Q1->X == 0) && (Q1->Y == 0) && (Q1->Z == 0)) { printf("dividing by zero occured in dvector_inv"); exit(1); } return 1 / sqrt(Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z); } /* */ /* */ double dnorm_inv(Q *Q1) { if((Q1->T == 0) && (Q1->X == 0) && (Q1->Y == 0) && (Q1->Z == 0)) { printf("dividing by zero occured in dnorm_inv"); exit(1); } return 1 / (Q1->T * Q1->T + Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z); } /* */ /* */ double dnorm_vector_inv(Q *Q1) { if((Q1->X == 0) && (Q1->Y == 0) && (Q1->Z == 0)) { printf("dividing by zero occured in dnorm_vector_inv"); exit(1); } return 1 / (Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z); } /* */ /* */ double ddet_inv(Q *Q1) { double D1 = 0; if((Q1->T == 0) && (Q1->X == 0) && (Q1->Y == 0) && (Q1->Z == 0)) { printf("dividing by zero occured in ddet_inv"); exit(1); } D1 = 1 / (Q1->T * Q1->T + Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z); D1 *= D1; return D1; } /* */ /* Unary functions */ /* qscalar, qvector, qconj, q_inv, qadj, qunit */ /* rqscalar, rqvector, rqconj, rq_inv, rqadj, rqunit */ /* */ 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); } /* */ /* */ void rqscalar(Q *Q1, Q *Q2) { replaceQ(Q1->T,0,0,0, Q2); } /* */ /* */ void rqvector(Q *Q1, Q *Q2) { replaceQ(0, Q1->X, Q1->Y, Q1->Z, Q2); } /* */ /* */ void rqconj(Q *Q1, Q *Q2) { replaceQ(Q1->T, Q1->X * -1, Q1->Y * -1, Q1->Z * -1, Q2); } /* */ /* */ void rqinv(Q *Q1, Q *Q2) { 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; replaceQ(Q1->T * norm_inv, Q1->X * norm_neg, Q1->Y * norm_neg, Q1->Z * norm_neg, Q2); } /* */ /* */ void rqadj(Q *Q1, Q *Q2) { 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; replaceQ(Q1->T * norm, Q1->X * norm_neg, Q1->Y * norm_neg, Q1->Z * norm_neg, Q2); } /* */ /* */ void rqunit(Q *Q1, Q *Q2) { double C = 0; C = dabs_inv(Q1); replaceQ(Q1->T * C, Q1->X * C, Q1->Y * C, Q1->Z * C, Q2); } /* */ /* Unary trig functions */ /* qsin, qcos, qtan, qasin, qacos, qatan */ /* rqsin, rqcos, ratan, rqasin, rqacos, rqatan */ /* qsinh, qacosh, qatanh, qasinh, qacosh, qatanh */ /* rqsinh, rqcosh, rqtanh, rqasinh, rqacosh, rqatanh */ /* */ 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; } /* */ /* */ void rqsin(Q *Q1, Q *Q2) { double C = 0; double abs_v = 0; abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { replaceQ(sin(Q1->T), 0, 0, 0, Q2); return; } C = cos(Q1->T) * sinh(abs_v) / abs_v; replaceQ(sin(Q1->T) * cosh(abs_v), C * Q1->X, C * Q1->Y, C * Q1->Z, Q2); } /* */ /* */ void rqcos(Q *Q1, Q *Q2) { double C = 0; double abs_v = 0; abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { replaceQ(cos(Q1->T),0,0,0, Q2); return; } C = - sin(Q1->T) * sinh(abs_v) / abs_v; replaceQ(cos(Q1->T) * cosh(abs_v), C * Q1->X, C * Q1->Y, C * Q1->Z, Q2); } /* */ /* */ void rqtan(Q *Q1, Q *Q2) { double abs_v = 0; abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { replaceQ(tan(Q1->T),0,0,0, Q2); return; } rqcopy(qxinv(qsin(Q1), qcos(Q1)), Q2); } /* */ /* */ void rqasin(Q *Q1, Q *Q2) { double C = 0; double abs_v = 0; Q *Qv; Qv = constructQ(0,1,0,0); abs_v = dabs_vector(Q1); if (abs_v != 0) { Qv = qvector(qxs(Q1, 1/abs_v)); } rqcopy(qx(Qv, qasinh(qx(Q1, qconj(Qv)))), Q2); } /* */ /* */ void rqacos(Q *Q1, Q *Q2) { double C = 0; double abs_v = 0; Q *Qv; Qv = constructQ(0,1,0,0); abs_v = dabs_vector(Q1); if (abs_v != 0) { Qv = qvector(qxs(Q1, -1/abs_v)); } rqcopy(qx(Qv, qacosh(Q1)), Q2); } /* */ /* */ void rqatan(Q *Q1, Q *Q2) { double C = 0; double abs_v = 0; Q *Qv; Qv = constructQ(0,1,0,0); abs_v = dabs_vector(Q1); if (abs_v != 0) { Qv = qvector(qxs(Q1, 1/abs_v)); } rqcopy(qx(qconj(Qv), qatanh(qx(Q1, Qv))), Q2); } /* */ /* */ void rqsinh(Q *Q1, Q *Q2) { double C = 0; double abs_v = 0; abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { replaceQ(sinh(Q1->T),0,0,0, Q2); return; } C = cosh(Q1->T) * sin(abs_v) / abs_v; replaceQ(sinh(Q1->T) * cos(abs_v), C * Q1->X, C * Q1->Y, C * Q1->Z, Q2); } /* */ /* */ void rqcosh(Q *Q1, Q *Q2) { double C = 0; double abs_v = 0; abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { replaceQ(cosh(Q1->T),0,0,0, Q2); return; } C = sinh(Q1->T) * sin(abs_v) / abs_v; replaceQ(cosh(Q1->T) * cos(abs_v), C * Q1->X, C * Q1->Y, C * Q1->Z, Q2); } /* */ /* */ void rqtanh(Q *Q1, Q *Q2) { double abs_v = 0; abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { Q2->T = tanh(Q1->T); return; } rqcopy(qxinv(qsinh(Q1), qcosh(Q1)), Q2); } /* */ /* */ void rqasinh(Q *Q1, Q *Q2) { Q *Qone; Qone = constructQ(1,0,0,0); rqcopy(qln(qsum(qtotheN(qsum(qx(Q1,Q1), Qone), .5), Q1)), Q2); } /* */ /* */ void rqacosh(Q *Q1, Q *Q2) { Q *Qone; Q *Qsumroot; Q *Qdifroot; Qone = constructQ(1,0,0,0); Qsumroot = constructQ(0,0,0,0); Qdifroot = constructQ(0,0,0,0); rqcopy(qtotheN(qdif(qx(Q1, Q1), Qone), .5), Q2); rqsum(Q1, Q2, Qsumroot); rqdif(Q1, Q2, Qdifroot); if (dnorm(Qsumroot) > dnorm(Qdifroot)) { rqcopy(qln(Qsumroot), Q2); } else { rqcopy(qln(Qdifroot), Q2); } } /* */ /* */ void rqatanh(Q *Q1, Q *Q2) { double vnorm = 0; Q *Qone; Qone = constructQ(1,0,0,0); rqcopy(qxs(qln(qxinv(qsum(Qone, Q1), qdif(Qone, Q1))), 0.5), Q2); /* this feels like a hack... */ vnorm = dnorm_vector(Q1); if (vnorm == 0 && Q1->T > 1) { Q2->X *= -1; } } /* */ /* Exponentials, logs */ /* qexp, qtotheN, qtotheQ, qln, qlog */ /* rqexp, rqtotheN, rqtotheQ, rqln, rqlog */ /* */ 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; } /* */ /* */ void rqexp(Q *Q1, Q *Q2) { double C = 0; double abs_v = 0; abs_v = dabs_vector(Q1); /* if the quaternion is a pure scalar */ if (abs_v == 0) { replaceQ(exp(Q1->T),0,0,0, Q2); return; } C = exp(Q1->T) * sin(abs_v) / abs_v; replaceQ(exp(Q1->T) * cos(abs_v), C * Q1->X, C * Q1->Y, C * Q1->Z, Q2); } /* */ /* */ void rqtotheN(Q *Q1, double D, Q *Q2) { rqexp(qxs(qln(Q1), D), Q2); } /* */ /* */ void rqtotheQ(Q *Q1, Q *Q2, Q *Q3) { rqexp(qx(qln(Q1), Q2), Q3); } /* */ /* */ void rqln(Q *Q1, Q *Q2) { 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) { replaceQ(log(Q1->T),0,0,0, Q2); return; } /* wish I knew why this was valid */ else { replaceQ(log(-1 * Q1->T),PI,0,0, Q2); return; } } C = atan2(abs_v, Q1->T) / abs_v; replaceQ(.5 * log(Q1->T * Q1->T + abs_v * abs_v), C * Q1->X, C * Q1->Y, C * Q1->Z, Q2); } /* */ /* */ void rqlog(Q *Q1, Q *Q2) { rqcopy(qxs(qln(Q1), 1 / log(10)), Q2); } /* */ /* addition and subtraction */ /* qsum, qdif */ /* rqsum, rqdif */ /* */ 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); } /* */ /* */ void rqsum(Q *Q1, Q *Q2, Q *Q3) { replaceQ(Q1->T + Q2->T, Q1->X + Q2->X, Q1->Y + Q2->Y, Q1->Z + Q2->Z, Q3); } /* */ /* */ void rqdif(Q *Q1, Q *Q2, Q *Q3) { replaceQ(Q1->T - Q2->T, Q1->X - Q2->X, Q1->Y - Q2->Y, Q1->Z - Q2->Z, Q3); } /* */ /* Products */ /* qxs, qx, qxinv, qinvx, qxeven, qxodd */ /* rqxs, rqx, rqxinv, rqinvx, rqxeven, rqxodd */ /* qcx, qcxeven, qcxodd, qxc, qxceven, qxcodd */ /* rqcx, rqcxeven, rqcxodd, rqxc, rqxceven, rqxcodd */ /* */ 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 = dnorm_inv(Q2); 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 rqxs(Q *Q1, double S, Q *Q2) { replaceQ(Q1->T * S, Q1->X * S, Q1->Y * S, Q1->Z * S, Q2); } /* */ /* */ void rqx(Q *Q1, Q *Q2, Q *Q3) { replaceQ( 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, Q3); } /* */ /* */ void rqxinv(Q *Q1, Q *Q2, Q *Q3) { double norm_inv = 0; norm_inv = 1/(Q2->T * Q2->T + Q2->X * Q2->X + Q2->Y * Q2->Y + Q2->Z * Q2->Z); replaceQ( (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, Q3); } /* */ /* */ void rqinvx(Q *Q1, Q *Q2, Q *Q3) { double norm_inv = 0; norm_inv = 1/(Q1->T * Q1->T + Q1->X * Q1->X + Q1->Y * Q1->Y + Q1->Z * Q1->Z); replaceQ( (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, Q3); } /* */ /* */ void rqxeven(Q *Q1, Q *Q2, Q *Q3) { replaceQ( 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, Q3); } /* */ /* */ void rqxodd(Q *Q1, Q *Q2, Q *Q3) { replaceQ(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, Q3); } /* */ /* */ void rqcx(Q *Q1, Q *Q2, Q *Q3) { replaceQ( 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, Q3); } /* */ /* */ void rqcxeven(Q *Q1, Q *Q2, Q *Q3) { replaceQ( Q1->T * Q2->T + Q1->X * Q2->X + Q1->Y * Q2->Y + Q1->Z * Q2->Z, 0,0,0, Q3); } /* */ /* */ void rqcxodd(Q *Q1, Q *Q2, Q *Q3) { replaceQ(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, Q3); } /* */ /* */ void rqxc(Q *Q1, Q *Q2, Q *Q3) { replaceQ( 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, Q3); } /* */ /* */ void rqxceven(Q *Q1, Q *Q2, Q *Q3) { replaceQ( Q1->T * Q2->T + Q1->X * Q2->X + Q1->Y * Q2->Y + Q1->Z * Q2->Z, 0,0,0, Q3); } /* */ /* */ void rqxcodd(Q *Q1, Q *Q2, Q *Q3) { replaceQ(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, Q3); } /* */ /* rotations */ /* qrot_x_by_angle qrot_y_by_angle qrot_z_by_angle qrot_xyz_by_angle */ /* rqrot_x_by_angle rqrot_y_by_angle rqrot_z_by_angle rqrot_xyz_by_angle */ /* q3D_rotation qcxq */ /* rq3D_rotation rqcxq */ /* */ Q* qrot_x_by_angle(Q *Q1, double D1) { double T = 0; double X = 0; /* to decrease dublicate multiplications */ double T2 = 0; double X2 = 0; double TX = 0; if (D1 == 0) { return Q1; } T = cos(D1 * DEGREES_TO_RADIANS / 2); X = sin(D1 * DEGREES_TO_RADIANS / 2); T2 = T * T; X2 = X * X; TX = T * X; return constructQ( Q1->T * (T2 + X2), Q1->X * (T2 + X2), Q1->Y * (T2 - X2) + 2 * Q1->Z * TX, Q1->Z * (T2 - X2) - 2 * Q1->Y * TX); } /* */ /* */ Q* qrot_y_by_angle(Q *Q1, double D1) { double T = 0; double Y = 0; /* to decrease dublicate multiplications */ double T2 = 0; double Y2 = 0; double TY = 0; if (D1 == 0) { return Q1; } T = cos(D1 * DEGREES_TO_RADIANS / 2); Y = sin(D1 * DEGREES_TO_RADIANS / 2); T2 = T * T; Y2 = Y * Y; TY = T * Y; return constructQ( Q1->T * (T2 + Y2), Q1->X * (T2 - Y2) - 2 * Q1->Z * TY, Q1->Y * (T2 + Y2), Q1->Z * (T2 - Y2) + 2 * Q1->X * TY); } /* */ /* */ Q* qrot_z_by_angle(Q *Q1, double D1) { double T = 0; double Z = 0; /* to decrease dublicate multiplications */ double T2 = 0; double Z2 = 0; double TZ = 0; if (D1 == 0) { return Q1; } T = cos(D1 * DEGREES_TO_RADIANS / 2); Z = sin(D1 * DEGREES_TO_RADIANS / 2); T2 = T * T; Z2 = Z * Z; TZ = T * Z; return constructQ( Q1->T * (T2 + Z2), Q1->X * (T2 - Z2) + 2 * Q1->Y * TZ, Q1->Y * (T2 - Z2) - 2 * Q1->X * TZ, Q1->Z * (T2 + Z2)); } /* */ /* */ Q* qrot_xyz_by_angle(Q *Q1, double DX, double DY, double DZ, double D1) { double T = 0; double X = 0; double Y = 0; double Z = 0; /* to decrease dublicate multiplications */ double T2 = 0; double X2 = 0; double Y2 = 0; double Z2 = 0; double TY = 0; double XY = 0; double TZ = 0; double XZ = 0; double TX = 0; double YZ = 0; double n = 0; double angle = 0; if (D1 == 0 || (DX == 0 && DY == 0 && DZ == 0) ) { return Q1; } /* normalize the 3-vector */ n = 1 / sqrt(DX * DX + DY * DY + DZ * DZ); DX = n * DX; DY = n * DY; DZ = n * DZ; angle = D1 * DEGREES_TO_RADIANS / 2; T = cos(angle); X = DX * sin(angle); Y = DY * sin(angle); Z = DZ * sin(angle); T2 = T * T; X2 = X * X; Y2 = Y * Y; Z2 = Z * Z; TY = T * Y; XY = X * Y; TZ = T * Z; XZ = X * Z; TX = T * X; YZ = Y * Z; return constructQ( Q1->T * (T2 + X2 + Y2 + Z2), Q1->X * (T2 + X2 - Y2 - Z2) + 2 * (Q1->Y * (XY + TZ) + Q1->Z * (XZ -TY)), Q1->Y * (T2 - X2 + Y2 - Z2) + 2 * (Q1->Z * (YZ + TX) + Q1->X * (XY -TZ)), Q1->Z * (T2 - X2 - Y2 + Z2) + 2 * (Q1->X * (XZ + TY) + Q1->Y * (YZ -TX))); } /* */ /* */ Q* q3D_rotation(Q *Q1, Q *Q2) { /* to decrease dublicate multiplications */ double T2 = 0; double X2 = 0; double Y2 = 0; double Z2 = 0; double TY = 0; double XY = 0; double TZ = 0; double XZ = 0; double TX = 0; double YZ = 0; double n = 0; n = dnorm_inv(Q1); T2 = Q1->T * Q1->T; X2 = Q1->X * Q1->X; Y2 = Q1->Y * Q1->Y; Z2 = Q1->Z * Q1->Z; TY = Q1->T * Q1->Y; XY = Q1->X * Q1->Y; TZ = Q1->T * Q1->Z; XZ = Q1->X * Q1->Z; TX = Q1->T * Q1->X; YZ = Q1->Y * Q1->Z; return constructQ( Q2->T, (Q2->X * (T2 + X2 - Y2 - Z2) + 2 * (Q2->Y * (XY + TZ) + Q2->Z * (XZ -TY))) * n, (Q2->Y * (T2 - X2 + Y2 - Z2) + 2 * (Q2->Z * (YZ + TX) + Q2->X * (XY -TZ))) * n, (Q2->Z * (T2 - X2 - Y2 + Z2) + 2 * (Q2->X * (XZ + TY) + Q2->Y * (YZ -TX))) * n); } /* */ /* */ Q* qcxq(Q *Q1, Q *Q2) { /* to decrease dublicate multiplications */ double T2 = 0; double X2 = 0; double Y2 = 0; double Z2 = 0; double TY = 0; double XY = 0; double TZ = 0; double XZ = 0; double TX = 0; double YZ = 0; T2 = Q1->T * Q1->T; X2 = Q1->X * Q1->X; Y2 = Q1->Y * Q1->Y; Z2 = Q1->Z * Q1->Z; TY = Q1->T * Q1->Y; XY = Q1->X * Q1->Y; TZ = Q1->T * Q1->Z; XZ = Q1->X * Q1->Z; TX = Q1->T * Q1->X; YZ = Q1->Y * Q1->Z; return constructQ( Q2->T * (T2 + X2 + Y2 + Z2), Q2->X * (T2 + X2 - Y2 - Z2) + 2 * (Q2->Y * (XY + TZ) + Q2->Z * (XZ -TY)), Q2->Y * (T2 - X2 + Y2 - Z2) + 2 * (Q2->Z * (YZ + TX) + Q2->X * (XY -TZ)), Q2->Z * (T2 - X2 - Y2 + Z2) + 2 * (Q2->X * (XZ + TY) + Q2->Y * (YZ -TX))); } /* */ /* */ void rqrot_x_by_angle(Q *Q1, double D1, Q *Q2) { double T = 0; double X = 0; /* to decrease dublicate multiplications */ double T2 = 0; double X2 = 0; double TX = 0; if (D1 == 0) { rqcopy(Q1, Q2); } T = cos(D1 * DEGREES_TO_RADIANS / 2); X = sin(D1 * DEGREES_TO_RADIANS / 2); T2 = T * T; X2 = X * X; TX = T * X; replaceQ( Q1->T * (T2 + X2), Q1->X * (T2 + X2), Q1->Y * (T2 - X2) + 2 * Q1->Z * TX, Q1->Z * (T2 - X2) - 2 * Q1->Y * TX, Q2); } /* */ /* */ void rqrot_y_by_angle(Q *Q1, double D1, Q *Q2) { double T = 0; double Y = 0; /* to decrease dublicate multiplications */ double T2 = 0; double Y2 = 0; double TY = 0; if (D1 == 0) { rqcopy(Q1, Q2); } T = cos(D1 * DEGREES_TO_RADIANS / 2); Y = sin(D1 * DEGREES_TO_RADIANS / 2); T2 = T * T; Y2 = Y * Y; TY = T * Y; replaceQ( Q1->T * (T2 + Y2), Q1->X * (T2 - Y2) - 2 * Q1->Z * TY, Q1->Y * (T2 + Y2), Q1->Z * (T2 - Y2) + 2 * Q1->X * TY, Q2); } /* */ /* */ void rqrot_z_by_angle(Q *Q1, double D1, Q *Q2) { double T = 0; double Z = 0; /* to decrease dublicate multiplications */ double T2 = 0; double Z2 = 0; double TZ = 0; if (D1 == 0) { rqcopy(Q1, Q2); } T = cos(D1 * DEGREES_TO_RADIANS / 2); Z = sin(D1 * DEGREES_TO_RADIANS / 2); T2 = T * T; Z2 = Z * Z; TZ = T * Z; replaceQ( Q1->T * (T2 + Z2), Q1->X * (T2 - Z2) + 2 * Q1->Y * TZ, Q1->Y * (T2 - Z2) - 2 * Q1->X * TZ, Q1->Z * (T2 + Z2), Q2); } /* */ /* */ void rqrot_xyz_by_angle(Q *Q1, double DX, double DY, double DZ, double D1, Q *Q2) { double T = 0; double X = 0; double Y = 0; double Z = 0; /* to decrease dublicate multiplications */ double T2 = 0; double X2 = 0; double Y2 = 0; double Z2 = 0; double TY = 0; double XY = 0; double TZ = 0; double XZ = 0; double TX = 0; double YZ = 0; double n = 0; double angle = 0; if (D1 == 0 || (DX == 0 && DY == 0 && DZ == 0) ) { rqcopy(Q1, Q2); } /* normalize the 3-vector */ n = 1 / sqrt(DX * DX + DY * DY + DZ * DZ); DX = n * DX; DY = n * DY; DZ = n * DZ; angle = D1 * DEGREES_TO_RADIANS / 2; T = cos(angle); X = DX * sin(angle); Y = DY * sin(angle); Z = DZ * sin(angle); T2 = T * T; X2 = X * X; Y2 = Y * Y; Z2 = Z * Z; TY = T * Y; XY = X * Y; TZ = T * Z; XZ = X * Z; TX = T * X; YZ = Y * Z; replaceQ( Q1->T * (T2 + X2 + Y2 + Z2), Q1->X * (T2 + X2 - Y2 - Z2) + 2 * (Q1->Y * (XY + TZ) + Q1->Z * (XZ -TY)), Q1->Y * (T2 - X2 + Y2 - Z2) + 2 * (Q1->Z * (YZ + TX) + Q1->X * (XY -TZ)), Q1->Z * (T2 - X2 - Y2 + Z2) + 2 * (Q1->X * (XZ + TY) + Q1->Y * (YZ -TX)), Q2); } /* */ /* */ void rq3D_rotation(Q *Q1, Q *Q2, Q *Q3) { /* to decrease dublicate multiplications */ double T2 = 0; double X2 = 0; double Y2 = 0; double Z2 = 0; double TY = 0; double XY = 0; double TZ = 0; double XZ = 0; double TX = 0; double YZ = 0; double n = 0; n = dnorm_inv(Q1); T2 = Q1->T * Q1->T; X2 = Q1->X * Q1->X; Y2 = Q1->Y * Q1->Y; Z2 = Q1->Z * Q1->Z; TY = Q1->T * Q1->Y; XY = Q1->X * Q1->Y; TZ = Q1->T * Q1->Z; XZ = Q1->X * Q1->Z; TX = Q1->T * Q1->X; YZ = Q1->Y * Q1->Z; replaceQ( Q2->T, (Q2->X * (T2 + X2 - Y2 - Z2) + 2 * (Q2->Y * (XY + TZ) + Q2->Z * (XZ -TY))) * n, (Q2->Y * (T2 - X2 + Y2 - Z2) + 2 * (Q2->Z * (YZ + TX) + Q2->X * (XY -TZ))) * n, (Q2->Z * (T2 - X2 - Y2 + Z2) + 2 * (Q2->X * (XZ + TY) + Q2->Y * (YZ -TX))) * n, Q3); } /* */ /* */ void rqcxq(Q *Q1, Q *Q2, Q *Q3) { /* to decrease dublicate multiplications */ double T2 = 0; double X2 = 0; double Y2 = 0; double Z2 = 0; double TY = 0; double XY = 0; double TZ = 0; double XZ = 0; double TX = 0; double YZ = 0; T2 = Q1->T * Q1->T; X2 = Q1->X * Q1->X; Y2 = Q1->Y * Q1->Y; Z2 = Q1->Z * Q1->Z; TY = Q1->T * Q1->Y; XY = Q1->X * Q1->Y; TZ = Q1->T * Q1->Z; XZ = Q1->X * Q1->Z; TX = Q1->T * Q1->X; YZ = Q1->Y * Q1->Z; replaceQ( Q2->T * (T2 + X2 + Y2 + Z2), Q2->X * (T2 + X2 - Y2 - Z2) + 2 * (Q2->Y * (XY + TZ) + Q2->Z * (XZ -TY)), Q2->Y * (T2 - X2 + Y2 - Z2) + 2 * (Q2->Z * (YZ + TX) + Q2->X * (XY -TZ)), Q2->Z * (T2 - X2 - Y2 + Z2) + 2 * (Q2->X * (XZ + TY) + Q2->Y * (YZ -TX)), Q3); } /* */ /* */ void printq(Q *Q1) { printf("%g %g %g %g", Q1->T, Q1->X, Q1->Y, Q1->Z); } /* */ /* */ void printlnq(Q *Q1) { printf("%g %g %g %g\n", Q1->T, Q1->X, Q1->Y, Q1->Z); } /* */ /* This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2 of the License. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */