body.cc

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· CC 代码 · 共 736 行 · 第 1/2 页

    //printf("v   %6.2f %6.2f %6.2f %6.2f\n", v[0], v[1], v[2], v[3]);
  
    // Compute angular velocity quaternion in the body frame
    vel = GzQuaternMul(GzQuaternSet(0, (double)(v[0])/2, (double)(v[1])/2, 
                                    (double)(v[2])/2), this->GetRotation());

    // TODO: confused
    // Compute angular velocity quaternion in the global frame
    //vel = GzQuaternMul(GzQuaternInverse(this->GetRotation()),
    //GzQuaternMul(vel, this->GetRotation()));

    return vel;
  }
}


//////////////////////////////////////////////////////////////////////////////
// Set the linear velocity (global cs)
void Body::SetLinearVel( GzVector vel ) 
{
  if (this->bodyId)
    dBodySetLinearVel( this->bodyId, vel.x, vel.y, vel.z );
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Set the angular velocity (global cs)
void Body::SetAngularVel( GzVector vel ) 
{
  if (this->bodyId)
    dBodySetAngularVel( this->bodyId, vel.x, vel.y, vel.z );
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Get the id of the body
dBodyID Body::GetBodyId() const
{
  return this->bodyId;
}


//////////////////////////////////////////////////////////////////////////////
// Apply a force to the body
void Body::SetForce( GzVector f )
{
  if (this->bodyId)
    dBodySetForce( this->bodyId, (dReal)f.x, (dReal)f.y ,(dReal)f.z );
}


//////////////////////////////////////////////////////////////////////////////
// Apply a torque to the body
void Body::SetTorque( double x, double y, double z )
{
  if (this->bodyId)
    dBodySetTorque( this->bodyId, (dReal)x, (dReal)y, (dReal)z);
}


//////////////////////////////////////////////////////////////////////////////
// Get body force
GzVector Body::GetForce(void)
{
  if (!this->bodyId)
  {
    return GzVectorZero();
  }
  else
  {
    GzVector gvec;
    const double* vec = (double*)dBodyGetForce( this->bodyId);
    gvec.x=vec[0];
    gvec.y=vec[1];
    gvec.z=vec[2];
    return gvec;
  }
}


//////////////////////////////////////////////////////////////////////////////
// Get body torque
GzVector Body::GetTorque(void)
{ 
  const dReal *vec = dBodyGetTorque( this->bodyId);

  return GzVectorSet((double)vec[0],(double)vec[1],(double)vec[2]);
}


//////////////////////////////////////////////////////////////////////////////
// Add a force to the body
void Body::AddForce( double x, double y, double z )
{
  if (this->bodyId)
    dBodyAddForce( this->bodyId, (dReal)x, (dReal)y , (dReal)z );
}


//////////////////////////////////////////////////////////////////////////////
// Add a torque to the body
void Body::AddTorque( double x, double y, double z )
{
  if (this->bodyId)
    dBodyAddTorque( this->bodyId, (dReal)x, (dReal)y, (dReal)z);
}


//////////////////////////////////////////////////////////////////////////////
// Add a relative force to the body
void Body::AddRelForce( double x, double y, double z )
{
  if (this->bodyId)
    dBodyAddRelForce( this->bodyId, (dReal)x, (dReal)y, (dReal)z );
}


//////////////////////////////////////////////////////////////////////////////
// Add a relative torque to the body
void Body::AddRelTorque( double x, double y, double z )
{
  if (this->bodyId)
    dBodyAddRelTorque( this->bodyId, (dReal)x, (dReal)y, (dReal)z);
}


//////////////////////////////////////////////////////////////////////////////
// Apply a force to the body at position
void Body::AddForceAtPos( double x, double y, double z,double pos_x,double pos_y,double pos_z )
{
  if (this->bodyId)
    dBodyAddForceAtPos( this->bodyId, (dReal)x, (dReal)y, (dReal)z,
                        (dReal)pos_x, (dReal)pos_y, (dReal)pos_z );
}


//////////////////////////////////////////////////////////////////////////////
// Apply a force to the body at relative position 
void Body::AddForceAtRelPos( double x, double y, double z,double pos_x,double pos_y,double pos_z )
{
  if (this->bodyId)
    dBodyAddForceAtRelPos( this->bodyId, (dReal)x, (dReal)y, (dReal)z,
                           (dReal)pos_x, (dReal)pos_y, (dReal)pos_z);
}


//////////////////////////////////////////////////////////////////////////////
// Non-zero mode == affected by gravity, zero mode == not affected by
// gravity
void Body::SetGravityMode(bool enable)
{
  if (this->bodyId)
    dBodySetGravityMode(this->bodyId,(int) enable);
}


//////////////////////////////////////////////////////////////////////////////
// Return the gravity mode. See SetGravityMode
int Body::GetGravityMode()
{
  if (!this->bodyId)
    return 0;
  else
    return dBodyGetGravityMode(this->bodyId);
}


//////////////////////////////////////////////////////////////////////////////
// Wrappers for the body Finite Rotation Axis functions
void Body::SetFiniteRotationAxis(double x, double y, double z)
{
  if (this->bodyId)
    dBodySetFiniteRotationAxis(this->bodyId, (dReal)x, (dReal)y, (dReal)z);
} 


//////////////////////////////////////////////////////////////////////////////
// Mode Description:
// 0: An ``infitesimal'' orientation update is used. This is fast to compute, 
//    but it can occasionally cause inaccuracies for bodies that are rotating 
//    at high speed, especially when those bodies are joined to other bodies. 
//    This is the default for every new body that is created.
//
// 1: A ``finite'' orientation update is used. This is more costly to compute, 
//    but will be more accurate for high speed rotations. Note however that 
//    high speed rotations can result in many types of error in a simulation, 
//    and this mode will only fix one of those sources of error. 
//
void Body::SetFiniteRotationMode(int mode)
{
  if (this->bodyId)
    dBodySetFiniteRotationMode(this->bodyId,mode);
}


//////////////////////////////////////////////////////////////////////////////
// Set the pick id.
void Body::SetPickId( GLuint id )
{
  for (int i=0; i<this->geomCount; i++)
  {
    this->geoms[i]->SetPickId( id );
  }

  return;
}

void Body::Render( int pass, RenderOptions *renderOpt )
{
  int i;
  Geom *geom;

  if (renderOpt->displayAxes || renderOpt->displayCoM)
    this->RenderAxes(renderOpt);

  for (i = 0; i < this->GetNumGeoms(); i++)
  {
    geom = this->geoms[i];

    if (geom->renderOrder == pass)
    {
      geom->PreRender(renderOpt);

      if (renderOpt->displaySkins)
      {
        if (geom->HasSkin())
          geom->RenderSkin(renderOpt);
        else if (!geom->HasSkinNull())
          geom->Render(renderOpt);
      }
      else
        geom->Render(renderOpt);

      geom->PostRender(renderOpt);
    }
  }
}

//////////////////////////////////////////////////////////////////////////////
// Render a set of axes on a body (useful for debugging)
void Body::RenderAxes(RenderOptions *renderOpt)
{
  GzPose pose;
  
  // Dont display axes on dummy bodies
  if (this->GetBodyId() == NULL)
    return;

  // Dont display axes on un-named bodies
  if (this->bodyName == NULL)
    return;
  
  // Draw axis for the body cs
  if (renderOpt->displayAxes)
  {
    pose = this->GetPose();
    this->DrawAxis(pose, 1.0, this->bodyName);
  }

  // Draw axis for the CoM cs
  if (renderOpt->displayCoM)
  {
    pose = this->GetCoMPose();
    this->DrawAxis(pose, 0.5, "CoM");
  }
  
  return;
}

//////////////////////////////////////////////////////////////////////////////
// Draw an axis with the given pose and size
void Body::DrawAxis(GzPose pose, double size, const char *text)
{
  int i;
  GLfloat color[4];
  GzQuatern rot;
  
  glPushMatrix();

  glTranslatef(pose.pos.x, pose.pos.y, pose.pos.z);

  rot = GzQuaternToAxis(pose.rot);
  glRotatef(rot.u * 180 / M_PI, rot.x, rot.y, rot.z);

  // Set default material properties
  GZ_COLOR_COPY(color, GzColor(0, 0, 0, 1));  
  glMaterialfv( GL_FRONT_AND_BACK, GL_AMBIENT, color);
  glMaterialfv( GL_FRONT_AND_BACK, GL_DIFFUSE, color);
  glMaterialfv( GL_FRONT_AND_BACK, GL_SPECULAR, color);

  // Z axis
  GZ_COLOR_COPY(color, GzColor(0, 0, 1, 1));
  glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, color);
  glBegin(GL_LINES);
  glVertex3f(0, 0, 0);
  glVertex3f(0, 0, size);
  glEnd();
  glBegin(GL_LINE_LOOP);
  glVertex3f(0, 0, size);
  glVertex3f(-0.05, 0, 0.9 * size);
  glVertex3f(+0.05, 0, 0.9 * size);
  glEnd();
  glBegin(GL_LINE_LOOP);
  glVertex3f(0, 0, size);
  glVertex3f(0, -0.05, 0.9 * size);
  glVertex3f(0, +0.05, 0.9 * size);
  glEnd();

  glRotatef(90, 0, 1, 0);

  // X axis
  GZ_COLOR_COPY(color, GzColor(1, 0, 0, 1));
  glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, color);
  glBegin(GL_LINES);
  glVertex3f(0, 0, 0);
  glVertex3f(0, 0, size);
  glEnd();
  glBegin(GL_LINE_LOOP);
  glVertex3f(0, 0, size);
  glVertex3f(-0.05, 0, 0.9 * size);
  glVertex3f(+0.05, 0, 0.9 * size);
  glEnd();
  glBegin(GL_LINE_LOOP);
  glVertex3f(0, 0, size);
  glVertex3f(0, -0.05, 0.9 * size);
  glVertex3f(0, +0.05, 0.9 * size);
  glEnd();

  // Display the body name
  glPushMatrix();
  glRotatef(-90, 0, 1, 0);
  glTranslatef(0.8 * size, 0.1, 0.1);
  glRasterPos3f(0, 0, 0);
  for (i = 0; i < (int) strlen(text); i++)
    glutBitmapCharacter(GLUT_BITMAP_9_BY_15, text[i]);
  glPopMatrix();

  glRotatef(-90, 1, 0, 0);

  // Y axis
  GZ_COLOR_COPY(color, GzColor(0, 1, 0, 1));
  glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, color);
  glBegin(GL_LINES);
  glVertex3f(0, 0, 0);
  glVertex3f(0, 0, size);
  glEnd();
  glBegin(GL_LINE_LOOP);
  glVertex3f(0, 0, size);
  glVertex3f(-0.05, 0, 0.9 * size);
  glVertex3f(+0.05, 0, 0.9 * size);
  glEnd();
  glBegin(GL_LINE_LOOP);
  glVertex3f(0, 0, size);
  glVertex3f(0, -0.05, 0.9 * size);
  glVertex3f(0, +0.05, 0.9 * size);
  glEnd();

  // Reset the emission color
  GZ_COLOR_COPY(color, GzColor(0, 0, 0, 1));
  glMaterialfv(GL_FRONT, GL_EMISSION, color);

  glPopMatrix();

  return;
}