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;
}
