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

Linux Cross Reference
Tina5/tina-libs/tina/math/mathTran_euler.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    :  
 37  * Date    :  
 38  * Version :  
 39  * CVS Id  :  
 40  *
 41  * Author  :  paul.bromiley@man.ac.uk
 42  *
 43  * Notes :    Conversion between rotation matrices and Euler angles
 44  *
 45  *********
 46 */
 47 /**
 48  * @file  
 49  * @brief Routines to process and convert Euler angle representations of rotations
 50  * 
 51  * Two sets of functions: conv_euler_to_rot and conv_rot_to_euler convert
 52  * between mat3 rotation matrices and Euler angle representations. The functions 
 53  * with _pab appended to the name do the same maths, but take double vectors containing 
 54  * the rows of the rotation matrices as arguments to increase the accuracy.
 55  * 
 56  *********
 57 */
 58 
 59 #include "mathTran_euler.h"
 60 
 61 #if HAVE_CONFIG_H
 62 #include <config.h>
 63 #endif
 64 
 65 
 66 #include <math.h>
 67 #include <tina/sys/sysDef.h>
 68 #include <tina/math/math_GenDef.h>
 69 #include <tina/math/math_GeomDef.h>
 70 
 71 
 72 void conv_rot_to_euler(Mat3 *mat, double *euler)
 73 {
 74         double c, trx, try;
 75 
 76         euler[1] = -asin(mat->el[0][2]);
 77         c = cos(euler[1]);
 78         euler[1] = euler[1]*RADIAN;
 79 
 80         if(fabs(c)>0.0005) /* Has gimbal lock occured? */
 81         {
 82                 trx = mat->el[2][2]/c;
 83                 try = -mat->el[1][2]/c;
 84                 euler[0] = atan2(try, trx)*RADIAN;
 85                 trx = mat->el[0][0]/c;
 86                 try = -mat->el[0][1]/c;
 87                 euler[2] = atan2(try, trx)*RADIAN;
 88         }
 89         else
 90         {
 91                 euler[0] = 0.0;
 92                 trx = mat->el[1][1];
 93                 try = mat->el[1][0];
 94                 euler[2] = atan2(try, trx)*RADIAN;
 95         }
 96 }
 97 
 98 void conv_euler_to_rot(double *euler, Mat3 *mat)
 99 {
100         double a, b, c, d, e, f, ad, bd;
101 
102         a = cos(euler[0]/RADIAN);
103         b = sin(euler[0]/RADIAN);
104         c = cos(euler[1]/RADIAN);
105         d = sin(euler[1]/RADIAN);
106         e = cos(euler[2]/RADIAN);
107         f = sin(euler[2]/RADIAN);
108         ad = a*d;
109         bd = b*d;
110 
111         mat->el[0][0] = c*e;
112         mat->el[0][1] = -c*f;
113         mat->el[0][2] = -d;
114         mat->el[1][0] = -bd*e+a*f;
115         mat->el[1][1] = bd*f+a*e;
116         mat->el[1][2] = -b*c;
117         mat->el[2][0] = ad*e+b*f;
118         mat->el[2][1] = -ad*f+b*e;
119         mat->el[2][2] = a*c;
120 }
121 
122 void conv_rot_to_euler_pab(double *ex, double *ey, double *ez, double *euler)
123 {
124         double c, trx, try;
125 
126         euler[1] = -asin(ex[2]);
127         c = cos(euler[1]);
128         euler[1] = euler[1]*RADIAN;
129 
130         if(fabs(c)>0.0005) /* Has gimbal lock occured? */
131         {
132                 trx = ez[2]/c;
133                 try = -ey[2]/c;
134                 euler[0] = atan2(try, trx)*RADIAN;
135                 trx = ex[0]/c;
136                 try = -ex[1]/c;
137                 euler[2] = atan2(try, trx)*RADIAN;
138         }
139         else
140         {
141                 euler[0] = 0.0;
142                 trx = ey[1];
143                 try = ey[0];
144                 euler[2] = atan2(try, trx)*RADIAN;
145         }
146 }
147 
148 void conv_euler_to_rot_pab(double *euler, double *ex, double *ey, double *ez)
149 {
150         double a, b, c, d, e, f, ad, bd;
151 
152         a = cos(euler[0]/RADIAN);
153         b = sin(euler[0]/RADIAN);
154         c = cos(euler[1]/RADIAN);
155         d = sin(euler[1]/RADIAN);
156         e = cos(euler[2]/RADIAN);
157         f = sin(euler[2]/RADIAN);
158         ad = a*d;
159         bd = b*d;
160 
161         ex[0] = c*e;
162         ex[1] = -c*f;
163         ex[2] = -d;
164         ey[0] = -bd*e+a*f;
165         ey[1] = bd*f+a*e;
166         ey[2] = -b*c;
167         ez[0] = ad*e+b*f;
168         ez[1] = -ad*f+b*e;
169         ez[2] = a*c;
170 }
171 

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