74 lines
1.9 KiB
C++
74 lines
1.9 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2023 Ondsel, Inc. *
|
|
* *
|
|
* This file is part of OndselSolver. *
|
|
* *
|
|
* See LICENSE file for details about copyright. *
|
|
***************************************************************************/
|
|
|
|
#pragma once
|
|
|
|
#include "EulerArray.h"
|
|
#include "FullColumn.h"
|
|
#include "FullMatrix.h"
|
|
|
|
namespace MbD {
|
|
|
|
template<typename T>
|
|
class EulerAngleszxz : public EulerArray<T>
|
|
{
|
|
//phiA theA psiA aA
|
|
//Used by EndFrameqct
|
|
public:
|
|
EulerAngleszxz() : EulerArray<T>(3) {}
|
|
void initialize() override;
|
|
void calc() override;
|
|
|
|
FMatDsptr phiA, theA, psiA, aA;
|
|
};
|
|
template<typename T>
|
|
inline void EulerAngleszxz<T>::initialize()
|
|
{
|
|
phiA = FullMatrixDouble::identitysptr(3);
|
|
theA = FullMatrixDouble::identitysptr(3);
|
|
psiA = FullMatrixDouble::identitysptr(3);
|
|
}
|
|
template<typename T>
|
|
inline void EulerAngleszxz<T>::calc()
|
|
{
|
|
double zero = 0.0;
|
|
double phi = this->at(0);
|
|
double sphi = sin(phi);
|
|
double cphi = cos(phi);
|
|
double the = this->at(1);
|
|
double sthe = sin(the);
|
|
double cthe = cos(the);
|
|
double psi = this->at(2);
|
|
double spsi = sin(psi);
|
|
double cpsi = cos(psi);
|
|
FRowDsptr phiAi;
|
|
phiAi = phiA->at(0);
|
|
phiAi->at(0) = cphi;
|
|
phiAi->at(1) = -sphi;
|
|
phiAi = phiA->at(1);
|
|
phiAi->at(0) = sphi;
|
|
phiAi->at(1) = cphi;
|
|
FRowDsptr theAi;
|
|
theAi = theA->at(1);
|
|
theAi->at(1) = cthe;
|
|
theAi->at(2) = -sthe;
|
|
theAi = theA->at(2);
|
|
theAi->at(1) = sthe;
|
|
theAi->at(2) = cthe;
|
|
FRowDsptr psiAi;
|
|
psiAi = psiA->at(0);
|
|
psiAi->at(0) = cpsi;
|
|
psiAi->at(1) = -spsi;
|
|
psiAi = psiA->at(1);
|
|
psiAi->at(0) = spsi;
|
|
psiAi->at(1) = cpsi;
|
|
aA = phiA->timesFullMatrix(theA->timesFullMatrix(psiA));
|
|
}
|
|
}
|
|
|