/*!
Extract the rotation as a quaternion.
*/
void
vpHomogeneousMatrix::extract(vpQuaternionVector &q) const
{
vpRotationMatrix R;
(*this).extract(R);
q.buildFrom(R);
}
/*!
Quaternion substraction.
Substracts a quaternion from another. Substraction is component-wise.
\param q : quaternion to substract.
*/
vpQuaternionVector vpQuaternionVector::operator-( vpQuaternionVector &q)
{
return vpQuaternionVector(x()-q.x(), y()-q.y(), z()-q.z(), w()-q.w());
}
//! Multiply two quaternions.
vpQuaternionVector vpQuaternionVector::operator* ( vpQuaternionVector &rq) {
return vpQuaternionVector(w() * rq.x() + x() * rq.w() + y() * rq.z() - z() * rq.y(),
w() * rq.y() + y() * rq.w() + z() * rq.x() - x() * rq.z(),
w() * rq.z() + z() * rq.w() + x() * rq.y() - y() * rq.x(),
w() * rq.w() - x() * rq.x() - y() * rq.y() - z() * rq.z());
}
/*!
Quaternion addition.
Adds two quaternions. Addition is component-wise.
\param q : quaternion to add.
*/
vpQuaternionVector vpQuaternionVector::operator+(const vpQuaternionVector &q) const
{
return vpQuaternionVector(x()+q.x(), y()+q.y(), z()+q.z(), w()+q.w());
}
