Program Listing for File RotationMatrix.h

↰ Return to documentation for file (include/Karana/Math/RotationMatrix.h)

/*
*Copyright(c)2024-2025KaranaDynamicsPtyLtd.Allrightsreserved.
*
*NOTICETOUSER:
*
*Thissourcecodeand/ordocumentation(the"LicensedMaterials")is
*theconfidentialandproprietaryinformationofKaranaDynamicsInc.
*UseoftheseLicensedMaterialsisgovernedbythetermsandconditions
*ofaseparatesoftwarelicenseagreementbetweenKaranaDynamicsandthe
*Licensee("LicenseAgreement").Unlessexpresslypermittedunderthat
*agreement,anyreproduction,modification,distribution,ordisclosure
*oftheLicensedMaterials,inwholeorinpart,toanythirdparty
*withoutthepriorwrittenconsentofKaranaDynamicsisstrictlyprohibited.
*
*THELICENSEDMATERIALSAREPROVIDED"ASIS"WITHOUTWARRANTYOFANYKIND.
*KARANADYNAMICSDISCLAIMSALLWARRANTIES,EXPRESSORIMPLIED,INCLUDING
*BUTNOTLIMITEDTOWARRANTIESOFMERCHANTABILITY,NON-INFRINGEMENT,AND
*FITNESSFORAPARTICULARPURPOSE.
*
*INNOEVENTSHALLKARANADYNAMICSBELIABLEFORANYDAMAGESWHATSOEVER,
*INCLUDINGBUTNOTLIMITEDTOLOSSOFPROFITS,DATA,ORUSE,EVENIF
*ADVISEDOFTHEPOSSIBILITYOFSUCHDAMAGES,WHETHERINCONTRACT,TORT,
*OROTHERWISEARISINGOUTOFORINCONNECTIONWITHTHELICENSEDMATERIALS.
*
*U.S.GovernmentEndUsers:TheLicensedMaterialsarea"commercialitem"
*asdefinedat48C.F.R.2.101,andareprovidedtotheU.S.Government
*onlyasacommercialenditemunderthetermsofthislicense.
*
*AnyuseoftheLicensedMaterialsinindividualorcommercialsoftwaremust
*include,intheuserdocumentationandinternalsourcecodecomments,
*thisNotice,Disclaimer,andU.S.GovernmentUseProvision.
*/


#pragmaonce

#include"Karana/Math/Defs.h"
#include<csignal>
#include<format>

namespaceKarana::Math{

usingnamespaceEigen;

classRotationVector;

classRotationMatrix:publicMat33{

//Constructors
public:
RotationMatrix(){};

RotationMatrix(constMat33&mat,boolorthogonalize=false,boolskip_values_check=false)
:Mat33(mat){
_setValues(orthogonalize,skip_values_check);
}

RotationMatrix(constRotationVector&rotvec);

//Copyconstructor,moveconstructor,andassignmentoperators
public:
RotationMatrix(constRotationMatrix&rv)
:Mat33(rv){};

RotationMatrix&operator=(constRotationMatrix&other){
if(this!=&other){
this->Mat33::operator=(other);
}
return*this;
}

RotationMatrix(RotationMatrix&&rv)noexcept
:Mat33(std::move(rv)){};

RotationMatrix&operator=(RotationMatrix&&other){
if(this!=&other){
this->Mat33::operator=(other);
}
return*this;
}

public:
booloperator==(constRotationMatrix&other){returnthis->Mat33::operator==(other);}

boolisApprox(constRotationMatrix&other,doubleprec=MATH_EPSILON){
returnthis->Mat33::isApprox(other,prec);
}

std::stringtypeString()const{return"RotationMatrix";}

void
setValues(constMat33&mat,boolorthogonalize=false,boolskip_values_check=false){
this->Mat33::operator=(mat);
_setValues(orthogonalize,skip_values_check);
}

private:
void_setValues(boolorthogonalize=false,boolskip_values_check=false){
if(orthogonalize){
/*orthogonalizethematrix*/
_orthogonalize();
}else{
if(notskip_values_check){
/*checkthatthematrixisorthogonalwithdeterminant1*/
_isOrthogonal();
}
}
//_is_identity=_checkIdentityValues(Base::epsilon);
}

void_isOrthogonal(doubleepsilon=MATH_EPSILON)const{
/*checkthatthedeterminantofthematrixis1*/
if(std::abs(determinant()-1)>=epsilon){
doubledet=determinant();
throwstd::invalid_argument(
std::format("Rotationmatrixfailedthedetermenant=1check.Thedeterminant"
"is{},whichisnotequalto1.",
det));
};

//Checkiftheproductofthematrixanditstransposeisclose
//totheidentity
Mat33product=(*this)*transpose();
if(notproduct.isIdentity(epsilon)){
throwstd::invalid_argument("Rotationmatrixfailedtheorthogonalitycheck.");
}
}

void_orthogonalize(){
/*UsetheQRdecompositiontoextracttheorthogonalpartof
thismatrix*/
Eigen::HouseholderQR<Eigen::Matrix3d>qr(*this);
Mat33Q=qr.householderQ();

/*Flipthesignonacolumnifthedeterminatofthematrixis
negative*/
if(Q.determinant()<0){
Q.col(2)=-Q.col(2);
}
this->Mat33::operator=(Q);
}
};

}//namespaceKarana::Math