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.x = m.element[1][0] * v.x + m.element[1][1] * v.y + m.element[1][2]*v.z;
  result.x = m.element[2][0] * v.x + m.element[2][1] * v.y + m.element[2][2]*v.z;

  return result;
}  
Revision:	1268
Committed:	Fri Jun 11 17:16:21 2004 UTC (21 years, 4 months ago) by tim
File size:	7322 byte(s)
Log Message:	roll in progress
#	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	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
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	result.x = m.element[1][0] * v.x + m.element[1][1] * v.y + m.element[1][2]*v.z;
235	result.x = m.element[2][0] * v.x + m.element[2][1] * v.y + m.element[2][2]*v.z;
236
237	return result;
238	}