void RigidBody::step(double dt){
FILE_LOG(logDEBUG)<<"entered RigidBody::step(double dt)"<<std::endl;
// set com
// NOTE: com is transform r in THIS case
for(unsigned i=0;i<3;i++)
com[i] = Tws(i,3);
// NOTE: set accel for integration function
vd_ = &vd;
FILE_LOG(logDEBUG) << "q (before) = " << q << std::endl;
FILE_LOG(logDEBUG) << "v (before) = " << v << std::endl;
FILE_LOG(logDEBUG) << "vd (before) = " << vd << std::endl;
// Set forces to apply
// apply drag force
{
double k = 0.1;
Ravelin::Vector3d drag;
v.get_sub_vec(0,3,drag);
drag = -drag *= (drag.dot(drag) * k);
apply_force(drag,com);
}
// apply forces
calc_fwd_dynamics();
// Set up state
Vec state;
state.set_zero(q.rows() + v.rows());
state.set_sub_vec(0,q);
state.set_sub_vec(q.rows(),v);
// Integrate state forward 1 step
sim_->integrate(sim_->time,dt,&fn,state);
// re-normalize quaternion state
state.get_sub_vec(0,q.rows(),q);
state.get_sub_vec(q.rows(),state.rows(),v);
Vec e;
q.get_sub_vec(3,7,e);
e.normalize();
q.set_sub_vec(3,e);
FILE_LOG(logDEBUG) << "q (after) = " << q << std::endl;
FILE_LOG(logDEBUG) << "v (after) = " << v << std::endl;
FILE_LOG(logDEBUG) << "vd (after) = " << vd << std::endl;
// Update graphics transform
update();
FILE_LOG(logDEBUG)<<"exited RigidBody::step(.)"<<std::endl;
reset_accumulators();
}
Ravelin::Vector3d Utility::slerp( const Ravelin::Vector3d& v0,const Ravelin::Vector3d& v1,double t){
double a = v0.dot(v1);
return (v0*sin((1-t)*a)/sin(a) + v1*sin(t*a)/sin(a));
}
double Utility::distance_from_plane(const Ravelin::Vector3d& normal,const Ravelin::Vector3d& point, const Ravelin::Vector3d& x){
Ravelin::Vector3d v;
v = x - point;
return normal.dot(v);
}