hesuicong
/
openMVS
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
28 Commits
1 Branch
0 Tags
13 MiB
 
 
 
 
 
 
Tree: af8100f851
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'af8100f851'
${ noResults }
openMVS/libs/Common/Rotation.inl

1563 lines
44 KiB
Raw Blame History

// D E F I N E S ///////////////////////////////////////////////////
// S T R U C T S ///////////////////////////////////////////////////
// G L O B A L S ///////////////////////////////////////////////////
namespace SEACAVE {
// C L A S S //////////////////////////////////////////////////////
template <typename TYPE>
inline TQuaternion<TYPE>::~TQuaternion()
{}
template <typename TYPE>
inline TQuaternion<TYPE>::TQuaternion()
: Base()
{}
template <typename TYPE>
inline TQuaternion<TYPE>::TQuaternion(const Base& q)
: Base(q)
{}
template <typename TYPE>
inline TQuaternion<TYPE>::TQuaternion(TYPE i, TYPE j, TYPE k, TYPE r)
: Base(cv::Vec<TYPE,4>(i, j, k, r))
{}
template <typename TYPE>
inline TQuaternion<TYPE>::TQuaternion(const Rot& R)
: Base(((const TRMatrixBase<TYPE>&)R).GetQuaternion())
{}
template <typename TYPE>
inline void TQuaternion<TYPE>::Invert()
{
Base::operator()(0) *=-1;
Base::operator()(1) *=-1;
Base::operator()(2) *=-1;
}
template <typename TYPE>
inline TQuaternion<TYPE> TQuaternion<TYPE>::Inverse() const
{
TQuaternion<TYPE> res(*this);
res.Invert();
return res;
}
template <typename TYPE>
inline void TQuaternion<TYPE>::MakeUnique()
{
//project quaternion to upper hemisphere (first ima.part >= 0)
TYPE *qThis = this->val;
if ((qThis[3] <= QUATERNION_EPSILON)
&&(qThis[3] >= -QUATERNION_EPSILON)) {
if ((qThis[1] <= QUATERNION_EPSILON)
&&(qThis[1] >= -QUATERNION_EPSILON)) {
if ((qThis[2] <= QUATERNION_EPSILON)
&&(qThis[2] >= -QUATERNION_EPSILON)) {
if (qThis[0] < 0.0) (*this) *= -1.0;
} else if (qThis[2] < 0.0) {
(*this) *= -1.0;
}
} else if (qThis[1] < 0.0) {
(*this) *= -1.0;
}
} else if (qThis[3] < 0.0) {
(*this) *= -1.0;
}
}
template <typename TYPE>
inline void TQuaternion<TYPE>::Mult(const TQuaternion<TYPE> &quat)
{
TYPE s1, s2, s3, s4, s5, s6, s7, s8, s9, t;
TYPE *qThis = this->val;
s1 = (qThis[2] - qThis[1]) * (quat[1] - quat[2]);
s2 = (qThis[3] + qThis[0]) * (quat[3] + quat[0]);
s3 = (qThis[3] - qThis[0]) * (quat[1] + quat[2]);
s4 = (qThis[2] + qThis[1]) * (quat[3] - quat[0]);
s5 = (qThis[2] - qThis[0]) * (quat[0] - quat[1]);
s6 = (qThis[2] + qThis[0]) * (quat[0] + quat[1]);
s7 = (qThis[3] + qThis[1]) * (quat[3] - quat[2]);
s8 = (qThis[3] - qThis[1]) * (quat[3] + quat[2]);
s9 = s6 + s7 + s8;
t = (s5 + s9) / 2.0f;
qThis[3] = s1 + t - s6;
qThis[0] = s2 + t - s9;
qThis[1] = s3 + t - s8;
qThis[2] = s4 + t - s7;
}
template <typename TYPE>
inline void TQuaternion<TYPE>::Mult(const TQuaternion<TYPE> &arg, TQuaternion<TYPE> &res) const
{
res = *this;
res.Mult(arg);
}
template <typename TYPE>
inline void TQuaternion<TYPE>::MultLeft(const TQuaternion<TYPE> &quatLeft)
{
TYPE s1, s2, s3, s4, s5, s6, s7, s8, s9, t;
TYPE *qThis = this->val;
s1 = (quatLeft[2] - quatLeft[1]) * (qThis[1] - qThis[2]);
s2 = (quatLeft[3] + quatLeft[0]) * (qThis[3] + qThis[0]);
s3 = (quatLeft[3] - quatLeft[0]) * (qThis[1] + qThis[2]);
s4 = (quatLeft[2] + quatLeft[1]) * (qThis[3] - qThis[0]);
s5 = (quatLeft[2] - quatLeft[0]) * (qThis[0] - qThis[1]);
s6 = (quatLeft[2] + quatLeft[0]) * (qThis[0] + qThis[1]);
s7 = (quatLeft[3] + quatLeft[1]) * (qThis[3] - qThis[2]);
s8 = (quatLeft[3] - quatLeft[1]) * (qThis[3] + qThis[2]);
s9 = s6 + s7 + s8;
t = (s5 + s9) / 2.0f;
qThis[3] = s1 + t - s6;
qThis[0] = s2 + t - s9;
qThis[1] = s3 + t - s8;
qThis[2] = s4 + t - s7;
}
/** rotates the given Vector with the quaternion ( q v q* )
the resulting vector is given in res
@returns 0 in case of no error
@author Daniel Grest, June 2003
*/
template <typename TYPE>
inline int TQuaternion<TYPE>::MultVec(const Vec &vec, Vec &res) const {
TYPE rx,ry,rz;
TYPE Vx= vec[0], Vy=vec[1], Vz=vec[2];
const TYPE *q = this->val;
{
TYPE QwQx, QwQy, QwQz, QxQy, QxQz, QyQz;
QwQx = q[3] * q[0]; QwQy = q[3] * q[1]; QwQz = q[3] * q[2];
QxQy = q[0] * q[1]; QxQz = q[0] * q[2]; QyQz = q[1] * q[2];
rx = 2* (Vy * (-QwQz + QxQy) + Vz *( QwQy + QxQz));
ry = 2* (Vx * ( QwQz + QxQy) + Vz *(-QwQx + QyQz));
rz = 2* (Vx * (-QwQy + QxQz) + Vy *( QwQx + QyQz));
}
TYPE QwQw, QxQx, QyQy, QzQz;
QwQw= q[3]*q[3]; QxQx = q[0]*q[0]; QyQy = q[1]*q[1]; QzQz= q[2]*q[2];
rx+= Vx * (QwQw + QxQx - QyQy - QzQz);
ry+= Vy * (QwQw - QxQx + QyQy - QzQz);
rz+= Vz * (QwQw - QxQx - QyQy + QzQz);
res[0]=rx; res[1]=ry; res[2]=rz;
return 0;
}
template <typename TYPE>
inline typename TQuaternion<TYPE>::Vec TQuaternion<TYPE>::MultVec(const Vec &vec) const
{
Vec r;
MultVec(vec,r);
return r;
}
template <typename TYPE>
inline void TQuaternion<TYPE>::SetIdentity()
{
Base::operator()(0) = Base::operator()(1) = Base::operator()(2) = 0;
Base::operator()(3) = 1;
}
template <typename TYPE>
inline void TQuaternion<TYPE>::SetQuaternion(TYPE real, TYPE i,
TYPE j, TYPE k)
{
//Components are stored in Vec4 in order imaginary part 1,2,3 and real part
this->Set(i,j,k,real);
}
template <typename TYPE>
inline void TQuaternion<TYPE>::SetValueAsAxisRad(const Vec &axis, TYPE angle)
{
SetValueAsAxisRad(axis[0], axis[1], axis[2], angle);
}
template <typename TYPE>
inline void TQuaternion<TYPE>::SetValueAsAxisRad(TYPE x, TYPE y, TYPE z, TYPE w)
{
TYPE rTmp = TYPE(sqrt(x * x + y * y + z * z));
if(rTmp > QUATERNION_EPSILON)
{
rTmp = TYPE(sin(w / 2.0f)) / rTmp;
Base::operator()(0) = x * rTmp;
Base::operator()(1) = y * rTmp;
Base::operator()(2) = z * rTmp;
Base::operator()(3) = TYPE(cos(w / 2.0f));
}
else
{
SetIdentity();
}
}
template <typename TYPE>
typename TQuaternion<TYPE>::Mat TQuaternion<TYPE>::GetQuaternionMultMatrixLeft() const
{
Mat res;
res(0,0) = (*this)[3];
res(0,1) = -(*this)[2];
res(0,2) = (*this)[1];
res(0,3) = (*this)[0];
res(1,0) = (*this)[2];
res(1,1) = (*this)[3];
res(1,2) = -(*this)[0];
res(1,3) = (*this)[1];
res(2,0) = -(*this)[1];
res(2,1) = (*this)[0];
res(2,2) = (*this)[3];
res(2,3) = (*this)[2];
res(3,0) = -(*this)[0];
res(3,1) = -(*this)[1];
res(3,2) = -(*this)[2];
res(3,3) = (*this)[3];
return res;
}
template <typename TYPE>
typename TQuaternion<TYPE>::Mat TQuaternion<TYPE>::GetQuaternionMultMatrixRight() const
{
Mat res;
res(0,0) = (*this)[3];
res(0,1) = (*this)[2];
res(0,2) = -(*this)[1];
res(0,3) = (*this)[0];
res(1,0) = -(*this)[2];
res(1,1) = (*this)[3];
res(1,2) = (*this)[0];
res(1,3) = (*this)[1];
res(2,0) = (*this)[1];
res(2,1) = -(*this)[0];
res(2,2) = (*this)[3];
res(2,3) = (*this)[2];
res(3,0) = -(*this)[0];
res(3,1) = -(*this)[1];
res(3,2) = -(*this)[2];
res(3,3) = (*this)[3];
return res;
}
template <typename TYPE>
int TQuaternion<TYPE>::GetAxisAngle(Vec& axis, TYPE& angle) const
{
// nicked from OpenSG
int res=0;
Vec q(cv::Vec<TYPE,3>((*this)[0], (*this)[1], (*this)[2]));
TYPE len = TYPE(norm(q));
if(len > QUATERNION_EPSILON) {
q *= TYPE(1.0) / len;
axis[0] = q[0];
axis[1] = q[1];
axis[2] = q[2];
angle = TYPE(2.0 * acos((*this)[3]));
} else {
axis[0] = 0.0;
axis[1] = 0.0;
axis[2] = 1.0;
angle = 0.0;
}
return res;
}
template <typename TYPE>
typename TQuaternion<TYPE>::Vec TQuaternion<TYPE>::GetRotationAxis() const
{
Vec r(cv::Vec<TYPE,3>((*this)[0], (*this)[1], (*this)[2]));
TYPE len = TYPE(norm(r));
if (len > QUATERNION_EPSILON)
return r * (1.0 / len);
else
return Vec(cv::Vec<TYPE,3>(0,0,1));
}
template <typename TYPE>
TYPE TQuaternion<TYPE>::GetRotationAngle() const
{
const TYPE angle = TYPE(2 * acos((*this)[3]));
return std::isnan(angle) && ABS(ABS((*this)[3]) - TYPE(1)) < QUATERNION_EPSILON ?
TYPE(0) :
angle;
}
template <typename TYPE>
TQuaternion<TYPE> TQuaternion<TYPE>::Power(const TYPE & scale) const
{
TQuaternion<TYPE> res = *this;
if ((scale < 1e-7) && (scale > -1e-7)) {
//scale near zero -> return identity
res.SetIdentity();
} else if ((res[3] > 0.999999) || (res[3] < -0.999999)) {
//this is near identity (numerical unstable) -> return identity
//res = *this
} else {
//else use slerp interpolation between identity and this with scale
TYPE temp1 = sin(scale * acos(res[3]))/sin(acos(res[3]));
TYPE res3 = res[3];
res *= temp1;
res[3] += TYPE(sin((1.0-scale) * acos(res3))/sin(acos(res3)));
}
return res;
}
template <typename TYPE>
TQuaternion<TYPE> TQuaternion<TYPE>::InterpolateLinear(const TQuaternion<TYPE> &to, const TYPE & t) const
{
BIASASSERT(t >= 0.0 && t <= 1.0);
TQuaternion<TYPE> p = *this;
TQuaternion<TYPE> q = to;
//p.Normalize();
//q.Normalize();
if (t < 1e-7) {
//t near zero -> return p
return p;
} else if (1.0-t < 1e-7) {
//t near one -> return q
return q;
}
//else use linear interpolation between this and q
TQuaternion<TYPE> res;
const TYPE cosPhi = (TYPE)p.ScalarProduct(q);
const TYPE a = (TYPE)1.0 - t;
const TYPE b = (cosPhi < 0 ? -t : t);
res[0] = a*p[0] + b*q[0];
res[1] = a*p[1] + b*q[1];
res[2] = a*p[2] + b*q[2];
res[3] = a*p[3] + b*q[3];
res.Normalize();
return res;
}
template <typename TYPE>
TQuaternion<TYPE> TQuaternion<TYPE>::
Interpolate(const TQuaternion<TYPE> &to, const TYPE & t) const
{
BIASASSERT(t >= 0.0 && t <= 1.0);
TQuaternion<TYPE> p = *this;
TQuaternion<TYPE> q = to;
//p.Normalize();
//q.Normalize();
if (t < 1e-7) {
//t near zero -> return p
return p;
} else if (1.0-t < 1e-7) {
//t near one -> return q
return q;
}
TQuaternion<TYPE> res;
TYPE a, b;
TYPE cosPhi = (TYPE)p.ScalarProduct(q);
//adjust angle if necessary
if (cosPhi < 0) {
q.MultiplyIP(-1.0);
cosPhi = -cosPhi;
}
if (cosPhi > 0.9999 || cosPhi < -0.9999) {
// p is very close to q, do linear interpolation instead of slerp
a = (TYPE)1.0 - t;
b = (cosPhi < 0 ? -t : t);
} else {
//else use slerp interpolation between p and q
const TYPE phi = acos(cosPhi < 0 ? -cosPhi : cosPhi);
const TYPE sinPhi = sin(phi);
a = sin(((TYPE)1.0 - t) * phi) / sinPhi;
b = sin(t * phi) / sinPhi;
if (cosPhi < 0) b = -b;
}
res[0] = a*p[0] + b*q[0];
res[1] = a*p[1] + b*q[1];
res[2] = a*p[2] + b*q[2];
res[3] = a*p[3] + b*q[3];
res.Normalize();
return res;
}
template <typename TYPE>
int TQuaternion<TYPE>::SetXYZ(TYPE radX, TYPE radY, TYPE radZ)
{
TQuaternion<TYPE> q_x;
TQuaternion<TYPE> q_y;
TQuaternion<TYPE> q_z;
q_x.SetValueAsAxisRad( 1.,0.,0., radX );
q_y.SetValueAsAxisRad( 0.,1.,0., radY );
q_z.SetValueAsAxisRad( 0.,0.,1., radZ );
q_x.Mult(q_y);
q_x.Mult(q_z);
*this = q_x;
return 0;
}
template <typename TYPE>
int TQuaternion<TYPE>::SetZYX(TYPE radX, TYPE radY, TYPE radZ)
{
TQuaternion<TYPE> q_x;
TQuaternion<TYPE> q_y;
TQuaternion<TYPE> q_z;
q_x.SetValueAsAxisRad( 1.,0.,0., radX );
q_y.SetValueAsAxisRad( 0.,1.,0., radY );
q_z.SetValueAsAxisRad( 0.,0.,1., radZ );
q_z.Mult(q_y);
q_z.Mult(q_x);
*this = q_z;
return 0;
}
template <typename TYPE>
TQuaternion<TYPE>& TQuaternion<TYPE>::operator=(const TQuaternion<TYPE>& vec)
{
memcpy((void *)val, (void *)vec.val, sizeof(TQuaternion<TYPE>));
return *this;
}
template <typename TYPE>
void TQuaternion<TYPE>::EnforceRigidCouplingConstraint(TQuaternion<TYPE> &other)
{
// @todo check unit norm constraint of this and other quaternion!!
// make this and other quaternion unique
MakeUnique();
other.MakeUnique();
// get aliases for both quaternions
TQuaternion<TYPE> &q0 = *this, &q1 = other;
// interpolate equal angle
q0[3] = q1[3] = TYPE(0.5) * (q0[3] + q1[3]);
// re-enforce unit length constraint for q0 and q1
TYPE q0norm = q0[0]*q0[0] + q0[1]*q0[1] + q0[2]*q0[2];
TYPE q1norm = q1[0]*q1[0] + q1[1]*q1[1] + q1[2]*q1[2];
if (q0norm > TYPE(1e-8) && q1norm > TYPE(1e-8)) {
TYPE qscale0 = (TYPE)sqrt(double((1-q0[3]*q0[3])/q0norm));
TYPE qscale1 = (TYPE)sqrt(double((1-q1[3]*q1[3])/q1norm));
for (int l = 0; l < 3; l++) {
q0[l] *= qscale0;
q1[l] *= qscale1;
}
}
// @todo check unit norm constraint of resulting quaternions!!
}
template <typename TYPE>
void TQuaternion<TYPE>::EnforceRigidCouplingConstraint(std::vector<TQuaternion<TYPE> > &quats)
{
// @todo check unit norm constraint of all quaternions!!
// make all quaternions unique
const int n = quats.size();
for (int i = 0; i < n; i++)
quats[i].MakeUnique();
// interpolate equal angle
double a = 0;
for (int i = 0; i < n; i++)
a += quats[i][3];
a /= TYPE(n);
for (int i = 0; i < n; i++)
quats[i][3] = a;
// re-enforce unit length constraint for all quaternions
for (int i = 0; i < n; i++) {
TQuaternion<TYPE> &q = quats[i];
TYPE qnorm = q[0]*q[0] + q[1]*q[1] + q[2]*q[2];
if (qnorm > TYPE(1e-8)) {
TYPE qscale = (TYPE)sqrt(double((1-q[3]*q[3])/qnorm));
for (int l = 0; l < 3; l++)
q[l] *= qscale;
}
}
// @todo check unit norm constraint of resulting quaternions!!
}
/*----------------------------------------------------------------*/
// C L A S S //////////////////////////////////////////////////////
// accuracy for constraint check in Check() function calls
#define R_CONSTRAINT_ACCURACY 1e-5 //ZEROTOLERANCE<TYPE>()
// if rotation angle is below this value, assume zero rotation
#define MINIMUM_ROTATION 1e-7
template <typename TYPE>
inline TRMatrixBase<TYPE>::TRMatrixBase()
: Base(Base::IDENTITY)
{}
template <typename TYPE>
inline TRMatrixBase<TYPE>::TRMatrixBase(const Mat& mat)
: Base(mat)
{
ASSERT(Check(R_CONSTRAINT_ACCURACY));
}
template <typename TYPE>
inline TRMatrixBase<TYPE>::TRMatrixBase(const Vec& w, TYPE phi)
{
Set(w, phi);
}
template <typename TYPE>
inline TRMatrixBase<TYPE>::TRMatrixBase(const Vec& rot)
{
SetRotationAxisAngle(rot);
}
template <typename TYPE>
inline TRMatrixBase<TYPE>::TRMatrixBase(const Quat& q)
{
SetFromQuaternion(q);
}
template <typename TYPE>
inline TRMatrixBase<TYPE>::TRMatrixBase(TYPE roll, TYPE pitch, TYPE yaw)
{
SetXYZ(roll, pitch, yaw);
}
template <typename TYPE>
inline TRMatrixBase<TYPE>::TRMatrixBase(const Vec& dir0, const Vec& dir1)
{
SetFromDir2Dir(dir0, dir1);
}
template <typename TYPE>
void TRMatrixBase<TYPE>::SetXYZ(TYPE PhiX, TYPE PhiY, TYPE PhiZ)
{
#if 0
TRMatrixBase<TYPE> Rx(TRMatrixBase<TYPE>::IDENTITY);
TRMatrixBase<TYPE> Ry(TRMatrixBase<TYPE>::IDENTITY);
TRMatrixBase<TYPE> Rz(TRMatrixBase<TYPE>::IDENTITY);
/* set Rz, Ry, Rx as rotation matrices */
Rz(0,0) = cos(PhiZ);
Rz(0,1) = -sin(PhiZ);
Rz(1,0) = -Rz(0,1);
Rz(1,1) = Rz(0,0);
//Rz(2,2) = 1;
Ry(0,0) = cos(PhiY);
Ry(0,2) = sin(PhiY);
//Ry(1,1) = 1;
Ry(2,0) = -Ry(0,2); //-sin(PhiY);
Ry(2,2) = Ry(0,0); //cos(PhiY);
//Rx(0,0) = 1;
Rx(1,1) = cos(PhiX);
Rx(1,2) = -sin(PhiX);
Rx(2,1) = -Rx(1,2); //sin(PhiX);
Rx(2,2) = Rx(1,1); //cos(PhiX);
*this = Rx*Ry*Rz;
#else
const TYPE sin_x = sin(PhiX);
const TYPE sin_y = sin(PhiY);
const TYPE sin_z = sin(PhiZ);
const TYPE cos_x = cos(PhiX);
const TYPE cos_y = cos(PhiY);
const TYPE cos_z = cos(PhiZ);
val[0] = cos_y * cos_z;
val[1] = -cos_y * sin_z;
val[2] = sin_y;
val[3] = cos_x * sin_z + cos_z * sin_x * sin_y;
val[4] = cos_x * cos_z - sin_x * sin_y * sin_z;
val[5] = -cos_y * sin_x;
val[6] = sin_x * sin_z - cos_x * cos_z * sin_y;
val[7] = cos_z * sin_x + cos_x * sin_y * sin_z;
val[8] = cos_x * cos_y;
#endif
}
template <typename TYPE>
void TRMatrixBase<TYPE>::SetZYX(TYPE PhiX, TYPE PhiY, TYPE PhiZ)
{
TRMatrixBase<TYPE> Rx(TRMatrixBase<TYPE>::IDENTITY);
TRMatrixBase<TYPE> Ry(TRMatrixBase<TYPE>::IDENTITY);
TRMatrixBase<TYPE> Rz(TRMatrixBase<TYPE>::IDENTITY);
/* set Rz, Ry, Rx as rotation matrices */
Rz(0,0) = cos(PhiZ);
Rz(0,1) = -sin(PhiZ);
Rz(1,0) = -Rz(0,1);
Rz(1,1) = Rz(0,0);
//Rz(2,2) = 1;
Ry(0,0) = cos(PhiY);
Ry(0,2) = sin(PhiY);
//Ry(1,1) = 1;
Ry(2,0) = -Ry(0,2); //-sin(PhiY);
Ry(2,2) = Ry(0,0); //cos(PhiY);
//Rx(0,0) = 1;
Rx(1,1) = cos(PhiX);
Rx(1,2) = -sin(PhiX);
Rx(2,1) = -Rx(1,2); //sin(PhiX);
Rx(2,2) = Rx(1,1); //cos(PhiX);
*this = Rz*Ry*Rx;
}
template <typename TYPE>
void TRMatrixBase<TYPE>::SetYXZ(TYPE PhiY, TYPE PhiX, TYPE PhiZ)
{
TRMatrixBase<TYPE> Rx(TRMatrixBase<TYPE>::IDENTITY);
TRMatrixBase<TYPE> Ry(TRMatrixBase<TYPE>::IDENTITY);
TRMatrixBase<TYPE> Rz(TRMatrixBase<TYPE>::IDENTITY);
/* set Rz, Ry, Rx as rotation matrices */
Rz(0,0) = cos(PhiZ);
Rz(0,1) = -sin(PhiZ);
Rz(1,0) = -Rz(0,1);
Rz(1,1) = Rz(0,0);
Ry(0,0) = cos(PhiY);
Ry(0,2) = sin(PhiY);
Ry(2,0) = -Ry(0,2); //-sin(PhiY);
Ry(2,2) = Ry(0,0); //cos(PhiY);
Rx(1,1) = cos(PhiX);
Rx(1,2) = -sin(PhiX);
Rx(2,1) = -Rx(1,2); //sin(PhiX);
Rx(2,2) = Rx(1,1); //cos(PhiX);
*this = Ry*Rx*Rz;
}
template <typename TYPE>
void TRMatrixBase<TYPE>::SetZXY(TYPE PhiX, TYPE PhiY, TYPE PhiZ)
{
TRMatrixBase<TYPE> Rx(TRMatrixBase<TYPE>::IDENTITY);
TRMatrixBase<TYPE> Ry(TRMatrixBase<TYPE>::IDENTITY);
TRMatrixBase<TYPE> Rz(TRMatrixBase<TYPE>::IDENTITY);
/* set Rz, Ry, Rx as rotation matrices */
Rz(0,0) = cos(PhiZ);
Rz(0,1) = -sin(PhiZ);
Rz(1,0) = -Rz(0,1);
Rz(1,1) = Rz(0,0);
Ry(0,0) = cos(PhiY);
Ry(0,2) = sin(PhiY);
Ry(2,0) = -Ry(0,2); //-sin(PhiY);
Ry(2,2) = Ry(0,0); //cos(PhiY);
Rx(1,1) = cos(PhiX);
Rx(1,2) = -sin(PhiX);
Rx(2,1) = -Rx(1,2); //sin(PhiX);
Rx(2,2) = Rx(1,1); //cos(PhiX);
*this = Rz*Rx*Ry;
}
template <typename TYPE>
void TRMatrixBase<TYPE>::Set(const Vec& wa, TYPE phi)
{
// zero rotation results in identity matrix
if (ISZERO(phi)) {
*this = Base::IDENTITY;
return;
}
const TYPE wnorm(norm(wa));
if (wnorm < std::numeric_limits<TYPE>::epsilon()) {
CPC_ERROR("Vector "<<wa<<" is close to zero (norm = "<<wnorm<<"), solution is instable!");
}
const Vec w(wa*(TYPE(1)/wnorm));
Mat Omega;
Omega(0,0) = TYPE(0);
Omega(0,1) = -w[2];
Omega(0,2) = w[1];
Omega(1,0) = w[2];
Omega(1,1) = TYPE(0);
Omega(1,2) = -w[0];
Omega(2,0) = -w[1] ;
Omega(2,1) = w[0];
Omega(2,2) = TYPE(0);
const Mat Sin_O(Omega * sin(phi));
const Mat Cos_O_O((Omega * Omega) * (TYPE(1)-cos(phi)));
*this = Base::IDENTITY + Sin_O + Cos_O_O;
}
template <typename TYPE>
void TRMatrixBase<TYPE>::SetFromColumnVectors(const Vec& v0, const Vec& v1, const Vec& v2)
{
val[0] = v0[0];
val[1] = v1[0];
val[2] = v2[0];
val[3] = v0[1];
val[4] = v1[1];
val[5] = v2[1];
val[6] = v0[2];
val[7] = v1[2];
val[8] = v2[2];
ASSERT(Check(R_CONSTRAINT_ACCURACY));
}
template <typename TYPE>
void TRMatrixBase<TYPE>::SetFromRowVectors(const Vec& v0, const Vec& v1, const Vec& v2)
{
val[0] = v0[0];
val[1] = v0[1];
val[2] = v0[2];
val[3] = v1[0];
val[4] = v1[1];
val[5] = v1[2];
val[6] = v2[0];
val[7] = v2[1];
val[8] = v2[2];
ASSERT(Check(R_CONSTRAINT_ACCURACY));
}
template <typename TYPE>
void TRMatrixBase<TYPE>::SetFromAxisAngle(const Vec& w)
{
#if 0
const TYPE dAngle(norm(w));
Set(w*INVERT(dAngle), dAngle);
#else
SetRotationAxisAngle(w);
#endif
}
template <typename TYPE>
void TRMatrixBase<TYPE>::GetQuaternion(Quat& quat) const
{
ASSERT(Check(R_CONSTRAINT_ACCURACY));
#if 1
// Conversion method from OpenSG
// @attention Removed old conversion in Linux build which had numerical
// issues for rotations close to 180 degree! (esquivel 03/2011)
TYPE tr = Base::operator()(0,0) + Base::operator()(1,1) + Base::operator()(2,2);
if (tr > 0.0)
{
// Use default formula if trace is large enough
TYPE s = sqrt(tr + 1.0);
quat[3] = s * 0.5;
s *= 2.0;
quat[0] = (Base::operator()(2,1) - Base::operator()(1,2)) / s;
quat[1] = (Base::operator()(0,2) - Base::operator()(2,0)) / s;
quat[2] = (Base::operator()(1,0) - Base::operator()(0,1)) / s;
}
else
{
// Use alternate formula using largest trace element
TYPE s;
unsigned i;
unsigned j;
unsigned k;
// Find largest trace element i and its successors j, k
if(Base::operator()(1,1) > Base::operator()(0,0))
i = 1;
else
i = 0;
if(Base::operator()(2,2) > Base::operator()(i,i))
i = 2;
j = (i + 1) % 3;
k = (j + 1) % 3;
// Compute discriminator using largest trace element
s = sqrt(Base::operator()(i,i) - (Base::operator()(j,j) + Base::operator()(k,k)) + 1.0);
// Compute quaternion entries
quat[i] = s * 0.5;
s *= 2.0;
quat[j] = (Base::operator()(i,j) + Base::operator()(j,i)) / s;
quat[k] = (Base::operator()(i,k) + Base::operator()(k,i)) / s;
quat[3] = (Base::operator()(k,j) - Base::operator()(j,k) ) / s;
}
#elif 1
// Conversion method using derivation depending on max. denominator
// @see http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29#Rotation_matrix_.E2.86.94_quaternion
// @deprecated
// Determine max. denominator
TYPE d[4] = { 1 + Base::operator()(0,0) - Base::operator()(1,1) - Base::operator()(2,2),
1 - Base::operator()(0,0) + Base::operator()(1,1) - Base::operator()(2,2),
1 - Base::operator()(0,0) - Base::operator()(1,1) + Base::operator()(2,2),
1 + Base::operator()(0,0) + Base::operator()(1,1) + Base::operator()(2,2) };
int maxidx = 0;
for (int i = 1; i < 4; i++) {
if (d[maxidx] < d[i]) maxidx = i;
}
// Compute quaternion starting with max. denominator
if (maxidx == 0) {
quat[0]=0.5*sqrt(1 + Base::operator()(0,0) - Base::operator()(1,1) - Base::operator()(2,2));
quat[3]=0.25*(Base::operator()(2,1)-Base::operator()(1,2))/quat[0];
quat[1]=0.25*(Base::operator()(0,1)+Base::operator()(1,0))/quat[0];
quat[2]=0.25*(Base::operator()(0,2)+Base::operator()(2,0))/quat[0];
} else if (maxidx == 1) {
quat[1]=0.5*sqrt(1 - Base::operator()(0,0) + Base::operator()(1,1) - Base::operator()(2,2));
quat[3]=0.25*(Base::operator()(0,2)-Base::operator()(2,0))/quat[1];
quat[2]=0.25*(Base::operator()(1,2)+Base::operator()(2,1))/quat[1];
quat[0]=0.25*(Base::operator()(1,0)+Base::operator()(0,1))/quat[1];
} else if (maxidx == 2) {
quat[2]=0.5*sqrt(1 - Base::operator()(0,0) - Base::operator()(1,1) + Base::operator()(2,2));
quat[3]=0.25*(Base::operator()(1,0)-Base::operator()(0,1))/quat[2];
quat[0]=0.25*(Base::operator()(2,0)+Base::operator()(0,2))/quat[2];
quat[1]=0.25*(Base::operator()(2,1)+Base::operator()(1,2))/quat[2];
} else {
quat[3]=0.5*sqrt(1 + Base::operator()(0,0) + Base::operator()(1,1) + Base::operator()(2,2));
quat[0]=0.25*(Base::operator()(2,1)-Base::operator()(1,2))/quat[3];
quat[1]=0.25*(Base::operator()(0,2)-Base::operator()(2,0))/quat[3];
quat[2]=0.25*(Base::operator()(1,0)-Base::operator()(0,1))/quat[3];
}
#elif 1
// More compact implementation of the conversion method from OpenSG
// @deprecated
TYPE trace = Base::operator()(0,0) + Base::operator()(1,1) + Base::operator()(2,2);
if ( trace > 0 ) {
TYPE s = 2.0 * sqrt(trace + 1.0);
quat[3] = 0.25 * s;
quat[0] = ( Base::operator()(2,1) - Base::operator()(1,2) ) / s;
quat[1] = ( Base::operator()(0,2) - Base::operator()(2,0) ) / s;
quat[2] = ( Base::operator()(1,0) - Base::operator()(0,1) ) / s;
} else {
if ( Base::operator()(0,0) > Base::operator()(1,1) && Base::operator()(0,0) > Base::operator()(2,2) ) {
TYPE s = 2.0 * sqrt( 1.0 + Base::operator()(0,0) - Base::operator()(1,1) - Base::operator()(2,2));
quat[3] = (Base::operator()(2,1) - Base::operator()(1,2) ) / s;
quat[0] = 0.25 * s;
quat[1] = (Base::operator()(0,1) + Base::operator()(1,0) ) / s;
quat[2] = (Base::operator()(0,2) + Base::operator()(2,0) ) / s;
} else if (Base::operator()(1,1) > Base::operator()(2,2)) {
TYPE s = 2.0 * sqrt( 1.0 + Base::operator()(1,1) - Base::operator()(0,0) - Base::operator()(2,2));
quat[3] = (Base::operator()(0,2) - Base::operator()(2,0) ) / s;
quat[0] = (Base::operator()(0,1) + Base::operator()(1,0) ) / s;
quat[1] = 0.25 * s;
quat[2] = (Base::operator()(1,2) + Base::operator()(2,1) ) / s;
} else {
TYPE s = 2.0 * sqrt( 1.0 + Base::operator()(2,2) - Base::operator()(0,0) - Base::operator()(1,1) );
quat[3] = (Base::operator()(1,0) - Base::operator()(0,1) ) / s;
quat[0] = (Base::operator()(0,2) + Base::operator()(2,0) ) / s;
quat[1] = (Base::operator()(1,2) + Base::operator()(2,1) ) / s;
quat[2] = 0.25 * s;
}
}
#else
// Old conversion method
// @see http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
// @deprecated
// @attention Computes wrong quaternion for rotation close to 180 degree!
quat[3] = sqrt( max( 0.0, 1.0 + Base::operator()(0,0) + Base::operator()(1,1) + Base::operator()(2,2) ) ) / 2.0;
quat[0] = sqrt( max( 0.0, 1.0 + Base::operator()(0,0) - Base::operator()(1,1) - Base::operator()(2,2) ) ) / 2.0;
quat[1] = sqrt( max( 0.0, 1.0 - Base::operator()(0,0) + Base::operator()(1,1) - Base::operator()(2,2) ) ) / 2.0;
quat[2] = sqrt( max( 0.0, 1.0 - Base::operator()(0,0) - Base::operator()(1,1) + Base::operator()(2,2) ) ) / 2.0;
quat[0] = copysign( quat[0], Base::operator()(2,1) - Base::operator()(1,2) ) ;
quat[1] = copysign( quat[1], Base::operator()(0,2) - Base::operator()(2,0) ) ;
quat[2] = copysign( quat[2], Base::operator()(1,0) - Base::operator()(0,1) ) ;
#endif
// Normalize resulting quaternion
quat.Normalize();
// Correct quaternion so that scalar part is non-negative
if (quat[3] < 0) quat *= -1.0;
}
template <typename TYPE>
void TRMatrixBase<TYPE>::GetRotationAxisAngle(Vec& Axis, TYPE& Angle) const
{
// nicked from OpenSG
GetQuaternion().GetAxisAngle(Axis, Angle);
}
template <typename TYPE>
typename TRMatrixBase<TYPE>::Vec TRMatrixBase<TYPE>::GetRotationAxis() const
{
Vec Axis;
TYPE Angle;
GetRotationAxisAngle(Axis, Angle);
return Axis;
}
template <typename TYPE>
TYPE TRMatrixBase<TYPE>::GetRotationAngle() const
{
Vec Axis;
TYPE Angle;
GetRotationAxisAngle(Axis, Angle);
return Angle;
}
template <typename TYPE>
int TRMatrixBase<TYPE>::GetRotationAnglesXYZ(TYPE& PhiX, TYPE& PhiY, TYPE& PhiZ) const
{
#if 1
if (Base::operator()(0,2) < 1)
{
if (Base::operator()(0,2) > -1)
{
PhiY = asin(Base::operator()(0,2));
PhiX = atan2(-Base::operator()(1,2), Base::operator()(2,2));
PhiZ = atan2(-Base::operator()(0,1), Base::operator()(0,0));
}
else
{ // WARNING. Not a unique solution.
PhiY = -FHALF_PI;
PhiX = -atan2(Base::operator()(1,0), Base::operator()(1,1));
PhiZ = 0.f; // any angle works
}
}
else
{ // WARNING. Not a unique solution.
PhiY = FHALF_PI;
PhiX = atan2(Base::operator()(1,0), Base::operator()(1,1));
PhiZ = 0.f; // any angle works
}
#else
if ((Base::operator()(0,2) == 1.0)||(Base::operator()(0,2) == -1.0)) {
ASSERT("Rotation for Y-axis is +-PI/2, can not decompose X-/Z-component!" == NULL);
return -1;
}
PhiY = asin(Base::operator()(0,2));
if (ISINFORNAN(PhiY)) return -1;
// changed this from atan to atan2 (koeser)
PhiX = atan2(-Base::operator()(1,2),Base::operator()(2,2));
if (ISINFORNAN(PhiX)) return -2;
PhiZ = atan2(-Base::operator()(0,1),Base::operator()(0,0));
if (ISINFORNAN(PhiZ)) return -3;
#endif
// #ifdef _CPC_DEBUG
// TRMatrixBase<TYPE> R;
// R.SetXYZ(PhiX, PhiY, PhiZ);
// if (R == (*this))
// res = 0;
// else
// res = -1;
// #endif
return 0;
}
template <typename TYPE>
int TRMatrixBase<TYPE>::GetRotationAnglesZXY(TYPE& PhiX, TYPE& PhiY, TYPE& PhiZ) const
{
if ((Base::operator()(0,2) == 1.0)||(Base::operator()(0,2) == -1.0)) {
DEBUG("Rotation for Y-axis is +-PI/2, can not decompose X-/Z-component!");
return -1;
}
PhiZ = atan2(-Base::operator()(0,1),Base::operator()(1,1));
if (ISINFORNAN(PhiZ)) return -3;
PhiX = asin(Base::operator()(2,1));
if (ISINFORNAN(PhiX)) return -2;
PhiY = atan2(-Base::operator()(2,0),Base::operator()(2,2));
if (ISINFORNAN(PhiY)) return -1;
// #ifdef _CPC_DEBUG
// TRMatrixBase<TYPE> R;
// R.SetZXY(PhiX, PhiY, PhiZ);
// cout<<"Erg R: "<<R<<endl;
// if (R == (*this))
// res = 0;
// else
// res = -1;
// #endif
return 0;
}
template <typename TYPE>
int TRMatrixBase<TYPE>::GetRotationAnglesYXZ(TYPE& PhiY, TYPE& PhiX, TYPE& PhiZ) const
{
if ((Base::operator()(0,2) == 1.0)||(Base::operator()(0,2) == -1.0)) {
DEBUG("Rotation for Y-axis is +-PI/2, can not decompose X-/Z-component!");
return -1;
}
PhiZ = atan2(Base::operator()(0,2),Base::operator()(2,2));
if (ISINFORNAN(PhiZ)) return -3;
PhiX = asin(-Base::operator()(1,2));
if (ISINFORNAN(PhiX)) return -2;
PhiY = atan2(Base::operator()(1,0),Base::operator()(1,1));
if (ISINFORNAN(PhiY)) return -1;
// #ifdef _CPC_DEBUG
// TRMatrixBase<TYPE> R;
// R.SetZXY(PhiX, PhiY, PhiZ);
// cout<<"Erg R: "<<R<<endl;
// if (R == (*this))
// res = 0;
// else
// res = -1;
// #endif
return 0;
}
template <typename TYPE>
int TRMatrixBase<TYPE>::GetRotationAnglesZYX(TYPE& PhiX, TYPE& PhiY, TYPE& PhiZ) const
{
if ((Base::operator()(0,2) == 1.0)||(Base::operator()(0,2) == -1.0)) {
DEBUG("Rotation for Y-axis is +-PI/2, cannot decompose X-/Z-component!");
return -1;
}
PhiY = asin(-Base::operator()(2,0));
if (ISINFORNAN(PhiY)) return -1;
// changed this from atan to atan2 (koeser)
PhiX = atan2(Base::operator()(2,1),Base::operator()(2,2));
if (ISINFORNAN(PhiX)) return -2;
PhiZ = atan2(Base::operator()(1,0),Base::operator()(0,0));
if (ISINFORNAN(PhiZ)) return -3;
// #ifdef _CPC_DEBUG
// TRMatrixBase<TYPE> R;
// R.SetZYX(PhiX, PhiY, PhiZ);
// if (R == (*this))
// res = 0;
// else
// res = -1;
// #endif
return 0;
}
template <typename TYPE>
void TRMatrixBase<TYPE>::SetFromQuaternion(const Quat& qu)
{
// fast version, see below for old code:
const TYPE nrm = 1.0/norm(qu);
const TYPE q0 = qu[0]*nrm;
const TYPE q1 = qu[1]*nrm;
const TYPE q2 = qu[2]*nrm;
const TYPE q3 = qu[3]*nrm;
TYPE* pData = val;
*pData++ = 1.0 - 2.0 * (q1 * q1 + q2 * q2);
*pData++ = 2.0 * (q0 * q1 - q2 * q3);
*pData++ = 2.0 * (q2 * q0 + q1 * q3);
*pData++ = 2.0 * (q0 * q1 + q2 * q3);
*pData++ = 1.0 - 2.0 * (q2 * q2 + q0 * q0);
*pData++ = 2.0 * (q1 * q2 - q0 * q3);
*pData++ = 2.0 * (q2 * q0 - q1 * q3);
*pData++ = 2.0 * (q1 * q2 + q0 * q3);
*pData++ = 1.0 - 2.0 * (q1 * q1 + q0 * q0);
#ifdef WANT_TO_COMPARE_WITH_OLD_CODE
Quat q = qu; // de-const
q.Normalize();
// nicked from OpenSG
TRMatrixBase<TYPE> RCompare(MatrixZero);
RCompare[0][0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
RCompare[1][0] = 2.0 * (q[0] * q[1] + q[2] * q[3]);
RCompare[2][0] = 2.0 * (q[2] * q[0] - q[1] * q[3]);
RCompare[0][1] = 2.0 * (q[0] * q[1] - q[2] * q[3]);
RCompare[1][1] = 1.0 - 2.0 * (q[2] * q[2] + q[0] * q[0]);
RCompare[2][1] = 2.0 * (q[1] * q[2] + q[0] * q[3]);
RCompare[0][2] = 2.0 * (q[2] * q[0] + q[1] * q[3]);
RCompare[1][2] = 2.0 * (q[1] * q[2] - q[0] * q[3]);
RCompare[2][2] = 1.0 - 2.0 * (q[1] * q[1] + q[0] * q[0]);
for (unsigned i=0; i<3; i++) {
for (unsigned j=0; j<3; j++) {
if (!equal(Base::operator()(i,j), RCompare[i][j], TYPE(1e-12))) {
SLOG("Matrices are not same !"<<*this<<" "<<RCompare<<std::endl);
SLOG(" at "<<i<<" "<<j<<" "<< (Base::operator()(i,j))-(RCompare[i][j])<<std::endl);
ABORT("BIASABORT");
}
}
}
#endif
}
template <typename TYPE>
void TRMatrixBase<TYPE>::SetFromOrthogonalBasis(const Vec& xxx, const Vec& yyy, const Vec& zzz)
{
SetFromRowVectors(normalized(xxx),
normalized(yyy),
normalized(zzz));
}
template <typename TYPE>
void TRMatrixBase<TYPE>::SetFromHV(const Vec& xxx, const Vec& yyy)
{
// normalize
const Vec x0(normalized(xxx));
// compute base z in rhs:
const Vec z0(normalized(cross(xxx, yyy)));
// compute y base orthogonal in rhs
const Vec y0(normalized(cross(z0, x0)));
SetFromColumnVectors(x0, y0, z0);
}
template <typename TYPE>
TRMatrixBase<TYPE>& TRMatrixBase<TYPE>::SetFromDir2Dir(const Vec& dir0, const Vec& dir1)
{
ASSERT(ISEQUAL(norm(dir0), TYPE(1)));
ASSERT(ISEQUAL(norm(dir1), TYPE(1)));
const TYPE cos01(CLAMP(dir1.dot(dir0), TYPE(-1), TYPE(1)));
const TYPE sin01Sq(TYPE(1) - SQUARE(cos01));
if (sin01Sq > EPSILONTOLERANCE<TYPE>()) {
const Vec v(cross(dir0, dir1));
const Mat V(CreateCrossProductMatrix3T<TYPE>(v));
*this = Mat::IDENTITY + V + (V*V)*((TYPE(1)-cos01)/sin01Sq);
} else {
*this = Mat::ZERO;
if (cos01 > TYPE(0)) {
Base::operator()(0,0) = TYPE(1);
Base::operator()(1,1) = TYPE(1);
Base::operator()(2,2) = TYPE(1);
} else {
Base::operator()(0,0) = TYPE(-1);
Base::operator()(1,1) = TYPE(-1);
Base::operator()(2,2) = TYPE(-1);
}
}
return *this;
}
template <typename TYPE>
void TRMatrixBase<TYPE>::SetFromDirUpGL(const Vec& viewDir, const Vec& viewUp)
{
ASSERT(ISEQUAL(norm(viewDir), TYPE(1)));
ASSERT(ISEQUAL(norm(viewUp), TYPE(1)));
const Vec right(normalized(cross(viewDir, viewUp)));
const Vec up(normalized(cross(right, viewDir)));
const Vec forward(viewDir * TYPE(-1)); // convert to right handed system
SetFromColumnVectors(right, up, forward);
}
template <typename TYPE>
void TRMatrixBase<TYPE>::SetFromDirUp(const Vec& viewDir, const Vec& viewUp)
{
ASSERT(ISEQUAL(norm(viewDir), TYPE(1)));
ASSERT(ISEQUAL(norm(viewUp), TYPE(1)));
const Vec right(normalized(cross(viewDir, viewUp)));
const Vec up(normalized(cross(viewDir, right)));
const Vec& forward(viewDir);
SetFromColumnVectors(right, up, forward);
}
template <typename TYPE>
void TRMatrixBase<TYPE>::LookAt(const Vec& from, const Vec& to, const Vec& up)
{
SetFromDirUp(normalized(to-from), up);
}
template <typename TYPE>
inline void TRMatrixBase<TYPE>::SetRotationAxisAngle(const Vec& rot)
{
#if 0
// set rotation using Rodriguez formula
*this = Mat::IDENTITY;
const TYPE thetaSq = normSq(rot);
if (thetaSq < TYPE(1e-12))
return;
const TYPE theta = sqrt(thetaSq);
// make cross product matrix of rot
Mat J;
J(0,0) = TYPE(0); J(0,1) = -rot.z; J(0,2) = rot.y;
J(1,0) = rot.z; J(1,1) = TYPE(0); J(1,2) = -rot.x;
J(2,0) = -rot.y; J(2,1) = rot.x; J(2,2) = TYPE(0);
// compute R matrix
const Mat J2 = J * J;
const TYPE c1 = sin(theta)/theta;
const TYPE c2 = (TYPE(1)-cos(theta))/thetaSq;
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
(*this)(i,j) += c1*J(i,j) + c2*J2(i,j);
#elif !defined(_USE_EIGEN)
// set rotation using Rodriguez formula
const TYPE x2 = rot.x*rot.x;
const TYPE y2 = rot.y*rot.y;
const TYPE z2 = rot.z*rot.z;
const TYPE t2 = x2+y2+z2;
if (t2 > TYPE(1e-12)) {
// We want to be careful to only evaluate the square root if the
// norm of the angle_axis vector is greater than zero. Otherwise
// we get a division by zero.
const TYPE t = sqrt(t2);
const TYPE ct = /*t==0.0?0.5:*/(TYPE(1)-cos(t))/t2;
const TYPE st = /*t==0.0?1.0:*/sin(t)/t;
val[0] = TYPE(1) - (y2 + z2)*ct;
val[1] = rot.x*rot.y*ct - rot.z*st;
val[2] = rot.z*rot.x*ct + rot.y*st;
val[3] = rot.x*rot.y*ct + rot.z*st;
val[4] = TYPE(1) - (z2 + x2)*ct;
val[5] = rot.y*rot.z*ct - rot.x*st;
val[6] = rot.z*rot.x*ct - rot.y*st;
val[7] = rot.y*rot.z*ct + rot.x*st;
val[8] = TYPE(1) - (x2 + y2)*ct;
} else {
// At zero, we switch to using the first order Taylor expansion.
val[0] = TYPE(1);
val[1] =-rot.z;
val[2] = rot.y;
val[3] = rot.z;
val[4] = TYPE(1);
val[5] =-rot.x;
val[6] =-rot.y;
val[7] = rot.x;
val[8] = TYPE(1);
}
#else
*this = Eigen::SO3<TYPE>(rot).get_matrix();
#endif
}
template <typename TYPE>
inline void TRMatrixBase<TYPE>::Apply(const Vec& delta)
{
const TRMatrixBase dR(delta);
*this = dR * (*this);
}
template <typename TYPE>
inline typename TRMatrixBase<TYPE>::Vec TRMatrixBase<TYPE>::GetRotationAxisAngle() const
{
#if 0
// get parametrized rotation using quaternions
Vec angle_axis;
TYPE angle;
GetQuaternion().GetAxisAngle(angle_axis, angle);
angle_axis.Normalize();
angle_axis *= angle;
return angle_axis;
#elif 0
// get parametrized rotation using Rodriguez formula
Vec angle_axis;
// x = k * 2 * sin(theta), where k is the axis of rotation.
angle_axis[0] = (*this)(2, 1) - (*this)(1, 2);
angle_axis[1] = (*this)(0, 2) - (*this)(2, 0);
angle_axis[2] = (*this)(1, 0) - (*this)(0, 1);
static const TYPE kZero(0);
static const TYPE kOne(1);
static const TYPE kTwo(2);
// Since the right hand side may give numbers just above 1.0 or
// below -1.0 leading to atan misbehaving, we threshold.
const TYPE costheta(MINF(MAXF(((*this)(0, 0) + (*this)(1, 1) + (*this)(2, 2) - kOne)/kTwo, TYPE(-1.0)), kOne));
// sqrt is guaranteed to give non-negative results, so we only
// threshold above.
const TYPE sintheta(MINF(norm(angle_axis)/kTwo, kOne));
// Use the arctan2 to get the right sign on theta
const TYPE theta(atan2(sintheta, costheta));
// Case 1: sin(theta) is large enough, so dividing by it is not a
// problem. We do not use abs here, because while jets.h imports
// std::abs into the namespace, here in this file, abs resolves to
// the int version of the function, which returns zero always.
//
// We use a threshold much larger then the machine epsilon, because
// if sin(theta) is small, not only do we risk overflow but even if
// that does not occur, just dividing by a small number will result
// in numerical garbage. So we play it safe.
static const TYPE kThreshold(TYPE(1e-12));
if ((sintheta > kThreshold) || (sintheta < -kThreshold)) {
angle_axis *= theta / (kTwo * sintheta);
return angle_axis;
}
// Case 2: theta ~ 0, means sin(theta) ~ theta to a good
// approximation.
if (costheta > kZero) {
angle_axis *= TYPE(0.5);
return angle_axis;
}
// Case 3: theta ~ pi, this is the hard case. Since theta is large,
// and sin(theta) is small. Dividing theta by sin(theta) will either
// give an overflow or worse still numerically meaningless results.
// Thus we use an alternate more complicated and expensive formula.
#ifndef _USE_EIGEN
// Since cos(theta) is negative, division by (1-cos(theta)) cannot
// overflow.
const TYPE inv_one_minus_costheta(kOne / (kOne - costheta));
// We now compute the absolute value of coordinates of the axis
// vector using the diagonal entries of R. To resolve the sign of
// these entries, we compare the sign of angle_axis[i]*sin(theta)
// with the sign of sin(theta). If they are the same, then
// angle_axis[i] should be positive, otherwise negative.
for (int i = 0; i < 3; ++i) {
angle_axis[i] = theta * sqrt(((*this)(i, i) - costheta) * inv_one_minus_costheta);
if (((sintheta < kZero) && (angle_axis[i] > kZero)) ||
((sintheta > kZero) && (angle_axis[i] < kZero)))
angle_axis[i] = -angle_axis[i];
}
#else
Eigen::AngleAxis<TYPE> aa;
aa.fromRotationMatrix(Base::EMatMap((TYPE*)this));
angle_axis[0] = aa.angle() * aa.axis()[0];
angle_axis[1] = aa.angle() * aa.axis()[1];
angle_axis[2] = aa.angle() * aa.axis()[2];
#endif
return angle_axis;
#else
return reinterpret_cast<const Eigen::SO3<TYPE>*>(this)->ln();
#endif
}
template <typename TYPE>
bool TRMatrixBase<TYPE>::Check(const TYPE eps, int verbose) const
{
// check determinant
bool ok = equal(TYPE(cv::determinant(*this)), TYPE(1), eps);
// check unit column vector length
Vec cl[3];
for (unsigned i = 0; i < 3 && ok; i++) {
cl[i] = Base::col(i);
ok = equal(norm(cl[i]), TYPE(1), eps);
}
// check orthogonality
if (ok)
ok = equal(cl[0].dot(cl[1]), TYPE(0), eps);
if (ok)
ok = equal(cl[0].dot(cl[2]), TYPE(0), eps);
if (ok)
ok = equal(cl[1].dot(cl[2]), TYPE(0), eps);
#if TD_VERBOSE != TD_VERBOSE_OFF
// print the reason in case of failure
if (!ok && VERBOSITY_LEVEL > verbose) {
std::ostringstream res(" ");
const TYPE det(cv::determinant(*this));
if (!equal(det, TYPE(1), eps)) {
res << "det != 1 (det=" << std::setprecision(30) << det << ") ";
}
for (unsigned i = 0; i < 3; i++) {
cl[i] = Base::col(i);
if (!equal(norm(cl[i]), TYPE(1), eps)) {
res << "col[" << i << "].NormL2() != 1) (= "
<< norm(cl[i]) << ") ";
}
}
if (!equal(cl[0].dot(cl[1]), TYPE(0), eps)) {
res << "col[0].ScalarProduct(c[1]) != 0 (= "
<< cl[0].dot(cl[1]) << ") ";
}
if (!equal(cl[0].dot(cl[2]), TYPE(0), eps)) {
res << "col[0].ScalarProduct(c[2]) != 0 (= "
<< cl[0].dot(cl[2]) << ") ";
}
if (!equal(cl[1].dot(cl[2]), TYPE(0), eps)) {
res << "col[1].ScalarProduct(c[2]) != 0 (= "
<< cl[1].dot(cl[2]) << ") ";
}
SLOG("Rotation matrix is invalid: " << res.str() << std::endl);
}
#endif
return ok;
}
template <typename TYPE>
void TRMatrixBase<TYPE>::EnforceOrthogonality()
{
#if 1
// use SVD decomposition to find the closest orthogonal matrix
const cv::SVD svd(*this, cv::SVD::MODIFY_A|cv::SVD::FULL_UV);
*this = svd.u*svd.vt;
#elif 1
// replaces the 3 x 3 matrix with the nearest (least squares) orthogonal matrix;
// a combination of Babylonian iteration and Berthold KP Horn's formula R = M (MT M)^-1/2
TYPE* r = this->val;
TYPE m[9], ri[9], x[9];
memcpy(m, r, 9*sizeof(TYPE));
//By trial and error I found that 4 iterates always works unless your matrix is waaay off. Limit 10.
for (unsigned iterate = 0; iterate != 10; iterate++) {
//Invert r. Use inverse and not transpose since r is only approximately orthogonal (transpose will result in slow, oscillating convergence).
InvertMatrix3x3(r, ri);
//Get x = ri m + mt r.
x[0] = m[6]*r[6]+ri[2]*m[6]+m[3]*r[3]+ri[1]*m[3]+m[0]*ri[0]+m[0]*r[0];
x[1] = m[6]*r[7]+ri[2]*m[7]+m[3]*r[4]+ri[1]*m[4]+m[0]*r[1]+ri[0]*m[1];
x[2] = m[6]*r[8]+ri[2]*m[8]+m[3]*r[5]+ri[1]*m[5]+m[0]*r[2]+ri[0]*m[2];
x[3] = r[6]*m[7]+ri[5]*m[6]+m[3]*ri[4]+r[3]*m[4]+m[0]*ri[3]+r[0]*m[1];
x[4] = m[7]*r[7]+ri[5]*m[7]+m[4]*ri[4]+m[4]*r[4]+m[1]*ri[3]+m[1]*r[1];
x[5] = m[7]*r[8]+ri[5]*m[8]+m[4]*r[5]+ri[4]*m[5]+m[2]*ri[3]+m[1]*r[2];
x[6] = m[6]*ri[8]+r[6]*m[8]+m[3]*ri[7]+m[0]*ri[6]+r[3]*m[5]+r[0]*m[2];
x[7] = m[7]*ri[8]+r[7]*m[8]+m[4]*ri[7]+m[1]*ri[6]+r[4]*m[5]+r[1]*m[2];
x[8] = m[8]*ri[8]+m[8]*r[8]+m[5]*ri[7]+m[2]*ri[6]+m[5]*r[5]+m[2]*r[2];
//Invert into ri.
InvertMatrix3x3(x, ri);
//Replace r with 2 m ri, where ri is as replaced above.
r[0] = 2*(m[2]*ri[6]+m[1]*ri[3]+m[0]*ri[0]);
r[1] = 2*(m[2]*ri[7]+m[1]*ri[4]+m[0]*ri[1]);
r[2] = 2*(m[2]*ri[8]+m[1]*ri[5]+m[0]*ri[2]);
r[3] = 2*(m[5]*ri[6]+ri[3]*m[4]+ri[0]*m[3]);
r[4] = 2*(m[5]*ri[7]+m[4]*ri[4]+ri[1]*m[3]);
r[5] = 2*(m[5]*ri[8]+m[4]*ri[5]+ri[2]*m[3]);
r[6] = 2*(ri[6]*m[8]+ri[3]*m[7]+ri[0]*m[6]);
r[7] = 2*(ri[7]*m[8]+ri[4]*m[7]+ri[1]*m[6]);
r[8] = 2*(m[8]*ri[8]+ri[5]*m[7]+ri[2]*m[6]);
//Check how off it is.
x[0] = ABS(r[0]*r[0] + r[3]*r[3] + r[6]*r[6] - TYPE(1));
x[1] = ABS(r[1]*r[1] + r[4]*r[4] + r[7]*r[7] - TYPE(1));
x[2] = ABS(r[2]*r[2] + r[5]*r[5] + r[8]*r[8] - TYPE(1));
x[3] = ABS(r[0]*r[1] + r[3]*r[4] + r[6]*r[7]);
x[4] = ABS(r[1]*r[2] + r[4]*r[5] + r[7]*r[8]);
x[5] = ABS(r[2]*r[0] + r[5]*r[3] + r[8]*r[6]);
//Max norm, by all means, really make it as accurate as possible.
TYPE maxElement(-FLT_MAX);
for (unsigned n=6; n != 0; )
if(x[--n] > maxElement)
maxElement = x[n];
if (maxElement < TYPE(1e-15)) return;
}
#else
// Matrix Orthogonalization
// by Eric Raible from "Graphics Gems", Academic Press, 1990
//
// Reorthogonalize matrix R - that is find an orthogonal matrix that is
// "close" to R by computing an approximation to the orthogonal matrix
//
// T -1/2
// RC = R(R R)
// T -1
// [RC is orthogonal because (RC) = (RC) ]
// -1/2
// To compute C, we evaluate the Taylor expansion of F(x) = (I + x)
// (where x = C - I) about x=0.
// This gives C = I - (1/2)x + (3/8)x^2 - (5/16)x^3 + ...
// By default, limit should be 8.
const unsigned max_limit = 8;
static const TYPE coef[10] = { 1, -1/2., 3/8., -5/16., 35/128., -63/256., 231/1024., -429/2048., 6435/32768., -12155/65536. };
Base& R(*this);
const Base RtR(R.t()*R);
const Base X(RtR - Matrix3x3::IDENTITY); /* RtR - I */
Base X_power(Matrix3x3::IDENTITY); /* X^0 */
Base Sum(Matrix3x3::IDENTITY); /* coef[0] * X^0 */
const unsigned limit(MINF(max_limit, (unsigned)10));
for (unsigned power=1; power<limit; ++power) {
X_power = X_power * X;
Sum += coef[power] * X_power;
}
R = R * Sum;
#endif
if (cv::determinant(*this) < 0)
*this *= TYPE(-1);
ASSERT(Check(R_CONSTRAINT_ACCURACY));
}
/*----------------------------------------------------------------*/
// if the matrix is a rotation, then the the transpose should be the inverse
template<typename TYPE>
inline bool IsRotationMatrix(const TMatrix<TYPE,3,3>& R) {
ASSERT(sizeof(TMatrix<TYPE,3,3>) == sizeof(TRMatrixBase<TYPE>));
return ((const TRMatrixBase<TYPE>&)R).IsValid();
} // IsRotationMatrix
template<typename TYPE, int O>
inline bool IsRotationMatrix(const Eigen::Matrix<TYPE,3,3,O>& R) {
// the trace should be three and the determinant should be one
return (ISEQUAL(R.determinant(), TYPE(1)) && ISEQUAL((R*R.transpose()).trace(), TYPE(3)));
} // IsRotationMatrix
/*----------------------------------------------------------------*/
// enforce matrix orthogonality
template<typename TYPE>
inline void EnsureRotationMatrix(TMatrix<TYPE,3,3>& R) {
ASSERT(sizeof(TMatrix<TYPE,3,3>) == sizeof(TRMatrixBase<TYPE>));
((TRMatrixBase<TYPE>&)R).EnforceOrthogonality();
} // EnsureRotationMatrix
/*----------------------------------------------------------------*/
// compute the distance on SO(3) between the two given rotations
// using log(R) as in: "Efficient and Robust Large-Scale Rotation Averaging", 2013
// same result as above, but returns directly the angle
template<typename TYPE>
inline TYPE ComputeAngleSO3(const TMatrix<TYPE,3,3>& I) {
return TYPE(norm(((const TRMatrixBase<TYPE>&)I).GetRotationAxisAngle()));
} // ComputeAngleSO3
template<typename TYPE>
FORCEINLINE TYPE ComputeAngleSO3(const TMatrix<TYPE,3,3>& R1, const TMatrix<TYPE,3,3>& R2) {
return ComputeAngleSO3(TMatrix<TYPE,3,3>(R1*R2.t()));
} // ComputeAngleSO3
/*----------------------------------------------------------------*/
} // namespace SEACAVE