void sortSingularValues(glm::mat3 &B, glm::mat3 &V)
{
// used in step 2
glm::vec3 b1 = glm::column(B,0); glm::vec3 v1 = glm::column(V,0);
glm::vec3 b2 = glm::column(B,1); glm::vec3 v2 = glm::column(V,1);
glm::vec3 b3 = glm::column(B,2); glm::vec3 v3 = glm::column(V,2);
float rho1 = glm::length2(b1);
float rho2 = glm::length2(b2);
float rho3 = glm::length2(b3);
bool c;
c = rho1 < rho2;
condNegSwap(c,b1,b2); condNegSwap(c,v1,v2);
condSwap(c,rho1,rho2);
c = rho1 < rho3;
condNegSwap(c,b1,b3); condNegSwap(c,v1,v3);
condSwap(c,rho1,rho3);
c = rho2 < rho3;
condNegSwap(c,b2,b3); condNegSwap(c,v2,v3);
// re-build B,V
B = glm::mat3(b1,b2,b3);
V = glm::mat3(v1,v2,v3);
}
static inline void
QRGivensQuaternion(double a1, double a2, double *ch, double *sh)
{
// a1 = pivot point on diagonal
// a2 = lower triangular entry we want to annihilate
double epsilon = EPSILON;
double rho = sqrt(a1*a1 + a2*a2);
(*sh) = rho > epsilon ? a2 : 0;
(*ch) = fabs(a1) + fmax(rho, epsilon);
bool b = a1 < 0;
condSwap(b, sh, ch);
double w = mtx_rsqrt((*ch)*(*ch)+(*sh)*(*sh));
(*ch) *= w;
(*sh) *= w;
}
void QRGivensQuaternion(float a1, float a2, float &ch, float &sh)
{
// a1 = pivot point on diagonal
// a2 = lower triangular entry we want to annihilate
float epsilon = EPSILON;
float rho = sqrt(a1*a1 + a2*a2);
sh = rho > epsilon ? a2 : 0;
ch = fabs(a1) + fmax(rho,epsilon);
bool b = a1 < 0;
condSwap(b,sh,ch);
float w = glm::inversesqrt(ch*ch+sh*sh);
//float w = glm::fastInverseSqrt(ch*ch+sh*sh);
ch *= w;
sh *= w;
}
