void Omnicam::PutCameraData()
{
  int i, j;
  int len;
  const unsigned char *src;
  unsigned char *dst;
  int width, height;
  GzPose pose;
  gz_camera_data_t *data;

  gz_camera_lock(this->cameraIface, 1);

  data = this->cameraIface->data;

  // Get image dimensions
  this->camera->GetImageSize(&width, &height);
  
  data->time = this->world->GetSimTime();
  data->width = width * 4;
  data->height = height;
  data->image_size = width * height * 3 * 4;

  // Make sure there is room to store the image
  assert(data->image_size <= sizeof(data->image));
 
  len = width * 3;
  // Copy the pixel data to the interface, but flip the y axis
  for(j = 0; j < 4; j++) {
    // 4 passes at 90 degrees each to give cube map texture of panorama
    dst = data->image + len * j;
    this->camera->Update();
    src = this->camera->GetImageData() + (height - 1) * len;
    for (i = 0; i < height; i++, src -= len, dst += len) {
      memcpy(dst, src, len);
      dst += len * 3;
    }
    pose = this->camera->GetPose();
    pose.rot.z += M_PI / 2;
    this->camera->SetPose(pose);
  }

  gz_camera_unlock(this->cameraIface);
  gz_camera_post(this->cameraIface);

  return;
}