birkeh
/
o3de
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
development
monroegm-disable-blank-issue-2
main
2111.2
2111.1
2107.1
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'd03f946609'
${ noResults }
o3de/Code/Tools/CryCommonTools/Decompose.cpp

515 lines
19 KiB
C++
Raw Blame History

// Modifications copyright Amazon.com, Inc. or its affiliates.
#include <platform.h>
// Taken from http://tog.acm.org/GraphicsGems/gemsiv/polar_decomp/Decompose.c
/**** Decompose.c ****/
/* Ken Shoemake, 1993 */
#include <math.h>
#include "Decompose.h"
#pragma warning(disable:4244) // conversion from 'double' to 'float', possible loss of data
#pragma warning(disable:4305) // 'initializing' : truncation from 'double' to 'float'
namespace decomp {
/******* Matrix Preliminaries *******/
/** Fill out 3x3 matrix to 4x4 **/
#define mat_pad(A) (A[W][X]=A[X][W]=A[W][Y]=A[Y][W]=A[W][Z]=A[Z][W]=0,A[W][W]=1)
/** Copy nxn matrix A to C using "gets" for assignment **/
#define mat_copy(C,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
C[i][j] gets (A[i][j]);}
/** Copy transpose of nxn matrix A to C using "gets" for assignment **/
#define mat_tpose(AT,gets,A,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
AT[i][j] gets (A[j][i]);}
/** Assign nxn matrix C the element-wise combination of A and B using "op" **/
#define mat_binop(C,gets,A,op,B,n) {int i,j; for(i=0;i<n;i++) for(j=0;j<n;j++)\
C[i][j] gets (A[i][j]) op (B[i][j]);}
/** Multiply the upper left 3x3 parts of A and B to get AB **/
void mat_mult(HMatrix A, HMatrix B, HMatrix AB)
{
int i, j;
for (i = 0; i < 3; i++) for (j = 0; j < 3; j++)
AB[i][j] = A[i][0] * B[0][j] + A[i][1] * B[1][j] + A[i][2] * B[2][j];
}
/** Return dot product of length 3 vectors va and vb **/
float vdot(float* va, float* vb)
{
return (va[0] * vb[0] + va[1] * vb[1] + va[2] * vb[2]);
}
/** Set v to cross product of length 3 vectors va and vb **/
void vcross(float* va, float* vb, float* v)
{
v[0] = va[1] * vb[2] - va[2] * vb[1];
v[1] = va[2] * vb[0] - va[0] * vb[2];
v[2] = va[0] * vb[1] - va[1] * vb[0];
}
/** Set MadjT to transpose of inverse of M times determinant of M **/
void adjoint_transpose(HMatrix M, HMatrix MadjT)
{
vcross(M[1], M[2], MadjT[0]);
vcross(M[2], M[0], MadjT[1]);
vcross(M[0], M[1], MadjT[2]);
}
/******* Quaternion Preliminaries *******/
/* Construct a (possibly non-unit) quaternion from real components. */
Quat Qt_(float x, float y, float z, float w)
{
Quat qq;
qq.x = x; qq.y = y; qq.z = z; qq.w = w;
return (qq);
}
/* Return conjugate of quaternion. */
Quat Qt_Conj(Quat q)
{
Quat qq;
qq.x = -q.x; qq.y = -q.y; qq.z = -q.z; qq.w = q.w;
return (qq);
}
/* Return quaternion product qL * qR. Note: order is important!
* To combine rotations, use the product Mul(qSecond, qFirst),
* which gives the effect of rotating by qFirst then qSecond. */
Quat Qt_Mul(Quat qL, Quat qR)
{
Quat qq;
qq.w = qL.w * qR.w - qL.x * qR.x - qL.y * qR.y - qL.z * qR.z;
qq.x = qL.w * qR.x + qL.x * qR.w + qL.y * qR.z - qL.z * qR.y;
qq.y = qL.w * qR.y + qL.y * qR.w + qL.z * qR.x - qL.x * qR.z;
qq.z = qL.w * qR.z + qL.z * qR.w + qL.x * qR.y - qL.y * qR.x;
return (qq);
}
/* Return product of quaternion q by scalar w. */
Quat Qt_Scale(Quat q, float w)
{
Quat qq;
qq.w = q.w * w; qq.x = q.x * w; qq.y = q.y * w; qq.z = q.z * w;
return (qq);
}
/* Construct a unit quaternion from rotation matrix. Assumes matrix is
* used to multiply column vector on the left: vnew = mat vold. Works
* correctly for right-handed coordinate system and right-handed rotations.
* Translation and perspective components ignored. */
Quat Qt_FromMatrix(HMatrix mat)
{
/* This algorithm avoids near-zero divides by looking for a large component
* - first w, then x, y, or z. When the trace is greater than zero,
* |w| is greater than 1/2, which is as small as a largest component can be.
* Otherwise, the largest diagonal entry corresponds to the largest of |x|,
* |y|, or |z|, one of which must be larger than |w|, and at least 1/2. */
Quat qu;
double tr, s;
tr = mat[X][X] + mat[Y][Y] + mat[Z][Z];
if (tr >= 0.0) {
s = sqrt(tr + mat[W][W]);
qu.w = s * 0.5;
s = 0.5 / s;
qu.x = (mat[Z][Y] - mat[Y][Z]) * s;
qu.y = (mat[X][Z] - mat[Z][X]) * s;
qu.z = (mat[Y][X] - mat[X][Y]) * s;
} else {
int h = X;
if (mat[Y][Y] > mat[X][X]) h = Y;
if (mat[Z][Z] > mat[h][h]) h = Z;
switch (h) {
#define caseMacro(i,j,k,I,J,K) \
case I:\
s = sqrt( (mat[I][I] - (mat[J][J]+mat[K][K])) + mat[W][W] );\
qu.i = s*0.5;\
s = 0.5 / s;\
qu.j = (mat[I][J] + mat[J][I]) * s;\
qu.k = (mat[K][I] + mat[I][K]) * s;\
qu.w = (mat[K][J] - mat[J][K]) * s;\
break
caseMacro(x, y, z, X, Y, Z);
caseMacro(y, z, x, Y, Z, X);
caseMacro(z, x, y, Z, X, Y);
}
}
if (mat[W][W] != 1.0) qu = Qt_Scale(qu, 1 / sqrt(mat[W][W]));
return (qu);
}
/******* Decomp Auxiliaries *******/
static HMatrix mat_id = { {1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1} };
/** Compute either the 1 or infinity norm of M, depending on tpose **/
float mat_norm(HMatrix M, int tpose)
{
int i;
float sum, max;
max = 0.0;
for (i = 0; i < 3; i++) {
if (tpose) sum = fabs(M[0][i]) + fabs(M[1][i]) + fabs(M[2][i]);
else sum = fabs(M[i][0]) + fabs(M[i][1]) + fabs(M[i][2]);
if (max < sum) max = sum;
}
return max;
}
float norm_inf(HMatrix M) { return mat_norm(M, 0); }
float norm_one(HMatrix M) { return mat_norm(M, 1); }
/** Return index of column of M containing maximum abs entry, or -1 if M=0 **/
int find_max_col(HMatrix M)
{
float abs, max;
int i, j, col;
max = 0.0; col = -1;
for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) {
abs = M[i][j]; if (abs < 0.0) abs = -abs;
if (abs > max) { max = abs; col = j; }
}
return col;
}
/** Setup u for Household reflection to zero all v components but first **/
void make_reflector(float* v, float* u)
{
float s = sqrt(vdot(v, v));
u[0] = v[0]; u[1] = v[1];
u[2] = v[2] + ((v[2] < 0.0) ? -s : s);
s = sqrt(2.0 / vdot(u, u));
u[0] = u[0] * s; u[1] = u[1] * s; u[2] = u[2] * s;
}
/** Apply Householder reflection represented by u to column vectors of M **/
void reflect_cols(HMatrix M, float* u)
{
int i, j;
for (i = 0; i < 3; i++) {
float s = u[0] * M[0][i] + u[1] * M[1][i] + u[2] * M[2][i];
for (j = 0; j < 3; j++) M[j][i] -= u[j] * s;
}
}
/** Apply Householder reflection represented by u to row vectors of M **/
void reflect_rows(HMatrix M, float* u)
{
int i, j;
for (i = 0; i < 3; i++) {
float s = vdot(u, M[i]);
for (j = 0; j < 3; j++) M[i][j] -= u[j] * s;
}
}
/** Find orthogonal factor Q of rank 1 (or less) M **/
void do_rank1(HMatrix M, HMatrix Q)
{
float v1[3], v2[3], s;
int col;
mat_copy(Q, =, mat_id, 4);
/* If rank(M) is 1, we should find a non-zero column in M */
col = find_max_col(M);
if (col < 0) return; /* Rank is 0 */
v1[0] = M[0][col]; v1[1] = M[1][col]; v1[2] = M[2][col];
make_reflector(v1, v1); reflect_cols(M, v1);
v2[0] = M[2][0]; v2[1] = M[2][1]; v2[2] = M[2][2];
make_reflector(v2, v2); reflect_rows(M, v2);
s = M[2][2];
if (s < 0.0) Q[2][2] = -1.0;
reflect_cols(Q, v1); reflect_rows(Q, v2);
}
/** Find orthogonal factor Q of rank 2 (or less) M using adjoint transpose **/
void do_rank2(HMatrix M, HMatrix MadjT, HMatrix Q)
{
float v1[3], v2[3];
float w, x, y, z, c, s, d;
int col;
/* If rank(M) is 2, we should find a non-zero column in MadjT */
col = find_max_col(MadjT);
if (col < 0) { do_rank1(M, Q); return; } /* Rank<2 */
v1[0] = MadjT[0][col]; v1[1] = MadjT[1][col]; v1[2] = MadjT[2][col];
make_reflector(v1, v1); reflect_cols(M, v1);
vcross(M[0], M[1], v2);
make_reflector(v2, v2); reflect_rows(M, v2);
w = M[0][0]; x = M[0][1]; y = M[1][0]; z = M[1][1];
if (w * z > x* y) {
c = z + w; s = y - x; d = sqrt(c * c + s * s); c = c / d; s = s / d;
Q[0][0] = Q[1][1] = c; Q[0][1] = -(Q[1][0] = s);
} else {
c = z - w; s = y + x; d = sqrt(c * c + s * s); c = c / d; s = s / d;
Q[0][0] = -(Q[1][1] = c); Q[0][1] = Q[1][0] = s;
}
Q[0][2] = Q[2][0] = Q[1][2] = Q[2][1] = 0.0; Q[2][2] = 1.0;
reflect_cols(Q, v1); reflect_rows(Q, v2);
}
/******* Polar Decomposition *******/
/* Polar Decomposition of 3x3 matrix in 4x4,
* M = QS. See Nicholas Higham and Robert S. Schreiber,
* Fast Polar Decomposition of An Arbitrary Matrix,
* Technical Report 88-942, October 1988,
* Department of Computer Science, Cornell University.
*/
float polar_decomp(HMatrix M, HMatrix Q, HMatrix S)
{
#define TOL 1.0e-6
HMatrix Mk, MadjTk, Ek;
float det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2;
int i, j;
mat_tpose(Mk, =, M, 3);
M_one = norm_one(Mk); M_inf = norm_inf(Mk);
do {
adjoint_transpose(Mk, MadjTk);
det = vdot(Mk[0], MadjTk[0]);
if (det == 0.0) { do_rank2(Mk, MadjTk, Mk); break; }
MadjT_one = norm_one(MadjTk); MadjT_inf = norm_inf(MadjTk);
gamma = sqrt(sqrt((MadjT_one * MadjT_inf) / (M_one * M_inf)) / fabs(det));
g1 = gamma * 0.5;
g2 = 0.5 / (gamma * det);
mat_copy(Ek, =, Mk, 3);
mat_binop(Mk, =, g1 * Mk, +, g2 * MadjTk, 3);
mat_copy(Ek, -=, Mk, 3);
E_one = norm_one(Ek);
M_one = norm_one(Mk); M_inf = norm_inf(Mk);
} while (E_one > (M_one * TOL));
mat_tpose(Q, =, Mk, 3); mat_pad(Q);
mat_mult(Mk, M, S); mat_pad(S);
for (i = 0; i < 3; i++) for (j = i; j < 3; j++)
S[i][j] = S[j][i] = 0.5 * (S[i][j] + S[j][i]);
return (det);
}
/******* Spectral Decomposition *******/
/* Compute the spectral decomposition of symmetric positive semi-definite S.
* Returns rotation in U and scale factors in result, so that if K is a diagonal
* matrix of the scale factors, then S = U K (U transpose). Uses Jacobi method.
* See Gene H. Golub and Charles F. Van Loan. Matrix Computations. Hopkins 1983.
*/
HVect spect_decomp(HMatrix S, HMatrix U)
{
HVect kv;
double Diag[3], OffD[3]; /* OffD is off-diag (by omitted index) */
double g, h, fabsh, fabsOffDi, t, theta, c, s, tau, ta, OffDq, a, b;
static char nxt[] = { Y,Z,X };
int sweep, i, j;
mat_copy(U, =, mat_id, 4);
Diag[X] = S[X][X]; Diag[Y] = S[Y][Y]; Diag[Z] = S[Z][Z];
OffD[X] = S[Y][Z]; OffD[Y] = S[Z][X]; OffD[Z] = S[X][Y];
for (sweep = 20; sweep > 0; sweep--) {
float sm = fabs(OffD[X]) + fabs(OffD[Y]) + fabs(OffD[Z]);
if (sm == 0.0) break;
for (i = Z; i >= X; i--) {
int p = nxt[i]; int q = nxt[p];
fabsOffDi = fabs(OffD[i]);
g = 100.0 * fabsOffDi;
if (fabsOffDi > 0.0) {
h = Diag[q] - Diag[p];
fabsh = fabs(h);
if (fabsh + g == fabsh) {
t = OffD[i] / h;
} else {
theta = 0.5 * h / OffD[i];
t = 1.0 / (fabs(theta) + sqrt(theta * theta + 1.0));
if (theta < 0.0) t = -t;
}
c = 1.0 / sqrt(t * t + 1.0); s = t * c;
tau = s / (c + 1.0);
ta = t * OffD[i]; OffD[i] = 0.0;
Diag[p] -= ta; Diag[q] += ta;
OffDq = OffD[q];
OffD[q] -= s * (OffD[p] + tau * OffD[q]);
OffD[p] += s * (OffDq - tau * OffD[p]);
for (j = Z; j >= X; j--) {
a = U[j][p]; b = U[j][q];
U[j][p] -= s * (b + tau * a);
U[j][q] += s * (a - tau * b);
}
}
}
}
kv.x = Diag[X]; kv.y = Diag[Y]; kv.z = Diag[Z]; kv.w = 1.0;
return (kv);
}
/******* Spectral Axis Adjustment *******/
/* Given a unit quaternion, q, and a scale vector, k, find a unit quaternion, p,
* which permutes the axes and turns freely in the plane of duplicate scale
* factors, such that q p has the largest possible w component, i.e. the
* smallest possible angle. Permutes k's components to go with q p instead of q.
* See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
* Proceedings of Graphics Interface 1992. Details on p. 262-263.
*/
Quat snuggle(Quat q, HVect* k)
{
#define SQRTHALF (0.7071067811865475244f)
#define sgn(n,v) ((n)?-(v):(v))
#define swap(a,i,j) {a[3]=a[i]; a[i]=a[j]; a[j]=a[3];}
#define cycle(a,p) if (p) {a[3]=a[0]; a[0]=a[1]; a[1]=a[2]; a[2]=a[3];}\
else {a[3]=a[2]; a[2]=a[1]; a[1]=a[0]; a[0]=a[3];}
Quat p;
float ka[4];
int i, turn = -1;
ka[X] = k->x; ka[Y] = k->y; ka[Z] = k->z;
if (ka[X] == ka[Y]) { if (ka[X] == ka[Z]) turn = W; else turn = Z; }
else { if (ka[X] == ka[Z]) turn = Y; else if (ka[Y] == ka[Z]) turn = X; }
if (turn >= 0) {
Quat qtoz, qp;
unsigned neg[3], win;
double mag[3], t;
static Quat qxtoz = { 0,SQRTHALF,0,SQRTHALF };
static Quat qytoz = { SQRTHALF,0,0,SQRTHALF };
static Quat qppmm = { 0.5, 0.5,-0.5,-0.5 };
static Quat qpppp = { 0.5, 0.5, 0.5, 0.5 };
static Quat qmpmm = { -0.5, 0.5,-0.5,-0.5 };
static Quat qpppm = { 0.5, 0.5, 0.5,-0.5 };
static Quat q0001 = { 0.0, 0.0, 0.0, 1.0 };
static Quat q1000 = { 1.0, 0.0, 0.0, 0.0 };
switch (turn) {
default: return (Qt_Conj(q));
case X: q = Qt_Mul(q, qtoz = qxtoz); swap(ka, X, Z) break;
case Y: q = Qt_Mul(q, qtoz = qytoz); swap(ka, Y, Z) break;
case Z: qtoz = q0001; break;
}
q = Qt_Conj(q);
mag[0] = (double)q.z * q.z + (double)q.w * q.w - 0.5;
mag[1] = (double)q.x * q.z - (double)q.y * q.w;
mag[2] = (double)q.y * q.z + (double)q.x * q.w;
for (i = 0; i < 3; i++) if (neg[i] = (mag[i] < 0.0)) mag[i] = -mag[i];
if (mag[0] > mag[1]) { if (mag[0] > mag[2]) win = 0; else win = 2; }
else { if (mag[1] > mag[2]) win = 1; else win = 2; }
switch (win) {
case 0: if (neg[0]) p = q1000; else p = q0001; break;
case 1: if (neg[1]) p = qppmm; else p = qpppp; cycle(ka, 0) break;
case 2: if (neg[2]) p = qmpmm; else p = qpppm; cycle(ka, 1) break;
}
qp = Qt_Mul(q, p);
t = sqrt(mag[win] + 0.5);
p = Qt_Mul(p, Qt_(0.0, 0.0, -qp.z / t, qp.w / t));
p = Qt_Mul(qtoz, Qt_Conj(p));
} else {
float qa[4], pa[4];
unsigned lo, hi, neg[4], par = 0;
double all, big, two;
qa[0] = q.x; qa[1] = q.y; qa[2] = q.z; qa[3] = q.w;
for (i = 0; i < 4; i++) {
pa[i] = 0.0;
if (neg[i] = (qa[i] < 0.0)) qa[i] = -qa[i];
par ^= neg[i];
}
/* Find two largest components, indices in hi and lo */
if (qa[0] > qa[1]) lo = 0; else lo = 1;
if (qa[2] > qa[3]) hi = 2; else hi = 3;
if (qa[lo] > qa[hi]) {
if (qa[lo ^ 1] > qa[hi]) { hi = lo; lo ^= 1; }
else { hi ^= lo; lo ^= hi; hi ^= lo; }
} else {if (qa[hi^1]>qa[lo]) lo = hi^1;}
all = (qa[0] + qa[1] + qa[2] + qa[3]) * 0.5;
two = (qa[hi] + qa[lo]) * SQRTHALF;
big = qa[hi];
if (all > two) {
if (all > big) {/*all*/
{int i; for (i = 0; i < 4; i++) pa[i] = sgn(neg[i], 0.5); }
cycle(ka, par)
} else {/*big*/ pa[hi] = sgn(neg[hi],1.0);}
} else {
if (two > big) {/*two*/
pa[hi] = sgn(neg[hi], SQRTHALF); pa[lo] = sgn(neg[lo], SQRTHALF);
if (lo > hi) { hi ^= lo; lo ^= hi; hi ^= lo; }
if (hi == W) { hi = "\001\002\000"[lo]; lo = 3 - hi - lo; }
swap(ka, hi, lo)
} else {/*big*/ pa[hi] = sgn(neg[hi],1.0);}
}
p.x = -pa[0]; p.y = -pa[1]; p.z = -pa[2]; p.w = pa[3];
}
k->x = ka[X]; k->y = ka[Y]; k->z = ka[Z];
return (p);
}
/******* Decompose Affine Matrix *******/
/* Decompose 4x4 affine matrix A as TFRUK(U transpose), where t contains the
* translation components, q contains the rotation R, u contains U, k contains
* scale factors, and f contains the sign of the determinant.
* Assumes A transforms column vectors in right-handed coordinates.
* See Ken Shoemake and Tom Duff. Matrix Animation and Polar Decomposition.
* Proceedings of Graphics Interface 1992.
*/
void decomp_affine(HMatrix A, AffineParts* parts)
{
HMatrix Q, S, U;
Quat p;
float det;
parts->t = Qt_(A[X][W], A[Y][W], A[Z][W], 0);
det = polar_decomp(A, Q, S);
if (det < 0.0) {
mat_copy(Q, =, -Q, 3);
parts->f = -1;
} else parts->f = 1;
parts->q = Qt_FromMatrix(Q);
parts->k = spect_decomp(S, U);
parts->u = Qt_FromMatrix(U);
p = snuggle(parts->u, &parts->k);
parts->u = Qt_Mul(parts->u, p);
}
/******* Invert Affine Decomposition *******/
/* Compute inverse of affine decomposition.
*/
void invert_affine(AffineParts* parts, AffineParts* inverse)
{
Quat t, p;
inverse->f = parts->f;
inverse->q = Qt_Conj(parts->q);
inverse->u = Qt_Mul(parts->q, parts->u);
inverse->k.x = (parts->k.x == 0.0) ? 0.0 : 1.0 / parts->k.x;
inverse->k.y = (parts->k.y == 0.0) ? 0.0 : 1.0 / parts->k.y;
inverse->k.z = (parts->k.z == 0.0) ? 0.0 : 1.0 / parts->k.z;
inverse->k.w = parts->k.w;
t = Qt_(-parts->t.x, -parts->t.y, -parts->t.z, 0);
t = Qt_Mul(Qt_Conj(inverse->u), Qt_Mul(t, inverse->u));
t = Qt_(inverse->k.x * t.x, inverse->k.y * t.y, inverse->k.z * t.z, 0);
p = Qt_Mul(inverse->q, inverse->u);
t = Qt_Mul(p, Qt_Mul(t, Qt_Conj(p)));
inverse->t = (inverse->f > 0.0) ? t : Qt_(-t.x, -t.y, -t.z, 0);
}
}
Reference in New Issue View Git Blame Copy Permalink