CNum::Counter &CNum::Counter::operator<<=(const Counter &rhs) { assert(rhs.value.size() == 1); if (*this == 0) { return *this; } const Unit div = rhs[0] / UNIT_BIT_SIZE; const Unit rem = rhs[0] % UNIT_BIT_SIZE; // fine shift if (rem) { Unit filler = 0; for (Unit i = 0; i < value.size(); ++i) { CNum::left_shift(value[i], rem, filler); } if (filler) { value.push_back(filler); } } assert(isNormalized()); // coarse shift for (Unit i = 0; i < div; ++i) { value.insert(value.cbegin(), 0); } assert(isNormalized()); return *this; }
Spectrum Spectrum::convolve(const Spectrum& other, double threshold) const { if (this->isConvolutionUnit()) return other; if (other.isConvolutionUnit()) return *this; const auto& p1 = *this, p2 = other; size_t n1 = p1.size(), n2 = p2.size(); assert(isNormalized(p1)); assert(isNormalized(p2)); Spectrum result; for (size_t i = 0; i < n1; i++) for (size_t j = 0; j < n2; j++) { auto abundance = p1.intensities[i] * p2.intensities[j]; if (abundance > threshold) { result.masses.push_back(p1.masses[i] + p2.masses[j]); result.intensities.push_back(abundance); } else { if (j == 0) break; n2 = j; } } return result.normalize(); }
CNum::Counter &CNum::Counter::operator%=(const Counter &rhs) { assert(isNormalized()); assert(rhs.isNormalized()); assert(rhs > 0); *this -= (*this / rhs * rhs); assert(isNormalized()); return *this; }
Transform operator*(const Transform &lhs, const Transform &rhs) { assert(isNormalized(lhs.rotation, 1e-5)); assert(isNormalized(rhs.rotation, 1e-5)); auto lv = Eigen::Map<const Eigen::Vector3d>(&lhs.translation.x); auto rv = Eigen::Map<const Eigen::Vector3d>(&rhs.translation.x); auto lq = Eigen::Map<const Eigen::Quaterniond>(&lhs.rotation.x); auto rq = Eigen::Map<const Eigen::Quaterniond>(&rhs.rotation.x); Transform result; Eigen::Map<Eigen::Quaterniond>(&result.rotation.x) = lq * rq; Eigen::Map<Eigen::Vector3d>(&result.translation.x) = lv + lq * rv; return result; }
// Doing in place computation could be dangerous... CNum::Counter &CNum::Counter::operator-=(const Counter &rhs) { assert(isNormalized()); assert(rhs.isNormalized()); assert(rhs <= *this); Unit borrow = 0; for (Index pos = 0; borrow || pos < rhs.value.size(); ++pos) { Unit r = pos >= rhs.value.size() ? 0 : rhs.value[pos]; CNum::sub(value[pos], r, borrow); } normalize(); assert(isNormalized()); return *this; }
// Doing in place computation could be dangerous... CNum::Counter &CNum::Counter::operator+=(const Counter &rhs) { assert(isNormalized()); assert(rhs.isNormalized()); Unit carry = 0; for (Index pos = 0; carry || pos < rhs.value.size(); ++pos) { if (pos >= value.size()) { value.push_back(0); } Unit r = (pos >= rhs.value.size() ? 0 : rhs.value[pos]); CNum::add(value[pos], r, carry); } assert(isNormalized()); return *this; }
void CNum::Counter::normalize() { // trim leading zero while (value.size() > 1 && value.back() == 0) { value.pop_back(); } assert(isNormalized()); }
CNum::Counter CNum::Counter::log2() const { assert(isNormalized()); if (*this == 0) { throw; } return bitSize() - 1; }
CNum::Counter CNum::Counter::pow(const Counter &rhs) const { assert(isNormalized()); if (*this == 0 && rhs == 0) throw; if (*this == 0) return 0; if (rhs == 0) return 1; // *this >=1 && rhs >=1 here // Do repeated square algorithm const Unit SQ_SIZE = rhs.bitSize(); assert(SQ_SIZE >= 1); std::vector<Counter> sq(SQ_SIZE); sq[0] = *this; for (Unit i = 1; i < SQ_SIZE; ++i) { sq[i] = sq[i - 1] * sq[i - 1]; } Counter rv(1); for (Unit i = 0; i < SQ_SIZE; ++i) { if (rhs.isSet(i)) { rv *= sq[i]; } } return rv; }
//------------------------------------------------------------------------ void ParameterChangesCheck::checkNormalized (float normVal) { if (!isNormalized (normVal)) { mEventLogger->addLogEvent (kLogIdInvalidParamValue); } }
inline Eigen::Affine3d toEigenAffine3d(const Transform &tf) { assert(isNormalized(tf.rotation, 1e-5)); const auto& t = tf.translation; const auto& r = tf.rotation; return Eigen::Affine3d(Eigen::Translation3d(t.x, t.y, t.z) * Eigen::Quaterniond(r.w, r.x, r.y, r.z)); }
bool CNum::Counter::isSet(const Unit &idx) const { assert(isNormalized()); if (idx >= bitSize()) { return false; } Unit div = idx / CNum::UNIT_BIT_SIZE; Unit rem = idx % CNum::UNIT_BIT_SIZE; const Unit FILTER = 1; return (value[div] & (FILTER << rem)) != 0; }
RevoluteJoint::RevoluteJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child, const Transform3f& transform_to_parent, const std::string& name, const Vec3f& axis) : Joint(link_parent, link_child, transform_to_parent, name) { BOOST_ASSERT(isNormalized(axis) && "Axis is not normalized."); setAxis(axis); init(); }
Quaternion makeNormalizedQuaternion(double x, double y, double z, double w) { auto q = makeQuaternion(x, y, z, w); if (!isNormalized(q, 1e-5f)) { std::stringstream err; err << "Quaternion(" << x << ", " << y << ", " << z << ", " << w << ") is not normalized"; throw std::runtime_error(err.str()); } return q; }
CNum::Counter &CNum::Counter::operator*=(const Counter &rhs) { assert(isNormalized()); assert(rhs.isNormalized()); Counter sol = 0; Unit shift1 = 0; for (Index rpos = 0; rpos < rhs.value.size(); ++rpos) { Unit shift2 = 0; Unit carry = 0; for (Index lpos = 0; carry || lpos < value.size(); ++lpos) { Unit l = lpos < value.size() ? value[lpos] : 0; CNum::mul(l, rhs.value[rpos], carry); sol += (Counter(l) << (shift1 + shift2)); shift2 += UNIT_BIT_SIZE; } shift1 += UNIT_BIT_SIZE; } assert(sol.isNormalized()); *this = sol; assert(isNormalized()); return *this; }
CNum::Unit CNum::Counter::bitSize() const { assert(isNormalized()); assert(value.size() >= 1); Unit filter = 0x1; Unit count = 0; for (Unit i = UNIT_BIT_SIZE; i > 0; --i) { if (value[value.size() - 1] & (filter << (i - 1))) { count = i; break; } } return count + (value.size() - 1) * UNIT_BIT_SIZE; }
// Doing division by naive search... O(n^3) CNum::Counter &CNum::Counter::operator/=(const Counter &rhs) { assert(isNormalized()); assert(rhs.isNormalized()); assert(rhs > 0); // find upperbound ub such that rhs*ub > lhs >= rhs*(ub/2) Counter rem(*this); Counter sol = 0; while (rem >= rhs) { Counter bit(1); Counter ub(rhs); while (ub <= rem) { ub <<= 1; bit <<= 1; } ub >>= 1; bit >>= 1; sol += bit; rem -= ub; } *this = sol; assert(isNormalized()); return *this; }
// Simply use printf? std::string CNum::Counter::hex() const { assert(isNormalized()); assert(UNIT_BIT_SIZE % HEX_CHAR_SIZE == 0); const Unit CHAR_NUM = UNIT_BIT_SIZE / HEX_CHAR_SIZE; const Unit filter = 0xF; std::string rv("0x"); for (auto itr = value.crbegin(); itr != value.crend(); ++itr) { for (int i = 0; i < CHAR_NUM; ++i) { Unit shift = HEX_CHAR_SIZE * (CHAR_NUM - i - 1); Unit u = (*itr & (filter << shift)) >> shift; if (u <= 9) { rv.push_back(static_cast<char>(u) + '0'); } else { assert(0xA <= u && u <= 0xF); rv.push_back(static_cast<char>(u) - 10 + 'A'); } } } while (rv.size() >= 4 && rv[2] == '0') { rv.erase(rv.begin() + 2, rv.begin() + 3); } assert(rv == "0x0" || rv[2] != '0'); return rv; }
void CNum::Counter::setZero() { value.clear(); value.push_back(0); assert(isNormalized()); }
Radian Vector4::angleBetweenNorm(const Vector4& normalizedVector) const { aproassert(isNormalized() && normalizedVector.isNormalized(), "Vectors are not normalized !"); return Angle::ACos(dot(normalizedVector)); }
CNum::Counter::Counter(const std::string &s) : value() { if (s.size() == 0) { setZero(); return; } // Parse the correct base auto rBegin = s.crbegin(); auto rEnd = s.crend(); Unit base = 10; if (s.size() >= 2 && s[0] == '0') { if (s[1] == 'x') { base = 16; rEnd -= 2; } else if (s[1] == 'b') { base = 2; rEnd -= 2; } } // rBegin and rEnd is set if (base == 16) { // 1 character in hex occupied 4 bits Index shift = 0; Unit unit = 0; for (auto itr = rBegin; itr != rEnd; itr++) { Unit c = *itr; if ('0' <= c && c <= '9') { c -= '0'; } else if ('a' <= c && c <= 'f') { c = c - 'a' + 10; } else if ('A' <= c && c <= 'F') { c = c - 'A' + 10; } else { throw; } unit |= (c << shift); shift = (shift + HEX_CHAR_SIZE) % UNIT_BIT_SIZE; if (shift == 0) { value.push_back(unit); unit = 0; } } value.push_back(unit); } else if (base == 10) { // Naive algorithm that make use of multiplication and addition *this = 0; Counter exp = 1; for (auto itr = rBegin; itr != rEnd; itr++, exp *= base) { Unit c = *itr; if ('0' <= c && c <= '9') { c -= '0'; } else { throw; } *this += (c * exp); } } else { throw; } normalize(); assert(isNormalized()); }
CNum::Counter::Counter(unsigned long long v) : value() { assert(sizeof(unsigned long long) <= sizeof(Unit)); value.push_back(v); assert(isNormalized()); }
// Implement with long division std::string CNum::Counter::dec() const { assert(isNormalized()); std::string rv(""); return rv; }
CNum::Unit CNum::Counter::size() const { assert(isNormalized()); return value.size(); }
void Joint::setAxis(const Vec3f& axis) { BOOST_ASSERT(isNormalized(axis) && "Axis is not normalized."); axis_ = axis; }