void realizePosition(const SBStateDigest& sbs) const {
SBTreePositionCache& pc = sbs.updTreePositionCache();
const Transform& X_MB = getX_MB(); // fixed
const Transform& X_PF = getX_PF(); // fixed
const Transform& X_GP = getX_GP(pc); // already calculated
updX_FM(pc).setToZero();
updX_PB(pc) = X_PF * X_MB;
updX_GB(pc) = X_GP * getX_PB(pc);
const Vec3 p_PB_G = getX_GP(pc).R() * getX_PB(pc).p();
// The Phi matrix conveniently performs child-to-parent (inward) shifting
// on spatial quantities (forces); its transpose does parent-to-child
// (outward) shifting for velocities.
updPhi(pc) = PhiMatrix(p_PB_G);
// Calculate spatial mass properties. That means we need to transform
// the local mass moments into the Ground frame and reconstruct the
// spatial inertia matrix Mk.
const Rotation& R_GB = getX_GB(pc).R();
const Vec3& p_GB = getX_GB(pc).p();
// reexpress inertia in ground (57 flops)
const UnitInertia G_Bo_G = getUnitInertia_OB_B().reexpress(~R_GB);
const Vec3 p_BBc_G = R_GB*getCOM_B(); // 15 flops
updCOM_G(pc) = p_GB + p_BBc_G; // 3 flops
// Calc Mk: the spatial inertia matrix about the body origin.
// Note: we need to calculate this now so that we'll be able to calculate
// kinetic energy without going past the Velocity stage.
updMk_G(pc) = SpatialInertia(getMass(), p_BBc_G, G_Bo_G);
}
/**
Gradient Domain HDR tone mapping operator
@param Y Image luminance values
@param alpha Parameter alpha of the paper (suggested value is 0.1)
@param beta Parameter beta of the paper (suggested value is between 0.8 and 0.9)
@return returns the tone mapped luminance
*/
static FIBITMAP* tmoFattal02(FIBITMAP *Y, float alpha, float beta) {
const unsigned MIN_PYRAMID_SIZE = 32; // minimun size (width or height) of the coarsest level of the pyramid
FIBITMAP *H = NULL;
FIBITMAP **pyramid = NULL;
FIBITMAP **gradients = NULL;
FIBITMAP *phy = NULL;
FIBITMAP *divG = NULL;
FIBITMAP *U = NULL;
float *avgGrad = NULL;
int k;
int nlevels = 0;
try {
// get the normalized luminance
FIBITMAP *H = LogLuminance(Y);
if(!H) throw(1);
// get the number of levels for the pyramid
const unsigned width = FreeImage_GetWidth(H);
const unsigned height = FreeImage_GetHeight(H);
unsigned minsize = MIN(width, height);
while(minsize >= MIN_PYRAMID_SIZE) {
nlevels++;
minsize /= 2;
}
// create the Gaussian pyramid
pyramid = (FIBITMAP**)malloc(nlevels * sizeof(FIBITMAP*));
if(!pyramid) throw(1);
memset(pyramid, 0, nlevels * sizeof(FIBITMAP*));
if(!GaussianPyramid(H, pyramid, nlevels)) throw(1);
// calculate gradient magnitude and its average value on each pyramid level
gradients = (FIBITMAP**)malloc(nlevels * sizeof(FIBITMAP*));
if(!gradients) throw(1);
memset(gradients, 0, nlevels * sizeof(FIBITMAP*));
avgGrad = (float*)malloc(nlevels * sizeof(float));
if(!avgGrad) throw(1);
if(!GradientPyramid(pyramid, nlevels, gradients, avgGrad)) throw(1);
// free the Gaussian pyramid
for(k = 0; k < nlevels; k++) {
if(pyramid[k]) FreeImage_Unload(pyramid[k]);
}
free(pyramid); pyramid = NULL;
// compute the gradient attenuation function PHI(x, y)
phy = PhiMatrix(gradients, avgGrad, nlevels, alpha, beta);
if(!phy) throw(1);
// free the gradient pyramid
for(k = 0; k < nlevels; k++) {
if(gradients[k]) FreeImage_Unload(gradients[k]);
}
free(gradients); gradients = NULL;
free(avgGrad); avgGrad = NULL;
// compute gradients in x and y directions, attenuate them with the attenuation matrix,
// then compute the divergence div G from the attenuated gradient.
divG = Divergence(H, phy);
if(!divG) throw(1);
// H & phy no longer needed
FreeImage_Unload(H); H = NULL;
FreeImage_Unload(phy); phy = NULL;
// solve the PDE (Poisson equation) using a multigrid solver and 3 cycles
FIBITMAP *U = FreeImage_MultigridPoissonSolver(divG, 3);
if(!U) throw(1);
FreeImage_Unload(divG);
// perform exponentiation and recover the log compressed image
ExpLuminance(U);
return U;
} catch(int) {
if(H) FreeImage_Unload(H);
if(pyramid) {
for(int i = 0; i < nlevels; i++) {
if(pyramid[i]) FreeImage_Unload(pyramid[i]);
}
free(pyramid);
}
if(gradients) {
for(int i = 0; i < nlevels; i++) {
if(gradients[i]) FreeImage_Unload(gradients[i]);
}
free(gradients);
}
if(avgGrad) free(avgGrad);
if(phy) FreeImage_Unload(phy);
if(divG) FreeImage_Unload(divG);
if(U) FreeImage_Unload(U);
return NULL;
}
}
