dgJacobian dgDynamicBody::IntegrateForceAndToque(const dgVector& force, const dgVector& torque, const dgVector& timestep)
{
dgJacobian velocStep;
if (m_gyroTorqueOn) {
dgVector dtHalf(timestep * dgVector::m_half);
dgMatrix matrix(m_gyroRotation, dgVector::m_wOne);
dgVector localOmega(matrix.UnrotateVector(m_omega));
dgVector localTorque(matrix.UnrotateVector(torque - m_gyroTorque));
// derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy)
dgVector dw(localOmega * dtHalf);
dgMatrix jacobianMatrix(
dgVector(m_mass[0], (m_mass[2] - m_mass[1]) * dw[2], (m_mass[2] - m_mass[1]) * dw[1], dgFloat32(0.0f)),
dgVector((m_mass[0] - m_mass[2]) * dw[2], m_mass[1], (m_mass[0] - m_mass[2]) * dw[0], dgFloat32(1.0f)),
dgVector((m_mass[1] - m_mass[0]) * dw[1], (m_mass[1] - m_mass[0]) * dw[0], m_mass[2], dgFloat32(1.0f)),
dgVector::m_wOne);
// and solving for alpha we get the angular acceleration at t + dt
// calculate gradient at a full time step
//dgVector gradientStep(localTorque * timestep);
dgVector gradientStep(jacobianMatrix.SolveByGaussianElimination(localTorque * timestep));
dgVector omega(matrix.RotateVector(localOmega + gradientStep));
dgAssert(omega.m_w == dgFloat32(0.0f));
// integrate rotation here
dgFloat32 omegaMag2 = omega.DotProduct(omega).GetScalar() + dgFloat32(1.0e-12f);
dgFloat32 invOmegaMag = dgRsqrt(omegaMag2);
dgVector omegaAxis(omega.Scale(invOmegaMag));
dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep.GetScalar();
dgQuaternion deltaRotation(omegaAxis, omegaAngle);
m_gyroRotation = m_gyroRotation * deltaRotation;
dgAssert((m_gyroRotation.DotProduct(m_gyroRotation) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
matrix = dgMatrix(m_gyroRotation, dgVector::m_wOne);
localOmega = matrix.UnrotateVector(omega);
//dgVector angularMomentum(inertia * localOmega);
//body->m_gyroTorque = matrix.RotateVector(localOmega.CrossProduct(angularMomentum));
//body->m_gyroAlpha = body->m_invWorldInertiaMatrix.RotateVector(body->m_gyroTorque);
dgVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
m_gyroTorque = matrix.RotateVector(localGyroTorque);
m_gyroAlpha = matrix.RotateVector(localGyroTorque * m_invMass);
velocStep.m_angular = matrix.RotateVector(gradientStep);
} else {
velocStep.m_angular = m_invWorldInertiaMatrix.RotateVector(torque) * timestep;
//velocStep.m_angular = velocStep.m_angular * dgVector::m_half;
}
velocStep.m_linear = force.Scale(m_invMass.m_w) * timestep;
return velocStep;
}
void dgDynamicBody::IntegrateImplicit(dgFloat32 timestep)
{
// using simple backward Euler or implicit integration, this is.
// f'(t + dt) = (f(t + dt) - f(t)) / dt
// therefore:
// f(t + dt) = f(t) + f'(t + dt) * dt
// approximate f'(t + dt) by expanding the Taylor of f(w + dt)
// f(w + dt) = f(w) + f'(w) * dt + f''(w) * dt^2 / 2! + ....
// assume dt^2 is negligible, therefore we can truncate the expansion to
// f(w + dt) ~= f(w) + f'(w) * dt
// calculating dw as the f'(w) = d(wx, wy, wz) | dt
// dw/dt = a = (Tl - (wl x (wl * Il)) * Il^1
// expanding f(w)
// f'(wx) = Ix * ax = Tx - (Iz - Iy) * wy * wz
// f'(wy) = Iy * ay = Ty - (Ix - Iz) * wz * wx
// f'(wz) = Iz * az = Tz - (Iy - Ix) * wx * wy
//
// calculation the expansion
// Ix * ax = (Tx - (Iz - Iy) * wy * wz) - ((Iz - Iy) * wy * az + (Iz - Iy) * ay * wz) * dt
// Iy * ay = (Ty - (Ix - Iz) * wz * wx) - ((Ix - Iz) * wz * ax + (Ix - Iz) * az * wx) * dt
// Iz * az = (Tz - (Iy - Ix) * wx * wy) - ((Iy - Ix) * wx * ay + (Iy - Ix) * ax * wy) * dt
//
// factorizing a we get
// Ix * ax + (Iz - Iy) * dwy * az + (Iz - Iy) * dwz * ay = Tx - (Iz - Iy) * wy * wz
// Iy * ay + (Ix - Iz) * dwz * ax + (Ix - Iz) * dwx * az = Ty - (Ix - Iz) * wz * wx
// Iz * az + (Iy - Ix) * dwx * ay + (Iy - Ix) * dwy * ax = Tz - (Iy - Ix) * wx * wy
dgVector localOmega(m_matrix.UnrotateVector(m_omega));
dgVector localTorque(m_matrix.UnrotateVector(m_externalTorque - m_gyroTorque));
// and solving for alpha we get the angular acceleration at t + dt
// calculate gradient at a full time step
dgVector gradientStep(localTorque.Scale(timestep));
// derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy)
dgVector dw(localOmega.Scale(dgFloat32(0.5f) * timestep));
//dgVector dw(localOmega.Scale(dgFloat32(1.0f) * timestep));
// calculates Jacobian matrix (
// dWx / dwx, dWx / dwy, dWx / dwz
// dWy / dwx, dWy / dwy, dWy / dwz
// dWz / dwx, dWz / dwy, dWz / dwz
//
// dWx / dwx = Ix, dWx / dwy = (Iz - Iy) * wz * dt, dWx / dwz = (Iz - Iy) * wy * dt)
// dWy / dwx = (Ix - Iz) * wz * dt, dWy / dwy = Iy, dWy / dwz = (Ix - Iz) * wx * dt
// dWz / dwx = (Iy - Ix) * wy * dt, dWz / dwy = (Iy - Ix) * wx * dt, dWz / dwz = Iz
dgMatrix jacobianMatrix(
dgVector(m_mass[0], (m_mass[2] - m_mass[1]) * dw[2], (m_mass[2] - m_mass[1]) * dw[1], dgFloat32(0.0f)),
dgVector((m_mass[0] - m_mass[2]) * dw[2], m_mass[1], (m_mass[0] - m_mass[2]) * dw[0], dgFloat32(0.0f)),
dgVector((m_mass[1] - m_mass[0]) * dw[1], (m_mass[1] - m_mass[0]) * dw[0], m_mass[2], dgFloat32(0.0f)),
dgVector::m_wOne);
gradientStep = jacobianMatrix.SolveByGaussianElimination(gradientStep);
localOmega += gradientStep;
m_accel = m_externalForce.Scale(m_invMass.m_w);
m_alpha = m_matrix.RotateVector(localTorque * m_invMass);
m_veloc += m_accel.Scale(timestep);
m_omega = m_matrix.RotateVector(localOmega);
}
size_t SmoothDualDecompositionFistaDescent::run(double rho)
{
//double rho_end = rho;
//rho = 10;
// step length
double omega;
omega = _lipschitz_constant_optimistic;
// objectives
double obj_lambda, obj_y_old;
obj_y_old = -std::numeric_limits<double>::max();
// gradient
Eigen::VectorXd gradient;
double gnorm = std::numeric_limits<double>::max();
//// TODO: should possibly consider pruning all the labels that have an
//// inf in theta, but this would get quite messy! Could use the property
//// if a var is once inf it stays inf in the whole LPQP framework.
//// TODO: also investigate how much slower the computations get because
//// of the infs!
//size_t num_inf = 0;
//for (size_t v_idx=0; v_idx<_num_vertices; v_idx++) {
// for (size_t k=0; k<_num_states[v_idx]; k++) {
// if (std::isinf(_theta_unary[v_idx][k])) {
// num_inf++;
// }
// }
//}
//std::cout << "num_inf: " << num_inf << std::endl;
size_t iter = 0;
while(!stoppingCriteriaMet(iter, gnorm)){
//// TODO: play around with this! Seems to work, but we need to improve this!
//if ((iter % 10) == 0) {
// printf("rho before: %f.\n", rho);
// rho = adaptRho(_lambda, rho, rho_end, 1e-1);
// printf("changed rho: %f.\n", rho);
// obj_y_old = computeObjective(rho, _y);
//}
_y_old = _y;
double obj_new;
obj_new = gradientStep(rho, _lambda, obj_lambda, gradient, gnorm, _u, omega);
printProgress(iter, obj_new, gnorm, 1.0/omega);
if (obj_new > obj_y_old) {
_y = _u;
}
else {
_y = _y_old;
}
double theta = 2/(static_cast<double>(iter)+2.0);
_v = _y_old+1.0/theta*(_u-_y_old);
theta = 2/(static_cast<double>(iter)+3.0);
_lambda = (1-theta)*_y+theta*_v;
obj_y_old = obj_new;
iter++;
}
// _y is the final solution
_lambda = _y;
// due to the line search we have to call inference
// again before returning to actually get the latest marginals of the
// final value of lambda!
obj_lambda = computeObjective(rho, _lambda);
setMarginalsUnary();
setMarginalsPair();
return iter;
}
