ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/group/trunk/OOPSE/libmdtools/Mat3x3d.cpp
Revision: 1254
Committed: Wed Jun 9 16:16:33 2004 UTC (20 years, 1 month ago) by tim
File size: 6986 byte(s)
Log Message: 
1. adding some useful math classes(Mat3x3d, Vector3d, Quaternion, Euler3)
 these classes use anonymous union and struct to support
 double[3], double[3][3] and double[4]
2. adding roll constraint algorithm

File Contents

# Content
1 #include <cmath>
2 #include "Mat3x3d.hpp"
3 #include "Vector3d.hpp"
4 #include "Quaternion.hpp"
5 #include "Euler3.hpp"
6
7 Mat3x3d::Mat3x3d(const Vector3d& v1, const Vector3d& v2, const Vector3d& v3){
8 element[0][0] = v1.x;
9 element[0][1] = v1.y;
10 element[0][2] = v1.z;
11
12 element[1][0] = v2.x;
13 element[1][1] = v2.y;
14 element[1][2] = v2.z;
15
16 element[2][0] = v3.x;
17 element[2][1] = v3.y;
18 element[2][2] = v3.z;
19 }
20 Mat3x3d::Mat3x3d(const Quaternion& q){
21
22 double q0Sqr;
23 double q1Sqr;
24 double q2Sqr;
25 double q3Sqr;
26
27 q0Sqr = q.quat[0] * q.quat[0];
28 q1Sqr = q.quat[1] * q.quat[1];
29 q2Sqr = q.quat[2] * q.quat[2];
30 q3Sqr = q.quat[3] * q.quat[3];
31
32
33 element[0][0]= q0Sqr + q1Sqr - q2Sqr - q3Sqr;
34 element[0][1] = 2.0 * ( q.quat[1] * q.quat[2] + q.quat[0] * q.quat[3] );
35 element[0][2] = 2.0 * ( q.quat[1] * q.quat[3] - q.quat[0] * q.quat[2] );
36
37 element[1][0] = 2.0 * ( q.quat[1] * q.quat[2] - q.quat[0] * q.quat[3] );
38 element[1][1] = q0Sqr - q1Sqr + q2Sqr - q3Sqr;
39 element[1][2] = 2.0 * ( q.quat[2] * q.quat[3] + q.quat[0] * q.quat[1] );
40
41 element[2][0] = 2.0 * ( q.quat[1] * q.quat[3] + q.quat[0] * q.quat[2] );
42 element[2][1] = 2.0 * ( q.quat[2] * q.quat[3] - q.quat[0] * q.quat[1] );
43 element[2][2] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;
44
45 }
46
47 Mat3x3d::Mat3x3d(const Euler3& e){
48 double sinTheta;
49 double sinPhi;
50 double sinPsi;
51 double cosTheta;
52 double cosPhi;
53 double cosPsi;
54
55 sinTheta = sin(e.theta);
56 sinPhi = sin(e.phi);
57 sinPsi = sin(e.psi);
58
59 cosTheta = cos(e.theta);
60 cosPhi = cos(e.phi);
61 cosPsi = cos(e.psi);
62
63 element[0][0] = (cosPhi * cosPsi) - (sinPhi * cosTheta * sinPsi);
64 element[0][1] = (sinPhi * cosPsi) + (cosPhi * cosTheta * sinPsi);
65 element[0][2] = sinTheta * sinPsi;
66
67 element[1][0] = -(cosPhi * sinPsi) - (sinPhi * cosTheta * cosPsi);
68 element[1][1] = -(sinPhi * sinPsi) + (cosPhi * cosTheta * cosPsi);
69 element[1][2] = sinTheta * cosPsi;
70
71 element[2][0] = sinPhi * sinTheta;
72 element[2][1] = -cosPhi * sinTheta;
73 element[2][2] = cosTheta;
74 }
75
76 Mat3x3d Mat3x3d::inverse() const{
77
78 Mat3x3d invMat;
79
80 double determinant = det();
81
82 invMat.element[0][0] = element[1][1]*element[2][2] - element[1][2]*element[2][1];
83 invMat.element[1][0] = element[1][2]*element[2][0] - element[1][0]*element[2][2];
84 invMat.element[2][0] = element[1][0]*element[2][1] - element[1][1]*element[2][0];
85 invMat.element[0][1] = element[2][1]*element[0][2] - element[2][2]*element[0][1];
86 invMat.element[1][1] = element[2][2]*element[0][0] - element[2][0]*element[0][2];
87 invMat.element[2][1] = element[2][0]*element[0][1] - element[2][1]*element[0][0];
88 invMat.element[0][2] = element[0][1]*element[1][2] - element[0][2]*element[1][1];
89 invMat.element[1][2] = element[0][2]*element[1][0] - element[0][0]*element[1][2];
90 invMat.element[2][2] = element[0][0]*element[1][1] - element[0][1]*element[1][0];
91
92 invMat /= determinant;
93
94 return(invMat);
95 }
96
97 Mat3x3d Mat3x3d::transpose(void) const{
98 Mat3x3d transposeMat;
99
100 for(unsigned int i=0; i<3; i++)
101 for(unsigned int j=0; j<3; j++)
102 transposeMat.element[i][j] = element[j][i];
103
104 return(transposeMat);
105
106 }
107
108 double Mat3x3d::det() const{
109 double x;
110 double y;
111 double z;
112
113 x = element[0][0] * (element[1][1] * element[2][2] - element[1][2] * element[2][1]);
114 y = element[0][1] * (element[1][2] * element[2][0] - element[1][0] * element[2][2]);
115 z = element[0][2] * (element[1][0] * element[2][1] - element[1][1] * element[2][0]);
116
117 return(x + y + z);
118 }
119
120 void Mat3x3d::diagonalize(Vector3d& v, Mat3x3d& m){
121 diagonalize(v.vec, m.element);
122 }
123
124 void Mat3x3d::diagonalize(Vector3d& v, double m[3][3]){
125 diagonalize(v.vec, m);
126 }
127
128 void Mat3x3d::diagonalize(double v[3], Mat3x3d& m){
129 diagonalize(v, m.element);
130 }
131
132 void Mat3x3d::diagonalize(double v[3], double m[3][3]){
133
134 }
135
136 Quaternion Mat3x3d::toQuaternion(){
137 Quaternion q;
138 double t, s;
139 double ad1, ad2, ad3;
140
141 t = element[0][0] + element[1][1] + element[2][2] + 1.0;
142 if( t > 0.0 ){
143
144 s = 0.5 / sqrt( t );
145 q.quat[0] = 0.25 / s;
146 q.quat[1] = (element[1][2] - element[2][1]) * s;
147 q.quat[2] = (element[2][0] - element[0][2]) * s;
148 q.quat[3] = (element[0][1] - element[1][0]) * s;
149 }
150 else{
151
152 ad1 = fabs( element[0][0] );
153 ad2 = fabs( element[1][1] );
154 ad3 = fabs( element[2][2] );
155
156 if( ad1 >= ad2 && ad1 >= ad3 ){
157 s = 2.0 * sqrt( 1.0 + element[0][0] - element[1][1] - element[2][2] );
158 q.quat[0] = (element[1][2] + element[2][1]) / s;
159 q.quat[1] = 0.5 / s;
160 q.quat[2] = (element[0][1] + element[1][0]) / s;
161 q.quat[3] = (element[0][2] + element[2][0]) / s;
162 }
163 else if( ad2 >= ad1 && ad2 >= ad3 ){
164 s = sqrt( 1.0 + element[1][1] - element[0][0] - element[2][2] ) * 2.0;
165 q.quat[0] = (element[0][2] + element[2][0]) / s;
166 q.quat[1] = (element[0][1] + element[1][0]) / s;
167 q.quat[2] = 0.5 / s;
168 q.quat[3] = (element[1][2] + element[2][1]) / s;
169 }
170 else{
171 s = sqrt( 1.0 + element[2][2] - element[0][0] - element[1][1] ) * 2.0;
172 q.quat[0] = (element[0][1] + element[1][0]) / s;
173 q.quat[1] = (element[0][2] + element[2][0]) / s;
174 q.quat[2] = (element[1][2] + element[2][1]) / s;
175 q.quat[3] = 0.5 / s;
176 }
177 }
178 return q;
179 }
180
181 Euler3 Mat3x3d::toEuler(){
182 // We use so-called "x-convention", which is the most common definition.
183 // In this convention, the rotation given by Euler angles (phi, theta, psi), where the first
184 // rotation is by an angle phi about the z-axis, the second is by an angle
185 // theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the
186 //z-axis (again).
187
188 Euler3 e;
189 Mat3x3d m;
190 double cosTheta;
191 double sinTheta;
192 const double eps = 1.0e-8;
193 // set the tolerance for Euler angles and rotation elements
194
195 e.theta = acos(min(1.0,max(-1.0, element[2][2])));
196 cosTheta = element[2][2];
197 sinTheta = sqrt(1.0 - cosTheta * cosTheta);
198
199 // when sin(theta) is close to 0, we need to consider singularity
200 // In this case, we can assign an arbitary value to phi (or psi), and then determine
201 // the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0
202 // in cases of singularity.
203 // we use atan2 instead of atan, since atan2 will give us -Pi to Pi.
204 // Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never
205 // change the sign of both of the parameters passed to atan2.
206
207 if (fabs(sinTheta) <= eps){
208 e.psi = 0.0;
209 //e.phi = atan2(-Amat[Ayx], Amat[Axx]);
210 e.phi = atan2(-element[1][0], element[0][0]);
211 }
212 // we only have one unique solution
213 else{
214 //e.phi = atan2(Amat[Azx], -Amat[Azy]);
215 //e.psi = atan2(Amat[Axz], Amat[Ayz]);
216 e.phi = atan2(element[2][0], -element[2][1]);
217 e.psi = atan2(element[0][2], -element[1][2]);
218 }
219
220 //wrap phi and psi, make sure they are in the range from 0 to 2*Pi
221 //if (phi < 0)
222 // phi += M_PI;
223
224 //if (psi < 0)
225 // psi += M_PI
226
227 return e;
228 }

Properties

Name Value
svn:executable *