1 /*
2  *_________________________________________________________________________*
3  *      POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE     *
4  *      DESCRIPTION: SEE READ-ME                                           *
5  *      FILE NAME: eulerparameters.cpp                                     *
6  *      AUTHORS: See Author List                                           *
7  *      GRANTS: See Grants List                                            *
8  *      COPYRIGHT: (C) 2005 by Authors as listed in Author's List          *
9  *      LICENSE: Please see License Agreement                              *
10  *      DOWNLOAD: Free at www.rpi.edu/~anderk5                             *
11  *      ADMINISTRATOR: Prof. Kurt Anderson                                 *
12  *                     Computational Dynamics Lab                          *
13  *                     Rensselaer Polytechnic Institute                    *
14  *                     110 8th St. Troy NY 12180                           *
15  *      CONTACT:        anderk5@rpi.edu                                    *
16  *_________________________________________________________________________*/
17 
18 #include "eulerparameters.h"
19 #include "math.h"
20 
21 using namespace std;
22 
EP_Derivatives(ColMatrix & q,ColMatrix & u,ColMatrix & qdot)23 void EP_Derivatives(ColMatrix& q, ColMatrix& u, ColMatrix& qdot){
24   EP_Normalize(q);
25   int num=u.GetNumRows();
26   if (3<num){
27 	  for (int i=4; i<=num; i++){
28 		  qdot.elements[i]=u.elements[i-1];
29 	  }
30   }
31 
32   qdot.elements[0] = 0.5 *(q.elements[3]*u.elements[0] - q.elements[2]*u.elements[1] + q.elements[1]*u.elements[2]);
33   qdot.elements[1] = 0.5 *(q.elements[2]*u.elements[0] + q.elements[3]*u.elements[1] - q.elements[0]*u.elements[2]);
34   qdot.elements[2] = 0.5 *(-q.elements[1]*u.elements[0] + q.elements[0]*u.elements[1] + q.elements[3]*u.elements[2]);
35   qdot.elements[3] = -0.5 *(q.elements[0]*u.elements[0] + q.elements[1]*u.elements[1] + q.elements[2]*u.elements[2]);
36 
37   }
38 
EP_Transformation(ColMatrix & q,Mat3x3 & C)39 void EP_Transformation(ColMatrix& q, Mat3x3& C){
40   EP_Normalize(q);
41 
42   double q11 = q.elements[0]*q.elements[0];
43   double q22 = q.elements[1]*q.elements[1];
44   double q33 = q.elements[2]*q.elements[2];
45   double q44 = q.elements[3]*q.elements[3];
46 
47   double q12 = q.elements[0]*q.elements[1];
48   double q13 = q.elements[0]*q.elements[2];
49   double q14 = q.elements[0]*q.elements[3];
50   double q23 = q.elements[1]*q.elements[2];
51   double q24 = q.elements[1]*q.elements[3];
52   double q34 = q.elements[2]*q.elements[3];
53 
54   C.elements[0][0] = q11 - q22 - q33 + q44;
55   C.elements[1][1] = -q11 + q22 - q33 + q44;
56   C.elements[2][2] = -q11 - q22 + q33 + q44;
57 
58   C.elements[0][1] = 2*(q12 - q34);
59   C.elements[1][0] = 2*(q12 + q34);
60 
61   C.elements[0][2] = 2*(q13 + q24);
62   C.elements[2][0] = 2*(q13 - q24);
63 
64   C.elements[1][2] = 2*(q23 - q14);
65   C.elements[2][1] = 2*(q23 + q14);
66 }
67 
EP_FromTransformation(ColMatrix & q,Mat3x3 & C)68 void EP_FromTransformation(ColMatrix& q, Mat3x3& C){
69   double b[4];
70 
71   // condition indicators
72   b[0] = C.elements[0][0] - C.elements[1][1] - C.elements[2][2] + 1;
73   b[1] = -C.elements[0][0] + C.elements[1][1] - C.elements[2][2] + 1;
74   b[2] = -C.elements[0][0] - C.elements[1][1] + C.elements[2][2] + 1;
75   b[3] = C.elements[0][0] + C.elements[1][1] + C.elements[2][2] + 1;
76 
77   int max = 0;
78   for(int i=1;i<4;i++){
79     if( b[i] > b[max] ) max = i;
80   }
81 
82   if( max == 3 ){
83     q.elements[3] = 0.5 * sqrt( b[3] );
84     q.elements[0] = ( C.elements[2][1] - C.elements[1][2] ) / ( 4 * q.elements[3] );
85     q.elements[1] = ( C.elements[0][2] - C.elements[2][0] ) / ( 4 * q.elements[3] );
86     q.elements[2] = ( C.elements[1][0] - C.elements[0][1] ) / ( 4 * q.elements[3] );
87     return;
88   }
89 
90   if( max == 0 ){
91     q.elements[0] = 0.5 * sqrt( b[0] );
92     q.elements[1] = ( C.elements[0][1] + C.elements[1][0] ) / ( 4 * q.elements[0] );
93     q.elements[2] = ( C.elements[0][2] + C.elements[2][0] ) / ( 4 * q.elements[0] );
94     q.elements[3] = ( C.elements[2][1] - C.elements[1][2] ) / ( 4 * q.elements[0] );
95     return;
96   }
97 
98   if( max == 1 ){
99     q.elements[1] = 0.5 * sqrt( b[1] );
100     q.elements[0] = ( C.elements[0][1] + C.elements[1][0] ) / ( 4 * q.elements[1] );
101     q.elements[2] = ( C.elements[1][2] + C.elements[2][1] ) / ( 4 * q.elements[1] );
102     q.elements[3] = ( C.elements[0][2] - C.elements[2][0] ) / ( 4 * q.elements[1] );
103     return;
104   }
105 
106   if( max == 2 ){
107     q.elements[2] = 0.5 * sqrt( b[2] );
108     q.elements[0] = ( C.elements[0][2] + C.elements[2][0] ) / ( 4 * q.elements[2] );
109     q.elements[1] = ( C.elements[1][2] + C.elements[2][1] ) / ( 4 * q.elements[2] );
110     q.elements[3] = ( C.elements[1][0] - C.elements[0][1] ) / ( 4 * q.elements[2] );
111     return;
112   }
113   EP_Normalize(q);
114 }
115 
EP_Normalize(ColMatrix & q)116 void EP_Normalize(ColMatrix& q){
117   double one = 1.0/sqrt(q.elements[0]*q.elements[0] + q.elements[1]*q.elements[1] + q.elements[2]*q.elements[2] + q.elements[3]*q.elements[3]);
118   q.elements[0] = one*q.elements[0];
119   q.elements[1] = one*q.elements[1];
120   q.elements[2] = one*q.elements[2];
121   q.elements[3] = one*q.elements[3];
122 }
123 
124 
125 
126 
EPdotdot_udot(ColMatrix & Audot,ColMatrix & Aqdot,ColMatrix & Aq,ColMatrix & Aqddot)127 void EPdotdot_udot(ColMatrix& Audot, ColMatrix& Aqdot, ColMatrix& Aq, ColMatrix& Aqddot){
128   int num=Audot.GetNumRows();
129   if (3<num){
130 	  for (int i=4; i<=num; i++){
131 		  Aqddot.elements[i]=Audot.elements[i-1];
132 	  }
133   }
134 
135   double AA;
136   AA=Aqdot.elements[0]*Aqdot.elements[0]+Aqdot.elements[1]*Aqdot.elements[1]+Aqdot.elements[2]*Aqdot.elements[2]+Aqdot.elements[3]*Aqdot.elements[3];
137 
138   Aqddot.elements[0] = 0.5 *(Aq.elements[3]*Audot.elements[0] - Aq.elements[2]*Audot.elements[1] + Aq.elements[1]*Audot.elements[2]-2*Aq.elements[0]*AA);
139 
140   Aqddot.elements[1] = 0.5 *(Aq.elements[2]*Audot.elements[0] + Aq.elements[3]*Audot.elements[1] - Aq.elements[0]*Audot.elements[2]-2*Aq.elements[1]*AA);
141 
142   Aqddot.elements[2] = 0.5 *(-Aq.elements[1]*Audot.elements[0] + Aq.elements[0]*Audot.elements[1] + Aq.elements[3]*Audot.elements[2]-2*Aq.elements[2]*AA);
143 
144   Aqddot.elements[3] = -0.5 *(Aq.elements[0]*Audot.elements[0] + Aq.elements[1]*Audot.elements[1] + Aq.elements[2]*Audot.elements[2]+2*Aq.elements[3]*AA);
145 
146 }
147 
148 
qdot_to_u(ColMatrix & q,ColMatrix & u,ColMatrix & qdot)149 void qdot_to_u(ColMatrix& q, ColMatrix& u, ColMatrix& qdot){
150 	EP_Normalize(q);
151 	int num=qdot.GetNumRows();
152       	if (4<num){
153       		for (int i=5; i<=num; i++){
154       			u.elements[i-2]=qdot.elements[i-1];
155       		}
156       	}
157       	u.elements[0]=2*(q.elements[3]*qdot.elements[0]+q.elements[2]*qdot.elements[1]-q.elements[1]*qdot.elements[2]-q.elements[0]*qdot.elements[3]);
158       	u.elements[1]=2*(-q.elements[2]*qdot.elements[0]+q.elements[3]*qdot.elements[1]+q.elements[0]*qdot.elements[2]-q.elements[1]*qdot.elements[3]);
159       	u.elements[2]=2*(q.elements[1]*qdot.elements[0]-q.elements[0]*qdot.elements[1]+q.elements[3]*qdot.elements[2]-q.elements[2]*qdot.elements[3]);
160 
161 }
162 
163 
164