~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

Linux Cross Reference
Tina4/src/math/transform/quat.c

Version: ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

  1 /**@(#)Quaternion handling
  2 **/
  3 #include <math.h>
  4 #include <tina/sys.h>
  5 #include <tina/math.h>
  6 #include <tina/mathfuncs.h>
  7 
  8 Vec3    vec3_quat(Vec4 q)
  9 {
 10     Vec3    v = {Vec3_id};
 11 
 12     vec3_x(v) = vec4_x(q);
 13     vec3_y(v) = vec4_y(q);
 14     vec3_z(v) = vec4_z(q);
 15     return (v);
 16 }
 17 
 18 Vec4    quat_vec3(Vec3 v)
 19 {
 20     Vec4    q = {Vec4_id};
 21 
 22     vec4_x(q) = vec3_x(v);
 23     vec4_y(q) = vec3_y(v);
 24     vec4_z(q) = vec3_z(v);
 25     vec4_w(q) = 0.0;
 26     return (q);
 27 }
 28 
 29 Vec4    quat_rot3(Mat3 r)
 30 {
 31     double  theta;
 32     Vec3    axis = {Vec3_id};
 33     Vec4    q = {Vec4_id};
 34 
 35     rot3_angle_axis(r, &theta, &axis);
 36     q = quat_vec3(vec3_times(sin(0.5 * theta), axis));
 37     vec4_w(q) = cos(0.5 * theta);
 38     return (q);
 39 }
 40 
 41 Mat3    rot3_quat(Vec4 q)
 42 {
 43     double  theta, s;
 44     Vec3    axis = {Vec3_id};
 45 
 46     theta = 2.0 * acos(vec4_w(q));
 47     s = sin(0.5 * theta);
 48     if (s == 0.0)
 49         return (mat3_unit());
 50     s = 1.0 / s;
 51     vec3_x(axis) = s * vec4_x(q);
 52     vec3_y(axis) = s * vec4_y(q);
 53     vec3_z(axis) = s * vec4_z(q);
 54     return (rot3(theta, axis));
 55 }
 56 
 57 Vec4    quat_prod(Vec4 p, Vec4 q)
 58 {
 59     Vec4    prod = {Vec4_id};
 60     double  px = vec4_x(p), qx = vec4_x(q);
 61     double  py = vec4_y(p), qy = vec4_y(q);
 62     double  pz = vec4_z(p), qz = vec4_z(q);
 63     double  p0 = vec4_w(p), q0 = vec4_w(q);
 64 
 65     vec4_x(prod) = p0 * qx + px * q0 + py * qz - pz * qy;
 66     vec4_y(prod) = p0 * qy + py * q0 + pz * qx - px * qz;
 67     vec4_z(prod) = p0 * qz + pz * q0 + px * qy - py * qx;
 68     vec4_w(prod) = p0 * q0 - px * qx - py * qy - pz * qz;
 69 
 70     return (prod);
 71 }
 72 
 73 Vec4    quat_conj(Vec4 q)
 74 {
 75     Vec4    conj = {Vec4_id};
 76 
 77     vec4_x(conj) = -vec4_x(q);
 78     vec4_y(conj) = -vec4_y(q);
 79     vec4_z(conj) = -vec4_z(q);
 80     vec4_w(conj) = vec4_w(q);
 81     return (conj);
 82 }
 83 
 84 Vec4    quat_inverse(Vec4 q)
 85 {
 86     double  mod2 = vec4_sqrmod(q);
 87 
 88     return (vec4_times(1.0 / mod2, quat_conj(q)));
 89 }
 90 
 91 Vec3    vec3_rot_quat(Vec4 q, Vec3 v)
 92 {
 93     Vec4    p = {Vec4_id};
 94 
 95     p = quat_vec3(v);
 96     p = quat_prod(q, p);
 97     p = quat_prod(p, quat_inverse(q));
 98     return (vec3_quat(p));
 99 }
100 

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

This page was automatically generated by the LXR engine.
Visit the LXR main site for more information.