/*!
Returns true if cell spanning given volume overlaps
this sphere, false otherwise.
*/
bool overlaps(
const Vector_T& cell_start,
const Vector_T& cell_end
) {
for (size_t i = 0; i < size_t(cell_start.size()); i++) {
if (cell_start[i] > cell_end[i]) {
std::cerr << __FILE__ << "(" << __LINE__<< ") "
<< "Starting coordinate of cell at index " << i
<< " is larger than ending coordinate: "
<< cell_start[i] << " > " << cell_end[i]
<< std::endl;
abort();
}
}
// https://stackoverflow.com/questions/4578967
Scalar_T distance2 = 0;
for (size_t i = 0; i < size_t(cell_start.size()); i++) {
if (this->center[i] < cell_start[i]) {
distance2 += std::pow(this->center[i] - cell_start[i], 2);
} else if (this->center[i] > cell_end[i]) {
distance2 += std::pow(this->center[i] - cell_end[i], 2);
}
}
return distance2 < std::pow(this->radius, 2);
}
Quaternion_T<T>::Quaternion_T(Vector_T<T, 3> const & vec, T s) noexcept
{
this->x() = vec.x();
this->y() = vec.y();
this->z() = vec.z();
this->w() = s;
}
/*!
Returns true if cell spanning given volume overlaps
this box, false otherwise.
*/
bool overlaps(
const Vector_T& cell_start,
const Vector_T& cell_end
) {
for (size_t i = 0; i < size_t(cell_start.size()); i++) {
if (cell_start[i] > cell_end[i]) {
std::cerr << __FILE__ << "(" << __LINE__<< ") "
<< "Starting coordinate of cell at index " << i
<< " is larger than ending coordinate: "
<< cell_start[i] << " > " << cell_end[i]
<< std::endl;
abort();
}
}
bool overlaps = true;
for (size_t i = 0; i < size_t(cell_start.size()); i++) {
if (
cell_start[i] >= this->end[i]
or cell_end[i] <= this->start[i]
) {
overlaps = false;
break;
}
}
return overlaps;
}
state(const state& mean, const Vector_T& error, bool)
: gyro_bias(mean.gyro_bias + error.segment(0, 3))
, orientation(mean.orientation * exp<double>(error.segment(3, 3)))
, position(mean.position + error.segment(6, 3).template cast<double>())
, velocity(mean.velocity + error.segment(9, 3))
{
assert(!has_nan());
}
int main()
{
int check, errorCounter = 0;
Vector_T testClass;
check = testClass.initializationTest();
errorCounter += check;
check = testClass.operatorTest();
std::cout << "Total Failures for " << __FILE__ << ": " << errorCounter << std::endl;
return errorCounter; //Return the total number of errors
}
/*!
Returns magnetic field from a point dipole.
\param [dipole_moments_positions] List of dipole moments and
their corresponding positions
\param [field_position] Position of the returned total magnetic field
Assumes Vector_T is a 3d Eigen vector or similar.
Example usage:
\code
const Eigen::Vector3d field
= background_B::get_dipole_field_0d(
background_B::get_earth_dipole_moment<Eigen::Vector3d>(),
{0, 0, 0},
{6.3712e7, 0, 0} // at 10 Earth radii in +x
);
\endcode
\see background_B::get_dipole_field()
*/
template <class Vector_T> Vector_T get_dipole_field_0d(
const std::vector<std::pair<Vector_T, Vector_T>>& dipole_moments_positions,
const Vector_T& field_position
) {
static_assert(
Vector_T::SizeAtCompileTime == 3,
"ERROR: Only 3 component vectors supported"
);
Vector_T ret_val;
ret_val.setZero();
constexpr double vacuum_permeability = 1.256637061e-6;
for (const auto dip_mom_pos: dipole_moments_positions) {
const Vector_T r = field_position - dip_mom_pos.second;
bool far_enough = false;
for (size_t i = 0; i < size_t(r.size()); i++) {
if (fabs(r[i]) > std::numeric_limits<typename Vector_T::Scalar>::epsilon()) {
far_enough = true;
break;
}
}
if (not far_enough) {
ret_val += 2.0 / 3.0 * vacuum_permeability * dip_mom_pos.first;
continue;
}
const double r3 = r.norm() * r.squaredNorm();
const Vector_T
r_unit = r / r.norm(),
projected_dip_mom = dip_mom_pos.first.dot(r_unit) * r_unit;
ret_val += 1e-7 * (3 * projected_dip_mom - dip_mom_pos.first) / r3;
}
return ret_val;
}
bool set_geometry(
const Vector_T& given_start,
const Vector_T& given_end
) {
for (size_t i = 0; i < size_t(given_start.size()); i++) {
if (given_end[i] <= given_start[i]) {
return false;
}
}
this->start = given_start;
this->end = given_end;
return true;
}
bool Rect_T<T>::PtInRect(Vector_T<T, 2> const & pt) const
{
return MathLib::in_bound(pt.x(), this->left(), this->right())
&& MathLib::in_bound(pt.y(), this->top(), this->bottom());
}
void Quaternion_T<T>::v(Vector_T<T, 3> const & rhs) noexcept
{
this->x() = rhs.x();
this->y() = rhs.y();
this->z() = rhs.z();
}
void Plane_T<T>::Normal(Vector_T<T, 3> const & rhs)
{
this->a() = rhs.x();
this->b() = rhs.y();
this->c() = rhs.z();
}
