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; }