void DContact::ResolveCollisions(const float2& Ncoll, float t, float fCoF, float fCoR,
const float2& C0, const float2& P0, float2& V0, float& w0, float m0, float i0,
const float2& C1, const float2& P1, float2& V1, float& w1, float m1, float i1)
{
// pre-computations
float tcoll = t > 0.0f ? t : 0.0f;
float2 Q0 = P0 + V0 * tcoll;
float2 Q1 = P1 + V1 * tcoll;
float2 R0 = C0 - Q0;
float2 R1 = C1 - Q1;
float2 T0(-R0.y, R0.x);
float2 T1(-R1.y, R1.x);
float2 VP0 = V0 - T0 * w0;
float2 VP1 = V1 - T1 * w1;
// impact velocity
float2 Vcoll = VP0 - VP1;
float vn = Vcoll.dot(Ncoll);
float2 Vn = Ncoll * vn;
float2 Vt = Vcoll - Vn;
if(vn > 0.0f) { return; }
float vt = Vt.magnitude();
Vt = Vt.normalize();
// compute impulse (friction and restitution)
float2 J, Jt(0.0f, 0.0f), Jn(0.0f, 0.0f);
float t0 = (R0 ^ Ncoll) * (R0 ^ Ncoll) * i0;
float t1 = (R1 ^ Ncoll) * (R1 ^ Ncoll) * i1;
float m = m0 + m1;
float denom = m + t0 + t1;
float jn = vn / denom;
Jn = Ncoll * (-(1.0f + fCoR) * jn); // restitution
Jt = Vt.normalize() * (fCoF * jn); // friction
J = Jn + Jt;
// changes in momentum
float2 dV0 = J * m0;
float2 dV1 = -J * m1;
float dw0 =-(R0 ^ J) * i0;
float dw1 = (R1 ^ J) * i1;
if(m0 > 0.0f) { V0 += dV0; w0 += dw0; }; // apply changes in momentum
if(m1 > 0.0f) { V1 += dV1; w1 += dw1; }; // apply changes in momentum
// Check for static frcition
if (vn < 0.0f && fCoF > 0.0f)
{
float cone = -vt / vn;
if (cone < fCoF)
{
float2 Nfriction = -Vt.normalize();
float fCoS = cmat.coStaticFriction;
ResolveCollisions(Nfriction, 0.0f, 0.0f, fCoS,
C0, P0, V0, w0, m0, i0,
C1, P1, V1, w1, m1, i1);
}
}
};
MatrixXd ContactDynamics::getJacobian(kinematics::BodyNode* node, const Vector3d& p) {
int nDofs = node->getSkel()->getNumDofs();
MatrixXd Jt( MatrixXd::Zero(nDofs, 3) );
Vector3d invP = dart_math::xformHom(node->getWorldInvTransform(), p);
for(int dofIndex = 0; dofIndex < node->getNumDependentDofs(); dofIndex++) {
int i = node->getDependentDof(dofIndex);
Jt.row(i) = dart_math::xformHom(node->getDerivWorldTransform(dofIndex), invP);
}
return Jt;
}
////////////////////////////////////////////////////////////////
// ResolveCollision
////////////////////////////////////////////////////////////////
// calculates the change in angular and linear velocity of two
// colliding objects
////////////////////////////////////////////////////////////////
// parameters :
// ------------
// Ncoll : normal of collision
// t : time of collision, (if negative, it's a penetration depth)
// fCoF : coefficient of friction
// fCoR : coefficient of restitution
// C0 : point of contact on object 0
// P0 : centre of gravity (position) of object 0
// V0 : linear Velocity of object 0
// w0 : angular velocity of object 0
// m0 : inverse mass of object 0
// i0 : inverse inertia of object 0
// C1 : point of contact on object 1
// P1 : centre of gravity (position) of object 1
// V1 : linear Velocity of object 1
// w1 : angular velocity of object 1
// m1 : inverse mass of object 1
// i1 : inverse inertia of object 1
//
// return values : V0, w0, V1, w1 will change upon a collision
// -------------
///////////////////////////////////////////////////////////////
void ResolveCollision(const glm::vec2& Ncoll, float t, float fCoF, float fCoR,
const glm::vec2& C0, const glm::vec2& P0, glm::vec2& V0, float& w0, float m0, float i0,
const glm::vec2& C1, const glm::vec2& P1, glm::vec2& V1, float& w1, float m1, float i1)
{
//------------------------------------------------------------------------------------------------------
// pre-computations
//------------------------------------------------------------------------------------------------------
float tcoll = (t > 0.0f)? t : 0.0f;
glm::vec2 Q0 = P0 + V0 * tcoll;
glm::vec2 Q1 = P1 + V1 * tcoll;
glm::vec2 R0 = C0 - Q0;
glm::vec2 R1 = C1 - Q1;
glm::vec2 T0 = glm::vec2(-R0.y, R0.x);
glm::vec2 T1 = glm::vec2(-R1.y, R1.x);
glm::vec2 VP0 = V0 - T0 * w0; // point velocity (SIGN IS WRONG)
glm::vec2 VP1 = V1 - T1 * w1; // point velocity (SIGN IS WRONG)
//------------------------------------------------------------------------------------------------------
// impact velocity
//------------------------------------------------------------------------------------------------------
glm::vec2 Vcoll = VP0 - VP1;
float vn = glm::dot(Vcoll, Ncoll);
glm::vec2 Vn = vn * Ncoll;
glm::vec2 Vt = Vcoll - Vn;
// separation
if (vn > 0.0f)
return;
float vt = glm::length(Vt);
//------------------------------------------------------------------------------------------------------
// compute impulse (frction and restitution).
// ------------------------------------------
//
// -(1+Cor)(Vel.norm)
// j = ------------------------------------------------------------
// [1/Ma + 1/Mb] + [Ia' * (ra x norm)²] + [Ib' * (rb x norm)²]
//------------------------------------------------------------------------------------------------------
glm::vec2 J;
glm::vec2 Jt(0.0f, 0.0f);
glm::vec2 Jn(0.0f, 0.0f);
float t0 = cross(R0, Ncoll) * cross(R0, Ncoll) * i0;
float t1 = cross(R1, Ncoll) * cross(R1, Ncoll) * i1;
float m = m0 + m1;
float denom = m + t0 + t1;
float jn = vn / denom;
Jn = (-(1.0f + fCoR) * jn) * Ncoll;
if (dbg_UseFriction)
{
Jt = (fCoF * jn) * glm::normalize(Vt);
}
J = Jn + Jt;
//------------------------------------------------------------------------------------------------------
// changes in momentum
//------------------------------------------------------------------------------------------------------
glm::vec2 dV0 = J * m0;
glm::vec2 dV1 =-J * m1;
float dw0 =-cross(R0, J) * i0; // (SIGN IS WRONG)
float dw1 = cross(R1, J) * i1; // (SIGN IS WRONG)
//------------------------------------------------------------------------------------------------------
// apply changes in momentum
//------------------------------------------------------------------------------------------------------
if (m0 > 0.0f) V0 += dV0;
if (m1 > 0.0f) V1 += dV1;
if (m0 > 0.0f) w0 += dw0;
if (m1 > 0.0f) w1 += dw1;
//------------------------------------------------------------------------------------------------------
// Check for static frcition
//------------------------------------------------------------------------------------------------------
if (vn < 0.0f && fCoF > 0.0f)
{
float cone = -vt / vn;
if (cone < fCoF)
{
// treat static friciton as a collision at the contact point
glm::vec2 Nfriction = glm::normalize(-Vt);
float fCoS = GetStaticFriction;
ResolveCollision(Nfriction, 0.0f, 0.0f, fCoS,
C0, P0, V0, w0, m0, i0,
C1, P1, V1, w1, m1, i1);
}
}
}
void Contact::ResolveCollision()
{
if (!m_pxBodies[0] || !m_pxBodies[1])
return;
//------------------------------------------------------------------------------------------------------
// parameters
//------------------------------------------------------------------------------------------------------
Point2f C0 = m_xContacts[0];
Point2f C1 = m_xContacts[1];
Point2f Ncoll = C1 - C0;
// assert(Ncoll.x != 0 || Ncoll.y != 0);
Ncoll.Normalize();
float m0 = m_pxBodies[0]->getInvMass();
float m1 = m_pxBodies[1]->getInvMass();
float i0 = m_pxBodies[0]->getInvInertia();
float i1 = m_pxBodies[1]->getInvInertia();
const Point2f& P0 = m_pxBodies[0]->getPosition();
const Point2f& P1 = m_pxBodies[1]->getPosition();
const Point2f& V0 = m_pxBodies[0]->getVelocity();
const Point2f& V1 = m_pxBodies[1]->getVelocity();
float w0 = m_pxBodies[0]->getAngVelocity();
float w1 = m_pxBodies[1]->getAngVelocity();
//------------------------------------------------------------------------------------------------------
// pre-computations
//------------------------------------------------------------------------------------------------------
Point2f R0 = C0 - P0;
Point2f R1 = C1 - P1;
Point2f T0 = Point2f(-R0.y, R0.x);
Point2f T1 = Point2f(-R1.y, R1.x);
Point2f VP0 = V0 + T0 * w0; // point velocity
Point2f VP1 = V1 + T1 * w1; // point velocity
//------------------------------------------------------------------------------------------------------
// impact velocity
//------------------------------------------------------------------------------------------------------
Point2f Vcoll = VP0 - VP1;
float vn = Vcoll * Ncoll;
Point2f Vn = Ncoll * vn;
Point2f Vt = Vcoll - Vn;
if (vn > 0.0f) // separation
return;
if (Vt * Vt < 0.0001f)
Vt = Point2f(0.0f, 0.0f);
// float vt = Vt.Length();
Vt.Normalize();
//------------------------------------------------------------------------------------------------------
// compute impulse (frction and restitution).
// ------------------------------------------
//
// -(1+Cor)(Vel.norm)
// j = ------------------------------------------------------------
// [1/Ma + 1/Mb] + [Ia' * (ra x norm)²] + [Ib' * (rb x norm)²]
//------------------------------------------------------------------------------------------------------
Point2f J;
Point2f Jt(0.0f, 0.0f);
Point2f Jn(0.0f, 0.0f);
float fCoR = s_xContactMaterial.GetRestitution();
float fCoF = s_xContactMaterial.GetFriction();
float t0 = (R0 ^ Ncoll) * (R0 ^ Ncoll) * i0;
float t1 = (R1 ^ Ncoll) * (R1 ^ Ncoll) * i1;
float m = m0 + m1;
float denom = m + t0 + t1;
float jn = vn / denom;
Jn = Ncoll * (-(1.0f + fCoR) * jn);
//if (dbg_UseFriction)
Jt = Vt * (fCoF * jn);
J = Jn + Jt;
//------------------------------------------------------------------------------------------------------
// changes in momentum
//------------------------------------------------------------------------------------------------------
Point2f dV0 = J * m0;
Point2f dV1 =-J * m1;
float dw0 = (R0 ^ J) * i0;
float dw1 =-(R1 ^ J) * i1;
//------------------------------------------------------------------------------------------------------
// apply changes in momentum
//------------------------------------------------------------------------------------------------------
if (!m_pxBodies[0]->isStaticBody())
{
if (m0 > 0.0f)
{
m_pxBodies[0]->setVelocity(V0 + dV0);
}
if (m0 > 0.0f)
{
m_pxBodies[0]->setAngVelocity(w0 + dw0);
}
}
if (!m_pxBodies[1]->isStaticBody())
{
if (m1 > 0.0f)
{
m_pxBodies[1]->setVelocity(V1 + dV1);
}
if (m1 > 0.0f)
{
m_pxBodies[1]->setAngVelocity(w1 + dw1);
}
}
// Render();
return;
/*
//------------------------------------------------------------------------------------------------------
// Check for static frcition
//------------------------------------------------------------------------------------------------------
float fRestingContactVelocity = 1.0f;
if (-vn < fRestingContactVelocity)
{
if (vt < fRestingContactVelocity * fCoF)
{
//------------------------------------------------------------------------------------------------------
// Cancel tangential velocity on the two bodies, so they stick
//------------------------------------------------------------------------------------------------------
Point2f dV = V1 - V0;
dV -= Ncoll * (dV * Ncoll);
if (m0 > 0.0f) V0 += dV * (m0 / m + 0.01f);
if (m1 > 0.0f) V1 -= dV * (m1 / m + 0.01f);
}
}
*/
}
CDenseVector<T> CLevenbergMarquardt<Matrix,T>::Iterate(size_t n, T epsilon1, T epsilon2, bool silent) {
// access to state
CDenseVector<T>& x = m_problem.Get();
// initial residual, Jacobian
CDenseVector<T> r(m_problem.GetNumberOfDataPoints());
Matrix J(m_problem.GetNumberOfDataPoints(),m_problem.GetNumberOfModelParameters());
m_problem.ComputeResidualAndJacobian(r,J);
// initial value for lambda, TODO: do this depending on trace of J'*J
m_lambda = m_tau*1;
// residual norm
T res = r.Norm2();
m_residuals.push_back(res);
// gradient norm
J.Transpose();
CDenseVector<T> grad = J*r;
J.Transpose();
T normgrad = grad.Norm2();
// init other quantities
T nu = m_params[2];
size_t k = 0;
// in verbose mode, print out initial residual, etc.
if(!silent) {
cout.setf(ios::scientific,ios::floatfield);
cout << "k\t f(x)\t ||grad f||\t ||h||\t lambda" << endl;
cout << k << "\t" << res << "\t" << normgrad << "\t" << 0.0000 << "\t" << m_lambda << endl;
}
while(true) {
// solve linear system after setting lmbda in the CGLS solver
CDenseVector<T> step(m_problem.GetNumberOfModelParameters());
m_solver.SetLambda(sqrt(m_lambda));
m_solver.Iterate(J,r,step);
// save old state before advancing
CDenseVector<T> xold = x.Clone();
// tentative point, everything is allocated so changing pointer is ok
x = x - step;
// compute tentative residual and Jacobian
CDenseVector<T> rt(m_problem.GetNumberOfDataPoints());
Matrix Jt(m_problem.GetNumberOfDataPoints(),m_problem.GetNumberOfModelParameters());
m_problem.ComputeResidualAndJacobian(rt,Jt);
// residual norm
res = rt.Norm2();
// compute rho
T normstep, descent, dres, dlres, rho;
dres = m_residuals.back() - res;
normstep = sqrt(CDenseVector<T>::InnerProduct(step,step));
descent = -CDenseVector<T>::InnerProduct(step,grad);
dlres = 0.5*(normstep*normstep*m_lambda - descent);
rho = dres/dlres;
// check step size criterion (lambda going to infinity)
if(normstep<epsilon2*xold.Norm2())
break;
// update state if a descent direction is found
if(rho>0) {
// it ok now to store the residual norm
m_residuals.push_back(res);
// keep Jacobian and residual, should be ok to move pointers
r = rt;
J = Jt;
// update gradient norm, this contains step size parameter (but maybe it should not?)
J.Transpose();
grad = J*r;
J.Transpose();
normgrad = grad.Norm2();
// push lambda towards Gauss-Newton step
nu = m_params[2];
T factor = max(m_params[3],1-(m_params[2]-1)*pow(2*rho-1,m_params[5]));
m_lambda *= factor;
k++;
// print out current state of optimization
if(!silent)
cout << k << "\t" << res << "\t" << normgrad << "\t" << normstep << "\t" << m_lambda << endl;
// check convergence criteria
if(normgrad<epsilon1 || k==n)
break;
}
else {
// push towards gradient descent
if(m_lambda>0)
m_lambda *= nu;
else
m_lambda = 1e-12;
nu *= 2;
// restore state because step was unsuccessful
x = xold;
// show how lambda develops
if(!silent)
cout << "Adjusting lambda to " << m_lambda << "." << endl;
}
if(std::isinf(m_lambda))
break;
}
return r;
}
