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

Linux Cross Reference
Tina5/tina-libs/tina/math/mathTran_quat.c

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

  1 /**********
  2  *
  3  * Copyright (c) 2003, Division of Imaging Science and Biomedical Engineering,
  4  * University of Manchester, UK.  All rights reserved.
  5  * 
  6  * Redistribution and use in source and binary forms, with or without modification, 
  7  * are permitted provided that the following conditions are met:
  8  * 
  9  *   . Redistributions of source code must retain the above copyright notice, 
 10  *     this list of conditions and the following disclaimer.
 11  *    
 12  *   . Redistributions in binary form must reproduce the above copyright notice,
 13  *     this list of conditions and the following disclaimer in the documentation 
 14  *     and/or other materials provided with the distribution.
 15  * 
 16  *   . Neither the name of the University of Manchester nor the names of its
 17  *     contributors may be used to endorse or promote products derived from this 
 18  *     software without specific prior written permission.
 19  * 
 20  * 
 21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 24  * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
 25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 31  * POSSIBILITY OF SUCH DAMAGE.
 32  *
 33  **********
 34  *
 35  * Program :    TINA
 36  * File    :  $Source: /home/tina/cvs/tina-libs/tina/math/mathTran_quat.c,v $
 37  * Date    :  $Date: 2005/01/23 19:10:21 $
 38  * Version :  $Revision: 1.6 $
 39  * CVS Id  :  $Id: mathTran_quat.c,v 1.6 2005/01/23 19:10:21 paul Exp $
 40  *
 41  * Author  :  Legacy TINA
 42  *
 43  * Notes : Quaternion handling
 44  *
 45  *********
 46 */
 47 /**
 48  * @file  
 49  * @brief Routines to process and convert quaternions 
 50  *  
 51  * The conv_quat_to_rot function and its inverse do not fully handle the angle of rotation: 
 52  * they extract the angle from the normalisation of the other elements of the 
 53  * quaternion, thus losing its sign and limiting the rotation to a 180 degree hemisphere. 
 54  * The new versions with _pab appended fix this, but beware that the angle parameter needs to get
 55  * passed around properly in higher level functions. They also take double vectors rather than mat3's 
 56  * (which are built from floats) as their arguments for accuracy.  PAB 20/2/2003 
 57  *
 58  * Here is a brief haiku, in the style of Mr. Biffo, on the old quaternion ocnversion routines:
 59  * 
 60  * Oh man! Messed up
 61  * quaternion representation
 62  * encodes major badness.
 63  * 
 64  *********
 65 */
 66 
 67 #include "mathTran_quat.h"
 68 
 69 #if HAVE_CONFIG_H
 70 #include <config.h>
 71 #endif
 72 
 73 
 74 #include <math.h>
 75 #include <tina/sys/sysDef.h>
 76 #include <tina/math/math_GeomDef.h>
 77 #include <tina/math/math_GeomPro.h>
 78 #include <tina/math/mathTran_rot3.h>
 79 
 80 Vec3            vec3_quat(Vec4 q)
 81 {
 82         Vec3            v = {Vec3_id};
 83 
 84         vec3_x(v) = vec4_x(q);
 85         vec3_y(v) = vec4_y(q);
 86         vec3_z(v) = vec4_z(q);
 87         return (v);
 88 }
 89 
 90 Vec4            quat_vec3(Vec3 v)
 91 {
 92         Vec4            q = {Vec4_id};
 93 
 94         vec4_x(q) = vec3_x(v);
 95         vec4_y(q) = vec3_y(v);
 96         vec4_z(q) = vec3_z(v);
 97         vec4_w(q) = 0.0;
 98         return (q);
 99 }
100 
101 Vec4            quat_rot3(Mat3 r)
102 {
103         double          theta;
104         Vec3            axis = {Vec3_id};
105         Vec4            q = {Vec4_id};
106 
107         rot3_angle_axis(r, &theta, &axis);
108         q = quat_vec3(vec3_times(sin(0.5 * theta), axis));
109         vec4_w(q) = cos(0.5 * theta);
110         return (q);
111 }
112 
113 Mat3            rot3_quat(Vec4 q)
114 {
115         double          theta, s;
116         Vec3            axis = {Vec3_id};
117 
118         theta = 2.0 * acos(vec4_w(q));
119         s = sin(0.5 * theta);
120         if (s == 0.0)
121                 return (mat3_unit());
122         s = 1.0 / s;
123         vec3_x(axis) = s * vec4_x(q);
124         vec3_y(axis) = s * vec4_y(q);
125         vec3_z(axis) = s * vec4_z(q);
126         return (rot3(theta, axis));
127 }
128 
129 Vec4            quat_prod(Vec4 p, Vec4 q)
130 {
131         Vec4            prod = {Vec4_id};
132         double          px = vec4_x(p), qx = vec4_x(q);
133         double          py = vec4_y(p), qy = vec4_y(q);
134         double          pz = vec4_z(p), qz = vec4_z(q);
135         double          p0 = vec4_w(p), q0 = vec4_w(q);
136 
137         vec4_x(prod) = p0 * qx + px * q0 + py * qz - pz * qy;
138         vec4_y(prod) = p0 * qy + py * q0 + pz * qx - px * qz;
139         vec4_z(prod) = p0 * qz + pz * q0 + px * qy - py * qx;
140         vec4_w(prod) = p0 * q0 - px * qx - py * qy - pz * qz;
141 
142         return (prod);
143 }
144 
145 Vec4            quat_conj(Vec4 q)
146 {
147         Vec4            conj = {Vec4_id};
148 
149         vec4_x(conj) = -vec4_x(q);
150         vec4_y(conj) = -vec4_y(q);
151         vec4_z(conj) = -vec4_z(q);
152         vec4_w(conj) = vec4_w(q);
153         return (conj);
154 }
155 
156 Vec4            quat_inverse(Vec4 q)
157 {
158         double          mod2 = vec4_sqrmod(q);
159 
160         return (vec4_times(1.0 / mod2, quat_conj(q)));
161 }
162 
163 Vec3            vec3_rot_quat(Vec4 q, Vec3 v)
164 {
165         Vec4            p = {Vec4_id};
166 
167         p = quat_vec3(v);
168         p = quat_prod(q, p);
169         p = quat_prod(p, quat_inverse(q));
170         return (vec3_quat(p));
171 }
172 
173 
174 void            conv_rot_to_quat(Mat3 * mat, double *q)
175 {
176         q[0] = 0.5 * sqrt(mat->el[0][0] + mat->el[1][1] + mat->el[2][2] + 1.0);
177         if (q[0] > 1.0)
178         {
179                 q[0] = 1.0;
180                 q[1] = 0.0;
181                 q[2] = 0.0;
182                 q[3] = 0.0;
183         } else if (q[0] == 0.0)
184         {
185                 q[1] = 1.0;
186                 q[2] = 0.0;
187                 q[3] = 0.0;
188         } else
189         {
190                 q[1] = (mat->el[2][1] - mat->el[1][2]) / (4.0 * q[0]);
191                 q[2] = (mat->el[0][2] - mat->el[2][0]) / (4.0 * q[0]);
192                 q[3] = (mat->el[1][0] - mat->el[0][1]) / (4.0 * q[0]);
193         }
194 }
195 
196 void            conv_quat_to_rot(double *q, Mat3 * mat)
197 {
198         mat->el[0][0] = (float) (q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
199         mat->el[1][0] = (float) (2.0 * (q[1] * q[2] + q[0] * q[3]));
200         mat->el[2][0] = (float) (2.0 * (q[1] * q[3] - q[0] * q[2]));
201         mat->el[0][1] = (float) (2.0 * (q[1] * q[2] - q[0] * q[3]));
202         mat->el[1][1] = (float) (q[0] * q[0] - q[1] * q[1] + q[2] * q[2] - q[3] * q[3]);
203         mat->el[2][1] = (float) (2.0 * (q[2] * q[3] + q[0] * q[1]));
204         mat->el[0][2] = (float) (2.0 * (q[1] * q[3] + q[0] * q[2]));
205         mat->el[1][2] = (float) (2.0 * (q[2] * q[3] - q[0] * q[1]));
206         mat->el[2][2] = (float) (q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
207 }
208 
209 
210 void    conv_rot_to_quat_pab(double *ex, double *ey, double *ez, double *q)
211 {
212     double t, s, tot;
213     int i;
214 
215     t = (ex[0] + ey[1] + ez[2] + 1.0);
216     
217     if(t>0)
218     {
219         s = 0.5/sqrt(t);
220         q[0] = 0.25/s;
221         q[1] = s*(ez[1] - ey[2]);
222         q[2] = s*(ex[2] - ez[0]);
223         q[3] = s*(ey[0] - ex[1]);
224     }
225     else
226     {
227         if((ex[0]>ey[1])&&(ex[0]>ez[2]))
228         {
229             s = 2*sqrt(1.0 + ex[0] - ey[1] - ez[2]);
230             q[0] = (ey[2] + ez[1])/s;
231             q[1] = 0.5/s;
232             q[2] = (ex[1] + ey[0])/s;
233             q[3] = (ex[2] + ez[0])/s;
234         }
235         else if(ey[1]>ez[2])
236         {
237             s = 2*sqrt(1.0 - ex[0] + ey[1] - ez[2]);
238             q[0] = (ex[2] + ez[0])/s;
239             q[1] = (ey[0] + ex[1])/s;
240             q[2] = 0.5/s;
241             q[3] = (ey[2] + ez[1])/s;
242         }
243         else
244         {
245             s = 2*sqrt(1.0 - ex[0] - ey[1] + ez[2]);
246             q[0] = (ex[1] + ey[0])/s;
247             q[1] = (ey[2] + ez[1])/s;
248             q[2] = (ex[2] + ez[0])/s;
249             q[3] = 0.5/s;
250         }
251     }
252 
253 /* Finally, normalise the quaternion */
254 
255     tot = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
256     for(i=0; i<4; i++)
257     {
258         q[i] = q[i]/tot;
259     }
260 }
261 
262 
263 void    conv_quat_to_rot_pab(double *q, double *ex, double *ey, double *ez)
264 {
265     double tot;
266     int i;
267 
268 /* First, normalise the quaternion */
269 
270     tot = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
271     for(i=0; i<4; i++)
272     {
273         q[i] = q[i]/tot;
274     }
275 
276     ex[0] = (double)(1 - 2.0*(q[2] * q[2] + q[3] * q[3]));
277     ey[0] = (double)(2.0 * (q[1] * q[2] + q[0] * q[3]));
278     ez[0] = (double)(2.0 * (q[1] * q[3] - q[0] * q[2]));
279     ex[1] = (double)(2.0 * (q[1] * q[2] - q[0] * q[3]));
280     ey[1] = (double)(1 - 2.0*(q[1] * q[1] + q[3] * q[3]));
281     ez[1] = (double)(2.0 * (q[2] * q[3] + q[0] * q[1]));
282     ex[2] = (double)(2.0 * (q[1] * q[3] + q[0] * q[2]));
283     ey[2] = (double)(2.0 * (q[2] * q[3] - q[0] * q[1]));
284     ez[2] = (double)(1 - 2.0*(q[1] * q[1] + q[2] * q[2]));
285 }
286 

~ [ 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.