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, 3 months 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

# User Rev Content
1 tim 1254 #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 *