C++ (Cpp) getHeightAndNormal Examples

Example #1

File: controlUtil.cpp Project: DArpinoRobotics/drake

void collisionDetect(void* map_ptr, Vector3d const &contact_pos, Vector3d &pos, Vector3d *normal, double terrain_height)
{
  if (map_ptr) {
#ifdef USE_MAPS    
    Vector3d oNormal;
    double height;
    auto state = static_cast<terrainmap::TerrainMap*>(map_ptr);
    if (state != NULL) {
      state->getHeightAndNormal(contact_pos(0), contact_pos(1), height, oNormal);
      pos << contact_pos.topRows(2), height;
      if (normal) {
        *normal = oNormal.cast<double>();
        if ((*normal)(2) < 0) {
          *normal = -(*normal);
        }
        return;
      }
    }
#else
    throw std::runtime_error("map_ptr is not NULL, but controlUtil was built without USE_MAPS support");
#endif      
  } else {
//    mexPrintf("Warning: using 0,0,1 as normal\n");
    pos << contact_pos.topRows(2), terrain_height;
    if (normal) *normal << 0,0,1;
  }
}

Example #2

File: controlUtil.cpp Project: blandry/drake

void collisionDetect(void* map_ptr, Vector3d const &contact_pos, Vector3d &pos, Vector3d *normal, double terrain_height)
{
  if (map_ptr) {
#ifdef USE_MAPS    
    Vector3d oNormal;
    double height;
    auto state = static_cast<terrainmap::TerrainMap*>(map_ptr);
    if (state != NULL) {
      state->getHeightAndNormal(contact_pos(0), contact_pos(1), height, oNormal);
      pos << contact_pos.topRows(2), height;
      if (normal) {
        *normal = oNormal.cast<double>();
        return;
      }
    }
#endif      
  } else {
//    mexPrintf("Warning: using 0,0,1 as normal\n");
    pos << contact_pos.topRows(2), terrain_height;
    if (normal) *normal << 0,0,1;
  }
}