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();
}
Beispiel #2
0
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));
}
Beispiel #3
0
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);
}