OOPSE/libmdtools/Mat3x3d.cpp

#include <cmath>
#include "Mat3x3d.hpp"
#include "Vector3d.hpp"
#include "Quaternion.hpp"
#include "Euler3.hpp"

Mat3x3d::Mat3x3d(const Vector3d& v1, const Vector3d& v2, const Vector3d& v3){
  element[0][0] = v1.x;
  element[0][1] = v1.y;
  element[0][2] = v1.z;
  
  element[1][0] = v2.x;
  element[1][1] = v2.y;
  element[1][2] = v2.z;

  element[2][0] = v3.x;
  element[2][1] = v3.y;
  element[2][2] = v3.z;
}
Mat3x3d::Mat3x3d(const Quaternion& q){

  double q0Sqr;
  double q1Sqr;
  double q2Sqr;
  double q3Sqr;

  q0Sqr = q.quat[0] * q.quat[0];
  q1Sqr = q.quat[1] * q.quat[1];
  q2Sqr = q.quat[2] * q.quat[2];
  q3Sqr = q.quat[3] * q.quat[3];
  
  
  element[0][0]= q0Sqr + q1Sqr - q2Sqr - q3Sqr;
  element[0][1] = 2.0 * ( q.quat[1] * q.quat[2] + q.quat[0] * q.quat[3] );
  element[0][2] = 2.0 * ( q.quat[1] * q.quat[3] - q.quat[0] * q.quat[2] );
  
  element[1][0] = 2.0 * ( q.quat[1] * q.quat[2] - q.quat[0] * q.quat[3] );
  element[1][1] = q0Sqr - q1Sqr + q2Sqr - q3Sqr;
  element[1][2] = 2.0 * ( q.quat[2] * q.quat[3] + q.quat[0] * q.quat[1] );
  
  element[2][0] = 2.0 * ( q.quat[1] * q.quat[3] + q.quat[0] * q.quat[2] );
  element[2][1] = 2.0 * ( q.quat[2] * q.quat[3] - q.quat[0] * q.quat[1] );
  element[2][2] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;
  
}

Mat3x3d::Mat3x3d(const Euler3& e){
  double sinTheta;
  double sinPhi;
  double sinPsi;
  double cosTheta;
  double cosPhi;
  double cosPsi;

  sinTheta = sin(e.theta);
  sinPhi = sin(e.phi);
  sinPsi = sin(e.psi);

  cosTheta = cos(e.theta);
  cosPhi = cos(e.phi);
  cosPsi = cos(e.psi);
  
  element[0][0] = (cosPhi * cosPsi) - (sinPhi * cosTheta * sinPsi);
  element[0][1] = (sinPhi * cosPsi) + (cosPhi * cosTheta * sinPsi);
  element[0][2] = sinTheta * sinPsi;
    
  element[1][0] = -(cosPhi * sinPsi) - (sinPhi * cosTheta * cosPsi);
  element[1][1] = -(sinPhi * sinPsi) + (cosPhi * cosTheta * cosPsi);
  element[1][2] = sinTheta * cosPsi;
  
  element[2][0] = sinPhi * sinTheta;
  element[2][1] = -cosPhi * sinTheta;
  element[2][2] = cosTheta;
}

Mat3x3d Mat3x3d::inverse() const{

  Mat3x3d invMat;
  
  double determinant = det();

  invMat.element[0][0] = element[1][1]*element[2][2] - element[1][2]*element[2][1];
  invMat.element[1][0] = element[1][2]*element[2][0] - element[1][0]*element[2][2];
  invMat.element[2][0] = element[1][0]*element[2][1] - element[1][1]*element[2][0];
  invMat.element[0][1] = element[2][1]*element[0][2] - element[2][2]*element[0][1];
  invMat.element[1][1] = element[2][2]*element[0][0] - element[2][0]*element[0][2];
  invMat.element[2][1] = element[2][0]*element[0][1] - element[2][1]*element[0][0];
  invMat.element[0][2] = element[0][1]*element[1][2] - element[0][2]*element[1][1];
  invMat.element[1][2] = element[0][2]*element[1][0] - element[0][0]*element[1][2];
  invMat.element[2][2] = element[0][0]*element[1][1] - element[0][1]*element[1][0];
  
  invMat /= determinant;
  
  return(invMat);
}

Mat3x3d Mat3x3d::transpose(void) const{
  Mat3x3d transposeMat;

  for(unsigned int i=0; i<3; i++)
    for(unsigned int j=0; j<3; j++)
      transposeMat.element[i][j] = element[j][i];
  
  return(transposeMat);

}

double Mat3x3d::det() const{
  double x;
  double y;
  double z;

  x = element[0][0] * (element[1][1] * element[2][2] - element[1][2] * element[2][1]);
  y = element[0][1] * (element[1][2] * element[2][0] - element[1][0] * element[2][2]);
  z = element[0][2] * (element[1][0] * element[2][1] - element[1][1] * element[2][0]);

  return(x + y + z);
}

void Mat3x3d::diagonalize(Vector3d& v, Mat3x3d& m){
  diagonalize(v.vec, m.element);
}

void Mat3x3d::diagonalize(Vector3d& v, double m[3][3]){
  diagonalize(v.vec, m);
}

void Mat3x3d::diagonalize(double v[3], Mat3x3d& m){
  diagonalize(v, m.element);
}

void Mat3x3d::diagonalize(double v[3], double m[3][3]){
  
}

Quaternion Mat3x3d::toQuaternion(){
  Quaternion q;
  double t, s;
  double ad1, ad2, ad3;
    
  t = element[0][0] + element[1][1] + element[2][2] + 1.0;
  if( t > 0.0 ){
    
    s = 0.5 / sqrt( t );
    q.quat[0] = 0.25 / s;
    q.quat[1] = (element[1][2] - element[2][1]) * s;
    q.quat[2] = (element[2][0] - element[0][2]) * s;
    q.quat[3] = (element[0][1] - element[1][0]) * s;
  }
  else{
    
    ad1 = fabs( element[0][0] );
    ad2 = fabs( element[1][1] );
    ad3 = fabs( element[2][2] );
    
    if( ad1 >= ad2 && ad1 >= ad3 ){     
        s = 2.0 * sqrt( 1.0 + element[0][0] - element[1][1] - element[2][2] );
        q.quat[0] = (element[1][2] + element[2][1]) / s;
        q.quat[1] = 0.5 / s;
        q.quat[2] = (element[0][1] + element[1][0]) / s;
        q.quat[3] = (element[0][2] + element[2][0]) / s;
    }
    else if( ad2 >= ad1 && ad2 >= ad3 ){                
      s = sqrt( 1.0 + element[1][1] - element[0][0] - element[2][2] ) * 2.0;
      q.quat[0] = (element[0][2] + element[2][0]) / s;
      q.quat[1] = (element[0][1] + element[1][0]) / s;
      q.quat[2] = 0.5 / s;
      q.quat[3] = (element[1][2] + element[2][1]) / s;
    }
    else{
      s = sqrt( 1.0 + element[2][2] - element[0][0] - element[1][1] ) * 2.0;
      q.quat[0] = (element[0][1] + element[1][0]) / s;
      q.quat[1] = (element[0][2] + element[2][0]) / s;
      q.quat[2] = (element[1][2] + element[2][1]) / s;
      q.quat[3] = 0.5 / s;
    }
  }  
  return q;
}

Euler3 Mat3x3d::toEuler(){
  // We use so-called "x-convention", which is the most common definition. 
  // In this convention, the rotation given by Euler angles (phi, theta, psi), where the first 
  // rotation is by an angle phi about the z-axis, the second is by an angle  
  // theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the
  //z-axis (again). 

  Euler3 e;
  double cosTheta;
  double sinTheta;
  const double eps = 1.0e-8;
  // set the tolerance for Euler angles and rotation elements

  e.theta = acos(min(1.0,max(-1.0, element[2][2])));
  cosTheta = element[2][2];
  sinTheta = sqrt(1.0 - cosTheta * cosTheta);

  // when sin(theta) is close to 0, we need to consider singularity
  // In this case, we can assign an arbitary value to phi (or psi), and then determine 
  // the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0
  // in cases of singularity.  
  // we use atan2 instead of atan, since atan2 will give us -Pi to Pi. 
  // Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never
  // change the sign of both of the parameters passed to atan2.
  
  if (fabs(sinTheta) <= eps){
    e.psi = 0.0;
    //e.phi = atan2(-Amat[Ayx], Amat[Axx]);  
    e.phi = atan2(-element[1][0], element[0][0]);
  }
  // we only have one unique solution
  else{    
      //e.phi = atan2(Amat[Azx], -Amat[Azy]);
      //e.psi = atan2(Amat[Axz], Amat[Ayz]);
      e.phi = atan2(element[2][0], -element[2][1]);
      e.psi = atan2(element[0][2], -element[1][2]);
  }

  //wrap phi and psi, make sure they are in the range from 0 to 2*Pi
  //if (phi < 0)
  //  phi += M_PI;

  //if (psi < 0)
  //  psi += M_PI

  return e;
}


Vector3d operator*(const Mat3x3d& m, const Vector3d& v){
  Vector3d result;
  
  result.x = m.element[0][0] * v.x + m.element[0][1] * v.y + m.element[0][2]*v.z;
  result.y = m.element[1][0] * v.x + m.element[1][1] * v.y + m.element[1][2]*v.z;
  result.z = m.element[2][0] * v.x + m.element[2][1] * v.y + m.element[2][2]*v.z;

  return result;
}  
Revision:	1319
Committed:	Tue Jun 29 22:52:24 2004 UTC (21 years, 4 months ago) by tim
File size:	7322 byte(s)
Log Message:	fixed a bug in Mat3x3d.cpp
#	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			double cosTheta;
190			double sinTheta;
191			const double eps = 1.0e-8;
192			// set the tolerance for Euler angles and rotation elements
193
194			e.theta = acos(min(1.0,max(-1.0, element[2][2])));
195			cosTheta = element[2][2];
196			sinTheta = sqrt(1.0 - cosTheta * cosTheta);
197
198			// when sin(theta) is close to 0, we need to consider singularity
199			// In this case, we can assign an arbitary value to phi (or psi), and then determine
200			// the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0
201			// in cases of singularity.
202			// we use atan2 instead of atan, since atan2 will give us -Pi to Pi.
203			// Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never
204			// change the sign of both of the parameters passed to atan2.
205
206			if (fabs(sinTheta) <= eps){
207			e.psi = 0.0;
208			//e.phi = atan2(-Amat[Ayx], Amat[Axx]);
209			e.phi = atan2(-element[1][0], element[0][0]);
210			}
211			// we only have one unique solution
212			else{
213			//e.phi = atan2(Amat[Azx], -Amat[Azy]);
214			//e.psi = atan2(Amat[Axz], Amat[Ayz]);
215			e.phi = atan2(element[2][0], -element[2][1]);
216			e.psi = atan2(element[0][2], -element[1][2]);
217			}
218
219			//wrap phi and psi, make sure they are in the range from 0 to 2*Pi
220			//if (phi < 0)
221			// phi += M_PI;
222
223			//if (psi < 0)
224			// psi += M_PI
225
226			return e;
227			}
228	tim	1268
229
230			Vector3d operator*(const Mat3x3d& m, const Vector3d& v){
231			Vector3d result;
232
233			result.x = m.element[0][0] * v.x + m.element[0][1] * v.y + m.element[0][2]*v.z;
234	tim	1319	result.y = m.element[1][0] * v.x + m.element[1][1] * v.y + m.element[1][2]*v.z;
235			result.z = m.element[2][0] * v.x + m.element[2][1] * v.y + m.element[2][2]*v.z;
236	tim	1268
237			return result;
238			}