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