Program Listing for File Defs.h

↰ Return to documentation for file (include/Karana/Math/Defs.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<bit>
#include<cmath>
#include<format>
#include<iomanip>
#include<iostream>

/*foruseofgccspecificfenableexcept*/
#ifdefined(PYBIND11_MKDOC_SKIP)||(defined(__GNUC__)&&!defined(__clang__))
#include<fenv.h>
#endif

#include<Eigen/Dense>
#include<Eigen/Geometry>
#include<chrono>

namespaceKarana::Math{

usingKtime=std::chrono::nanoseconds;

KtimesecondsToKtime(doublet);

doublektimeToSeconds(constKtime&kt);

constexprdoublebuildNaNWithPayload(uint64_tpayload){
returnstd::bit_cast<double>((0x7FFull<<52)|(1ull<<51)|
(payload&((1ull<<51)-1)));
}

constexprboolisNaNWithPayload(constdouble&candidate,uint64_tpayload){
if(!std::isnan(candidate)){
returnfalse;
}
autobits=std::bit_cast<uint64_t>(candidate);
constexpruint64_tpayload_mask=(1ull<<51)-1;
return(bits&payload_mask)==(payload&payload_mask);
}

inlineconstexpruint64_tuninitializedPayload=1954;

inlineconstexprdoubleuninitializedNaN=buildNaNWithPayload(uninitializedPayload);

constexprboolisUninitializedNaN(constdouble&candidate){
returnisNaNWithPayload(candidate,uninitializedPayload);
}

voiduninitialize(Ktime&kt);

boolisUninitialized(constKtime&kt);

usingnamespaceEigen;

//Vectordefinitions
usingVec=VectorXd;
usingVec3=Vector3d;
usingVec4=Vector4d;
usingVec6=Matrix<double,6,1>;

//Matrixdefinitions.WeuseRowMajor,sincethisisstandardforC++.
usingMat=Matrix<double,Dynamic,Dynamic,RowMajor>;
usingMat33=Matrix<double,3,3,RowMajor>;
usingMat44=Matrix<double,4,4,RowMajor>;
usingMat66=Matrix<double,6,6,RowMajor>;
usingMat6n=Matrix<double,6,Dynamic,RowMajor>;

//Vectorarraydefinitions
usingArrayVec=ArrayXd;
usingArray=Array<double, Dynamic, Dynamic, RowMajor>;

//Slicedefinitions
//Theseshouldbepreferredinsteadof&orconst&tosupportsliceswithoutcreatingacopy
usingVecSlice=Ref<Vec>;
usingConstVecSlice=constRef<constVec>;
usingVec3Slice=Ref<Vec3>;
usingConstVec3Slice=constRef<constVec3>;
usingMatSlice=Ref<Mat>;
usingConstMatSlice=constRef<constMat>;

#defineMATH_EPSILONEigen::NumTraits<double>::dummy_precision()

Mat33tilde(constVec3&v);

//Runonlywithg++orwhenpybind11-mkdocisrunning.Skipclangotherwise.
#ifdefined(PYBIND11_MKDOC_SKIP)||(defined(__GNUC__)&&!defined(__clang__))
voidfloatingPointExceptions(boolenable);
#endif

template<typenameDerived>voiduninitialize(Eigen::DenseBase<Derived>&base){
base.setConstant(uninitializedNaN);
}

template<typenameDerived>boolisInitialized(constEigen::MatrixBase<Derived>&mat){
returnnotmat.array().unaryExpr([](doublex){returnisUninitializedNaN(x);}).any();
}

template<typenameDerived>boolisInitialized(constEigen::ArrayBase<Derived>&array){
returnnotarray.unaryExpr([](doublex){returnisUninitializedNaN(x);}).any();
}

template<typenameDerived>
std::stringdumpString(constEigen::MatrixBase<Derived>&mat,
conststd::string&prefix="",
unsignedintprecision=10,
boolexponential=false){
constEigen::IOFormatprettyFormat(precision,0,",","\n","[","]","","");
//constEigen::IOFormatprettyFormat(precision,0,",","\n",std::format("{}[",prefix),
//"]","","");

std::ostringstreamoss;
if(exponential)
oss<<std::scientific;

//oss<<std::setfill('-')<<std::setw(80)<<"-"<<"\n";
if(!prefix.empty())
oss<<prefix<<"=";

if(mat.cols()==1)//Vector
oss<<mat.transpose().format(prettyFormat);//<<"\n";
else//Matrix
oss<<"\\\n"<<mat.format(prettyFormat)<<"\n";

//oss<<std::setfill('-')<<std::setw(80)<<"-"<<"\n";

returnoss.str();
}

template<typenameDerived>
voiddump(constEigen::MatrixBase<Derived>&mat,
conststd::string&prefix="",
unsignedintprecision=10,
boolexponential=false){
std::cout<<dumpString(mat,prefix,precision,exponential);
if(mat.cols()==1)
std::cout<<std::endl;
}

template<typenameDerived>unsignedintgetRank(constEigen::MatrixBase<Derived>&mat){
//PerformQRdecompositionwithcolumnpivoting
Eigen::ColPivHouseholderQR<Eigen::MatrixXd>qr(mat);

returnqr.rank();
}

template<typenameDerived>
std::vector<unsignedint>getIndepColIndices(constEigen::MatrixBase<Derived>&mat){
//PerformQRdecompositionwithcolumnpivoting
Eigen::ColPivHouseholderQR<Eigen::MatrixXd>qr(mat);
unsignedintfullrank=qr.rank();
#if0
//Getthepermutationvector
auto*permutation=qr.colsPermutation().indices().data();
std::vector<unsignedint>result;
for(size_ti=0;i<fullrank;++i){
result.push_back(permutation[i]);
}
//sortindicesinincreasingorder
std::sort(result.data(),result.data()+result.size());
#else
autoindices=qr.colsPermutation().indices().head(fullrank);
std::vector<unsignedint>result(indices.data(),indices.data()+fullrank);
//std::sort(result.data(),result.data()+result.size());
#endif

returnresult;
}

template<typenameDerived>
std::pair<std::vector<unsignedint>,std::vector<unsignedint>>
getIndepRowColIndices(constEigen::MatrixBase<Derived>&mat){
//PerformLUdecompositionwithcolumnpivoting
Eigen::FullPivLU<Eigen::MatrixXd>lu(mat);
lu.setThreshold(1e-10);
unsignedintfullrank=lu.rank();
#if0
//Gettherowspermutationvector
auto*row_permutation=lu.permutationP().indices().data();
std::vector<unsignedint>rows;
for(size_ti=0;i<fullrank;++i){
rows.push_back(row_permutation[i]);
}

//Getthecolumnspermutationvector
auto*col_permutation=lu.permutationQ().indices().data();
std::vector<unsignedint>cols;
for(size_ti=0;i<fullrank;++i){
cols.push_back(col_permutation[i]);
}
//sortindicesinincreasingorder
std::sort(rows.data(),rows.data()+rows.size());
std::sort(cols.data(),cols.data()+cols.size());
#else
autorindices=lu.permutationP().indices().head(fullrank);
autocindices=lu.permutationQ().indices().head(fullrank);
std::vector<unsignedint>rows(rindices.data(),rindices.data()+fullrank);
std::vector<unsignedint>cols(cindices.data(),cindices.data()+fullrank);
#endif

returnstd::pair<std::vector<unsignedint>,std::vector<unsignedint>>(rows,cols);
}

template<typenameDerived>VecgetSingularValues(constEigen::MatrixBase<Derived>&mat){
//PerformQRdecompositionwithcolumnpivoting
Eigen::JacobiSVD<Eigen::MatrixXd>svd(mat,Eigen::ComputeThinU|Eigen::ComputeFullV);

returnsvd.singularValues();
}

}//namespaceKarana::Math