void NewtonIntegrator::updateEnergy(const shared_ptr<Body>& b, const State* state, const Vector3r& fluctVel, const Vector3r& f, const Vector3r& m){
assert(b->isStandalone() || b->isClump());
// always positive dissipation, by-component: |F_i|*|v_i|*damping*dt (|T_i|*|ω_i|*damping*dt for rotations)
if(damping!=0. && state->isDamped){
scene->energy->add(fluctVel.cwise().abs().dot(f.cwise().abs())*damping*scene->dt,"nonviscDamp",nonviscDampIx,/*non-incremental*/false);
// when the aspherical integrator is used, torque is damped instead of ang acceleration; this code is only approximate
scene->energy->add(state->angVel.cwise().abs().dot(m.cwise().abs())*damping*scene->dt,"nonviscDamp",nonviscDampIx,false);
}
// kinetic energy
Real Etrans=.5*state->mass*fluctVel.squaredNorm();
Real Erot;
// rotational terms
if(b->isAspherical()){
Matrix3r mI; mI<<state->inertia[0],0,0, 0,state->inertia[1],0, 0,0,state->inertia[2];
Matrix3r T(state->ori);
Erot=.5*b->state->angVel.transpose().dot((T.transpose()*mI*T)*b->state->angVel);
} else { Erot=0.5*state->angVel.dot(state->inertia.cwise()*state->angVel); }
if(!kinSplit) scene->energy->add(Etrans+Erot,"kinetic",kinEnergyIx,/*non-incremental*/true);
else{ scene->energy->add(Etrans,"kinTrans",kinEnergyTransIx,true); scene->energy->add(Erot,"kinRot",kinEnergyRotIx,true); }
}
Vector3r NewtonIntegrator::computeAngAccel(const Vector3r& torque, const Vector3r& inertia, int blockedDOFs){
if(likely(blockedDOFs==0)) return torque.cwise()/inertia;
Vector3r ret(Vector3r::Zero());
for(int i=0; i<3; i++) if(!(blockedDOFs & State::axisDOF(i,true))) ret[i]+=torque[i]/inertia[i];
return ret;
}
