/**
* Determine if M has positive rank. The maximum-magnitude entry of M is
* returned. The row in which it is contained is also returned.
*
* Based on "Geometric Tools" by David Eberly.
*/
static bool eig3_rank(const Matrix3x3 &m, Float& maxEntry, Vector &maxRow) {
// Locate the maximum-magnitude entry of the matrix.
maxEntry = -1.0f;
int maxRowIndex = -1;
for (int row = 0; row < 3; ++row) {
for (int col = row; col < 3; ++col) {
Float absValue = std::abs(m(row, col));
if (absValue > maxEntry) {
maxEntry = absValue;
maxRowIndex = row;
}
}
}
// Return the row containing the maximum, to be used for eigenvector
// construction.
maxRow = m.row(maxRowIndex);
return maxEntry >= std::numeric_limits<Float>::epsilon();
}
vec3 operator*(const Matrix3x3 &A, const vec3 &x)
{
return vec3(dot(A.row(0), x),
dot(A.row(1), x),
dot(A.row(2), x));
}
void dthetaR_dtheta(Matrix3x3 &out, const Matrix3x3 &R, const Matrix3x3 &dR_dthetax, const Matrix3x3 &dR_dthetay, const Matrix3x3 &dR_dthetaz)
{
//doesn't show up in <cmath> until c++11
#ifndef INFINITY
const double INFINITY = std::numeric_limits<double>::infinity();
#endif
double theta_y = asin(-R(2,0));
Vector3 delta_x(Vector3::Zero());
Vector3 delta_y, delta_z;
//theta_y = asin(-R20)
//dtheta_y/dfoo =
// -1/sqrt(1-R20^2) * dR20/dfoo
delta_y.x() = dR_dthetax(2,0);
delta_y.y() = dR_dthetay(2,0);
delta_y.z() = dR_dthetaz(2,0);
if (fabs(fabs(R(2,0)) - 1.0) < S_EPSILON) {
//delta_y *= -INFINITY;
//preserve zeros and avoid 0*infinity=nan
delta_y.x() = (fabs(delta_y.x()) < S_EPSILON ? 0 : (delta_y.x() < 0 ? INFINITY : -INFINITY));
delta_y.y() = (fabs(delta_y.y()) < S_EPSILON ? 0 : (delta_y.y() < 0 ? INFINITY : -INFINITY));
delta_y.z() = (fabs(delta_y.z()) < S_EPSILON ? 0 : (delta_y.z() < 0 ? INFINITY : -INFINITY));
}
else
delta_y /= -sqrt(1.0-R(2,0)*R(2,0));
if (fabs(theta_y - M_PI_2) < S_EPSILON ||
fabs(theta_y + M_PI_2) < S_EPSILON) {
//theta_z = atan(R12/R02)
//dtheta_z/dfoo =
// 1/(1+(R12/R02)^2) * (R02*dR12/dfoo - R12*dR02/dfoo) / R02^2
delta_z.x() = R(0,2)*dR_dthetax(1,2) - R(1,2)*dR_dthetax(0,2);
delta_z.y() = R(0,2)*dR_dthetay(1,2) - R(1,2)*dR_dthetay(0,2);
delta_z.z() = R(0,2)*dR_dthetaz(1,2) - R(1,2)*dR_dthetaz(0,2);
double divisorz = R(1,2)*R(1,2)+R(0,2)*R(0,2);
if (fabs(divisorz) < S_EPSILON) {
//delta_z *= (divisorz >= 0 ? INFINITY : -INFINITY);
//preserve zeros and avoid 0*infinity=nan
delta_z.x() = (fabs(delta_z.x()) < S_EPSILON ? 0 : (delta_z.x() > 0 ? INFINITY : -INFINITY));
delta_z.y() = (fabs(delta_z.y()) < S_EPSILON ? 0 : (delta_z.y() > 0 ? INFINITY : -INFINITY));
delta_z.z() = (fabs(delta_z.z()) < S_EPSILON ? 0 : (delta_z.z() > 0 ? INFINITY : -INFINITY));
}
else
delta_z /= divisorz;
}
else {
//theta_x = atan(R21/R22)
//dtheta_x/dfoo =
// 1/(1+(R21/R22)^2) * (R22*dR21/dfoo - R21*dR22/dfoo) / R22^2
delta_x.x() = R(2,2)*dR_dthetax(2,1) - R(2,1)*dR_dthetax(2,2);
delta_x.y() = R(2,2)*dR_dthetay(2,1) - R(2,1)*dR_dthetay(2,2);
delta_x.z() = R(2,2)*dR_dthetaz(2,1) - R(2,1)*dR_dthetaz(2,2);
double divisorx = R(2,2)*R(2,2)+R(2,1)*R(2,1);
if (fabs(divisorx) < S_EPSILON) {
//delta_x *= (divisorx >= 0 ? INFINITY : -INFINITY);
//preserve zeros and avoid 0*infinity=nan
delta_x.x() = (fabs(delta_x.x()) < S_EPSILON ? 0 : (delta_x.x() > 0 ? INFINITY : -INFINITY));
delta_x.y() = (fabs(delta_x.y()) < S_EPSILON ? 0 : (delta_x.y() > 0 ? INFINITY : -INFINITY));
delta_x.z() = (fabs(delta_x.z()) < S_EPSILON ? 0 : (delta_x.z() > 0 ? INFINITY : -INFINITY));
}
else
delta_x /= divisorx;
//theta_z = atan(R10/R00)
//dtheta_z/dfoo =
// 1/(1+(R10/R00)^2) * (R00*dR10/dfoo - R10*dR00/dfoo) / R00^2
delta_z.x() = R(0,0)*dR_dthetax(1,0) - R(1,0)*dR_dthetax(0,0);
delta_z.y() = R(0,0)*dR_dthetay(1,0) - R(1,0)*dR_dthetay(0,0);
delta_z.z() = R(0,0)*dR_dthetaz(1,0) - R(1,0)*dR_dthetaz(0,0);
double divisorz = R(0,0)*R(0,0)+R(1,0)*R(1,0);
if (fabs(divisorz) < S_EPSILON) {
//delta_z *= (divisorz >= 0 ? INFINITY : -INFINITY);
//preserve zeros and avoid 0*infinity=nan
delta_z.x() = (fabs(delta_z.x()) < S_EPSILON ? 0 : (delta_z.x() > 0 ? INFINITY : -INFINITY));
delta_z.y() = (fabs(delta_z.y()) < S_EPSILON ? 0 : (delta_z.y() > 0 ? INFINITY : -INFINITY));
delta_z.z() = (fabs(delta_z.z()) < S_EPSILON ? 0 : (delta_z.z() > 0 ? INFINITY : -INFINITY));
}
else
delta_z /= divisorz;
}
out.row(0) = delta_x;
out.row(1) = delta_y;
out.row(2) = delta_z;
}
