52#ifndef MATH_SQUAREMATRIX3_HPP
53#define MATH_SQUAREMATRIX3_HPP
67 template<
typename Real>
70 using ElemType = Real;
71 using ElemPoinerType = Real*;
99 if (
this == &m)
return *
this;
114 setupRotMat(eulerAngles[0], eulerAngles[1], eulerAngles[2]);
124 Real sphi, stheta, spsi;
125 Real cphi, ctheta, cpsi;
134 this->data_[0][0] = cpsi * cphi - ctheta * sphi * spsi;
135 this->data_[0][1] = cpsi * sphi + ctheta * cphi * spsi;
136 this->data_[0][2] = spsi * stheta;
138 this->data_[1][0] = -spsi * cphi - ctheta * sphi * cpsi;
139 this->data_[1][1] = -spsi * sphi + ctheta * cphi * cpsi;
140 this->data_[1][2] = cpsi * stheta;
142 this->data_[2][0] = stheta * sphi;
143 this->data_[2][1] = -stheta * cphi;
144 this->data_[2][2] = ctheta;
167 void setupSkewMat(
Vector3<Real> v) { setupSkewMat(v[0], v[1], v[2]); }
169 void setupSkewMat(Real v1, Real v2, Real v3) {
170 this->data_[0][0] = 0;
171 this->data_[0][1] = -v3;
172 this->data_[0][2] = v2;
173 this->data_[1][0] = v3;
174 this->data_[1][1] = 0;
175 this->data_[1][2] = -v1;
176 this->data_[2][0] = -v2;
177 this->data_[2][1] = v1;
178 this->data_[2][2] = 0;
191 this->data_[0][0] = v1;
192 this->data_[1][1] = v2;
193 this->data_[2][2] = v3;
194 this->data_[1][2] = v4;
195 this->data_[2][1] = v4;
196 this->data_[0][2] = v5;
197 this->data_[2][0] = v5;
198 this->data_[0][1] = v6;
199 this->data_[1][0] = v6;
213 this->data_[0][0] = v1;
214 this->data_[1][1] = v2;
215 this->data_[2][2] = v3;
216 this->data_[1][2] = v4;
217 this->data_[0][2] = v5;
218 this->data_[0][1] = v6;
228 RealType ct = cos(angle);
229 RealType st = sin(angle);
245 t = this->data_[0][0] + this->data_[1][1] + this->data_[2][2] + 1.0;
247 if (t > std::numeric_limits<RealType>::epsilon()) {
250 q[1] = (this->data_[1][2] - this->data_[2][1]) * s;
251 q[2] = (this->data_[2][0] - this->data_[0][2]) * s;
252 q[3] = (this->data_[0][1] - this->data_[1][0]) * s;
254 ad1 = this->data_[0][0];
255 ad2 = this->data_[1][1];
256 ad3 = this->data_[2][2];
258 if (ad1 >= ad2 && ad1 >= ad3) {
259 s = 0.5 / sqrt(1.0 + this->data_[0][0] - this->data_[1][1] -
261 q[0] = (this->data_[1][2] - this->data_[2][1]) * s;
263 q[2] = (this->data_[0][1] + this->data_[1][0]) * s;
264 q[3] = (this->data_[0][2] + this->data_[2][0]) * s;
265 }
else if (ad2 >= ad1 && ad2 >= ad3) {
266 s = 0.5 / sqrt(1.0 + this->data_[1][1] - this->data_[0][0] -
268 q[0] = (this->data_[2][0] - this->data_[0][2]) * s;
269 q[1] = (this->data_[0][1] + this->data_[1][0]) * s;
271 q[3] = (this->data_[1][2] + this->data_[2][1]) * s;
273 s = 0.5 / sqrt(1.0 + this->data_[2][2] - this->data_[0][0] -
275 q[0] = (this->data_[0][1] - this->data_[1][0]) * s;
276 q[1] = (this->data_[0][2] + this->data_[2][0]) * s;
277 q[2] = (this->data_[1][2] + this->data_[2][1]) * s;
306 std::min((RealType)1.0, std::max((RealType)-1.0, this->data_[2][2])));
307 ctheta = this->data_[2][2];
308 stheta = sqrt(1.0 - ctheta * ctheta);
320 if (fabs(stheta) < 1e-6) {
322 phi = atan2(-this->data_[1][0], this->data_[0][0]);
326 phi = atan2(this->data_[2][0], -this->data_[2][1]);
327 psi = atan2(this->data_[0][2], this->data_[1][2]);
331 if (phi < 0) phi += 2.0 * Constants::PI;
333 if (psi < 0) psi += 2.0 * Constants::PI;
343 const Real& a,
const Real& b,
344 const Real& epsilon = std::numeric_limits<Real>::epsilon()) {
345 return (epsilon > std::abs(a - b));
348 Vector3<Real> toRPY() {
349 const Real PI = 3.14159265358979323846264;
351 if (closeEnough(this->data_[0][2], -1.0)) {
354 Real z = x + atan2(this->data_[1][0], this->data_[2][0]);
355 return Vector3<Real>(x, y, z);
356 }
else if (closeEnough(this->data_[0][2], 1.0)) {
359 Real z = -x + atan2(-this->data_[1][0], -this->data_[2][0]);
360 return Vector3<Real>(x, y, z);
363 Real x1 = -asin(this->data_[0][2]);
367 atan2(this->data_[1][2] / cos(x1), this->data_[2][2] / cos(x1));
369 atan2(this->data_[1][2] / cos(x2), this->data_[2][2] / cos(x2));
372 atan2(this->data_[0][1] / cos(x1), this->data_[0][0] / cos(x1));
374 atan2(this->data_[0][1] / cos(x2), this->data_[0][0] / cos(x2));
378 if ((std::abs(x1) + std::abs(y1) + std::abs(z1)) <=
379 (std::abs(x2) + std::abs(y2) + std::abs(z2))) {
380 return Vector3<Real>(x1, y1, z1);
382 return Vector3<Real>(x2, y2, z2);
387 Vector<Real, 6> toVoigtTensor() {
388 Vector<Real, 6> voigt;
389 voigt[0] = this->data_[0][0];
390 voigt[1] = this->data_[1][1];
391 voigt[2] = this->data_[2][2];
392 voigt[3] = 0.5 * (this->data_[1][2] + this->data_[2][1]);
393 voigt[4] = 0.5 * (this->data_[0][2] + this->data_[2][0]);
394 voigt[5] = 0.5 * (this->data_[0][1] + this->data_[1][0]);
402 x = this->data_[0][0] * (this->data_[1][1] * this->data_[2][2] -
403 this->data_[1][2] * this->data_[2][1]);
404 y = this->data_[0][1] * (this->data_[1][2] * this->data_[2][0] -
405 this->data_[1][0] * this->data_[2][2]);
406 z = this->data_[0][2] * (this->data_[1][0] * this->data_[2][1] -
407 this->data_[1][1] * this->data_[2][0]);
413 return this->data_[0][0] + this->data_[1][1] + this->data_[2][2];
425 m(0, 0) = this->data_[1][1] * this->data_[2][2] -
426 this->data_[1][2] * this->data_[2][1];
427 m(1, 0) = this->data_[1][2] * this->data_[2][0] -
428 this->data_[1][0] * this->data_[2][2];
429 m(2, 0) = this->data_[1][0] * this->data_[2][1] -
430 this->data_[1][1] * this->data_[2][0];
431 m(0, 1) = this->data_[2][1] * this->data_[0][2] -
432 this->data_[2][2] * this->data_[0][1];
433 m(1, 1) = this->data_[2][2] * this->data_[0][0] -
434 this->data_[2][0] * this->data_[0][2];
435 m(2, 1) = this->data_[2][0] * this->data_[0][1] -
436 this->data_[2][1] * this->data_[0][0];
437 m(0, 2) = this->data_[0][1] * this->data_[1][2] -
438 this->data_[0][2] * this->data_[1][1];
439 m(1, 2) = this->data_[0][2] * this->data_[1][0] -
440 this->data_[0][0] * this->data_[1][2];
441 m(2, 2) = this->data_[0][0] * this->data_[1][1] -
442 this->data_[0][1] * this->data_[1][0];
450 for (
unsigned int i = 0; i < 3; i++)
451 for (
unsigned int j = 0; j < 3; j++)
452 result(j, i) = this->data_[i][j];
486 template<
typename Real>
497 if (w[0] == w[1] && w[0] == w[2]) {
507 for (i = 0; i < 3; i++) {
508 if (w((i + 1) % 3) == w((i + 2) % 3)) {
510 maxVal = fabs(v(i, 0));
512 for (j = 1; j < 3; j++) {
513 if (maxVal < (tmp = fabs(v(i, j)))) {
528 if (v(maxI, maxI) < 0) {
529 v(maxI, 0) = -v(maxI, 0);
530 v(maxI, 1) = -v(maxI, 1);
531 v(maxI, 2) = -v(maxI, 2);
546 v_k =
cross(v_maxI, v_j);
548 v_j =
cross(v_k, v_maxI);
563 maxVal = fabs(v(0, 0));
565 for (i = 1; i < 3; i++) {
566 if (maxVal < (tmp = fabs(v(i, 0)))) {
580 if (fabs(v(1, 1)) < fabs(v(2, 1))) {
588 for (i = 0; i < 2; i++) {
614 template<
typename Real>
619 for (
unsigned int i = 0; i < 3; i++)
620 for (
unsigned int j = 0; j < 3; j++)
621 for (
unsigned int k = 0; k < 3; k++)
622 result(i, j) += m1(i, k) * m2(k, j);
627 template<
typename Real>
628 inline SquareMatrix3<Real> outProduct(
const Vector3<Real>& v1,
629 const Vector3<Real>& v2) {
630 SquareMatrix3<Real> result;
632 for (
unsigned int i = 0; i < 3; i++) {
633 for (
unsigned int j = 0; j < 3; j++) {
634 result(i, j) = v1[i] * v2[j];
641 using Mat3x3d = SquareMatrix3<RealType>;
642 using RotMat3x3d = SquareMatrix3<RealType>;
644 const Mat3x3d M3Zero(0.0);
Quaternion is a sort of a higher-level complex number.
Real x() const
Returns the value of the first element of this quaternion.
Real z() const
Returns the value of the fourth element of this quaternion.
SquareMatrix< Real, 3 > toRotationMatrix3()
Returns the corresponding rotation matrix (3x3)
Real w() const
Returns the value of the first element of this quaternion.
Real y() const
Returns the value of the thirf element of this quaternion.
Vector< Real, Row > getRow(unsigned int row)
Returns a row of this matrix as a vector.
void setRow(unsigned int row, const Vector< Real, Row > &v)
Sets a row of this matrix.
void swapRow(unsigned int i, unsigned int j)
swap two rows of this matrix
static void diagonalize(SquareMatrix3< Real > &a, Vector3< Real > &w, SquareMatrix3< Real > &v)
Extract the eigenvalues and eigenvectors from a 3x3 matrix.
SquareMatrix3< Real > inverse() const
Sets the value of this matrix to the inverse of itself.
SquareMatrix3(Real s)
Constructs and initializes every element of this matrix to a scalar.
Real determinant() const
Returns the determinant of this matrix.
SquareMatrix3()
default constructor
void axisAngle(Vector3d axis, RealType angle)
Uses Rodrigues' rotation formula for a rotation matrix.
SquareMatrix3< Real > & operator=(const SquareMatrix< Real, 3 > &m)
copy assignment operator
void setupRotMat(const Quaternion< Real > &quat)
Sets this matrix to a rotation matrix by quaternion.
SquareMatrix3(Real *array)
Constructs and initializes from an array.
Real trace() const
Returns the trace of this matrix.
Quaternion< Real > toQuaternion()
Returns the quaternion from this rotation matrix.
void setupUpperTriangularVoigtTensor(Vector< Real, 6 > vt)
Sets this matrix to an upper-triangular (asymmetric) tensor using Voigt Notation.
void setupRotMat(const Vector3< Real > &eulerAngles)
Sets this matrix to a rotation matrix by three euler angles @ param euler.
SquareMatrix3(const SquareMatrix< Real, 3 > &m)
copy constructor
Vector3< Real > toEulerAngles()
Returns the euler angles from this rotation matrix.
void setupRotMat(Real w, Real x, Real y, Real z)
Sets this matrix to a rotation matrix by quaternion.
void setupRotMat(Real phi, Real theta, Real psi)
Sets this matrix to a rotation matrix by three euler angles.
void setupVoigtTensor(Vector< Real, 6 > vt)
Sets this matrix to a symmetric tensor using Voigt Notation.
SquareMatrix< Real, Dim > & operator=(const RectMatrix< Real, Dim, Dim > &m)
copy assignment operator
static SquareMatrix< Real, Dim > identity()
static int jacobi(SquareMatrix< Real, Dim > &a, Vector< Real, Dim > &d, SquareMatrix< Real, Dim > &v)
void normalize()
Normalizes this vector in place.
This basic Periodic Table class was originally taken from the data.cpp file in OpenBabel.
Vector3< Real > cross(const Vector3< Real > &v1, const Vector3< Real > &v2)
Returns the cross product of two Vectors.
DynamicRectMatrix< Real > operator*(const DynamicRectMatrix< Real > &m, Real s)
Return the multiplication of scalar and matrix (m * s).