<commit_before>/* Source file for the R3 surfel scene class */



////////////////////////////////////////////////////////////////////////
// INCLUDE FILES
////////////////////////////////////////////////////////////////////////

#include "R3Surfels.h"



////////////////////////////////////////////////////////////////////////
// Namespace
////////////////////////////////////////////////////////////////////////

namespace gaps {



////////////////////////////////////////////////////////////////////////
// PRIVATE FLAGS
////////////////////////////////////////////////////////////////////////

#define R3_SURFEL_SCENE_DIRTY_FLAG                  0x01



////////////////////////////////////////////////////////////////////////
// CONSTRUCTORS/DESTRUCTORS
////////////////////////////////////////////////////////////////////////

R3SurfelScene::
R3SurfelScene(const char *name)
  : tree(NULL),
    objects(),
    labels(),
    object_properties(),
    label_properties(),
    object_relationships(),
    label_relationships(),
    assignments(),
    scans(),
    images(),
    features(),
    transformation(R4Matrix(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1), 0),
    filename(NULL),
    rwaccess(NULL),
    name((name) ? RNStrdup(name) : NULL),
    flags(R3_SURFEL_SCENE_DIRTY_FLAG)
{
  // Create tree
  tree = new R3SurfelTree();
  tree->scene = this;

  // Create root object
  R3SurfelObject *object = new R3SurfelObject("Root");
  InsertObject(object, NULL);

  // Create root label
  R3SurfelLabel *label = new R3SurfelLabel("Root");
  InsertLabel(label, NULL);
}



R3SurfelScene::
R3SurfelScene(const R3SurfelScene& scene)
{
  // Copy everything
  RNAbort("Not implemented");
}



R3SurfelScene::
~R3SurfelScene(void)
{
  // Delete everything
  while (NFeatures() > 0) delete Feature(NFeatures()-1);
  while (NLabelAssignments() > 0) delete LabelAssignment(NLabelAssignments()-1);
  while (NObjectRelationships() > 0) delete ObjectRelationship(NObjectRelationships()-1);
  while (NLabelRelationships() > 0) delete LabelRelationship(NLabelRelationships()-1);
  while (NObjectProperties() > 0) delete ObjectProperty(NObjectProperties()-1);
  while (NLabelProperties() > 0) delete LabelProperty(NLabelProperties()-1);
  while (NLabels() > 0) delete Label(NLabels()-1);
  while (NObjects() > 0) delete Object(NObjects()-1);
  while (NScans() > 0) delete Scan(NScans()-1);
  while (NImages() > 0) delete Image(NImages()-1);
  
  // Delete tree
  if (tree) delete tree;
  
  // Delete filename
  if (filename) free(filename);

  // Delete rwaccess
  if (rwaccess) free(rwaccess);

  // Delete name
  if (name) free(name);
}



////////////////////////////////////////////////////////////////////////
// ACCESS FUNCTIONS
////////////////////////////////////////////////////////////////////////

R3SurfelObject *R3SurfelScene::
FindObjectByName(const char *object_name) const
{
  // Search for object
  for (int i = 0; i < NObjects(); i++) {
    R3SurfelObject *object = Object(i);
    if (!object->Name()) continue;
    if (strcmp(object->Name(), object_name)) continue;
    return object;
  }

  // Not found
  return NULL;
}



R3SurfelObject *R3SurfelScene::
FindObjectByIdentifier(int identifier) const
{
  // Search for object
  for (int i = 0; i < NObjects(); i++) {
    R3SurfelObject *object = Object(i);
    if (object->Identifier() < 0) continue;
    if (object->Identifier() != identifier) continue;
    return object;
  }

  // Not found
  return NULL;
}



R3SurfelLabel *R3SurfelScene::
FindLabelByName(const char *label_name) const
{
  // Search for label
  for (int i = 0; i < NLabels(); i++) {
    R3SurfelLabel *label = Label(i);
    if (!label->Name()) continue;
    if (strcmp(label->Name(), label_name)) continue;
    return label;
  }

  // Not found
  return NULL;
}



R3SurfelLabel *R3SurfelScene::
FindLabelByIdentifier(int identifier) const
{
  // Search for label
  for (int i = 0; i < NLabels(); i++) {
    R3SurfelLabel *label = Label(i);
    if (label->Identifier() < 0) continue;
    if (label->Identifier() != identifier) continue;
    return label;
  }

  // Not found
  return NULL;
}



R3SurfelLabel *R3SurfelScene::
FindLabelByAssignmentKeystroke(int key) const
{
  // Search for label
  for (int i = 0; i < NLabels(); i++) {
    R3SurfelLabel *label = Label(i);
    if (label->AssignmentKeystroke() < 0) continue;
    if (label->AssignmentKeystroke() != key) continue;
    return label;
  }

  // Not found
  return NULL;
}



R3SurfelLabelAssignment *R3SurfelScene::
FindLabelAssignment(R3SurfelObject *object, R3SurfelLabel *label, RNScalar confidence, int originator) const
{
  // Search for label assignment
  for (int i = 0; i < object->NLabelAssignments(); i++) {
    R3SurfelLabelAssignment *assignment = object->LabelAssignment(i);
    if (assignment->Label() != label) continue;
    if (assignment->Confidence() != confidence) continue;
    if (assignment->Originator() != originator) continue;
    return assignment;
  }

  // Not found
  return NULL;
}



R3SurfelScan *R3SurfelScene::
FindScanByName(const char *scan_name) const
{
  // Search for scan
  for (int i = 0; i < NScans(); i++) {
    R3SurfelScan *scan = Scan(i);
    if (!scan->Name()) continue;
    if (strcmp(scan->Name(), scan_name)) continue;
    return scan;
  }

  // Not found
  return NULL;
}



R3SurfelImage *R3SurfelScene::
FindImageByName(const char *image_name) const
{
  // Search for image
  for (int i = 0; i < NImages(); i++) {
    R3SurfelImage *image = Image(i);
    if (!image->Name()) continue;
    if (strcmp(image->Name(), image_name)) continue;
    return image;
  }

  // Not found
  return NULL;
}



R3SurfelFeature *R3SurfelScene::
FindFeatureByName(const char *feature_name) const
{
  // Search for feature
  for (int i = 0; i < NFeatures(); i++) {
    R3SurfelFeature *feature = Feature(i);
    if (!feature->Name()) continue;
    if (strcmp(feature->Name(), feature_name)) continue;
    return feature;
  }

  // Not found
  return NULL;
}



////////////////////////////////////////////////////////////////////////
// PROPERTY MANIPULATION FUNCTIONS
////////////////////////////////////////////////////////////////////////

void R3SurfelScene::
SetName(const char *name)
{
  // Delete previous name
  if (this->name) free(this->name);

  // Set new name
  this->name = (name) ? RNStrdup(name) : NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
SetTransformation(const R3Affine& transformation, RNBoolean update_surfels)
{
  // Set transformation (does not change surfels)
  this->transformation = transformation;
}



void R3SurfelScene::
Transform(const R3Affine& transformation, RNBoolean update_surfels)
{
  // Check if should update surfels
  if (update_surfels) {
    // Compute total transformation
    R3Affine t = R3identity_affine;
    t.Transform(transformation);
    t.Transform(this->transformation);
    this->transformation = R3identity_affine;
    
    // Transform all nodes                                                                                                     
    R3SurfelTree *tree = Tree();
    for (int i = 0; i < tree->NNodes(); i++) {
      R3SurfelNode *node = tree->Node(i);
      node->Transform(t);
    }

    // Transform all scan poses                                                                                                
    for (int i = 0; i < NScans(); i++) {
      R3SurfelScan *scan = Scan(i);
      R3CoordSystem pose = scan->Pose();
      pose.Transform(t);
      scan->SetPose(pose);
    }

    // Transform all image poses                                                                                               
    for (int i = 0; i < NImages(); i++) {
      R3SurfelImage *image = Image(i);
      R3CoordSystem pose = image->Pose();
      pose.Transform(t);
      image->SetPose(pose);
    }
  }
  else {
    // Update transformation only
    R3Affine t = R3identity_affine;
    t.Transform(transformation);
    t.Transform(this->transformation);
    this->transformation = t;
  }
}



////////////////////////////////////////////////////////////////////////
// STRUCTURE MANIPULATION FUNCTIONS
////////////////////////////////////////////////////////////////////////

void R3SurfelScene::
InsertObject(R3SurfelObject *object, R3SurfelObject *parent)
{
  // Just checking
  assert(object);
  assert(object->scene == NULL);
  assert(object->scene_index == -1);

  // Insert object into scene
  object->scene = this;
  object->scene_index = objects.NEntries();
  objects.Insert(object);

  // Insert object into parent
  object->parent = parent;
  if (parent) parent->parts.Insert(object);

  // Update object
  object->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
MergeObject(R3SurfelObject *dst_object, R3SurfelObject *src_object)
{
  // Just checking
  assert(dst_object && src_object);
  assert(dst_object->scene == this);
  assert(dst_object->scene_index >= 0);
  assert(src_object->scene == this);
  assert(src_object->scene_index >= 0);

  // Move all nodes from src_object into dst_object
  while (src_object->NNodes() > 0) {
    R3SurfelNode *node = src_object->Node(src_object->NNodes()-1);
    src_object->RemoveNode(node);
    dst_object->InsertNode(node);
  }

  // Move all parts from src_object into dst_object
  while (src_object->NParts() > 0) {
    R3SurfelObject *part = src_object->Part(src_object->NParts()-1);
    RemoveObject(part);
    InsertObject(part, dst_object);
  }

  // Move all object properties from src_object into dst_object
  while (src_object->NObjectProperties() > 0) {
    R3SurfelObjectProperty *property = src_object->ObjectProperty(src_object->NObjectProperties()-1);
    src_object->UpdateBeforeRemoveObjectProperty(property);
    property->object = dst_object;
    dst_object->UpdateAfterInsertObjectProperty(property);
  }

  // Move all object relationships from src_object into dst_object
  while (src_object->NObjectRelationships() > 0) {
    R3SurfelObjectRelationship *relationship = src_object->ObjectRelationship(src_object->NObjectRelationships()-1);
    src_object->UpdateBeforeRemoveObjectRelationship(relationship);
    for (int i = 0; i < relationship->objects.NEntries(); i++) {
      if (relationship->objects.Kth(i) == src_object) {
        RNArrayEntry *entry = relationship->objects.KthEntry(i);
        relationship->objects.EntryContents(entry) = dst_object;
        break;
      }
    }
    dst_object->UpdateAfterInsertObjectRelationship(relationship);
  }

  // Move all label assignments from src_object into dst_object
  while (src_object->NLabelAssignments() > 0) {
    R3SurfelLabelAssignment *assignment = src_object->LabelAssignment(src_object->NLabelAssignments()-1);
    src_object->UpdateBeforeRemoveLabelAssignment(assignment);
    assignment->object = dst_object;
    dst_object->UpdateAfterInsertLabelAssignment(assignment);
  }

  // Remove src_object
  RemoveObject(src_object);

  // Delete src_object
  delete src_object;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveObject(R3SurfelObject *object)
{
  // Just checking
  assert(object);
  assert(object->scene == this);
  assert(object->scene_index >= 0);
  assert(objects.Kth(object->scene_index) == object);

  // Remove object propertys from scene
  while (object->NObjectProperties() > 0) {
    R3SurfelObjectProperty *property = object->ObjectProperty(object->NObjectProperties()-1);
    RemoveObjectProperty(property);
  }

  // Remove object relationships from scene
  while (object->NObjectRelationships() > 0) {
    R3SurfelObjectRelationship *relationship = object->ObjectRelationship(object->NObjectRelationships()-1);
    RemoveObjectRelationship(relationship);
  }

  // Remove label assignments
  while (object->NLabelAssignments() > 0) {
    R3SurfelLabelAssignment *assignment = object->LabelAssignment(object->NLabelAssignments()-1);
    RemoveLabelAssignment(assignment);
  }

  // Update object
  object->UpdateBeforeRemove(this);

  // Remove object from parent
  if (object->parent) {
    object->parent->parts.Remove(object);
    object->parent = NULL;
  }

  // Remove object from parts
  for (int i = 0; i < object->NParts(); i++) {
    R3SurfelObject *part = object->Part(i);
    part->parent = NULL;
  }

  // Remove parts from object
  object->parts.Empty();

  // Remove object from scene
  RNArrayEntry *entry = objects.KthEntry(object->scene_index);
  R3SurfelObject *tail = objects.Tail();
  tail->scene_index = object->scene_index;
  objects.EntryContents(entry) = tail;
  objects.RemoveTail();
  object->scene_index = -1;
  object->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertLabel(R3SurfelLabel *label, R3SurfelLabel *parent)
{
  // Just checking
  assert(label);
  assert(label->scene == NULL);
  assert(label->scene_index == -1);

  // Insert label 
  label->scene = this;
  label->scene_index = labels.NEntries();
  labels.Insert(label);

  // Insert label into parent
  label->parent = parent;
  if (parent) parent->parts.Insert(label);

  // Update label
  label->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveLabel(R3SurfelLabel *label)
{
  // Just checking
  assert(label);
  assert(label->scene == this);
  assert(label->scene_index >= 0);
  assert(labels.Kth(label->scene_index) == label);

  // Remove label properties from scene
  while (label->NLabelProperties() > 0) {
    R3SurfelLabelProperty *property = label->LabelProperty(label->NLabelProperties()-1);
    RemoveLabelProperty(property);
  }

  // Remove label relationships from scene
  while (label->NLabelRelationships() > 0) {
    R3SurfelLabelRelationship *relationship = label->LabelRelationship(label->NLabelRelationships()-1);
    RemoveLabelRelationship(relationship);
  }

  // Remove label assignments
  while (label->NLabelAssignments() > 0) {
    R3SurfelLabelAssignment *assignment = label->LabelAssignment(label->NLabelAssignments()-1);
    RemoveLabelAssignment(assignment);
  }

  // Update label
  label->UpdateBeforeRemove(this);

  // Remove label from parent
  if (label->parent) {
    label->parent->parts.Remove(label);
    label->parent = NULL;
  }

  // Remove label from parts
  for (int i = 0; i < label->NParts(); i++) {
    R3SurfelLabel *part = label->Part(i);
    part->parent = NULL;
  }

  // Remove parts from label
  label->parts.Empty();

  // Remove label from scene
  RNArrayEntry *entry = labels.KthEntry(label->scene_index);
  R3SurfelLabel *tail = labels.Tail();
  tail->scene_index = label->scene_index;
  labels.EntryContents(entry) = tail;
  labels.RemoveTail();
  label->scene_index = -1;
  label->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertObjectProperty(R3SurfelObjectProperty *property)
{
  // Just checking
  assert(property);
  assert(property->scene == NULL);
  assert(property->scene_index == -1);

  // Insert property into scene
  property->scene = this;
  property->scene_index = object_properties.NEntries();
  object_properties.Insert(property);

  // Update object
  R3SurfelObject *object = property->Object();
  object->UpdateAfterInsertObjectProperty(property);

  // Update object property
  property->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveObjectProperty(R3SurfelObjectProperty *property)
{
  // Just checking
  assert(property);
  assert(property->scene == this);
  assert(property->scene_index >= 0);
  assert(object_properties.Kth(property->scene_index) == property);

  // Update object property
  property->UpdateBeforeRemove(this);

  // Update object
  R3SurfelObject *object = property->Object();
  object->UpdateBeforeRemoveObjectProperty(property);

  // Remove property from scene
  RNArrayEntry *entry = object_properties.KthEntry(property->scene_index);
  R3SurfelObjectProperty *tail = object_properties.Tail();
  tail->scene_index = property->scene_index;
  object_properties.EntryContents(entry) = tail;
  object_properties.RemoveTail();
  property->scene_index = -1;
  property->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertLabelProperty(R3SurfelLabelProperty *property)
{
  // Just checking
  assert(property);
  assert(property->scene == this);
  assert(property->scene_index == -1);

  // Insert property into scene
  property->scene = this;
  property->scene_index = label_properties.NEntries();
  label_properties.Insert(property);

  // Update label
  R3SurfelLabel *label = property->Label();
  label->UpdateAfterInsertLabelProperty(property);

  // Update label property
  property->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveLabelProperty(R3SurfelLabelProperty *property)
{
  // Just checking
  assert(property);
  assert(property->scene == this);
  assert(property->scene_index >= 0);
  assert(label_properties.Kth(property->scene_index) == property);

  // Update label property
  property->UpdateAfterInsert(this);

  // Update label
  R3SurfelLabel *label = property->Label();
  label->UpdateBeforeRemoveLabelProperty(property);

  // Remove property from scene
  RNArrayEntry *entry = label_properties.KthEntry(property->scene_index);
  R3SurfelLabelProperty *tail = label_properties.Tail();
  tail->scene_index = property->scene_index;
  label_properties.EntryContents(entry) = tail;
  label_properties.RemoveTail();
  property->scene_index = -1;
  property->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertObjectRelationship(R3SurfelObjectRelationship *relationship)
{
  // Just checking
  assert(relationship);
  assert(relationship->scene == NULL);
  assert(relationship->scene_index == -1);

  // Insert relationship into scene
  relationship->scene = this;
  relationship->scene_index = object_relationships.NEntries();
  object_relationships.Insert(relationship);

  // Update objects
  for (int i = 0; i < relationship->NObjects(); i++) {
    R3SurfelObject *object = relationship->Object(i);
    object->UpdateAfterInsertObjectRelationship(relationship);
  }

  // Update object relationship
  relationship->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveObjectRelationship(R3SurfelObjectRelationship *relationship)
{
  // Just checking
  assert(relationship);
  assert(relationship->scene == this);
  assert(relationship->scene_index >= 0);
  assert(object_relationships.Kth(relationship->scene_index) == relationship);

  // Update object relationship
  relationship->UpdateBeforeRemove(this);

  // Update objects
  for (int i = 0; i < relationship->NObjects(); i++) {
    R3SurfelObject *object = relationship->Object(i);
    object->UpdateBeforeRemoveObjectRelationship(relationship);
  }

  // Remove relationship from scene
  RNArrayEntry *entry = object_relationships.KthEntry(relationship->scene_index);
  R3SurfelObjectRelationship *tail = object_relationships.Tail();
  tail->scene_index = relationship->scene_index;
  object_relationships.EntryContents(entry) = tail;
  object_relationships.RemoveTail();
  relationship->scene_index = -1;
  relationship->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertLabelRelationship(R3SurfelLabelRelationship *relationship)
{
  // Just checking
  assert(relationship);
  assert(relationship->scene == this);
  assert(relationship->scene_index == -1);

  // Insert relationship into scene
  relationship->scene = this;
  relationship->scene_index = label_relationships.NEntries();
  label_relationships.Insert(relationship);

  // Update labels
  for (int i = 0; i < relationship->NLabels(); i++) {
    R3SurfelLabel *label = relationship->Label(i);
    label->UpdateAfterInsertLabelRelationship(relationship);
  }

  // Update label relationship
  relationship->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveLabelRelationship(R3SurfelLabelRelationship *relationship)
{
  // Just checking
  assert(relationship);
  assert(relationship->scene == this);
  assert(relationship->scene_index >= 0);
  assert(label_relationships.Kth(relationship->scene_index) == relationship);

  // Update label relationship
  relationship->UpdateAfterInsert(this);

  // Update labels
  for (int i = 0; i < relationship->NLabels(); i++) {
    R3SurfelLabel *label = relationship->Label(i);
    label->UpdateBeforeRemoveLabelRelationship(relationship);
  }

  // Remove relationship from scene
  RNArrayEntry *entry = label_relationships.KthEntry(relationship->scene_index);
  R3SurfelLabelRelationship *tail = label_relationships.Tail();
  tail->scene_index = relationship->scene_index;
  label_relationships.EntryContents(entry) = tail;
  label_relationships.RemoveTail();
  relationship->scene_index = -1;
  relationship->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertLabelAssignment(R3SurfelLabelAssignment *assignment)
{
  // Just checking
  assert(assignment);
  assert(assignment->scene == NULL);
  assert(assignment->scene_index == -1);
  assert(assignment->Object());
  assert(assignment->Object()->Scene() == this);
  assert(assignment->Label());
  assert(assignment->Label()->Scene() == this);

  // Insert assignment into scene
  assignment->scene = this;
  assignment->scene_index = assignments.NEntries();
  assignments.Insert(assignment);

  // Update object and label
  assignment->Object()->UpdateAfterInsertLabelAssignment(assignment);
  assignment->Label()->UpdateAfterInsertLabelAssignment(assignment);

  // Update assignment
  assignment->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveLabelAssignment(R3SurfelLabelAssignment *assignment)
{
  // Just checking
  assert(assignment);
  assert(assignment->scene == this);
  assert(assignment->scene_index >= 0);
  assert(assignment->Object());
  assert(assignment->Object()->Scene() == this);
  assert(assignment->Label());
  assert(assignment->Label()->Scene() == this);

  // Update assignment
  assignment->UpdateBeforeRemove(this);

  // Update object and label
  assignment->Object()->UpdateBeforeRemoveLabelAssignment(assignment);
  assignment->Label()->UpdateBeforeRemoveLabelAssignment(assignment);

  // Remove assignment from scene
  assert(assignment->scene == this);
  RNArrayEntry *entry = assignments.KthEntry(assignment->scene_index);
  R3SurfelLabelAssignment *tail = assignments.Tail();
  tail->scene_index = assignment->scene_index;
  assignments.EntryContents(entry) = tail;
  assignments.RemoveTail();
  assignment->scene_index = -1;
  assignment->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertScan(R3SurfelScan *scan)
{
  // Just checking
  assert(scan);

  // Insert label 
  scan->scene = this;
  scan->scene_index = scans.NEntries();
  scans.Insert(scan);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveScan(R3SurfelScan *scan)
{
  // Just checking
  assert(scan);

  // Remove scan from scene
  RNArrayEntry *entry = scans.KthEntry(scan->scene_index);
  R3SurfelScan *tail = scans.Tail();
  tail->scene_index = scan->scene_index;
  scans.EntryContents(entry) = tail;
  scans.RemoveTail();
  scan->scene_index = -1;
  scan->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertImage(R3SurfelImage *image)
{
  // Just checking
  assert(image);

  // Insert label 
  image->scene = this;
  image->scene_index = images.NEntries();
  images.Insert(image);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveImage(R3SurfelImage *image)
{
  // Just checking
  assert(image);

  // Remove image from scene
  RNArrayEntry *entry = images.KthEntry(image->scene_index);
  R3SurfelImage *tail = images.Tail();
  tail->scene_index = image->scene_index;
  images.EntryContents(entry) = tail;
  images.RemoveTail();
  image->scene_index = -1;
  image->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertFeature(R3SurfelFeature *feature)
{
  // Just checking
  assert(feature);

  // Insert label 
  feature->scene = this;
  feature->scene_index = features.NEntries();
  features.Insert(feature);

  // Update all object feature vectors
  for (int i = 0; i < NObjects(); i++) {
    R3SurfelObject *object = Object(i);
    if (object->feature_vector.NValues() > 0) {
      object->feature_vector.Resize(NFeatures());
      feature->UpdateFeatureVector(object, object->feature_vector);
    }
  }

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveFeature(R3SurfelFeature *feature)
{
  // Just checking
  assert(feature);
  assert(feature->scene == this);
  assert(feature->scene_index >= 0);
  assert(feature->scene_index < NFeatures());

  // Remove feature from scene
  RNArrayEntry *entry = features.KthEntry(feature->scene_index);
  R3SurfelFeature *tail = features.Tail();
  tail->scene_index = feature->scene_index;
  features.EntryContents(entry) = tail;
  features.RemoveTail();
  feature->scene_index = -1;
  feature->scene = NULL;

  // Update all object feature vectors
  if (feature != tail) {
    for (int i = 0; i < NObjects(); i++) {
      R3SurfelObject *object = Object(i);
      R3SurfelFeatureVector& vector = object->feature_vector;
      if (vector.NValues() == 0) continue;
      assert(vector.NValues() == NFeatures()+1);
      RNScalar tail_value = vector.Value(vector.NValues()-1);
      object->feature_vector.SetValue(tail->scene_index, tail_value);
      object->feature_vector.Resize(NFeatures());
    }
  }

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertScene(const R3SurfelScene& scene2, 
  R3SurfelObject *parent_object1, 
  R3SurfelLabel *parent_label1,
  R3SurfelNode *parent_node1)
{
  // Get convenient variables
  R3SurfelScene& scene1 = *this;
  R3SurfelTree *tree1 = scene1.Tree();
  R3SurfelTree *tree2 = scene2.Tree();
  R3SurfelDatabase *database1 = tree1->Database();
  R3SurfelDatabase *database2 = tree2->Database();

  // Find parents in scene1
  if (!parent_node1) parent_node1 = tree1->RootNode();
  if (!parent_object1) parent_object1 = scene1.RootObject();
  if (!parent_label1) parent_label1 = scene1.RootLabel();

 // COPY FEATURES

#if 0
  // Copy label features from scene2
  for (int i = 0; i < scene2.NFeatures(); i++) {
    R3SurfelFeature *feature2 = scene2.Feature(i);
    R3SurfelFeature *feature1 = new R3SurfelFeature(feature2->Name(), feature2->Minimum(), feature2->Maximum());
    scene1.InsertFeature(feature1);
  }
#endif

  // COPY NODES

  // Create surfel tree nodes in scene1
  RNArray<R3SurfelNode *> nodes1;
  nodes1.Insert(tree1->RootNode());
  for (int i = 1; i < tree2->NNodes(); i++) {
    R3SurfelNode *node1 = new R3SurfelNode();
    nodes1.Insert(node1);
  }

  // Copy node stuff from scene2
  for (int i = 1; i < tree2->NNodes(); i++) {
    R3SurfelNode *node1 = nodes1.Kth(i);
    R3SurfelNode *node2 = tree2->Node(i);
    R3SurfelNode *parent2 = node2->Parent();
    R3SurfelNode *parent1 = parent_node1;
    if (parent2 && (!parent2->Name() || strcmp(parent2->Name(), "Root")))
      parent1 = nodes1.Kth(parent2->TreeIndex());
    tree1->InsertNode(node1, parent1);
    node1->SetName(node2->Name());
    for (int j = 0; j < node2->NBlocks(); j++) {
      R3SurfelBlock *block2 = node2->Block(j);
      database2->ReadBlock(block2);
      R3SurfelBlock *block1 = new R3SurfelBlock(*block2);
      database1->InsertBlock(block1);
      node1->InsertBlock(block1);
      database1->ReleaseBlock(block1);
      database2->ReleaseBlock(block2);
    }      
  }

  // COPY OBJECTS

  // Create objects in scene1
  RNArray<R3SurfelObject *> objects1;
  objects1.Insert(scene1.RootObject());
  for (int i = 1; i < scene2.NObjects(); i++) {
    R3SurfelObject *object1 = new R3SurfelObject();
    objects1.Insert(object1);
  }

  // Copy object stuff from scene2
  for (int i = 1; i < scene2.NObjects(); i++) {
    R3SurfelObject *object1 = objects1.Kth(i);
    R3SurfelObject *object2 = scene2.Object(i);
    R3SurfelObject *parent2 = object2->Parent();
    R3SurfelObject *parent1 = parent_object1;
    if (parent2 && (!parent2->Name() || strcmp(parent2->Name(), "Root")))
      parent1 = objects1.Kth(parent2->SceneIndex());
    scene1.InsertObject(object1, parent1);
    object1->SetName(object2->Name());
    object1->SetIdentifier(object2->Identifier());
    object1->SetFeatureVector(object2->feature_vector);
    for (int j = 0; j < object2->NNodes(); j++) {
      R3SurfelNode *node2 = object2->Node(j);
      R3SurfelNode *node1 = nodes1.Kth(node2->TreeIndex());
      object1->InsertNode(node1);
    }
  }

  // COPY LABELS 

  // Create labels in scene1
  RNArray<R3SurfelLabel *> labels1;
  labels1.Insert(scene1.RootLabel());
  for (int i = 1; i < scene2.NLabels(); i++) {
    R3SurfelLabel *label1 = new R3SurfelLabel();
    labels1.Insert(label1);
  }

  // Copy label stuff from scene2
  for (int i = 1; i < scene2.NLabels(); i++) {
    R3SurfelLabel *label1 = labels1.Kth(i);
    R3SurfelLabel *label2 = scene2.Label(i);
    R3SurfelLabel *parent2 = label2->Parent();
    R3SurfelLabel *parent1 = parent_label1;
    if (parent2 && (!parent2->Name() || strcmp(parent2->Name(), "Root")))
      parent1 = labels1.Kth(parent2->SceneIndex());
    label1->SetName(label2->Name());
    label1->SetIdentifier(label2->Identifier());
    label1->SetAssignmentKeystroke(label2->AssignmentKeystroke());
    label1->SetColor(label2->Color());
    scene1.InsertLabel(label1, parent1);
  }

  // COPY OBJECT PROPERTIES

  // Copy object properties from scene2
  for (int i = 0; i < scene2.NObjectProperties(); i++) {
    R3SurfelObjectProperty *property2 = scene2.ObjectProperty(i);
    int type = property2->Type();
    RNArray<R3SurfelObject *> objects1;
    R3SurfelObject *object2 = property2->Object();
    R3SurfelObject *object1 = objects1.Kth(object2->SceneIndex());
    objects1.Insert(object1);
    int noperands1 = 0;
    RNScalar *operands1 = NULL;
    if (property2->NOperands() > 0) {
      noperands1 = property2->NOperands();
      operands1 = new RNScalar [ noperands1 ];
      for (int j = 0; j < noperands1; j++) {
        RNScalar operand2 = property2->Operand(j);
        operands1[j] = operand2;
      }
    }
    R3SurfelObjectProperty *property1 = new R3SurfelObjectProperty(type, object1, operands1, noperands1);
    scene1.InsertObjectProperty(property1);
    delete [] operands1;
  }

  // COPY LABEL PROPERTIES

  // Copy label properties from scene2
  for (int i = 0; i < scene2.NLabelProperties(); i++) {
    R3SurfelLabelProperty *property2 = scene2.LabelProperty(i);
    int type = property2->Type();
    RNArray<R3SurfelLabel *> labels1;
    R3SurfelLabel *label2 = property2->Label();
    R3SurfelLabel *label1 = labels1.Kth(label2->SceneIndex());
    labels1.Insert(label1);
    int noperands1 = 0;
    RNScalar *operands1 = NULL;
    if (property2->NOperands() > 0) {
      noperands1 = property2->NOperands();
      operands1 = new RNScalar [ noperands1 ];
      for (int j = 0; j < noperands1; j++) {
        RNScalar operand2 = property2->Operand(j);
        operands1[j] = operand2;
      }
    }
    R3SurfelLabelProperty *property1 = new R3SurfelLabelProperty(type, label1, operands1, noperands1);
    scene1.InsertLabelProperty(property1);
    delete [] operands1;
  }

  // COPY OBJECT RELATIONSHIPS

  // Copy object relationships from scene2
  for (int i = 0; i < scene2.NObjectRelationships(); i++) {
    R3SurfelObjectRelationship *relationship2 = scene2.ObjectRelationship(i);
    int type = relationship2->Type();
    RNArray<R3SurfelObject *> objects1;
    for (int j = 0; j < relationship2->NObjects(); j++) {
      R3SurfelObject *object2 = relationship2->Object(j);
      R3SurfelObject *object1 = objects1.Kth(object2->SceneIndex());
      objects1.Insert(object1);
    }
    int noperands1 = 0;
    RNScalar *operands1 = NULL;
    if (relationship2->NOperands() > 0) {
      noperands1 = relationship2->NOperands();
      operands1 = new RNScalar [ noperands1 ];
      for (int j = 0; j < noperands1; j++) {
        RNScalar operand2 = relationship2->Operand(j);
        operands1[j] = operand2;
      }
    }
    R3SurfelObjectRelationship *relationship1 = new R3SurfelObjectRelationship(type, objects1, operands1, noperands1);
    scene1.InsertObjectRelationship(relationship1);
    delete [] operands1;
  }

  // COPY LABEL RELATIONSHIPS

  // Copy label relationships from scene2
  for (int i = 0; i < scene2.NLabelRelationships(); i++) {
    R3SurfelLabelRelationship *relationship2 = scene2.LabelRelationship(i);
    int type = relationship2->Type();
    RNArray<R3SurfelLabel *> labels1;
    for (int j = 0; j < relationship2->NLabels(); j++) {
      R3SurfelLabel *label2 = relationship2->Label(j);
      R3SurfelLabel *label1 = labels1.Kth(label2->SceneIndex());
      labels1.Insert(label1);
    }
    int noperands1 = 0;
    RNScalar *operands1 = NULL;
    if (relationship2->NOperands() > 0) {
      noperands1 = relationship2->NOperands();
      operands1 = new RNScalar [ noperands1 ];
      for (int j = 0; j < noperands1; j++) {
        RNScalar operand2 = relationship2->Operand(j);
        operands1[j] = operand2;
      }
    }
    R3SurfelLabelRelationship *relationship1 = new R3SurfelLabelRelationship(type, labels1, operands1);
    scene1.InsertLabelRelationship(relationship1);
    delete [] operands1;
  }

  // COPY ASSIGNMENTS

  // Copy label assignments from scene2
  for (int i = 0; i < scene2.NLabelAssignments(); i++) {
    R3SurfelLabelAssignment *assignment2 = scene2.LabelAssignment(i);
    R3SurfelObject *object2 = assignment2->Object();
    R3SurfelLabel *label2 = assignment2->Label();
    R3SurfelObject *object1 = objects1.Kth(object2->SceneIndex());
    R3SurfelLabel *label1 = labels1.Kth(label2->SceneIndex());
    R3SurfelLabelAssignment *assignment1 = new R3SurfelLabelAssignment(object1, label1, assignment2->Confidence(), assignment2->Originator());
    scene1.InsertLabelAssignment(assignment1);
  }

  // COPY SCANS

  // Copy scans from scene2
  RNArray<R3SurfelScan *> scans1;
  for (int i = 0; i < scene2.NScans(); i++) {
    R3SurfelScan *scan2 = scene2.Scan(i);
    R3SurfelScan *scan1 = new R3SurfelScan(scan2->Name());
    scan1->SetPose(scan2->Pose());
    scan1->SetTimestamp(scan2->Timestamp());
    scan1->SetXFocal(scan2->XFocal());
    scan1->SetYFocal(scan2->YFocal());
    scan1->SetImageDimensions(scan2->ImageWidth(), scan2->ImageHeight());
    scan1->SetImageCenter(scan2->ImageCenter());
    scan1->SetFlags(scan2->Flags());
    R3SurfelNode *node2 = scan2->Node();
    R3SurfelNode *node1 = nodes1.Kth(node2->TreeIndex());
    scan1->SetNode(node1);
    scene1.InsertScan(scan1);
    scans1.Insert(scan1);
  }

  // COPY IMAGES
  
  // Copy images from scene2
  for (int i = 0; i < scene2.NImages(); i++) {
    R3SurfelImage *image2 = scene2.Image(i);
    R3SurfelImage *image1 = new R3SurfelImage(image2->Name());
    image1->SetPose(image2->Pose());
    image1->SetTimestamp(image2->Timestamp());
    image1->SetXFocal(image2->XFocal());
    image1->SetYFocal(image2->YFocal());
    image1->SetImageDimensions(image2->ImageWidth(), image2->ImageHeight());
    image1->SetImageCenter(image2->ImageCenter());
    image1->SetFlags(image2->Flags());
    R3SurfelScan *scan2 = image2->Scan();
    R3SurfelScan *scan1 = (scan2) ? scans1.Kth(scan2->SceneIndex()) : NULL;
    image1->SetScan(scan1);
    scene1.InsertImage(image1);

    // Copy image channels?
    // XXX
  }
}



///////////////////////////////////////////////////////////////////////
// DISPLAY FUNCTIONS
////////////////////////////////////////////////////////////////////////

void R3SurfelScene::
Draw(RNFlags flags) const
{
  // Draw all objects
  for (int i = 0; i < NObjects(); i++) {
    R3SurfelObject *object = Object(i);
    object->Draw(flags);
  } 
}



void R3SurfelScene::
Print(FILE *fp, const char *prefix, const char *suffix) const
{
  // Check fp
  if (!fp) fp = stdout;

  // Print scene header
  if (prefix) fprintf(fp, "%s", prefix);
  fprintf(fp, "%s", (name) ? name : "Scene");
  if (suffix) fprintf(fp, "%s", suffix);
  fprintf(fp, "\n");

  // Add indentation to prefix
  char indented_prefix[1024];
  sprintf(indented_prefix, "%s  ", prefix);

  // Print all objects
  for (int i = 0; i < NObjects(); i++) {
    R3SurfelObject *object = Object(i);
    object->Print(fp, indented_prefix, suffix);
  }
}



////////////////////////////////////////////////////////////////////////
// I/O FUNCTIONS
////////////////////////////////////////////////////////////////////////

int R3SurfelScene::
OpenFile(const char *scene_filename, 
  const char *database_filename, 
  const char *scene_rwaccess,
  const char *database_rwaccess)
{
  // Remember scene file name
  if (this->filename) free(this->filename);
  this->filename = RNStrdup(scene_filename);

  // Remember scene file access mode
  if (this->rwaccess) free(this->rwaccess);
  if (!scene_rwaccess) this->rwaccess = RNStrdup("r");
  else if (!strcmp(scene_rwaccess, "w")) this->rwaccess = RNStrdup("w");
  else if (strstr(scene_rwaccess, "+")) this->rwaccess = RNStrdup("r+");
  else this->rwaccess = RNStrdup("r"); 

  // Open database file
  if (database_filename) {
    if (tree) {
      R3SurfelDatabase *database = tree->Database();
      if (!database->OpenFile(database_filename, database_rwaccess)) {
        return 0;
      }
    }
  }

  // Read scene file
  if (strcmp(this->rwaccess, "w")) {
    if (!ReadFile(scene_filename)) {
      return 0;
    }
  }

  // Return success
  return 1;
}



int R3SurfelScene::
SyncFile(const char *output_scene_filename)
{
  // Check if nothing changed
  if (!flags[R3_SURFEL_SCENE_DIRTY_FLAG]) return 1;

  // Sync surfels
  if (tree) {
    R3SurfelDatabase *database = tree->Database();
    if (database) {
      if (!database->SyncFile()) return 0;
    }
  }

  // Write scene file
  if (output_scene_filename) {
    // Write scene to new file
    if (!WriteFile(output_scene_filename)) {
      return 0;
    }
  }
  else if (this->filename && (strcmp(this->rwaccess, "r"))) {
    // Write scene over original file
    if (!WriteFile(filename)) {
      return 0;
    }
  }

  // Return success
  return 1;
}



int R3SurfelScene::
CloseFile(const char *output_scene_filename)
{
  // Sync file
  if (!SyncFile(output_scene_filename)) return 0;

  // Close surfel database file
  if (tree) {
    R3SurfelDatabase *database = tree->Database();
    if (database) {
      if (!database->CloseFile()) return 0;
    }
  }

  // Return success
  return 1;
}



int R3SurfelScene::
ReadFile(const char *filename)
{
  // Parse input filename extension
  const char *extension;
  if (!(extension = strrchr(filename, '.'))) {
    printf("Filename %s has no extension (e.g., .ssa)\n", filename);
    return 0;
  }

  // Read file of appropriate type
  if (!strncmp(extension, ".ssa", 4)) {
    return ReadAsciiFile(filename);
  }
  else if (!strncmp(extension, ".ssx", 4)) {
    return ReadBinaryFile(filename);
  }
  else { 
    RNFail("Unable to read file %s (unrecognized extension: %s)\n", filename, extension); 
    return 0; 
  }

  // Should never get here
  return 0;
}



int R3SurfelScene::
WriteFile(const char *filename) 
{
  // Parse input filename extension
  const char *extension;
  if (!(extension = strrchr(filename, '.'))) {
    printf("Filename %s has no extension (e.g., .ssa)\n", filename);
    return 0;
  }

  // Write file of appropriate type
  if (!strncmp(extension, ".ssa", 4)) {
    return WriteAsciiFile(filename);
  }
  else if (!strncmp(extension, ".ssx", 4)) {
    return WriteBinaryFile(filename);
  }
  else if (!strncmp(extension, ".arff", 5)) {
    return WriteARFFFile(filename);
  }
  else if (!strncmp(extension, ".tqn", 4)) {
    return WriteTianqiangFile(filename);
  }
  else { 
    RNFail("Unable to write file %s (unrecognized extension: %s)\n", filename, extension); 
    return 0; 
  }

  // Should never get here
  return 0;
}



////////////////////////////////////////////////////////////////////////
// Ascii I/O FUNCTIONS
////////////////////////////////////////////////////////////////////////

static int 
ReadAsciiName(FILE *fp, char *buffer)
{
  // Read string
  if (fscanf(fp, "%s", buffer) != (unsigned int) 1) {
    RNFail("Unable to read name\n");
    return 0;
  }

  // Replace '+' with ' '
  char *bufferp = buffer;
  while (*bufferp) {
    if (*bufferp == '+') *bufferp = ' ';
    bufferp++;
  }

  // Return success
  return 1;
}



static int 
WriteAsciiName(FILE *fp, const char *name)
{
  // Copy name
  char buffer[1024];
  if (name) strncpy(buffer, name, 1023);
  else strncpy(buffer, "None", 1023);

  // Replace ' ' with '+'
  char *bufferp = buffer;
  while (*bufferp) {
    if (*bufferp == ' ') *bufferp = '+';
    bufferp++;
  }

  // Write string
  fprintf(fp, "%s", buffer);

  // Replace '+' with ' '
  bufferp = buffer;
  while (*bufferp) {
    if (*bufferp == '+') *bufferp = ' ';
    bufferp++;
  }

  // Return success
  return 1;
}



int R3SurfelScene::
ReadAsciiFile(const char *filename)
{
  // Open file
  FILE *fp;
  if (!(fp = fopen(filename, "r"))) {
    RNFail("Unable to open file %s\n", filename);
    return 0;
  }

  // Read header
  char buffer[1024], version[1024];
  if (fscanf(fp, "%s%s", buffer, version) != (unsigned int) 2) {
    RNFail("Unable to read scene file %s\n", filename);
    return 0;
  }

  // Check header
  if (strcmp(buffer, "SSA") || strcmp(version, "1.0")) {
    RNFail("Wrong header line in Scene file %s\n", filename);
    return 0;
  }

  // Read scene header
  int nnodes;
  int nobjects, nlabels; 
  int nobject_properties, nlabel_properties;
  int nobject_relationships, nlabel_relationships;
  int nassignments, nfeatures, nscans, nimages, dummy;
  ReadAsciiName(fp, buffer);
  if (strcmp(buffer, "None")) SetName(buffer);
  fscanf(fp, "%d%d%d%d%d%d%d%d%d%d%d", &nnodes, &nobjects, &nlabels, &nfeatures, <commit_msg>sfl 1.1<commit_after>/* Source file for the R3 surfel scene class */



////////////////////////////////////////////////////////////////////////
// INCLUDE FILES
////////////////////////////////////////////////////////////////////////

#include "R3Surfels.h"



////////////////////////////////////////////////////////////////////////
// Namespace
////////////////////////////////////////////////////////////////////////

namespace gaps {



////////////////////////////////////////////////////////////////////////
// PRIVATE FLAGS
////////////////////////////////////////////////////////////////////////

#define R3_SURFEL_SCENE_DIRTY_FLAG                  0x01



////////////////////////////////////////////////////////////////////////
// CONSTRUCTORS/DESTRUCTORS
////////////////////////////////////////////////////////////////////////

R3SurfelScene::
R3SurfelScene(const char *name)
  : tree(NULL),
    objects(),
    labels(),
    object_properties(),
    label_properties(),
    object_relationships(),
    label_relationships(),
    assignments(),
    scans(),
    images(),
    features(),
    transformation(R4Matrix(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1), 0),
    filename(NULL),
    rwaccess(NULL),
    name((name) ? RNStrdup(name) : NULL),
    flags(R3_SURFEL_SCENE_DIRTY_FLAG)
{
  // Create tree
  tree = new R3SurfelTree();
  tree->scene = this;

  // Create root object
  R3SurfelObject *object = new R3SurfelObject("Root");
  InsertObject(object, NULL);

  // Create root label
  R3SurfelLabel *label = new R3SurfelLabel("Root");
  InsertLabel(label, NULL);
}



R3SurfelScene::
R3SurfelScene(const R3SurfelScene& scene)
{
  // Copy everything
  RNAbort("Not implemented");
}



R3SurfelScene::
~R3SurfelScene(void)
{
  // Delete everything
  while (NFeatures() > 0) delete Feature(NFeatures()-1);
  while (NLabelAssignments() > 0) delete LabelAssignment(NLabelAssignments()-1);
  while (NObjectRelationships() > 0) delete ObjectRelationship(NObjectRelationships()-1);
  while (NLabelRelationships() > 0) delete LabelRelationship(NLabelRelationships()-1);
  while (NObjectProperties() > 0) delete ObjectProperty(NObjectProperties()-1);
  while (NLabelProperties() > 0) delete LabelProperty(NLabelProperties()-1);
  while (NLabels() > 0) delete Label(NLabels()-1);
  while (NObjects() > 0) delete Object(NObjects()-1);
  while (NScans() > 0) delete Scan(NScans()-1);
  while (NImages() > 0) delete Image(NImages()-1);
  
  // Delete tree
  if (tree) delete tree;
  
  // Delete filename
  if (filename) free(filename);

  // Delete rwaccess
  if (rwaccess) free(rwaccess);

  // Delete name
  if (name) free(name);
}



////////////////////////////////////////////////////////////////////////
// ACCESS FUNCTIONS
////////////////////////////////////////////////////////////////////////

R3SurfelObject *R3SurfelScene::
FindObjectByName(const char *object_name) const
{
  // Search for object
  for (int i = 0; i < NObjects(); i++) {
    R3SurfelObject *object = Object(i);
    if (!object->Name()) continue;
    if (strcmp(object->Name(), object_name)) continue;
    return object;
  }

  // Not found
  return NULL;
}



R3SurfelObject *R3SurfelScene::
FindObjectByIdentifier(int identifier) const
{
  // Search for object
  for (int i = 0; i < NObjects(); i++) {
    R3SurfelObject *object = Object(i);
    if (object->Identifier() < 0) continue;
    if (object->Identifier() != identifier) continue;
    return object;
  }

  // Not found
  return NULL;
}



R3SurfelLabel *R3SurfelScene::
FindLabelByName(const char *label_name) const
{
  // Search for label
  for (int i = 0; i < NLabels(); i++) {
    R3SurfelLabel *label = Label(i);
    if (!label->Name()) continue;
    if (strcmp(label->Name(), label_name)) continue;
    return label;
  }

  // Not found
  return NULL;
}



R3SurfelLabel *R3SurfelScene::
FindLabelByIdentifier(int identifier) const
{
  // Search for label
  for (int i = 0; i < NLabels(); i++) {
    R3SurfelLabel *label = Label(i);
    if (label->Identifier() < 0) continue;
    if (label->Identifier() != identifier) continue;
    return label;
  }

  // Not found
  return NULL;
}



R3SurfelLabel *R3SurfelScene::
FindLabelByAssignmentKeystroke(int key) const
{
  // Search for label
  for (int i = 0; i < NLabels(); i++) {
    R3SurfelLabel *label = Label(i);
    if (label->AssignmentKeystroke() < 0) continue;
    if (label->AssignmentKeystroke() != key) continue;
    return label;
  }

  // Not found
  return NULL;
}



R3SurfelLabelAssignment *R3SurfelScene::
FindLabelAssignment(R3SurfelObject *object, R3SurfelLabel *label, RNScalar confidence, int originator) const
{
  // Search for label assignment
  for (int i = 0; i < object->NLabelAssignments(); i++) {
    R3SurfelLabelAssignment *assignment = object->LabelAssignment(i);
    if (assignment->Label() != label) continue;
    if (assignment->Confidence() != confidence) continue;
    if (assignment->Originator() != originator) continue;
    return assignment;
  }

  // Not found
  return NULL;
}



R3SurfelScan *R3SurfelScene::
FindScanByName(const char *scan_name) const
{
  // Search for scan
  for (int i = 0; i < NScans(); i++) {
    R3SurfelScan *scan = Scan(i);
    if (!scan->Name()) continue;
    if (strcmp(scan->Name(), scan_name)) continue;
    return scan;
  }

  // Not found
  return NULL;
}



R3SurfelImage *R3SurfelScene::
FindImageByName(const char *image_name) const
{
  // Search for image
  for (int i = 0; i < NImages(); i++) {
    R3SurfelImage *image = Image(i);
    if (!image->Name()) continue;
    if (strcmp(image->Name(), image_name)) continue;
    return image;
  }

  // Not found
  return NULL;
}



R3SurfelFeature *R3SurfelScene::
FindFeatureByName(const char *feature_name) const
{
  // Search for feature
  for (int i = 0; i < NFeatures(); i++) {
    R3SurfelFeature *feature = Feature(i);
    if (!feature->Name()) continue;
    if (strcmp(feature->Name(), feature_name)) continue;
    return feature;
  }

  // Not found
  return NULL;
}



////////////////////////////////////////////////////////////////////////
// PROPERTY MANIPULATION FUNCTIONS
////////////////////////////////////////////////////////////////////////

void R3SurfelScene::
SetName(const char *name)
{
  // Delete previous name
  if (this->name) free(this->name);

  // Set new name
  this->name = (name) ? RNStrdup(name) : NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
SetTransformation(const R3Affine& transformation, RNBoolean update_surfels)
{
  // Set transformation (does not change surfels)
  this->transformation = transformation;
}



void R3SurfelScene::
Transform(const R3Affine& transformation, RNBoolean update_surfels)
{
  // Check if should update surfels
  if (update_surfels) {
    // Compute total transformation
    R3Affine t = R3identity_affine;
    t.Transform(transformation);
    t.Transform(this->transformation);
    this->transformation = R3identity_affine;
    
    // Transform all nodes                                                                                                     
    R3SurfelTree *tree = Tree();
    for (int i = 0; i < tree->NNodes(); i++) {
      R3SurfelNode *node = tree->Node(i);
      node->Transform(t);
    }

    // Transform all scan poses                                                                                                
    for (int i = 0; i < NScans(); i++) {
      R3SurfelScan *scan = Scan(i);
      R3CoordSystem pose = scan->Pose();
      pose.Transform(t);
      scan->SetPose(pose);
    }

    // Transform all image poses                                                                                               
    for (int i = 0; i < NImages(); i++) {
      R3SurfelImage *image = Image(i);
      R3CoordSystem pose = image->Pose();
      pose.Transform(t);
      image->SetPose(pose);
    }
  }
  else {
    // Update transformation only
    R3Affine t = R3identity_affine;
    t.Transform(transformation);
    t.Transform(this->transformation);
    this->transformation = t;
  }
}



////////////////////////////////////////////////////////////////////////
// STRUCTURE MANIPULATION FUNCTIONS
////////////////////////////////////////////////////////////////////////

void R3SurfelScene::
InsertObject(R3SurfelObject *object, R3SurfelObject *parent)
{
  // Just checking
  assert(object);
  assert(object->scene == NULL);
  assert(object->scene_index == -1);

  // Insert object into scene
  object->scene = this;
  object->scene_index = objects.NEntries();
  objects.Insert(object);

  // Insert object into parent
  object->parent = parent;
  if (parent) parent->parts.Insert(object);

  // Update object
  object->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
MergeObject(R3SurfelObject *dst_object, R3SurfelObject *src_object)
{
  // Just checking
  assert(dst_object && src_object);
  assert(dst_object->scene == this);
  assert(dst_object->scene_index >= 0);
  assert(src_object->scene == this);
  assert(src_object->scene_index >= 0);

  // Move all nodes from src_object into dst_object
  while (src_object->NNodes() > 0) {
    R3SurfelNode *node = src_object->Node(src_object->NNodes()-1);
    src_object->RemoveNode(node);
    dst_object->InsertNode(node);
  }

  // Move all parts from src_object into dst_object
  while (src_object->NParts() > 0) {
    R3SurfelObject *part = src_object->Part(src_object->NParts()-1);
    RemoveObject(part);
    InsertObject(part, dst_object);
  }

  // Move all object properties from src_object into dst_object
  while (src_object->NObjectProperties() > 0) {
    R3SurfelObjectProperty *property = src_object->ObjectProperty(src_object->NObjectProperties()-1);
    src_object->UpdateBeforeRemoveObjectProperty(property);
    property->object = dst_object;
    dst_object->UpdateAfterInsertObjectProperty(property);
  }

  // Move all object relationships from src_object into dst_object
  while (src_object->NObjectRelationships() > 0) {
    R3SurfelObjectRelationship *relationship = src_object->ObjectRelationship(src_object->NObjectRelationships()-1);
    src_object->UpdateBeforeRemoveObjectRelationship(relationship);
    for (int i = 0; i < relationship->objects.NEntries(); i++) {
      if (relationship->objects.Kth(i) == src_object) {
        RNArrayEntry *entry = relationship->objects.KthEntry(i);
        relationship->objects.EntryContents(entry) = dst_object;
        break;
      }
    }
    dst_object->UpdateAfterInsertObjectRelationship(relationship);
  }

  // Move all label assignments from src_object into dst_object
  while (src_object->NLabelAssignments() > 0) {
    R3SurfelLabelAssignment *assignment = src_object->LabelAssignment(src_object->NLabelAssignments()-1);
    src_object->UpdateBeforeRemoveLabelAssignment(assignment);
    assignment->object = dst_object;
    dst_object->UpdateAfterInsertLabelAssignment(assignment);
  }

  // Remove src_object
  RemoveObject(src_object);

  // Delete src_object
  delete src_object;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveObject(R3SurfelObject *object)
{
  // Just checking
  assert(object);
  assert(object->scene == this);
  assert(object->scene_index >= 0);
  assert(objects.Kth(object->scene_index) == object);

  // Remove object propertys from scene
  while (object->NObjectProperties() > 0) {
    R3SurfelObjectProperty *property = object->ObjectProperty(object->NObjectProperties()-1);
    RemoveObjectProperty(property);
  }

  // Remove object relationships from scene
  while (object->NObjectRelationships() > 0) {
    R3SurfelObjectRelationship *relationship = object->ObjectRelationship(object->NObjectRelationships()-1);
    RemoveObjectRelationship(relationship);
  }

  // Remove label assignments
  while (object->NLabelAssignments() > 0) {
    R3SurfelLabelAssignment *assignment = object->LabelAssignment(object->NLabelAssignments()-1);
    RemoveLabelAssignment(assignment);
  }

  // Update object
  object->UpdateBeforeRemove(this);

  // Remove object from parent
  if (object->parent) {
    object->parent->parts.Remove(object);
    object->parent = NULL;
  }

  // Remove object from parts
  for (int i = 0; i < object->NParts(); i++) {
    R3SurfelObject *part = object->Part(i);
    part->parent = NULL;
  }

  // Remove parts from object
  object->parts.Empty();

  // Remove object from scene
  RNArrayEntry *entry = objects.KthEntry(object->scene_index);
  R3SurfelObject *tail = objects.Tail();
  tail->scene_index = object->scene_index;
  objects.EntryContents(entry) = tail;
  objects.RemoveTail();
  object->scene_index = -1;
  object->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertLabel(R3SurfelLabel *label, R3SurfelLabel *parent)
{
  // Just checking
  assert(label);
  assert(label->scene == NULL);
  assert(label->scene_index == -1);

  // Insert label 
  label->scene = this;
  label->scene_index = labels.NEntries();
  labels.Insert(label);

  // Insert label into parent
  label->parent = parent;
  if (parent) parent->parts.Insert(label);

  // Update label
  label->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveLabel(R3SurfelLabel *label)
{
  // Just checking
  assert(label);
  assert(label->scene == this);
  assert(label->scene_index >= 0);
  assert(labels.Kth(label->scene_index) == label);

  // Remove label properties from scene
  while (label->NLabelProperties() > 0) {
    R3SurfelLabelProperty *property = label->LabelProperty(label->NLabelProperties()-1);
    RemoveLabelProperty(property);
  }

  // Remove label relationships from scene
  while (label->NLabelRelationships() > 0) {
    R3SurfelLabelRelationship *relationship = label->LabelRelationship(label->NLabelRelationships()-1);
    RemoveLabelRelationship(relationship);
  }

  // Remove label assignments
  while (label->NLabelAssignments() > 0) {
    R3SurfelLabelAssignment *assignment = label->LabelAssignment(label->NLabelAssignments()-1);
    RemoveLabelAssignment(assignment);
  }

  // Update label
  label->UpdateBeforeRemove(this);

  // Remove label from parent
  if (label->parent) {
    label->parent->parts.Remove(label);
    label->parent = NULL;
  }

  // Remove label from parts
  for (int i = 0; i < label->NParts(); i++) {
    R3SurfelLabel *part = label->Part(i);
    part->parent = NULL;
  }

  // Remove parts from label
  label->parts.Empty();

  // Remove label from scene
  RNArrayEntry *entry = labels.KthEntry(label->scene_index);
  R3SurfelLabel *tail = labels.Tail();
  tail->scene_index = label->scene_index;
  labels.EntryContents(entry) = tail;
  labels.RemoveTail();
  label->scene_index = -1;
  label->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertObjectProperty(R3SurfelObjectProperty *property)
{
  // Just checking
  assert(property);
  assert(property->scene == NULL);
  assert(property->scene_index == -1);

  // Insert property into scene
  property->scene = this;
  property->scene_index = object_properties.NEntries();
  object_properties.Insert(property);

  // Update object
  R3SurfelObject *object = property->Object();
  object->UpdateAfterInsertObjectProperty(property);

  // Update object property
  property->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveObjectProperty(R3SurfelObjectProperty *property)
{
  // Just checking
  assert(property);
  assert(property->scene == this);
  assert(property->scene_index >= 0);
  assert(object_properties.Kth(property->scene_index) == property);

  // Update object property
  property->UpdateBeforeRemove(this);

  // Update object
  R3SurfelObject *object = property->Object();
  object->UpdateBeforeRemoveObjectProperty(property);

  // Remove property from scene
  RNArrayEntry *entry = object_properties.KthEntry(property->scene_index);
  R3SurfelObjectProperty *tail = object_properties.Tail();
  tail->scene_index = property->scene_index;
  object_properties.EntryContents(entry) = tail;
  object_properties.RemoveTail();
  property->scene_index = -1;
  property->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertLabelProperty(R3SurfelLabelProperty *property)
{
  // Just checking
  assert(property);
  assert(property->scene == this);
  assert(property->scene_index == -1);

  // Insert property into scene
  property->scene = this;
  property->scene_index = label_properties.NEntries();
  label_properties.Insert(property);

  // Update label
  R3SurfelLabel *label = property->Label();
  label->UpdateAfterInsertLabelProperty(property);

  // Update label property
  property->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveLabelProperty(R3SurfelLabelProperty *property)
{
  // Just checking
  assert(property);
  assert(property->scene == this);
  assert(property->scene_index >= 0);
  assert(label_properties.Kth(property->scene_index) == property);

  // Update label property
  property->UpdateAfterInsert(this);

  // Update label
  R3SurfelLabel *label = property->Label();
  label->UpdateBeforeRemoveLabelProperty(property);

  // Remove property from scene
  RNArrayEntry *entry = label_properties.KthEntry(property->scene_index);
  R3SurfelLabelProperty *tail = label_properties.Tail();
  tail->scene_index = property->scene_index;
  label_properties.EntryContents(entry) = tail;
  label_properties.RemoveTail();
  property->scene_index = -1;
  property->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertObjectRelationship(R3SurfelObjectRelationship *relationship)
{
  // Just checking
  assert(relationship);
  assert(relationship->scene == NULL);
  assert(relationship->scene_index == -1);

  // Insert relationship into scene
  relationship->scene = this;
  relationship->scene_index = object_relationships.NEntries();
  object_relationships.Insert(relationship);

  // Update objects
  for (int i = 0; i < relationship->NObjects(); i++) {
    R3SurfelObject *object = relationship->Object(i);
    object->UpdateAfterInsertObjectRelationship(relationship);
  }

  // Update object relationship
  relationship->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveObjectRelationship(R3SurfelObjectRelationship *relationship)
{
  // Just checking
  assert(relationship);
  assert(relationship->scene == this);
  assert(relationship->scene_index >= 0);
  assert(object_relationships.Kth(relationship->scene_index) == relationship);

  // Update object relationship
  relationship->UpdateBeforeRemove(this);

  // Update objects
  for (int i = 0; i < relationship->NObjects(); i++) {
    R3SurfelObject *object = relationship->Object(i);
    object->UpdateBeforeRemoveObjectRelationship(relationship);
  }

  // Remove relationship from scene
  RNArrayEntry *entry = object_relationships.KthEntry(relationship->scene_index);
  R3SurfelObjectRelationship *tail = object_relationships.Tail();
  tail->scene_index = relationship->scene_index;
  object_relationships.EntryContents(entry) = tail;
  object_relationships.RemoveTail();
  relationship->scene_index = -1;
  relationship->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertLabelRelationship(R3SurfelLabelRelationship *relationship)
{
  // Just checking
  assert(relationship);
  assert(relationship->scene == this);
  assert(relationship->scene_index == -1);

  // Insert relationship into scene
  relationship->scene = this;
  relationship->scene_index = label_relationships.NEntries();
  label_relationships.Insert(relationship);

  // Update labels
  for (int i = 0; i < relationship->NLabels(); i++) {
    R3SurfelLabel *label = relationship->Label(i);
    label->UpdateAfterInsertLabelRelationship(relationship);
  }

  // Update label relationship
  relationship->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveLabelRelationship(R3SurfelLabelRelationship *relationship)
{
  // Just checking
  assert(relationship);
  assert(relationship->scene == this);
  assert(relationship->scene_index >= 0);
  assert(label_relationships.Kth(relationship->scene_index) == relationship);

  // Update label relationship
  relationship->UpdateAfterInsert(this);

  // Update labels
  for (int i = 0; i < relationship->NLabels(); i++) {
    R3SurfelLabel *label = relationship->Label(i);
    label->UpdateBeforeRemoveLabelRelationship(relationship);
  }

  // Remove relationship from scene
  RNArrayEntry *entry = label_relationships.KthEntry(relationship->scene_index);
  R3SurfelLabelRelationship *tail = label_relationships.Tail();
  tail->scene_index = relationship->scene_index;
  label_relationships.EntryContents(entry) = tail;
  label_relationships.RemoveTail();
  relationship->scene_index = -1;
  relationship->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertLabelAssignment(R3SurfelLabelAssignment *assignment)
{
  // Just checking
  assert(assignment);
  assert(assignment->scene == NULL);
  assert(assignment->scene_index == -1);
  assert(assignment->Object());
  assert(assignment->Object()->Scene() == this);
  assert(assignment->Label());
  assert(assignment->Label()->Scene() == this);

  // Insert assignment into scene
  assignment->scene = this;
  assignment->scene_index = assignments.NEntries();
  assignments.Insert(assignment);

  // Update object and label
  assignment->Object()->UpdateAfterInsertLabelAssignment(assignment);
  assignment->Label()->UpdateAfterInsertLabelAssignment(assignment);

  // Update assignment
  assignment->UpdateAfterInsert(this);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveLabelAssignment(R3SurfelLabelAssignment *assignment)
{
  // Just checking
  assert(assignment);
  assert(assignment->scene == this);
  assert(assignment->scene_index >= 0);
  assert(assignment->Object());
  assert(assignment->Object()->Scene() == this);
  assert(assignment->Label());
  assert(assignment->Label()->Scene() == this);

  // Update assignment
  assignment->UpdateBeforeRemove(this);

  // Update object and label
  assignment->Object()->UpdateBeforeRemoveLabelAssignment(assignment);
  assignment->Label()->UpdateBeforeRemoveLabelAssignment(assignment);

  // Remove assignment from scene
  assert(assignment->scene == this);
  RNArrayEntry *entry = assignments.KthEntry(assignment->scene_index);
  R3SurfelLabelAssignment *tail = assignments.Tail();
  tail->scene_index = assignment->scene_index;
  assignments.EntryContents(entry) = tail;
  assignments.RemoveTail();
  assignment->scene_index = -1;
  assignment->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertScan(R3SurfelScan *scan)
{
  // Just checking
  assert(scan);

  // Insert label 
  scan->scene = this;
  scan->scene_index = scans.NEntries();
  scans.Insert(scan);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveScan(R3SurfelScan *scan)
{
  // Just checking
  assert(scan);

  // Remove scan from scene
  RNArrayEntry *entry = scans.KthEntry(scan->scene_index);
  R3SurfelScan *tail = scans.Tail();
  tail->scene_index = scan->scene_index;
  scans.EntryContents(entry) = tail;
  scans.RemoveTail();
  scan->scene_index = -1;
  scan->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertImage(R3SurfelImage *image)
{
  // Just checking
  assert(image);

  // Insert label 
  image->scene = this;
  image->scene_index = images.NEntries();
  images.Insert(image);

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveImage(R3SurfelImage *image)
{
  // Just checking
  assert(image);

  // Remove image from scene
  RNArrayEntry *entry = images.KthEntry(image->scene_index);
  R3SurfelImage *tail = images.Tail();
  tail->scene_index = image->scene_index;
  images.EntryContents(entry) = tail;
  images.RemoveTail();
  image->scene_index = -1;
  image->scene = NULL;

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertFeature(R3SurfelFeature *feature)
{
  // Just checking
  assert(feature);

  // Insert label 
  feature->scene = this;
  feature->scene_index = features.NEntries();
  features.Insert(feature);

  // Update all object feature vectors
  for (int i = 0; i < NObjects(); i++) {
    R3SurfelObject *object = Object(i);
    if (object->feature_vector.NValues() > 0) {
      object->feature_vector.Resize(NFeatures());
      feature->UpdateFeatureVector(object, object->feature_vector);
    }
  }

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
RemoveFeature(R3SurfelFeature *feature)
{
  // Just checking
  assert(feature);
  assert(feature->scene == this);
  assert(feature->scene_index >= 0);
  assert(feature->scene_index < NFeatures());

  // Remove feature from scene
  RNArrayEntry *entry = features.KthEntry(feature->scene_index);
  R3SurfelFeature *tail = features.Tail();
  tail->scene_index = feature->scene_index;
  features.EntryContents(entry) = tail;
  features.RemoveTail();
  feature->scene_index = -1;
  feature->scene = NULL;

  // Update all object feature vectors
  if (feature != tail) {
    for (int i = 0; i < NObjects(); i++) {
      R3SurfelObject *object = Object(i);
      R3SurfelFeatureVector& vector = object->feature_vector;
      if (vector.NValues() == 0) continue;
      assert(vector.NValues() == NFeatures()+1);
      RNScalar tail_value = vector.Value(vector.NValues()-1);
      object->feature_vector.SetValue(tail->scene_index, tail_value);
      object->feature_vector.Resize(NFeatures());
    }
  }

  // Mark scene as dirty
  flags.Add(R3_SURFEL_SCENE_DIRTY_FLAG);
}



void R3SurfelScene::
InsertScene(const R3SurfelScene& scene2, 
  R3SurfelObject *parent_object1, 
  R3SurfelLabel *parent_label1,
  R3SurfelNode *parent_node1)
{
  // Get convenient variables
  R3SurfelScene& scene1 = *this;
  R3SurfelTree *tree1 = scene1.Tree();
  R3SurfelTree *tree2 = scene2.Tree();
  R3SurfelDatabase *database1 = tree1->Database();
  R3SurfelDatabase *database2 = tree2->Database();

  // Find parents in scene1
  if (!parent_node1) parent_node1 = tree1->RootNode();
  if (!parent_object1) parent_object1 = scene1.RootObject();
  if (!parent_label1) parent_label1 = scene1.RootLabel();

 // COPY FEATURES

#if 0
  // Copy label features from scene2
  for (int i = 0; i < scene2.NFeatures(); i++) {
    R3SurfelFeature *feature2 = scene2.Feature(i);
    R3SurfelFeature *feature1 = new R3SurfelFeature(feature2->Name(), feature2->Minimum(), feature2->Maximum());
    scene1.InsertFeature(feature1);
  }
#endif

  // COPY NODES

  // Create surfel tree nodes in scene1
  RNArray<R3SurfelNode *> nodes1;
  nodes1.Insert(tree1->RootNode());
  for (int i = 1; i < tree2->NNodes(); i++) {
    R3SurfelNode *node1 = new R3SurfelNode();
    nodes1.Insert(node1);
  }

  // Copy node stuff from scene2
  for (int i = 1; i < tree2->NNodes(); i++) {
    R3SurfelNode *node1 = nodes1.Kth(i);
    R3SurfelNode *node2 = tree2->Node(i);
    R3SurfelNode *parent2 = node2->Parent();
    R3SurfelNode *parent1 = parent_node1;
    if (parent2 && (!parent2->Name() || strcmp(parent2->Name(), "Root")))
      parent1 = nodes1.Kth(parent2->TreeIndex());
    tree1->InsertNode(node1, parent1);
    node1->SetName(node2->Name());
    for (int j = 0; j < node2->NBlocks(); j++) {
      R3SurfelBlock *block2 = node2->Block(j);
      database2->ReadBlock(block2);
      R3SurfelBlock *block1 = new R3SurfelBlock(*block2);
      database1->InsertBlock(block1);
      node1->InsertBlock(block1);
      database1->ReleaseBlock(block1);
      database2->ReleaseBlock(block2);
    }      
  }

  // COPY OBJECTS

  // Create objects in scene1
  RNArray<R3SurfelObject *> objects1;
  objects1.Insert(scene1.RootObject());
  for (int i = 1; i < scene2.NObjects(); i++) {
    R3SurfelObject *object1 = new R3SurfelObject();
    objects1.Insert(object1);
  }

  // Copy object stuff from scene2
  for (int i = 1; i < scene2.NObjects(); i++) {
    R3SurfelObject *object1 = objects1.Kth(i);
    R3SurfelObject *object2 = scene2.Object(i);
    R3SurfelObject *parent2 = object2->Parent();
    R3SurfelObject *parent1 = parent_object1;
    if (parent2 && (!parent2->Name() || strcmp(parent2->Name(), "Root")))
      parent1 = objects1.Kth(parent2->SceneIndex());
    scene1.InsertObject(object1, parent1);
    object1->SetName(object2->Name());
    object1->SetIdentifier(object2->Identifier());
    object1->SetFeatureVector(object2->feature_vector);
    for (int j = 0; j < object2->NNodes(); j++) {
      R3SurfelNode *node2 = object2->Node(j);
      R3SurfelNode *node1 = nodes1.Kth(node2->TreeIndex());
      object1->InsertNode(node1);
    }
  }

  // COPY LABELS 

  // Create labels in scene1
  RNArray<R3SurfelLabel *> labels1;
  labels1.Insert(scene1.RootLabel());
  for (int i = 1; i < scene2.NLabels(); i++) {
    R3SurfelLabel *label1 = new R3SurfelLabel();
    labels1.Insert(label1);
  }

  // Copy label stuff from scene2
  for (int i = 1; i < scene2.NLabels(); i++) {
    R3SurfelLabel *label1 = labels1.Kth(i);
    R3SurfelLabel *label2 = scene2.Label(i);
    R3SurfelLabel *parent2 = label2->Parent();
    R3SurfelLabel *parent1 = parent_label1;
    if (parent2 && (!parent2->Name() || strcmp(parent2->Name(), "Root")))
      parent1 = labels1.Kth(parent2->SceneIndex());
    label1->SetName(label2->Name());
    label1->SetIdentifier(label2->Identifier());
    label1->SetAssignmentKeystroke(label2->AssignmentKeystroke());
    label1->SetColor(label2->Color());
    scene1.InsertLabel(label1, parent1);
  }

  // COPY OBJECT PROPERTIES

  // Copy object properties from scene2
  for (int i = 0; i < scene2.NObjectProperties(); i++) {
    R3SurfelObjectProperty *property2 = scene2.ObjectProperty(i);
    int type = property2->Type();
    RNArray<R3SurfelObject *> objects1;
    R3SurfelObject *object2 = property2->Object();
    R3SurfelObject *object1 = objects1.Kth(object2->SceneIndex());
    objects1.Insert(object1);
    int noperands1 = 0;
    RNScalar *operands1 = NULL;
    if (property2->NOperands() > 0) {
      noperands1 = property2->NOperands();
      operands1 = new RNScalar [ noperands1 ];
      for (int j = 0; j < noperands1; j++) {
        RNScalar operand2 = property2->Operand(j);
        operands1[j] = operand2;
      }
    }
    R3SurfelObjectProperty *property1 = new R3SurfelObjectProperty(type, object1, operands1, noperands1);
    scene1.InsertObjectProperty(property1);
    delete [] operands1;
  }

  // COPY LABEL PROPERTIES

  // Copy label properties from scene2
  for (int i = 0; i < scene2.NLabelProperties(); i++) {
    R3SurfelLabelProperty *property2 = scene2.LabelProperty(i);
    int type = property2->Type();
    RNArray<R3SurfelLabel *> labels1;
    R3SurfelLabel *label2 = property2->Label();
    R3SurfelLabel *label1 = labels1.Kth(label2->SceneIndex());
    labels1.Insert(label1);
    int noperands1 = 0;
    RNScalar *operands1 = NULL;
    if (property2->NOperands() > 0) {
      noperands1 = property2->NOperands();
      operands1 = new RNScalar [ noperands1 ];
      for (int j = 0; j < noperands1; j++) {
        RNScalar operand2 = property2->Operand(j);
        operands1[j] = operand2;
      }
    }
    R3SurfelLabelProperty *property1 = new R3SurfelLabelProperty(type, label1, operands1, noperands1);
    scene1.InsertLabelProperty(property1);
    delete [] operands1;
  }

  // COPY OBJECT RELATIONSHIPS

  // Copy object relationships from scene2
  for (int i = 0; i < scene2.NObjectRelationships(); i++) {
    R3SurfelObjectRelationship *relationship2 = scene2.ObjectRelationship(i);
    int type = relationship2->Type();
    RNArray<R3SurfelObject *> objects1;
    for (int j = 0; j < relationship2->NObjects(); j++) {
      R3SurfelObject *object2 = relationship2->Object(j);
      R3SurfelObject *object1 = objects1.Kth(object2->SceneIndex());
      objects1.Insert(object1);
    }
    int noperands1 = 0;
    RNScalar *operands1 = NULL;
    if (relationship2->NOperands() > 0) {
      noperands1 = relationship2->NOperands();
      operands1 = new RNScalar [ noperands1 ];
      for (int j = 0; j < noperands1; j++) {
        RNScalar operand2 = relationship2->Operand(j);
        operands1[j] = operand2;
      }
    }
    R3SurfelObjectRelationship *relationship1 = new R3SurfelObjectRelationship(type, objects1, operands1, noperands1);
    scene1.InsertObjectRelationship(relationship1);
    delete [] operands1;
  }

  // COPY LABEL RELATIONSHIPS

  // Copy label relationships from scene2
  for (int i = 0; i < scene2.NLabelRelationships(); i++) {
    R3SurfelLabelRelationship *relationship2 = scene2.LabelRelationship(i);
    int type = relationship2->Type();
    RNArray<R3SurfelLabel *> labels1;
    for (int j = 0; j < relationship2->NLabels(); j++) {
      R3SurfelLabel *label2 = relationship2->Label(j);
      R3SurfelLabel *label1 = labels1.Kth(label2->SceneIndex());
      labels1.Insert(label1);
    }
    int noperands1 = 0;
    RNScalar *operands1 = NULL;
    if (relationship2->NOperands() > 0) {
      noperands1 = relationship2->NOperands();
      operands1 = new RNScalar [ noperands1 ];
      for (int j = 0; j < noperands1; j++) {
        RNScalar operand2 = relationship2->Operand(j);
        operands1[j] = operand2;
      }
    }
    R3SurfelLabelRelationship *relationship1 = new R3SurfelLabelRelationship(type, labels1, operands1);
    scene1.InsertLabelRelationship(relationship1);
    delete [] operands1;
  }

  // COPY ASSIGNMENTS

  // Copy label assignments from scene2
  for (int i = 0; i < scene2.NLabelAssignments(); i++) {
    R3SurfelLabelAssignment *assignment2 = scene2.LabelAssignment(i);
    R3SurfelObject *object2 = assignment2->Object();
    R3SurfelLabel *label2 = assignment2->Label();
    R3SurfelObject *object1 = objects1.Kth(object2->SceneIndex());
    R3SurfelLabel *label1 = labels1.Kth(label2->SceneIndex());
    R3SurfelLabelAssignment *assignment1 = new R3SurfelLabelAssignment(object1, label1, assignment2->Confidence(), assignment2->Originator());
    scene1.InsertLabelAssignment(assignment1);
  }

  // COPY SCANS

  // Copy scans from scene2
  RNArray<R3SurfelScan *> scans1;
  for (int i = 0; i < scene2.NScans(); i++) {
    R3SurfelScan *scan2 = scene2.Scan(i);
    R3SurfelScan *scan1 = new R3SurfelScan(scan2->Name());
    scan1->SetPose(scan2->Pose());
    scan1->SetTimestamp(scan2->Timestamp());
    scan1->SetXFocal(scan2->XFocal());
    scan1->SetYFocal(scan2->YFocal());
    scan1->SetImageDimensions(scan2->ImageWidth(), scan2->ImageHeight());
    scan1->SetImageCenter(scan2->ImageCenter());
    scan1->SetFlags(scan2->Flags());
    R3SurfelNode *node2 = scan2->Node();
    R3SurfelNode *node1 = nodes1.Kth(node2->TreeIndex());
    scan1->SetNode(node1);
    scene1.InsertScan(scan1);
    scans1.Insert(scan1);
  }

  // COPY IMAGES
  
  // Copy images from scene2
  for (int i = 0; i < scene2.NImages(); i++) {
    R3SurfelImage *image2 = scene2.Image(i);
    R3SurfelImage *image1 = new R3SurfelImage(image2->Name());
    image1->SetPose(image2->Pose());
    image1->SetTimestamp(image2->Timestamp());
    image1->SetXFocal(image2->XFocal());
    image1->SetYFocal(image2->YFocal());
    image1->SetImageDimensions(image2->ImageWidth(), image2->ImageHeight());
    image1->SetImageCenter(image2->ImageCenter());
    image1->SetFlags(image2->Flags());
    R3SurfelScan *scan2 = image2->Scan();
    R3SurfelScan *scan1 = (scan2) ? scans1.Kth(scan2->SceneIndex()) : NULL;
    image1->SetScan(scan1);
    scene1.InsertImage(image1);

    // Copy image channels?
    // XXX
  }
}



///////////////////////////////////////////////////////////////////////
// DISPLAY FUNCTIONS
////////////////////////////////////////////////////////////////////////

void R3SurfelScene::
Draw(RNFlags flags) const
{
  // Draw all objects
  for (int i = 0; i < NObjects(); i++) {
    R3SurfelObject *object = Object(i);
    object->Draw(flags);
  } 
}



void R3SurfelScene::
Print(FILE *fp, const char *prefix, const char *suffix) const
{
  // Check fp
  if (!fp) fp = stdout;

  // Print scene header
  if (prefix) fprintf(fp, "%s", prefix);
  fprintf(fp, "%s", (name) ? name : "Scene");
  if (suffix) fprintf(fp, "%s", suffix);
  fprintf(fp, "\n");

  // Add indentation to prefix
  char indented_prefix[1024];
  sprintf(indented_prefix, "%s  ", prefix);

  // Print all objects
  for (int i = 0; i < NObjects(); i++) {
    R3SurfelObject *object = Object(i);
    object->Print(fp, indented_prefix, suffix);
  }
}



////////////////////////////////////////////////////////////////////////
// I/O FUNCTIONS
////////////////////////////////////////////////////////////////////////

int R3SurfelScene::
OpenFile(const char *scene_filename, 
  const char *database_filename, 
  const char *scene_rwaccess,
  const char *database_rwaccess)
{
  // Remember scene file name
  if (this->filename) free(this->filename);
  this->filename = RNStrdup(scene_filename);

  // Remember scene file access mode
  if (this->rwaccess) free(this->rwaccess);
  if (!scene_rwaccess) this->rwaccess = RNStrdup("r");
  else if (!strcmp(scene_rwaccess, "w")) this->rwaccess = RNStrdup("w");
  else if (strstr(scene_rwaccess, "+")) this->rwaccess = RNStrdup("r+");
  else this->rwaccess = RNStrdup("r"); 

  // Open database file
  if (database_filename) {
    if (tree) {
      R3SurfelDatabase *database = tree->Database();
      if (!database->OpenFile(database_filename, database_rwaccess)) {
        return 0;
      }
    }
  }

  // Read scene file
  if (strcmp(this->rwaccess, "w")) {
    if (!ReadFile(scene_filename)) {
      return 0;
    }
  }

  // Return success
  return 1;
}



int R3SurfelScene::
SyncFile(const char *output_scene_filename)
{
  // Check if nothing changed
  if (!flags[R3_SURFEL_SCENE_DIRTY_FLAG]) return 1;

  // Sync surfels
  if (tree) {
    R3SurfelDatabase *database = tree->Database();
    if (database) {
      if (!database->SyncFile()) return 0;
    }
  }

  // Write scene file
  if (output_scene_filename) {
    // Write scene to new file
    if (!WriteFile(output_scene_filename)) {
      return 0;
    }
  }
  else if (this->filename && (strcmp(this->rwaccess, "r"))) {
    // Write scene over original file
    if (!WriteFile(filename)) {
      return 0;
    }
  }

  // Return success
  return 1;
}



int R3SurfelScene::
CloseFile(const char *output_scene_filename)
{
  // Sync file
  if (!SyncFile(output_scene_filename)) return 0;

  // Close surfel database file
  if (tree) {
    R3SurfelDatabase *database = tree->Database();
    if (database) {
      if (!database->CloseFile()) return 0;
    }
  }

  // Return success
  return 1;
}



int R3SurfelScene::
ReadFile(const char *filename)
{
  // Parse input filename extension
  const char *extension;
  if (!(extension = strrchr(filename, '.'))) {
    printf("Filename %s has no extension (e.g., .ssa)\n", filename);
    return 0;
  }

  // Read file of appropriate type
  if (!strncmp(extension, ".ssa", 4)) {
    return ReadAsciiFile(filename);
  }
  else if (!strncmp(extension, ".ssx", 4)) {
    return ReadBinaryFile(filename);
  }
  else { 
    RNFail("Unable to read file %s (unrecognized extension: %s)\n", filename, extension); 
    return 0; 
  }

  // Should never get here
  return 0;
}



int R3SurfelScene::
WriteFile(const char *filename) 
{
  // Parse input filename extension
  const char *extension;
  if (!(extension = strrchr(filename, '.'))) {
    printf("Filename %s has no extension (e.g., .ssa)\n", filename);
    return 0;
  }

  // Write file of appropriate type
  if (!strncmp(extension, ".ssa", 4)) {
    return WriteAsciiFile(filename);
  }
  else if (!strncmp(extension, ".ssx", 4)) {
    return WriteBinaryFile(filename);
  }
  else if (!strncmp(extension, ".arff", 5)) {
    return WriteARFFFile(filename);
  }
  else if (!strncmp(extension, ".tqn", 4)) {
    return WriteTianqiangFile(filename);
  }
  else { 
    RNFail("Unable to write file %s (unrecognized extension: %s)\n", filename, extension); 
    return 0; 
  }

  // Should never get here
  return 0;
}



////////////////////////////////////////////////////////////////////////
// Ascii I/O FUNCTIONS
////////////////////////////////////////////////////////////////////////

static int 
ReadAsciiName(FILE *fp, char *buffer)
{
  // Read string
  if (fscanf(fp, "%s", buffer) != (unsigned int) 1) {
    RNFail("Unable to read name\n");
    return 0;
  }

  // Replace '+' with ' '
  char *bufferp = buffer;
  while (*bufferp) {
    if (*bufferp == '+') *bufferp = ' ';
    bufferp++;
  }

  // Return success
  return 1;
}



static int 
WriteAsciiName(FILE *fp, const char *name)
{
  // Copy name
  char buffer[1024];
  if (name) strncpy(buffer, name, 1023);
  else strncpy(buffer, "None", 1023);

  // Replace ' ' with '+'
  char *bufferp = buffer;
  while (*bufferp) {
    if (*bufferp == ' ') *bufferp = '+';
    bufferp++;
  }

  // Write string
  fprintf(fp, "%s", buffer);

  // Replace '+' with ' '
  bufferp = buffer;
  while (*bufferp) {
    if (*bufferp == '+') *bufferp = ' ';
    bufferp++;
  }

  // Return success
  return 1;
}



int R3SurfelScene::
ReadAsciiFile(const char *filename)
{
  // Open file
  FILE *fp;
  if (!(fp = fopen(filename, "r"))) {
    RNFail("Unable to open file %s\n", filename);
    return 0;
  }

  // Read header
  char buffer[1024], version[1024];
  if (fscanf(fp, "%s%s", buffer, version) != (unsigned int) 2) {
    RNFail("Unable to read scene file %s\n", filename);
    return 0;
  }

  // Check header
  if (strcmp(buffer, "SSA")) {
    RNFail("Wrong header line in Scene file %s\n", filename);
    return 0;
  }

  // Read scene header
  int nnodes;
  int nobjects, nlabels; 
  int nobject_properties, nlabel_properties;
  int nobject_relationships, nlabel_relationships;
  int nassignments, nfeatures, nscans, nimages, dummy;
  ReadAsciiName(fp, buffer);
  if (strcmp(buffer, "None")) SetName(buffer);
  fscanf(fp, "%d%d%d%d%d%d%d%d%d%d%d", &nnodes, &nobjects, &nlabels, &nfeatures, 
{'fraction_non_alphanumeric': 0.16576316530554774, 'fraction_numerical': 0.019525016427297474, 'mean_word_length': 3.19972404277337, 'pattern_counts': {'":': 0, '<': 117, '<?xml version=': 0, '>': 927, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 24, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4310836', 'n_tokens_mistral': 28034, 'n_tokens_neox': 27096, 'n_words': 5815}