double SpatialMotionVectorRaw::dot(const SpatialForceVectorRaw& other) const
{
Eigen::Map<const Vector6d> otherData(other.data());
Eigen::Map<const Vector6d> thisData(this->data());
return thisData.dot(otherData);
}
const SpatialMotionVectorRaw& SpatialMotionVectorRaw::changePoint(const PositionRaw& newPoint)
{
Eigen::Map<Vector6d> thisData(this->data());
Eigen::Map<const Eigen::Vector3d> pointData(newPoint.data());
thisData.segment<3>(0) = thisData.segment<3>(0) + pointData.cross(thisData.segment<3>(3));
return *this;
}
const SpatialMotionVectorRaw& SpatialMotionVectorRaw::changeCoordFrame(const RotationRaw& newCoordFrame)
{
Eigen::Map<Vector6d> thisData(this->data());
Eigen::Map<const Matrix3dRowMajor> rotData(newCoordFrame.data());
thisData.segment<3>(0) = rotData*thisData.segment<3>(0);
thisData.segment<3>(3) = rotData*thisData.segment<3>(3);
return *this;
}
Matrix<T>& Matrix<T>::operator+=( const Matrix<T>& Other )
{
Q_ASSERT(size() > 0 && Other.size() > 0 && rows() == Other.rows());
Q_ASSERT(Other.cols() == cols() || Other.cols() == 1);
bool genericProcessing(true);
if (Other.cols() == cols())
{
const int src1Stride1(bytesPerLine());
const int src1Stride2(bytesPerElement());
const int src2Stride1(Other.bytesPerLine());
const int src2Stride2(Other.bytesPerElement());
const int dstStride1(src1Stride1);
const int dstStride2(src1Stride2);
const int width(Other.cols());
const int height(Other.rows());
if (type() == CV_64F)
{
const Ipp64f* pSrc1(reinterpret_cast<const Ipp64f*>(constData()));
const Ipp64f* pSrc2(reinterpret_cast<const Ipp64f*>(Other.constData()));
Ipp64f* pDst(reinterpret_cast<Ipp64f*>(data()));
IppStatus status(ippmAdd_mm_64f(pSrc1, src1Stride1, src1Stride2,
pSrc2, src2Stride1, src2Stride2,
pDst, dstStride1, dstStride2, width, height));
Q_ASSERT(status == ippStsNoErr);
genericProcessing = false;
}
}
else if (Other.cols() == 1)
{
Q_ASSERT(!"CHECK");
const int dataSize(Other.size());
const int src1Stride(cols() * sizeof(T));
const int src2Stride(Other.cols() * sizeof(T));
const int dstStride(src1Stride);
if (type() == CV_64F)
{
const Ipp64f* pSrc1(reinterpret_cast<const Ipp64f*>(constData()));
const Ipp64f* pSrc2(reinterpret_cast<const Ipp64f*>(Other.constData()));
Ipp64f* pDst(reinterpret_cast<Ipp64f*>(data()));
for (int col(0); col < cols(); col++, pSrc1++, pDst++)
{
IppStatus status(ippmAdd_vv_64f(pSrc1, src1Stride, pSrc2, src2Stride, pDst, dstStride, dataSize));
Q_ASSERT(status == ippStsNoErr);
}
genericProcessing = false;
}
}
if (genericProcessing)
{
Q_ASSERT(!"CHECK");
const int thisCols(cols());
const int otherCols(Other.cols());
for (int row(0); row < rows(); row++)
{
T* thisData(data() + row*thisCols);
const T* otherData0(Other.constData() + row*otherCols);
const T* otherData(otherData0);
for (int thisCol(0), otherCol(0); thisCol < thisCols; thisCol++, otherCol++, thisData++, otherData++)
{
if (otherCol == otherCols)
{
otherData = otherData0;
otherCol = 0;
}
*thisData += *otherData;
}
}
}
return *this;
}
QString QByteArrayBuilder::toReadableString(){
QByteArray thisData(data(), size());
return toReadableString(thisData);
}
QString QByteArrayBuilder::toDecimalString(){
QByteArray thisData(data(), size());
return toDecimalString(thisData);
}