687 lines
23 KiB
C++
687 lines
23 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2023 Ondsel, Inc. *
|
|
* *
|
|
* This file is part of OndselSolver. *
|
|
* *
|
|
* See LICENSE file for details about copyright. *
|
|
***************************************************************************/
|
|
|
|
#include "FullMatrix.h"
|
|
#include "FullColumn.h"
|
|
#include "FullRow.h"
|
|
#include "DiagonalMatrix.h"
|
|
#include "EulerParameters.h"
|
|
|
|
namespace MbD {
|
|
FColsptr<double> FullMatrixDouble::timesFullColumn(FColsptr<double> fullCol)
|
|
{
|
|
// return this->timesFullColumn(fullCol.get());
|
|
auto nrow = this->nrow();
|
|
auto answer = std::make_shared<FullColumn<double>>(nrow);
|
|
for (int i = 0; i < nrow; i++)
|
|
{
|
|
answer->at(i) = this->at(i)->timesFullColumn(fullCol);
|
|
}
|
|
return answer;
|
|
}
|
|
// FColsptr<double> timesFullColumn(FullColumn<double>* fullCol) // local
|
|
// {
|
|
// //"a*b = a(i,j)b(j) sum j."
|
|
// auto nrow = this->nrow();
|
|
// auto answer = std::make_shared<FullColumn<double>>(nrow);
|
|
// for (int i = 0; i < nrow; i++)
|
|
// {
|
|
// answer->at(i) = this->at(i)->timesFullColumn(fullCol);
|
|
// }
|
|
// return answer;
|
|
// }
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatex(double the)
|
|
{
|
|
auto sthe = std::sin(the);
|
|
auto cthe = std::cos(the);
|
|
auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
|
|
auto row0 = rotMat->at(0);
|
|
row0->atiput(0, 1.0);
|
|
row0->atiput(1, 0.0);
|
|
row0->atiput(2, 0.0);
|
|
auto row1 = rotMat->at(1);
|
|
row1->atiput(0, 0.0);
|
|
row1->atiput(1, cthe);
|
|
row1->atiput(2, -sthe);
|
|
auto row2 = rotMat->at(2);
|
|
row2->atiput(0, 0.0);
|
|
row2->atiput(1, sthe);
|
|
row2->atiput(2, cthe);
|
|
return rotMat;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatey(double the)
|
|
{
|
|
auto sthe = std::sin(the);
|
|
auto cthe = std::cos(the);
|
|
auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
|
|
auto row0 = rotMat->at(0);
|
|
row0->atiput(0, cthe);
|
|
row0->atiput(1, 0.0);
|
|
row0->atiput(2, sthe);
|
|
auto row1 = rotMat->at(1);
|
|
row1->atiput(0, 0.0);
|
|
row1->atiput(1, 1.0);
|
|
row1->atiput(2, 0.0);
|
|
auto row2 = rotMat->at(2);
|
|
row2->atiput(0, -sthe);
|
|
row2->atiput(1, 0.0);
|
|
row2->atiput(2, cthe);
|
|
return rotMat;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatez(double the)
|
|
{
|
|
auto sthe = std::sin(the);
|
|
auto cthe = std::cos(the);
|
|
auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
|
|
auto row0 = rotMat->at(0);
|
|
row0->atiput(0, cthe);
|
|
row0->atiput(1, -sthe);
|
|
row0->atiput(2, 0.0);
|
|
auto row1 = rotMat->at(1);
|
|
row1->atiput(0, sthe);
|
|
row1->atiput(1, cthe);
|
|
row1->atiput(2, 0.0);
|
|
auto row2 = rotMat->at(2);
|
|
row2->atiput(0, 0.0);
|
|
row2->atiput(1, 0.0);
|
|
row2->atiput(2, 1.0);
|
|
return rotMat;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatexrotDot(double the, double thedot)
|
|
{
|
|
auto sthe = std::sin(the);
|
|
auto cthe = std::cos(the);
|
|
auto sthedot = cthe * thedot;
|
|
auto cthedot = -sthe * thedot;
|
|
auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
|
|
auto row0 = rotMat->at(0);
|
|
row0->atiput(0, 0.0);
|
|
row0->atiput(1, 0.0);
|
|
row0->atiput(2, 0.0);
|
|
auto row1 = rotMat->at(1);
|
|
row1->atiput(0, 0.0);
|
|
row1->atiput(1, cthedot);
|
|
row1->atiput(2, -sthedot);
|
|
auto row2 = rotMat->at(2);
|
|
row2->atiput(0, 0.0);
|
|
row2->atiput(1, sthedot);
|
|
row2->atiput(2, cthedot);
|
|
return rotMat;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotateyrotDot(double the, double thedot)
|
|
{
|
|
auto sthe = std::sin(the);
|
|
auto cthe = std::cos(the);
|
|
auto sthedot = cthe * thedot;
|
|
auto cthedot = -sthe * thedot;
|
|
auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
|
|
auto row0 = rotMat->at(0);
|
|
row0->atiput(0, cthedot);
|
|
row0->atiput(1, 0.0);
|
|
row0->atiput(2, sthedot);
|
|
auto row1 = rotMat->at(1);
|
|
row1->atiput(0, 0.0);
|
|
row1->atiput(1, 0.0);
|
|
row1->atiput(2, 0.0);
|
|
auto row2 = rotMat->at(2);
|
|
row2->atiput(0, -sthedot);
|
|
row2->atiput(1, 0.0);
|
|
row2->atiput(2, cthedot);
|
|
return rotMat;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatezrotDot(double the, double thedot)
|
|
{
|
|
auto sthe = std::sin(the);
|
|
auto cthe = std::cos(the);
|
|
auto sthedot = cthe * thedot;
|
|
auto cthedot = -sthe * thedot;
|
|
auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
|
|
auto row0 = rotMat->at(0);
|
|
row0->atiput(0, cthedot);
|
|
row0->atiput(1, -sthedot);
|
|
row0->atiput(2, 0.0);
|
|
auto row1 = rotMat->at(1);
|
|
row1->atiput(0, sthedot);
|
|
row1->atiput(1, cthedot);
|
|
row1->atiput(2, 0.0);
|
|
auto row2 = rotMat->at(2);
|
|
row2->atiput(0, 0.0);
|
|
row2->atiput(1, 0.0);
|
|
row2->atiput(2, 0.0);
|
|
return rotMat;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatexrotDotrotDDot(double the, double thedot, double theddot)
|
|
{
|
|
auto sthe = std::sin(the);
|
|
auto cthe = std::cos(the);
|
|
auto sthedot = cthe * thedot;
|
|
auto cthedot = -sthe * thedot;
|
|
auto stheddot = cthedot * thedot + (cthe * theddot);
|
|
auto ctheddot = -(sthedot * thedot) - (sthe * theddot);
|
|
auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
|
|
auto row0 = rotMat->at(0);
|
|
row0->atiput(0, 0.0);
|
|
row0->atiput(1, 0.0);
|
|
row0->atiput(2, 0.0);
|
|
auto row1 = rotMat->at(1);
|
|
row1->atiput(0, 0.0);
|
|
row1->atiput(1, ctheddot);
|
|
row1->atiput(2, -stheddot);
|
|
auto row2 = rotMat->at(2);
|
|
row2->atiput(0, 0.0);
|
|
row2->atiput(1, stheddot);
|
|
row2->atiput(2, ctheddot);
|
|
return rotMat;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotateyrotDotrotDDot(double the, double thedot, double theddot)
|
|
{
|
|
auto sthe = std::sin(the);
|
|
auto cthe = std::cos(the);
|
|
auto sthedot = cthe * thedot;
|
|
auto cthedot = -sthe * thedot;
|
|
auto stheddot = cthedot * thedot + (cthe * theddot);
|
|
auto ctheddot = -(sthedot * thedot) - (sthe * theddot);
|
|
auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
|
|
auto row0 = rotMat->at(0);
|
|
row0->atiput(0, ctheddot);
|
|
row0->atiput(1, 0.0);
|
|
row0->atiput(2, stheddot);
|
|
auto row1 = rotMat->at(1);
|
|
row1->atiput(0, 0.0);
|
|
row1->atiput(1, 0.0);
|
|
row1->atiput(2, 0.0);
|
|
auto row2 = rotMat->at(2);
|
|
row2->atiput(0, -stheddot);
|
|
row2->atiput(1, 0.0);
|
|
row2->atiput(2, ctheddot);
|
|
return rotMat;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::rotatezrotDotrotDDot(double the, double thedot, double theddot)
|
|
{
|
|
auto sthe = std::sin(the);
|
|
auto cthe = std::cos(the);
|
|
auto sthedot = cthe * thedot;
|
|
auto cthedot = -sthe * thedot;
|
|
auto stheddot = cthedot * thedot + (cthe * theddot);
|
|
auto ctheddot = -(sthedot * thedot) - (sthe * theddot);
|
|
auto rotMat = std::make_shared<FullMatrixDouble>(3, 3);
|
|
auto row0 = rotMat->at(0);
|
|
row0->atiput(0, ctheddot);
|
|
row0->atiput(1, -stheddot);
|
|
row0->atiput(2, 0.0);
|
|
auto row1 = rotMat->at(1);
|
|
row1->atiput(0, stheddot);
|
|
row1->atiput(1, ctheddot);
|
|
row1->atiput(2, 0.0);
|
|
auto row2 = rotMat->at(2);
|
|
row2->atiput(0, 0.0);
|
|
row2->atiput(1, 0.0);
|
|
row2->atiput(2, 0.0);
|
|
return rotMat;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::identitysptr(int n)
|
|
{
|
|
auto mat = std::make_shared<FullMatrixDouble>(n, n);
|
|
mat->identity();
|
|
return mat;
|
|
}
|
|
std::shared_ptr<MbD::FullMatrixFullMatrixDouble> FullMatrixFullMatrixDouble::identitysptr(int n)
|
|
{
|
|
auto mat = std::make_shared<FullMatrixFullMatrixDouble>(n, n);
|
|
mat->identity();
|
|
return mat;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::tildeMatrix(FColDsptr col)
|
|
{
|
|
//"tildeMatrix is skew symmetric matrix related to angular velocity and cross product."
|
|
if (col->size() != 3) throw std::runtime_error("Column is not of dimension 3");
|
|
auto tilde = std::make_shared<FullMatrixDouble>(3, 3);
|
|
auto c0 = col->at(0);
|
|
auto c1 = col->at(1);
|
|
auto c2 = col->at(2);
|
|
tilde->atijput(0, 0, 0.0);
|
|
tilde->atijput(1, 1, 0.0);
|
|
tilde->atijput(2, 2, 0.0);
|
|
tilde->atijput(1, 2, -c0);
|
|
tilde->atijput(0, 2, c1);
|
|
tilde->atijput(0, 1, -c2);
|
|
tilde->atijput(1, 0, c2);
|
|
tilde->atijput(2, 0, -c1);
|
|
tilde->atijput(2, 1, c0);
|
|
return tilde;
|
|
}
|
|
void FullMatrixDouble::zeroSelf()
|
|
{
|
|
for (int i = 0; i < this->size(); i++) {
|
|
this->at(i)->zeroSelf();
|
|
}
|
|
}
|
|
void FullMatrixFullMatrixDouble::zeroSelf()
|
|
{
|
|
for (int i = 0; i < this->size(); i++) {
|
|
this->at(i)->zeroSelf();
|
|
}
|
|
}
|
|
void FullMatrixFullColumnDouble::zeroSelf()
|
|
{
|
|
for (int i = 0; i < this->size(); i++) {
|
|
this->at(i)->zeroSelf();
|
|
}
|
|
}
|
|
void FullMatrixDouble::identity() {
|
|
this->zeroSelf();
|
|
for (int i = 0; i < this->size(); i++) {
|
|
this->at(i)->at(i) = 1.0;
|
|
}
|
|
}
|
|
void FullMatrixFullMatrixDouble::identity() {
|
|
assert(false);
|
|
// this->zeroSelf();
|
|
// for (int i = 0; i < this->size(); i++) {
|
|
// this->at(i)->at(i) = 1.0;
|
|
// }
|
|
}
|
|
// TODO: should there be a FullMatrixFullColumnDouble version of this?
|
|
FColsptr<double> FullMatrixDouble::column(int j) {
|
|
int n = (int)this->size();
|
|
auto answer = std::make_shared<FullColumn<double>>(n);
|
|
for (int i = 0; i < n; i++) {
|
|
answer->at(i) = this->at(i)->at(j);
|
|
}
|
|
return answer;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::timesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
|
|
{
|
|
int m = this->nrow();
|
|
auto answer = std::make_shared<FullMatrixDouble>(m);
|
|
for (int i = 0; i < m; i++) {
|
|
answer->at(i) = this->at(i)->timesFullMatrix(fullMat);
|
|
}
|
|
return answer;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::timesTransposeFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
|
|
{
|
|
int nrow = this->nrow();
|
|
auto answer = std::make_shared<FullMatrixDouble>(nrow);
|
|
for (int i = 0; i < nrow; i++) {
|
|
answer->at(i) = this->at(i)->timesTransposeFullMatrix(fullMat);
|
|
}
|
|
return answer;
|
|
}
|
|
// std::shared_ptr<FullMatrixFullMatrixDouble> FullMatrixFullMatrixDouble::timesTransposeFullMatrix(std::shared_ptr<FullMatrixFullMatrixDouble> fullMat)
|
|
// {
|
|
// int nrow = this->nrow();
|
|
// auto answer = std::make_shared<FullMatrixFullMatrixDouble>(nrow);
|
|
// for (int i = 0; i < nrow; i++) {
|
|
// answer->at(i) = this->at(i)->timesTransposeFullMatrixForFMFMDsptr(fullMat);
|
|
// }
|
|
// return answer;
|
|
// }
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::times(double a)
|
|
{
|
|
int m = this->nrow();
|
|
auto answer = std::make_shared<FullMatrixDouble>(m);
|
|
for (int i = 0; i < m; i++) {
|
|
// auto x = this->at(i);
|
|
answer->at(i) = this->at(i)->times(a);
|
|
}
|
|
return answer;
|
|
}
|
|
std::shared_ptr<FullMatrixFullMatrixDouble> FullMatrixFullMatrixDouble::times(double a)
|
|
{
|
|
assert(false);
|
|
}
|
|
std::shared_ptr<FullMatrixFullColumnDouble> FullMatrixFullColumnDouble::times(double a)
|
|
{
|
|
// int m = this->nrow();
|
|
// auto answer = std::make_shared<FullMatrixFullColumnDouble>(m);
|
|
// for (int i = 0; i < m; i++) {
|
|
// answer->at(i) = this->at(i)->times(a);
|
|
// }
|
|
// return answer;
|
|
assert(false);
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::transposeTimesFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
|
|
{
|
|
return this->transpose()->timesFullMatrix(fullMat);
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::plusFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
|
|
{
|
|
int n = (int)this->size();
|
|
auto answer = std::make_shared<FullMatrixDouble>(n);
|
|
for (int i = 0; i < n; i++) {
|
|
answer->at(i) = this->at(i)->plusFullRow(fullMat->at(i));
|
|
}
|
|
return answer;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::minusFullMatrix(std::shared_ptr<FullMatrixDouble> fullMat)
|
|
{
|
|
int n = (int)this->size();
|
|
auto answer = std::make_shared<FullMatrixDouble>(n);
|
|
for (int i = 0; i < n; i++) {
|
|
answer->at(i) = this->at(i)->minusFullRow(fullMat->at(i));
|
|
}
|
|
return answer;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::transpose() {
|
|
int nrow = this->nrow();
|
|
auto ncol = this->ncol();
|
|
auto answer = std::make_shared<FullMatrixDouble>(ncol, nrow);
|
|
for (int i = 0; i < nrow; i++) {
|
|
auto& row = this->at(i);
|
|
for (int j = 0; j < ncol; j++) {
|
|
answer->at(j)->at(i) = row->at(j);
|
|
}
|
|
}
|
|
return answer;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::negated()
|
|
{
|
|
return this->times(-1.0);
|
|
}
|
|
void FullMatrixDouble::symLowerWithUpper()
|
|
{
|
|
int n = (int)this->size();
|
|
for (int i = 0; i < n; i++) {
|
|
for (int j = i + 1; j < n; j++) {
|
|
this->at(j)->at(i) = this->at(i)->at(j);
|
|
}
|
|
}
|
|
}
|
|
void FullMatrixFullColumnDouble::symLowerWithUpper()
|
|
{
|
|
int n = (int)this->size();
|
|
for (int i = 0; i < n; i++) {
|
|
for (int j = i + 1; j < n; j++) {
|
|
this->at(j)->at(i) = this->at(i)->at(j);
|
|
}
|
|
}
|
|
}
|
|
void FullMatrixDouble::atiput(int i, FRowsptr<double> fullRow)
|
|
{
|
|
this->at(i) = fullRow;
|
|
}
|
|
void FullMatrixDouble::atijput(int i, int j, double value)
|
|
{
|
|
this->at(i)->atiput(j, value);
|
|
}
|
|
void FullMatrixDouble::atijputFullColumn(int i1, int j1, FColsptr<double> fullCol)
|
|
{
|
|
for (int ii = 0; ii < fullCol->size(); ii++)
|
|
{
|
|
this->at(i1 + ii)->at(j1) = fullCol->at(ii);
|
|
}
|
|
}
|
|
void FullMatrixDouble::atijplusFullRow(int i, int j, FRowsptr<double> fullRow)
|
|
{
|
|
this->at(i)->atiplusFullRow(j, fullRow);
|
|
}
|
|
void FullMatrixDouble::atijplusNumber(int i, int j, double value)
|
|
{
|
|
auto rowi = this->at(i);
|
|
rowi->at(j) += value;
|
|
}
|
|
void FullMatrixDouble::atijminusNumber(int i, int j, double value)
|
|
{
|
|
auto rowi = this->at(i);
|
|
rowi->at(j) -= value;
|
|
}
|
|
double FullMatrixDouble::sumOfSquares()
|
|
{
|
|
double sum = 0.0;
|
|
for (int i = 0; i < this->size(); i++)
|
|
{
|
|
sum += this->at(i)->sumOfSquares();
|
|
}
|
|
return sum;
|
|
}
|
|
double FullMatrixFullMatrixDouble::sumOfSquares()
|
|
{
|
|
double sum = 0.0;
|
|
for (int i = 0; i < this->size(); i++)
|
|
{
|
|
sum += this->at(i)->sumOfSquares();
|
|
}
|
|
return sum;
|
|
}
|
|
double FullMatrixFullColumnDouble::sumOfSquares()
|
|
{
|
|
double sum = 0.0;
|
|
for (int i = 0; i < this->size(); i++)
|
|
{
|
|
sum += this->at(i)->sumOfSquares();
|
|
}
|
|
return sum;
|
|
}
|
|
std::shared_ptr<FullMatrixDouble> FullMatrixDouble::copy()
|
|
{
|
|
auto m = (int)this->size();
|
|
auto answer = std::make_shared<FullMatrixDouble>(m);
|
|
for (int i = 0; i < m; i++)
|
|
{
|
|
answer->at(i) = this->at(i)->copy();
|
|
}
|
|
return answer;
|
|
}
|
|
FullMatrixDouble FullMatrixDouble::operator+(const FullMatrixDouble fullMat)
|
|
{
|
|
int n = (int)this->size();
|
|
FullMatrixDouble answer(n);
|
|
for (int i = 0; i < n; i++) {
|
|
answer.at(i) = this->at(i)->plusFullRow(fullMat.at(i));
|
|
}
|
|
return answer;
|
|
}
|
|
FColsptr<double> FullMatrixDouble::transposeTimesFullColumn(FColsptr<double> fullCol)
|
|
{
|
|
auto sptr = std::make_shared<FullMatrixDouble>(*this);
|
|
return fullCol->transpose()->timesFullMatrix(sptr)->transpose();
|
|
}
|
|
void FullMatrixDouble::magnifySelf(double factor)
|
|
{
|
|
for (int i = 0; i < this->size(); i++) {
|
|
this->at(i)->magnifySelf(factor);
|
|
}
|
|
}
|
|
std::ostream& FullMatrixDouble::printOn(std::ostream& s) const
|
|
{
|
|
s << "FullMat[" << std::endl;
|
|
for (int i = 0; i < this->size(); i++)
|
|
{
|
|
s << *(this->at(i)) << std::endl;
|
|
}
|
|
s << "]";
|
|
return s;
|
|
}
|
|
std::shared_ptr<EulerParameters<double>> FullMatrixDouble::asEulerParameters()
|
|
{
|
|
//"Given [A], compute Euler parameter."
|
|
|
|
auto traceA = this->trace();
|
|
double dum = 0.0;
|
|
double dumSq = 0.0;
|
|
//auto qE = CREATE<EulerParameters<double>>::With(4); //Cannot use CREATE.h in subclasses of std::vector. Why?
|
|
auto qE = std::make_shared<EulerParameters<double>>(4);
|
|
qE->initialize();
|
|
auto OneMinusTraceDivFour = (1.0 - traceA) / 4.0;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
dumSq = this->at(i)->at(i) / 2.0 + OneMinusTraceDivFour;
|
|
dum = (dumSq > 0.0) ? std::sqrt(dumSq) : 0.0;
|
|
qE->atiput(i, dum);
|
|
}
|
|
dumSq = (1.0 + traceA) / 4.0;
|
|
dum = (dumSq > 0.0) ? std::sqrt(dumSq) : 0.0;
|
|
qE->atiput(3, dum);
|
|
double max = 0.0;
|
|
int maxE = -1;
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
auto num = qE->at(i);
|
|
if (max < num) {
|
|
max = num;
|
|
maxE = i;
|
|
}
|
|
}
|
|
|
|
if (maxE == 0) {
|
|
auto FourE = 4.0 * qE->at(0);
|
|
qE->atiput(1, (this->at(0)->at(1) + this->at(1)->at(0)) / FourE);
|
|
qE->atiput(2, (this->at(0)->at(2) + this->at(2)->at(0)) / FourE);
|
|
qE->atiput(3, (this->at(2)->at(1) - this->at(1)->at(2)) / FourE);
|
|
}
|
|
else if (maxE == 1) {
|
|
auto FourE = 4.0 * qE->at(1);
|
|
qE->atiput(0, (this->at(0)->at(1) + this->at(1)->at(0)) / FourE);
|
|
qE->atiput(2, (this->at(1)->at(2) + this->at(2)->at(1)) / FourE);
|
|
qE->atiput(3, (this->at(0)->at(2) - this->at(2)->at(0)) / FourE);
|
|
}
|
|
else if (maxE == 2) {
|
|
auto FourE = 4.0 * qE->at(2);
|
|
qE->atiput(0, (this->at(0)->at(2) + this->at(2)->at(0)) / FourE);
|
|
qE->atiput(1, (this->at(1)->at(2) + this->at(2)->at(1)) / FourE);
|
|
qE->atiput(3, (this->at(1)->at(0) - this->at(0)->at(1)) / FourE);
|
|
}
|
|
else if (maxE == 3) {
|
|
auto FourE = 4.0 * qE->at(3);
|
|
qE->atiput(0, (this->at(2)->at(1) - this->at(1)->at(2)) / FourE);
|
|
qE->atiput(1, (this->at(0)->at(2) - this->at(2)->at(0)) / FourE);
|
|
qE->atiput(2, (this->at(1)->at(0) - this->at(0)->at(1)) / FourE);
|
|
}
|
|
qE->conditionSelf();
|
|
qE->calc();
|
|
return qE;
|
|
}
|
|
double FullMatrixDouble::trace()
|
|
{
|
|
double trace = 0.0;
|
|
for (int i = 0; i < this->size(); i++)
|
|
{
|
|
trace += this->at(i)->at(i);
|
|
}
|
|
return trace;
|
|
}
|
|
double FullMatrixDouble::maxMagnitude()
|
|
{
|
|
double max = 0.0;
|
|
for (int i = 0; i < this->size(); i++)
|
|
{
|
|
double element = this->at(i)->maxMagnitude();
|
|
if (max < element) max = element;
|
|
}
|
|
return max;
|
|
}
|
|
double FullMatrixFullMatrixDouble::maxMagnitude()
|
|
{
|
|
double max = 0.0;
|
|
for (int i = 0; i < this->size(); i++)
|
|
{
|
|
double element = this->at(i)->maxMagnitude();
|
|
if (max < element) max = element;
|
|
}
|
|
return max;
|
|
}
|
|
double FullMatrixFullColumnDouble::maxMagnitude()
|
|
{
|
|
double max = 0.0;
|
|
for (int i = 0; i < this->size(); i++)
|
|
{
|
|
double element = this->at(i)->maxMagnitude();
|
|
if (max < element) max = element;
|
|
}
|
|
return max;
|
|
}
|
|
FColsptr<double> FullMatrixDouble::bryantAngles()
|
|
{
|
|
auto answer = std::make_shared<FullColumn<double>>(3);
|
|
auto sthe1y = this->at(0)->at(2);
|
|
double the0x, the1y, the2z, cthe0x, sthe0x, y, x;
|
|
if (std::abs(sthe1y) > 0.9999) {
|
|
if (sthe1y > 0.0) {
|
|
the0x = std::atan2(this->at(1)->at(0), this->at(1)->at(1));
|
|
the1y = M_PI / 2.0;
|
|
the2z = 0.0;
|
|
}
|
|
else {
|
|
the0x = std::atan2(this->at(2)->at(1), this->at(2)->at(0));
|
|
the1y = M_PI / -2.0;
|
|
the2z = 0.0;
|
|
}
|
|
}
|
|
else {
|
|
the0x = std::atan2(-this->at(1)->at(2), this->at(2)->at(2));
|
|
cthe0x = std::cos(the0x);
|
|
sthe0x = std::sin(the0x);
|
|
y = sthe1y;
|
|
if (std::abs(cthe0x) > std::abs(sthe0x)) {
|
|
x = this->at(2)->at(2) / cthe0x;
|
|
}
|
|
else {
|
|
x = this->at(1)->at(2) / -sthe0x;
|
|
}
|
|
the1y = std::atan2(y, x);
|
|
the2z = std::atan2(-this->at(0)->at(1), this->at(0)->at(0));
|
|
}
|
|
answer->atiput(0, the0x);
|
|
answer->atiput(1, the1y);
|
|
answer->atiput(2, the2z);
|
|
return answer;
|
|
}
|
|
bool FullMatrixDouble::isDiagonal()
|
|
{
|
|
auto m = this->nrow();
|
|
auto n = this->ncol();
|
|
if (m != n) return false;
|
|
for (int i = 0; i < m; i++)
|
|
{
|
|
auto rowi = this->at(i);
|
|
for (int j = 0; j < n; j++)
|
|
{
|
|
if (i != j && rowi->at(j) != 0) return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
bool FullMatrixDouble::isDiagonalToWithin(double ratio)
|
|
{
|
|
double maxMag = this->maxMagnitude();
|
|
auto tol = ratio * maxMag;
|
|
auto nrow = this->nrow();
|
|
if (nrow == this->ncol()) {
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
for (int j = i + 1; j < 3; j++)
|
|
{
|
|
if (std::abs(this->at(i)->at(j)) > tol) return false;
|
|
if (std::abs(this->at(j)->at(i)) > tol) return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
else {
|
|
return false;
|
|
}
|
|
}
|
|
std::shared_ptr<DiagonalMatrix<double>> FullMatrixDouble::asDiagonalMatrix()
|
|
{
|
|
int nrow = this->nrow();
|
|
auto diagMat = std::make_shared<DiagonalMatrix<double>>(nrow);
|
|
for (int i = 0; i < nrow; i++)
|
|
{
|
|
diagMat->atiput(i, this->at(i)->at(i));
|
|
}
|
|
return diagMat;
|
|
}
|
|
void FullMatrixDouble::conditionSelfWithTol(double tol)
|
|
{
|
|
for (auto row : *this) {
|
|
row->conditionSelfWithTol(tol);
|
|
}
|
|
}
|
|
}
|